Skip to content

Commit

Permalink
fix(calibration): Update calibration related launch to use the new tool
Browse files Browse the repository at this point in the history 
Signed-off-by: Manato HIRABAYASHI <[email protected]>
  • Loading branch information
@manato
manato committed Sep 10, 2024
1 parent 5b4593d commit 5572599
Show file tree
Hide file tree
Showing 4 changed files with 86 additions and 114 deletions.
59 changes: 20 additions & 39 deletions edge_auto_launch/launch/calibration_extrinsic_at128_sample.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,17 +6,12 @@
<arg name="live_sensor" default="True"/>

<!-- variables -->
<let name="lidar_model" value="at128"/>
<let name="lidar_frame" value="lidar"/>
<let name="compressed_image_topic" value="/sensing/camera/$(var camera_name)/image_raw/compressed"/>
<let name="camera_info_topic" value="/sensing/camera/$(var camera_name)/camera_info"/>
<let name="pointcloud_topic" value="/sensing/lidar/at128/aw_points"/>
<let name="camera_frame" value="$(var camera_name)/camera_link"/>

<let name="rviz_profile" value=""/>
<let name="rviz_profile" value="$(find-pkg-share edge_auto_launch)/config/visualization/calibration_extrinsic_at128_camera0.rviz" if="$(eval &quot;'$(var camera_name)' == 'camera0' &quot;)"/>
<let name="rviz_profile" value="$(find-pkg-share edge_auto_launch)/config/visualization/calibration_extrinsic_at128_camera1.rviz" if="$(eval &quot;'$(var camera_name)' == 'camera1' &quot;)"/>

<!-- driver -->
<group>
<push-ros-namespace namespace="/sensing/lidar/at128"/>
Expand All @@ -29,42 +24,28 @@

<!-- calibrator -->
<group>
<push-ros-namespace namespace="$(var lidar_frame)/$(var camera_frame)"/>
<!-- image_decompressor -->
<node pkg="autoware_image_transport_decompressor" exec="image_transport_decompressor_node" name="decompressor" output="screen">
<remap from="decompressor/input/compressed_image" to="$(var compressed_image_topic)"/>
<remap from="decompressor/output/raw_image" to="image_raw"/>
</node>
<!-- tag_based_calibrator -->
<include file="$(find-pkg-share edge_auto_launch)/launch/component/extrinsic_tag_based_calibrator.launch.xml">
<arg name="lidar_model" value="$(var lidar_model)"/>
<arg name="parent_frame" value="$(var lidar_frame)"/>
<arg name="child_frame" value="$(var camera_frame)"/>
<arg name="camera_info_topic" value="$(var camera_info_topic)"/>
<arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
<arg name="image_topic" value="image_raw"/>
</include>
<!-- interactive -->
<node pkg="extrinsic_interactive_calibrator" exec="interactive_calibrator" name="interactive_calibrator" output="screen">
<remap from="pointcloud" to="$(var pointcloud_topic)"/>
<remap from="image" to="$(var compressed_image_topic)"/>
<remap from="camera_info" to="$(var camera_info_topic)"/>
<remap from="calibration_points_input" to="calibration_points"/>
<param name="camera_parent_frame" value="$(var lidar_frame)"/>
<param name="camera_frame" value="$(var camera_frame)"/>
<param name="use_calibration_api" value="false"/>
<param name="can_publish_tf" value="false"/>
</node>
<include file="$(find-pkg-share intrinsic_camera_calibration)/launch/optimizer.launch.xml"/>
<node pkg="sensor_calibration_manager" exec="sensor_calibration_manager" name="sensor_calibration_manager"/>
<node pkg="topic_tools" exec="relay" name="pointcloud_relay" args="$(var pointcloud_topic) /aw_points"/>
</group>

