Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(tier4_perception_launch): add centerpoint_vs_centerpoint-tiny launch file #2304

Merged
merged 10 commits into from
Dec 1, 2022
Original file line number Diff line number Diff line change
@@ -0,0 +1,140 @@
# Run lidar_centerpoint and lidar_centerpoint-tiny simultaneously

This tutorial is for showing `centerpoint` and `centerpoint_tiny`models’ results simultaneously, making it easier to visualize and compare the performance.

## Setup Development Environment

Follow the steps in the Source Installation ([link](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/)) in Autoware doc.

If you fail to build autoware environment according to lack of memory, then it is recommended to build autoware sequentially.

Source the ROS 2 Galactic setup script.

```bash
source /opt/ros/galactic/setup.bash
```

Build the entire autoware repository.

```bash
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers=1
```

Or you can use a constrained number of CPU to build only one package.

```bash
export MAKEFLAGS="-j 4" && MAKE_JOBS=4 colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 1 --packages-select PACKAGE_NAME
```

Source the package.

```bash
source install/setup.bash
```

## Data Preparation

### Using rosbag dataset

```bash
ros2 bag play /YOUR/ROSBAG/PATH/ --clock 100
```

Don't forget to add `clock` inorder to sync between two rviz display.

You can also use the sample rosbag provided by autoware [here](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/rosbag-replay-simulation/).

If you want to merge several rosbags into one, you can refer to [this tool](https://github.com/jerry73204/rosbag2-merge).

### Using realtime LiDAR dataset

Set up your Ethernet connection according to 1.1 - 1.3 in [this website](http://wiki.ros.org/velodyne/Tutorials/Getting%20Started%20with%20the%20Velodyne%20VLP16).

Download Velodyne ROS driver

```bash
git clone -b ros2 https://github.com/ros-drivers/velodyne.git
```

Source the ROS 2 Galactic setup script.

```bash
source /opt/ros/galactic/setup.bash
```

Compile Velodyne driver

```bash
cd velodyne
rosdep install -y --from-paths . --ignore-src --rosdistro $ROS_DISTRO
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
```

Edit the configuration file. Specify the LiDAR device IP address in `./velodyne_driver/config/VLP32C-velodyne_driver_node-params.yaml`

```yaml
velodyne_driver_node:
ros__parameters:
device_ip: 192.168.1.201 //change to your LiDAR device IP address
gps_time: false
time_offset: 0.0
enabled: true
read_once: false
read_fast: false
repeat_delay: 0.0
frame_id: velodyne
model: 32C
rpm: 600.0
port: 2368
```

Launch the velodyne driver.

```bash
# Terminal 1
ros2 launch velodyne_driver velodyne_driver_node-VLP32C-launch.py
```

Launch the velodyne_pointcloud.

```bash
# Terminal 2
ros2 launch velodyne_pointcloud velodyne_convert_node-VLP32C-launch.py
```

Point Cloud data will be available on topic `/velodyne_points`. You can check with `ros2 topic echo /velodyne_points`.

Check [this website](http://wiki.ros.org/velodyne/Tutorials/Getting%20Started%20with%20the%20Velodyne%20VLP16) if there is any unexpected issue.

## Launch file setting

Several fields to check in `centerpoint_vs_centerpoint-tiny.launch.xml` before running lidar centerpoint.

- `input/pointcloud` : set to the topic with input data you want to subscribe.
- `model_path` : set to the path of the model.
- `model_param_path` : set to the path of model's config file.

## Run CenterPoint and CenterPoint-tiny simultaneously

Run

```bash
ros2 launch lidar_centerpoint centerpoint_vs_centerpoint-tiny.launch.xml
```

Then you will see two rviz window show immediately. On the left is the result for lidar centerpoint tiny, and on the right is the result for lidar centerpoint.

![two rviz2 display centerpoint and centerpoint_tiny](https://i.imgur.com/YAYehrf.jpg)

## Troubleshooting

### Bounding Box blink on rviz

To avoid Bounding Boxs blinking on rviz, you can extend bbox marker lifetime.

Set `marker_ptr->lifetime` and `marker.lifetime` to a longer lifetime.

- `marker_ptr->lifetime` are in `PATH/autoware/src/universe/autoware.universe/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp`
- `marker.lifetime` are in `PATH/autoware/src/universe/autoware.universe/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp`

Make sure to rebuild packages after any change.
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
<?xml version="1.0"?>
<launch>
<arg name="rviz_path" default="$(find-pkg-share lidar_centerpoint)/launch/centerpoint_vs_centerpoint-tiny/rviz"/>
<arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
<arg name="score_threshold" default="0.35"/>
<arg name="rviz_config1" default="$(var rviz_path)/centerpoint_tiny.rviz" description="rviz config"/>
<arg name="rviz_config2" default="$(var rviz_path)/centerpoint.rviz" description="rviz config"/>

<set_parameter name="use_sim_time" value="true"/>
<group>
<!-- Rviz -->
<node pkg="rviz2" exec="rviz2" name="rviz2_centerpoint_tiny" output="screen" args="-d $(var rviz_config1)"/>
<node pkg="rviz2" exec="rviz2" name="rviz2_centerpoint" output="screen" args="-d $(var rviz_config2)"/>
</group>

<!-- CenterPoint -->
<group>
<push-ros-namespace namespace="centerpoint"/>
<group>
<include file="$(find-pkg-share lidar_centerpoint)/launch/lidar_centerpoint.launch.xml">
<arg name="input/pointcloud" value="$(var input/pointcloud)"/>
<arg name="score_threshold" value="$(var score_threshold)"/>
<arg name="model_name" value="centerpoint"/>
<arg name="model_path" value="$(find-pkg-share lidar_centerpoint)/data"/>
<arg name="model_param_path" value="$(find-pkg-share lidar_centerpoint)/config/$(var model_name).param.yaml"/>
</include>
</group>
</group>

<!-- CenterPoint-tiny -->
<group>
<push-ros-namespace namespace="centerpoint_tiny"/>
<group>
<include file="$(find-pkg-share lidar_centerpoint)/launch/lidar_centerpoint.launch.xml">
<arg name="input/pointcloud" value="$(var input/pointcloud)"/>
<arg name="score_threshold" value="$(var score_threshold)"/>
<arg name="model_name" value="centerpoint_tiny"/>
<arg name="model_path" value="$(find-pkg-share lidar_centerpoint)/data"/>
<arg name="model_param_path" value="$(find-pkg-share lidar_centerpoint)/config/$(var model_name).param.yaml"/>
</include>
</group>
</group>
</launch>
Loading