This repository is archived

YabLoc was merged into Autoware on June 22, 2023. For more details, please refer to this PR. Future development will be conducted on autoware.universe.

If you would like to perform a standalone verification of YabLoc, the "Quick Start Demo" provided in this README is useful. If you are interested in running YabLoc as a localization method in Autoware, the sample data provided in this PR can be handy.

YabLoc

Overview

YabLoc is vision-baed localization with vector map. https://youtu.be/Eaf6r_BNFfk

Yabloc has been developed as a new localization stack for Autoware.

Installation

Prerequisite

supporting Ubuntu 22.04 + ROS2 humble now.

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. The following submodules will be removed at the time.

Branches

main is a branch to work with minimal dependence on autoware.
- If you want to try the YabLoc demo, please use main branch.

Submodules

How to build

mkdir yabloc_ws/src -p
cd yabloc_ws
git clone git@github.com:tier4/YabLoc.git src/YabLoc --recursive
rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
sudo apt-get -y install ccache ninja-build
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

The author often use this build command

colcon build --symlink-install --cmake-args -GNinja -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_C_COMPILER_LAUNCHER=ccache -DCMAKE_CXX_COMPILER_LAUNCHER=ccache --continue-on-error

(optional) ccache (--cmake-args) -DCMAKE_C_COMPILER_LAUNCHER=ccache -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
(optional) clang-tidy (--cmake-args) -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
(optional) test (--cmake-args) -DBUILD_TESTING=ON

Quick Start Demo

sample rosbag: Google Drive link

# terminal 1
source install/setup.bash
ros2 launch yabloc_launch sample.launch.xml

# terminal 2
source install/setup.bash
ros2 launch yabloc_launch rviz.launch.xml

# terminal 3
source install/setup.bash
ros2 bag play awsim_yabloc_rosbag_sample_0.db3 -s sqlite3

If YabLoc runs successfully, you will see a screen like the following.

How to set initial pose

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

2D Pose Estimate in Rviz

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

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 runs `standalone:=false` (through Autoware's pose_initializer)

UNDER CONSTRUCTION

Architecture

click to more detail

Input topics

from sesnors

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`

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

click to open

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.

ros2 run tf2_ros tf2_echo base_link traffic_light_left_camera/camera_optical_link

If the wrong /tf_static are broadcasted due to using a prototype vehicle, not having accurate calibration data, or some other unavoidable reason, 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.

ros2 launch yabloc_launch sample_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 topics about pose

topic name	msg type	description
`/localicazation/pf/pose`	`geometry_msgs/msg/PoseStamped`	estimated pose
`/localicazation/pose_estimator/pose_with_covariance`	`geometry_msgs/msg/PoseStamped`	estimated pose with covariance

Output topics for visualization

This project contains original rviz plugins. rviz2_overlay_plugins

index	topic name	description
1	`/localicazation/imgproc/lanelet2_overlay_image`	Projection of lanelet2 (yellow lines) onto image based on estimated pose. If they match well with the actual road markings, it means that the localization performs well.
2	`/localicazation/imgproc/segmented_image`	result of graph-based segmetation. yellow area is identified as the road surface.
3	`/localicazation/pf/cost_map_image`	cost map generated from lanelet2.
4	`/localicazation/imgproc/image_with_line_segments`	detected line segments
5	`/localicazation/map/ground_status`	ground height and tilt estimatation status
6	`/localicazation/twist/kalman/status`	twist estimation status
7	`/localicazation/pf/predicted_particle_marker`	particle distribution of particle fitler (red means a probable candidate)
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

License

YabLoc is licensed under Apache License 2.0.

elkuno213/YabLoc