<!-- comment: this tf broadcast should be done elsewhere-->
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id base_link --child-frame-id lidar" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id camera0/camera_link --child-frame-id camera0/camera_optical_link" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id camera1/camera_link --child-frame-id camera1/camera_optical_link" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.0 --qy 0.0 --qz 0.0 --qw 1.0 --frame-id lidar --child-frame-id camera0/camera_link" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.0 --qy 0.0 --qz 0.0 --qw 1.0 --frame-id lidar --child-frame-id camera1/camera_link" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id base_link --child-frame-id lidar">
<param name="use_sim_time" value="true"/>
</node>
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id camera0/camera_link --child-frame-id camera0/camera_optical_link">
<param name="use_sim_time" value="true"/>
</node>
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id camera1/camera_link --child-frame-id camera1/camera_optical_link">
<param name="use_sim_time" value="true"/>
</node>
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id base_link --child-frame-id camera0/camera_link">
<param name="use_sim_time" value="true"/>
</node>
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id base_link --child-frame-id camera1/camera_link">
<param name="use_sim_time" value="true"/>
</node>
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id base_link --child-frame-id lidar">
<param name="use_sim_time" value="true"/>
</node>

<!-- visualization -->
<node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_profile)"/>
</launch>
59 changes: 20 additions & 39 deletions edge_auto_launch/launch/calibration_extrinsic_xt32_sample.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,17 +5,12 @@
<arg name="live_sensor" default="True"/>

<!-- variables -->
<let name="lidar_model" value="pandar_xt32"/>
<let name="lidar_frame" value="lidar"/>
<let name="compressed_image_topic" value="/sensing/camera/$(var camera_name)/image_raw/compressed"/>
<let name="camera_info_topic" value="/sensing/camera/$(var camera_name)/camera_info"/>
<let name="pointcloud_topic" value="/sensing/lidar/xt32/aw_points"/>
<let name="camera_frame" value="$(var camera_name)/camera_link"/>

<let name="rviz_profile" value=""/>
<let name="rviz_profile" value="$(find-pkg-share edge_auto_launch)/config/visualization/calibration_extrinsic_at128_camera0.rviz" if="$(eval &quot;'$(var camera_name)' == 'camera0' &quot;)"/>
<let name="rviz_profile" value="$(find-pkg-share edge_auto_launch)/config/visualization/calibration_extrinsic_at128_camera1.rviz" if="$(eval &quot;'$(var camera_name)' == 'camera1' &quot;)"/>

<!-- driver -->
<group>
<push-ros-namespace namespace="/sensing/lidar/xt32"/>
Expand All @@ -27,42 +22,28 @@

<!-- calibrator -->
<group>
<push-ros-namespace namespace="$(var lidar_frame)/$(var camera_frame)"/>
<!-- image_decompressor -->
<node pkg="autoware_image_transport_decompressor" exec="image_transport_decompressor_node" name="decompressor" output="screen">
<remap from="decompressor/input/compressed_image" to="$(var compressed_image_topic)"/>
<remap from="decompressor/output/raw_image" to="image_raw"/>
</node>
<!-- tag_based_calibrator -->
<include file="$(find-pkg-share edge_auto_launch)/launch/component/extrinsic_tag_based_calibrator.launch.xml">
<arg name="lidar_model" value="$(var lidar_model)"/>
<arg name="parent_frame" value="$(var lidar_frame)"/>
<arg name="child_frame" value="$(var camera_frame)"/>
<arg name="camera_info_topic" value="$(var camera_info_topic)"/>
<arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
<arg name="image_topic" value="image_raw"/>
</include>
<!-- interactive_calibrator -->
<node pkg="extrinsic_interactive_calibrator" exec="interactive_calibrator" name="interactive_calibrator" output="screen">
<remap from="pointcloud" to="$(var pointcloud_topic)"/>
<remap from="image" to="$(var compressed_image_topic)"/>
<remap from="camera_info" to="$(var camera_info_topic)"/>
<remap from="calibration_points_input" to="calibration_points"/>
<param name="camera_parent_frame" value="$(var lidar_frame)"/>
<param name="camera_frame" value="$(var camera_frame)"/>
<param name="use_calibration_api" value="false"/>
<param name="can_publish_tf" value="false"/>
</node>
<include file="$(find-pkg-share intrinsic_camera_calibration)/launch/optimizer.launch.xml"/>
<node pkg="sensor_calibration_manager" exec="sensor_calibration_manager" name="sensor_calibration_manager"/>
<node pkg="topic_tools" exec="relay" name="pointcloud_relay" args="$(var pointcloud_topic) /aw_points"/>
</group>

