kosuke55 / YabLoc

Open source visual localization for self-driving vehicles

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

• external/autoware_auto_msgs
• external/autoware_msgs
• external/septentrio_gnss_driver
• external/tier4_autoware_msgs

How to build

mkdir yabloc_ws/src -p
cd yabloc_ws
git clone [e-mail]:tier4/YabLoc.git src/YabLoc --recursive
rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
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 -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)

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 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.