Skip to content

Commit

Permalink
add quick start demo (autowarefoundation#8)
Browse files Browse the repository at this point in the history 
Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>
  • Loading branch information
@KYabuuchi
KYabuuchi committed Jun 6, 2023
1 parent d943a76 commit 54c19fa
Show file tree
Hide file tree
Showing 4 changed files with 609 additions and 86 deletions.
182 changes: 96 additions & 86 deletions localization/yabloc/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,71 +16,14 @@ This has been developed as a new localization stack for [Autoware](https://githu
**NOTE:** Currently, this software is assumed to be built in a separate workspace in order not to contaminate the autoware workspace.
Someday this will be located in the workspace where Autoware blongs. These submodules will be removed at the time.

## Architecture

![node_diagram](docs/node_diagram.png)

### Input topics from sesnors

This localizer requires following topics to work.

| topic name | msg type | description |
| ---- | ---- | -- |
| `/sensing/imu/tamagawa/imu_raw` | `sensor_msgs/msg/Imu` | |
| `/sensing/camera/traffic_light/image_raw/compressed` | `sensor_msgs/msg/CompressedImage` | |
| `/sensing/camera/traffic_light/camera_info` | `sensor_msgs/msg/CameraInfo` | |
| `/sensing/gnss/ublox/navpvt` | `ublox_msgs/msg/NavPVT` | If you use ublox |
| `/sensing/gnss/septentrio/poscovgeodetic` | `septentrio_gnss_driver_msgs/msg/PosCovGeodetic` | If you use Septentrio |
| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs/msg/VelocityReport` | |

### Input topics from autoware

| topic name | msg type | description |
| ---- | ---- | -- |
| `/tf_static` | `tf2_msgs/msg/TFMessage` | published from `sensor_kit` |
| `/map/vector_map` | `autoware_auto_mapping_msgs/msg/HADMapBin` | published from `/map/lanelet2_map_loader` |

#### about tf_static

Some nodes requires `/tf_static` from `/base_link` to the frame_id of `/sensing/camera/traffic_light/image_raw/compressed` (e.g. `/traffic_light_left_camera/camera_optical_link`).
You can verify that the tf_static is correct with the following command.

```shell
ros2 run tf2_ros tf2_echo base_link traffic_light_left_camera/camera_optical_link
```

If the wrong `/tf_static` are broadcasted because you are using a prototype vehicle, it is useful to give the frame_id in `override_camera_frame_id`.
If you give it a non-empty string, `/imgproc/undistort_node` will rewrite the frame_id in camera_info.
For example, you can give a different tf_static as follows.

```shell
ros2 launch pcdless_launch odaiba_launch.xml override_camera_frame_id:=fake_camera_optical_link
ros2 run tf2_ros static_transform_publisher \
--frame-id base_link \
--child-frame-id fake_camera_optical_link \
--roll -1.57 \
--yaw -1.570
```

### Output

| topic name | msg type | description |
| ---- | ---- | -- |
| `/localicazation/pf/pose` | `geometry_msgs/msg/PoseStamped` | estimated pose |
| `/localicazation/validation/overlay_image` | `sensor_msgs/msg/Image` | really nice image for demonstration |
| `/localicazation/pf/cost_map_image` | `sensor_msgs/msg/Image` | visualization of cost map for debug |
| `/localicazation/pf/predicted_particles_marker` | `visualization_msgs/msg/MarkerArray` | particles of particle filter |
| `/localicazation/imgproc/lsd_image` | `sensor_msgs/msg/Image` | image |
| `/localicazation/imgproc/projected_lsd_image` | `sensor_msgs/msg/Image` | image |

## How to build

**Supporting `Ubuntu 22.04` + `ROS2 humble` now.
Some branches might support `ROS2 galactic`.**

**NOTE:** Currently, this software is assumed to be built in a separate workspace in order not to contaminate the Autoware workspace.

```bash
```shell
mkdir yabloc_ws/src -p
cd yabloc_ws
git clone git@github.com:tier4/YabLoc.git src/YabLoc --recursive
Expand All @@ -102,21 +45,31 @@ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_E

</div></details>

## Sample data
## Quick Start Demo

[Google drive link](https://drive.google.com/drive/folders/1uNxQ2uPFEGbYXUODQMc7GRO5r9c3Fj6o?usp=share_link)
ROSBAG made by AWSIM: [Google drive link](https://drive.google.com/drive/folders/1XVWfkDoz-0fncYC-_I6cmv1gkB6EfJ2Y?usp=share_link)

The link contains *rosbag* and *lanelet2* but *pointcloud*.
![how_to_launch_with_rosbag](docs/how_to_launch_quick_start_demo.drawio.svg)

```shell
ros2 launch pcdless_launch odaiba_launch.xml standalone:=true
ros2 launch pcdless_launch rviz.launch.xml
ros2 bag play awsim_yabloc_rosbag_sample
```

## How to execute
## Demo with Autoware

**NOTE:** `use_sim_time` is TRUE as default.

### Run with ROSBAG
### Run with standard ROSBAG

Sample data: [Google drive link](https://drive.google.com/drive/folders/1uNxQ2uPFEGbYXUODQMc7GRO5r9c3Fj6o?usp=share_link)

The link contains *rosbag* and *lanelet2* but *pointcloud*.

![how_to_launch_with_rosbag](docs/how_to_launch_with_rosbag.drawio.svg)

```bash
```shell
ros2 launch pcdless_launch odaiba_launch.xml standalone:=true
ros2 launch pcdless_launch rviz.launch.xml
ros2 launch autoware_launch logging_simulator.launch.xml \
Expand All @@ -139,23 +92,96 @@ ros2 bag play sample_odaiba --clock 100

![how_to_launch_with_rosbag](docs/how_to_launch_in_real.drawio.svg)

**You have to change autoware.universe branch.**

```bash
```shell
ros2 launch pcdless_launch odaiba_launch.xml standalone:=true use_sim_time:=false
ros2 launch pcdless_launch rviz.launch.xml
ros2 launch autoware_launch autoware.launch.xml \
rviz:=false
```

### Run with AWSIM (UNDER CONSTRACTION)
### Run with AWSIM <ins>(UNDER CONSTRACTION)</ins>

```bash
ros2 launch pcdless_launch odaiba_launch.xml standalone:=true
**You have to change autoware.universe branch.**

```shell
ros2 launch pcdless_launch odaiba_launch.xml standalone:=false
ros2 launch pcdless_launch rviz.launch.xml
ros2 launch autoware_launch e2e_simulator.launch.xml
```

## How to set initialpose

### 1. When YabLoc works `standalone:=true` (without Autoware's pose_initializer)

1. 2D Pose Estimate in Rviz

You can inidcate x, y and yaw manually in rviz.

2. GNSS Doppler initialization

If doppler (`ublox_msgs/msg/navpvt`) is available and the vehicle moves enough fast, YabLoc will estiamte the initial pose **automatically**.

### 2. When Yabloc works `standalone:=false` (through Autoware's pose_initializer)

<ins>UNDER CONSTRUCTION</ins>

## Architecture

![node_diagram](docs/node_diagram.png)

### Input topics from sesnors

This localizer requires following topics to work.

| topic name | msg type | description |
| ---- | ---- | -- |
| `/sensing/imu/tamagawa/imu_raw` | `sensor_msgs/msg/Imu` | |
| `/sensing/camera/traffic_light/image_raw/compressed` | `sensor_msgs/msg/CompressedImage` | |
| `/sensing/camera/traffic_light/camera_info` | `sensor_msgs/msg/CameraInfo` | |
| `/sensing/gnss/ublox/navpvt` | `ublox_msgs/msg/NavPVT` | If you use ublox |
| `/sensing/gnss/septentrio/poscovgeodetic` | `septentrio_gnss_driver_msgs/msg/PosCovGeodetic` | If you use Septentrio |
| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs/msg/VelocityReport` | |

### Input topics from autoware

| topic name | msg type | description |
| ---- | ---- | -- |
| `/tf_static` | `tf2_msgs/msg/TFMessage` | published from `sensor_kit` |
| `/map/vector_map` | `autoware_auto_mapping_msgs/msg/HADMapBin` | published from `/map/lanelet2_map_loader` |

#### about tf_static

Some nodes requires `/tf_static` from `/base_link` to the frame_id of `/sensing/camera/traffic_light/image_raw/compressed` (e.g. `/traffic_light_left_camera/camera_optical_link`).
You can verify that the tf_static is correct with the following command.

```shell
ros2 run tf2_ros tf2_echo base_link traffic_light_left_camera/camera_optical_link
```

If the wrong `/tf_static` are broadcasted because you are using a prototype vehicle, it is useful to give the frame_id in `override_camera_frame_id`.
If you give it a non-empty string, `/imgproc/undistort_node` will rewrite the frame_id in camera_info.
For example, you can give a different tf_static as follows.

```shell
ros2 launch pcdless_launch odaiba_launch.xml override_camera_frame_id:=fake_camera_optical_link
ros2 run tf2_ros static_transform_publisher \
--frame-id base_link \
--child-frame-id fake_camera_optical_link \
--roll -1.57 \
--yaw -1.570
```

### Output

| topic name | msg type | description |
| ---- | ---- | -- |
| `/localicazation/pf/pose` | `geometry_msgs/msg/PoseStamped` | estimated pose |
| `/localicazation/validation/overlay_image` | `sensor_msgs/msg/Image` | really nice image for demonstration |
| `/localicazation/pf/cost_map_image` | `sensor_msgs/msg/Image` | visualization of cost map for debug |
| `/localicazation/pf/predicted_particles_marker` | `visualization_msgs/msg/MarkerArray` | particles of particle filter |
| `/localicazation/imgproc/lsd_image` | `sensor_msgs/msg/Image` | image |
| `/localicazation/imgproc/projected_lsd_image` | `sensor_msgs/msg/Image` | image |

## Visualization

(This project contains original rviz plugins.)
Expand All @@ -174,22 +200,6 @@ ros2 launch autoware_launch e2e_simulator.launch.xml
| 8 | `/localicazation/pf/gnss/range_marker` | particle weight distribution by GNSS |
| 9 | `/localicazation/pf/scored_cloud` | 3D projected line segments. the color means the how match they are |

## How to set initialpose

### 1. When YabLoc works `standalone:=true` (without Autoware's pose_initializer)

1. 2D Pose Estimate in Rviz

You can inidcate x, y and yaw manually in rviz.

2. GNSS Doppler initialization

If doppler (`ublox_msgs/msg/navpvt`) is available and the vehicle moves enough fast, YabLoc will estiamte the initial pose **automatically**.

### 2. When Yabloc works `standalone:=false` (through Autoware's pose_initializer)

<u>UNDER CONSTRUCTION</u>

## License

YabLoc is licensed under Apache License 2.0.
41 changes: 41 additions & 0 deletions localization/yabloc/docs/awsim_image_compress.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from sensor_msgs.msg import CompressedImage
from cv_bridge import CvBridge
from rclpy.qos import QoSProfile, QoSReliabilityPolicy


class ImageCompressor(Node):
def __init__(self):
super().__init__('image_compressor')
qos = QoSProfile(
reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
depth=10
)
self.publisher_ = self.create_publisher(
CompressedImage, '/sensing/camera/traffic_light/image_raw/compressed', qos)
self.subscription = self.create_subscription(
Image, '/sensing/camera/traffic_light/image_raw', self.on_image, qos)
self.bridge_ = CvBridge()

def on_image(self, msg):
self.get_logger().info('subscribed')
mat = self.bridge_.imgmsg_to_cv2(msg)
compressed_msg = self.bridge_.cv2_to_compressed_imgmsg(mat)
compressed_msg.header = msg.header
self.publisher_.publish(compressed_msg)
self.get_logger().info('published')


def main(args=None):
rclpy.init(args=args)
minimal_publisher = ImageCompressor()
rclpy.spin(minimal_publisher)
minimal_publisher.destroy_node()
rclpy.shutdown()


if __name__ == '__main__':
main()
13 changes: 13 additions & 0 deletions localization/yabloc/docs/awsim_rosbag_record.sh
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
#!/usr/bin/bash
ros2 bag record --use-sim-time\
/clock\
/tf_static\
/map/vector_map\
/map/vector_map_marker\
/vehicle/status/velocity_status\
/sensing/imu/tamagawa/imu_raw\
/sensing/gnss/pose\
/sensing/gnss/pose_with_covariance\
/sensing/camera/traffic_light/camera_info\
/sensing/camera/traffic_light/image_raw/compressed\
/initialpose
Loading

0 comments on commit 54c19fa

Please sign in to comment.