<!-- comment: this tf broadcast should be done elsewhere-->
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id base_link --child-frame-id lidar" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id camera0/camera_link --child-frame-id camera0/camera_optical_link" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id camera1/camera_link --child-frame-id camera1/camera_optical_link" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.0 --qy 0.0 --qz 0.0 --qw 1.0 --frame-id lidar --child-frame-id camera0/camera_link" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.0 --qy 0.0 --qz 0.0 --qw 1.0 --frame-id lidar --child-frame-id camera1/camera_link" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id base_link --child-frame-id lidar">
<!-- <param name="use_sim_time" value="true"/> -->
</node>
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id camera0/camera_link --child-frame-id camera0/camera_optical_link">
<!-- <param name="use_sim_time" value="true"/> -->
</node>
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id camera1/camera_link --child-frame-id camera1/camera_optical_link">
<!-- <param name="use_sim_time" value="true"/> -->
</node>
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id base_link --child-frame-id camera0/camera_link">
<!-- <param name="use_sim_time" value="true"/> -->
</node>
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id base_link --child-frame-id camera1/camera_link">
<!-- <param name="use_sim_time" value="true"/> -->
</node>
<node pkg="tf2_ros" exec="static_transform_publisher" name="estimated_base_link_broadcaster" output="screen" args="--x 0.0 --y 0.0 --z 0.0 --qx 0.5 --qy -0.5 --qz 0.5 --qw -0.5 --frame-id base_link --child-frame-id lidar">
<!-- <param name="use_sim_time" value="true"/> -->
</node>

<!-- visualization -->
<node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_profile)"/>
</launch>
31 changes: 0 additions & 31 deletions edge_auto_launch/launch/component/extrinsic_manual_calibrator.launch.xml
View file

This file was deleted.

51 changes: 46 additions & 5 deletions edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,17 +19,59 @@
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
import json
import yaml
import re
from pathlib import Path
import transforms3d

def parse_yaml(contents: dict):
get_first_key = lambda d: list(d.keys())[0]
frame_id = get_first_key(contents)
child_frame_id = get_first_key(contents[frame_id])

x = contents[frame_id][child_frame_id]['x']
y = contents[frame_id][child_frame_id]['y']
z = contents[frame_id][child_frame_id]['z']
roll = contents[frame_id][child_frame_id]['roll']
pitch = contents[frame_id][child_frame_id]['pitch']
yaw = contents[frame_id][child_frame_id]['yaw']

q_wxyz = transforms3d.euler.euler2quat(roll, pitch, yaw) # output format is wxyz

# pack contents into a similar format as json one
return {
'header': {'frame_id': frame_id},
'child_frame_id': child_frame_id,
'transform': {
'translation': {'x': x, 'y': y, 'z': z},
'rotation': {'w': q_wxyz[0], 'x': q_wxyz[1], 'y': q_wxyz[2], 'z': q_wxyz[3]}
}
}


def launch_setup(context, *args, **kwargs):
"""Return Launch Configuration for the TF between the lidar and the camera."""
tf_file = LaunchConfiguration('tf_file_path').perform(context)
with open(tf_file, 'r') as f:
tf_data = json.load(f)
file_format = Path(tf_file).suffix.lstrip('.')

'''
Here assumes json/yaml format generated by CalibrationTools
'''
if file_format == 'json':
with open(tf_file, 'r') as f:
tf_data = json.load(f)
elif file_format == 'yaml' or file_format == 'yml':
with open(tf_file, 'r') as f:
yaml_contents = yaml.safe_load(f)
tf_data = parse_yaml(yaml_contents)
else:
print(f'Invalid TF file ({tf_file}) was specified.')

camera_id = re.findall(r'\d+', str(tf_data['child_frame_id']))[0]

return [
Node(
name="lidar_camera_tf_publisher",
name=f'lidar_camera_tf_publisher{camera_id}',
package='tf2_ros',
executable='static_transform_publisher',
arguments=[
Expand All @@ -44,7 +86,7 @@ def launch_setup(context, *args, **kwargs):
'--child-frame-id', str(tf_data['child_frame_id'])
]),
Node(
name="camera_optical_link_publisher",
name=f'camera_optical_link_publisher{camera_id}',
package='tf2_ros',
executable='static_transform_publisher',
arguments=[
Expand All @@ -61,7 +103,6 @@ def launch_setup(context, *args, **kwargs):
])
]


def generate_launch_description():
return LaunchDescription(
[
Expand Down

0 comments on commit 5572599

Please sign in to comment.