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.

Sign up for GitHub

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(x2_gen2): add x2_gen2 project #204

Draft
wants to merge 7 commits into
base: tier4/universe
Choose a base branch
from
Draft

feat(x2_gen2): add x2_gen2 project #204

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
9286046
ground_plane_calibrator
muraki-t4 Oct 21, 2024
93f3e4b
mapping_based_lidar_lidar_calibrator
muraki-t4 Oct 21, 2024
f02fae5
tag_based_pnp_calibrator
muraki-t4 Oct 21, 2024
2dbdd26
marker_radar_lidar_calibrator
muraki-t4 Oct 21, 2024
5e67e95
tag_based_sfm_base_lidar_calibrator
muraki-t4 Oct 21, 2024
802f157
register calibrators
muraki-t4 Oct 21, 2024
87fd6b1
ci(pre-commit): autofix
pre-commit-ci[bot] Oct 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
54 changes: 54 additions & 0 deletions sensor_calibration_manager/launch/x2_gen2/ground_plane_calibrator.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,54 @@
<launch>
<let name="base_frame" value="base_link"/>
<let name="lidar_frame" value="left_upper/lidar"/>
<let name="pointcloud_topic" value="/sensing/lidar/left_upper/pointcloud_raw_ex"/>

<arg name="max_inlier_distance" default="0.03" description="Maximum distance in meters to consider inliers in plane estimation"/>
<arg name="min_plane_points" default="500" description="Minimum number of points for an hypothesis for be considered valid"/>
<arg name="min_plane_points_percentage" default="20.0" description="Minimum percentage of the total points to consider an hypothesis valid"/>
<arg name="max_cos_distance" default="0.2" description="Maximum cosine distance between an hypothesis and the initial calibration for an hypothesis to be considered valid"/>
<arg name="max_iterations" default="500" description="Number of maximum iterations in the plane estimation algorithm"/>
<arg name="verbose" default="true" description="Verbose mode"/>
<arg name="overwrite_xy_yaw" default="false" description="Overwrite x, y, and yaw values with the initial calibration"/>
<arg name="filter_estimations" default="true" description="Flag to enable filtering estimations"/>

<arg name="use_crop_box_filter" default="true" description="Use an optional crop box filter to accelerate processing and ignore walls or other planes"/>
<arg name="crop_box_min_x" default="-12.0" description="Minimum x value of the crop box filter"/>
<arg name="crop_box_min_y" default="-12.0" description="Minimum y value of the crop box filter"/>
<arg name="crop_box_min_z" default="-12.0" description="Minimum z value of the crop box filter"/>
<arg name="crop_box_max_x" default="12.0" description="Maximum x value of the crop box filter"/>
<arg name="crop_box_max_y" default="12.0" description="Maximum y value of the crop box filter"/>
<arg name="crop_box_max_z" default="-0.5" description="Maximum z value of the crop box filter"/>
<arg name="use_pca_rough_normal" default="false" description="Whether to use PCA or the initial calibration for normal plane estimation"/>

<arg name="rviz" default="true" description="Launch rviz"/>

<!-- base-lidar calibrator -->
<include file="$(find-pkg-share ground_plane_calibrator)/launch/calibrator.launch.xml">
<arg name="calibration_service_name" value="calibrate_base_lidar"/>

<arg name="rviz" value="$(var rviz)"/>
<arg name="base_frame" value="$(var base_frame)"/>
<arg name="lidar_frame" value="$(var lidar_frame)"/>
<arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
<arg name="calibration_service_name" value="$(var calibration_service_name)"/>

<arg name="max_inlier_distance" value="$(var max_inlier_distance)"/>
<arg name="min_plane_points" value="$(var min_plane_points)"/>
<arg name="min_plane_points_percentage" value="$(var min_plane_points_percentage)"/>
<arg name="max_cos_distance" value="$(var max_cos_distance)"/>
<arg name="max_iterations" value="$(var max_iterations)"/>
<arg name="verbose" value="$(var verbose)"/>
<arg name="overwrite_xy_yaw" value="$(var overwrite_xy_yaw)"/>
<arg name="filter_estimations" value="$(var filter_estimations)"/>

<arg name="use_crop_box_filter" value="$(var use_crop_box_filter)"/>
<arg name="crop_box_min_x" value="$(var crop_box_min_x)"/>
<arg name="crop_box_min_y" value="$(var crop_box_min_y)"/>
<arg name="crop_box_min_z" value="$(var crop_box_min_z)"/>
<arg name="crop_box_max_x" value="$(var crop_box_max_x)"/>
<arg name="crop_box_max_y" value="$(var crop_box_max_y)"/>
<arg name="crop_box_max_z" value="$(var crop_box_max_z)"/>
<arg name="use_pca_rough_normal" value="$(var use_pca_rough_normal)"/>
</include>
</launch>
106 changes: 106 additions & 0 deletions sensor_calibration_manager/launch/x2_gen2/mapping_based_lidar_lidar_calibrator.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,106 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="base_frame" default="base_link"/>

<arg name="mapping_registrator" default="gicp" description="ndt or gicp"/>
<arg name="rviz" default="true"/>

<arg name="lost_frame_max_acceleration" default="10.0"/>
<arg name="local_map_num_keyframes" default="30"/>
<arg name="dense_pointcloud_num_keyframes" default="20"/>
<arg name="mapper_resolution" default="0.5"/>
<arg name="mapper_max_iterations" default="500"/>
<arg name="mapper_epsilon" default="0.001"/>
<arg name="mapper_max_correspondence_distance" default="0.1"/>
<arg name="lidar_calibration_max_frames" default="4"/>
<arg name="calibration_eval_max_corr_distance" default="0.2"/>
<arg name="solver_iterations" default="100"/>
<arg name="calibration_skip_keyframes" default="10"/>

<arg name="mapping_max_range" default="80.0"/>
<arg name="max_calibration_range" default="60.0"/>

<arg name="calibration_use_only_last_frames" default="false"/>
<arg name="crop_z_calibration_pointclouds" default="true"/>
<arg name="crop_z_calibration_pointclouds_value" default="4.0"/>
<arg name="calibration_min_distance_between_frames" default="7.0"/>

<let name="calibration_camera_optical_link_frames" value="['']"/>

<let
name="calibration_lidar_frames"
value="[
left_lower/lidar,
right_upper/lidar,
right_lower/lidar,
front_upper/lidar,
front_lower/lidar,
rear_upper/lidar,
rear_lower/lidar]"
/>

<let name="mapping_lidar_frame" value="left_upper/lidar"/>
<let name="mapping_pointcloud" value="/sensing/lidar/left_upper/pointcloud"/>
<let name="detected_objects" value="/perception/object_recognition/detection/objects"/>

<let name="calibration_camera_info_topics" value="['']"/>

<let name="calibration_image_topics" value="[
'']"/>

<let
name="calibration_pointcloud_topics"
value="[
/sensing/lidar/left_lower/pointcloud,
/sensing/lidar/right_upper/pointcloud,
/sensing/lidar/right_lower/pointcloud,
/sensing/lidar/front_upper/pointcloud,
/sensing/lidar/front_lower/pointcloud,
/sensing/lidar/rear_upper/pointcloud,
/sensing/lidar/rear_lower/pointcloud]"
/>

<!-- mapping based calibrator -->
<include file="$(find-pkg-share mapping_based_calibrator)/launch/calibrator.launch.xml">
<arg name="ns" value=""/>
<arg name="calibration_service_name" value="calibrate_lidar_lidar"/>

<arg name="rviz" value="$(var rviz)"/>
<arg name="base_frame" value="$(var base_frame)"/>

<arg name="calibration_camera_optical_link_frames" value="$(var calibration_camera_optical_link_frames)"/>
<arg name="calibration_lidar_frames" value="$(var calibration_lidar_frames)"/>
<arg name="mapping_lidar_frame" value="$(var mapping_lidar_frame)"/>

<arg name="mapping_pointcloud" value="$(var mapping_pointcloud)"/>
<arg name="detected_objects" value="$(var detected_objects)"/>

<arg name="calibration_camera_info_topics" value="$(var calibration_camera_info_topics)"/>
<arg name="calibration_image_topics" value="$(var calibration_image_topics)"/>
<arg name="calibration_pointcloud_topics" value="$(var calibration_pointcloud_topics)"/>

<arg name="mapping_registrator" value="$(var mapping_registrator)"/>
<arg name="lost_frame_max_acceleration" value="$(var lost_frame_max_acceleration)"/>
<arg name="calibration_skip_keyframes" value="$(var calibration_skip_keyframes)"/>

<arg name="local_map_num_keyframes" value="$(var local_map_num_keyframes)"/>
<arg name="dense_pointcloud_num_keyframes" value="$(var dense_pointcloud_num_keyframes)"/>
<arg name="mapper_resolution" value="$(var mapper_resolution)"/>
<arg name="mapper_max_iterations" value="$(var mapper_max_iterations)"/>
<arg name="mapper_epsilon" value="$(var mapper_epsilon)"/>
<arg name="mapper_max_correspondence_distance" value="$(var mapper_max_correspondence_distance)"/>
<arg name="lidar_calibration_max_frames" value="$(var lidar_calibration_max_frames)"/>
<arg name="calibration_eval_max_corr_distance" value="$(var calibration_eval_max_corr_distance)"/>
<arg name="solver_iterations" value="$(var solver_iterations)"/>
<arg name="calibration_skip_keyframes" value="$(var calibration_skip_keyframes)"/>

<arg name="mapping_max_range" value="$(var mapping_max_range)"/>
<arg name="max_calibration_range" value="$(var max_calibration_range)"/>

<arg name="calibration_use_only_last_frames" value="$(var calibration_use_only_last_frames)"/>
<arg name="crop_z_calibration_pointclouds" value="$(var crop_z_calibration_pointclouds)"/>
<arg name="crop_z_calibration_pointclouds_value" value="$(var crop_z_calibration_pointclouds_value)"/>

<arg name="calibration_min_distance_between_frames" value="$(var calibration_min_distance_between_frames)"/>
</include>
</launch>
63 changes: 63 additions & 0 deletions sensor_calibration_manager/launch/x2_gen2/marker_radar_lidar_calibrator.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="radar_name" default="front_center">
<choice value="front_left"/>
<choice value="front_center"/>
<choice value="front_right"/>
<choice value="rear_left"/>
<choice value="rear_center"/>
<choice value="rear_right"/>
</arg>

<arg name="msg_type" default="radar_tracks">
<choice value="radar_tracks"/>
<choice value="radar_scan"/>
<choice value="radar_cloud"/>
</arg>

<arg name="transformation_type" default="svd_2d">
<choice value="svd_2d"/>
<choice value="yaw_only_rotation_2d"/>
<choice value="svd_3d"/>
<choice value="zero_roll_3d"/>
</arg>

<arg name="rviz" default="true"/>

<!-- For x2 project, do not change radar_optimization_frame to base_link, if you are using radar that doesn't support elevation or z axis-->
<let name="radar_optimization_frame" value="base_link"/>
<let name="radar_parent_frame" value="base_link"/>

<let name="radar_frame" value="$(var radar_name)/radar_link"/>

<let name="lidar_frame" value="front_upper/lidar_base_link" if="$(eval &quot;'$(var radar_name)' == 'front_left' &quot;)"/>
<let name="lidar_frame" value="front_upper/lidar_base_link" if="$(eval &quot;'$(var radar_name)' == 'front_center' &quot;)"/>
<let name="lidar_frame" value="front_upper/lidar_base_link" if="$(eval &quot;'$(var radar_name)' == 'front_right' &quot;)"/>
<let name="lidar_frame" value="rear_upper/lidar_base_link" if="$(eval &quot;'$(var radar_name)' == 'rear_left' &quot;)"/>
<let name="lidar_frame" value="rear_upper/lidar_base_link" if="$(eval &quot;'$(var radar_name)' == 'rear_center' &quot;)"/>
<let name="lidar_frame" value="rear_upper/lidar_base_link" if="$(eval &quot;'$(var radar_name)' == 'rear_right' &quot;)"/>

<let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/objects_raw" if="$(eval &quot;'$(var msg_type)'=='radar_tracks'&quot;)"/>
<let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/scan_raw" if="$(eval &quot;'$(var msg_type)'=='radar_scan'&quot;)"/>
<let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/detection_points" if="$(eval &quot;'$(var msg_type)'=='radar_cloud'&quot;)"/>

<let name="input_lidar_pointcloud" value="/sensing/lidar/front_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var radar_name)' == 'front_left' &quot;)"/>
<let name="input_lidar_pointcloud" value="/sensing/lidar/front_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var radar_name)' == 'front_center' &quot;)"/>
<let name="input_lidar_pointcloud" value="/sensing/lidar/front_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var radar_name)' == 'front_right' &quot;)"/>
<let name="input_lidar_pointcloud" value="/sensing/lidar/rear_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var radar_name)' == 'rear_left' &quot;)"/>
<let name="input_lidar_pointcloud" value="/sensing/lidar/rear_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var radar_name)' == 'rear_center' &quot;)"/>
<let name="input_lidar_pointcloud" value="/sensing/lidar/rear_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var radar_name)' == 'rear_right' &quot;)"/>

<node pkg="tf2_ros" exec="static_transform_publisher" name="lidar_broadcaster" output="screen" args="0 0 0 0 0 0 $(var radar_frame) radar_frame"/>

<!-- marker radar-lidar calibrator -->
<include file="$(find-pkg-share marker_radar_lidar_calibrator)/launch/calibrator.launch.xml">
<arg name="rviz" value="$(var rviz)"/>
<arg name="calibration_service_name" value="calibrate_radar_lidar"/>
<arg name="radar_optimization_frame" value="$(var radar_optimization_frame)"/>
<arg name="input_lidar_pointcloud" value="$(var input_lidar_pointcloud)"/>
<arg name="input_radar_msg" value="$(var input_radar_msg)"/>
<arg name="msg_type" value="$(var msg_type)"/>
<arg name="transformation_type" value="$(var transformation_type)"/>
</include>
</launch>
106 changes: 106 additions & 0 deletions sensor_calibration_manager/launch/x2_gen2/tag_based_pnp_calibrator.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,106 @@
<launch>
<arg name="camera_name" default="camera0">
<choice value="camera0"/>
<choice value="camera1"/>
<choice value="camera2"/>
<choice value="camera3"/>
<choice value="camera4"/>
<choice value="camera5"/>
<choice value="camera6"/>
<choice value="camera7"/>
</arg>

<arg name="view_only_ui" default="true" description="By default we use a minimal UI"/>
<arg name="calibration_pairs" default="9" description="Number of lidar-image pairs for the calibration to converge"/>
<arg name="calibration_pairs_min_distance" default="1.5" description="Minimum allowed between a new detection and current pairs"/>
<arg name="rviz" default="true" description="Launch rviz"/>

<!-- we do not use the standard image_raw name to avoid naming conflicts -->
<let name="image_decompressed_topic" value="/sensing/camera/$(var camera_name)/image_raw/decompressed"/>
<let name="image_compressed_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/left_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var camera_name)' == 'camera5' &quot;)"/>
<let name="pointcloud_topic" value="/sensing/lidar/right_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var camera_name)' == 'camera7' &quot;)"/>
<let name="pointcloud_topic" value="/sensing/lidar/right_lower/pointcloud_raw_ex" if="$(eval &quot;'$(var camera_name)' == 'camera2' &quot;)"/>
<let name="pointcloud_topic" value="/sensing/lidar/left_lower/pointcloud_raw_ex" if="$(eval &quot;'$(var camera_name)' == 'camera1' &quot;)"/>
<let name="pointcloud_topic" value="/sensing/lidar/left_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var camera_name)' == 'camera6' &quot;)"/>
<let name="pointcloud_topic" value="/sensing/lidar/left_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var camera_name)' == 'camera4' &quot;)"/>
<let name="pointcloud_topic" value="/sensing/lidar/rear_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var camera_name)' == 'camera0' &quot;)"/>
<let name="pointcloud_topic" value="/sensing/lidar/front_upper/pointcloud_raw_ex" if="$(eval &quot;'$(var camera_name)' == 'camera3' &quot;)"/>

<let name="camera_frame_id" value="top_front_left" if="$(eval &quot;'$(var camera_name)' == 'camera5' &quot;)"/>
<let name="camera_frame_id" value="right_front" if="$(eval &quot;'$(var camera_name)' == 'camera7' &quot;)"/>
<let name="camera_frame_id" value="right_center" if="$(eval &quot;'$(var camera_name)' == 'camera2' &quot;)"/>
<let name="camera_frame_id" value="left_center" if="$(eval &quot;'$(var camera_name)' == 'camera1' &quot;)"/>
<let name="camera_frame_id" value="left_front" if="$(eval &quot;'$(var camera_name)' == 'camera6' &quot;)"/>
<let name="camera_frame_id" value="top_front_center_right" if="$(eval &quot;'$(var camera_name)' == 'camera4' &quot;)"/>
<let name="camera_frame_id" value="top_rear_center" if="$(eval &quot;'$(var camera_name)' == 'camera0' &quot;)"/>
<let name="camera_frame_id" value="top_front_right" if="$(eval &quot;'$(var camera_name)' == 'camera3' &quot;)"/>

<let name="camera_frame" value="$(var camera_frame_id)/camera_link"/>
<let name="rviz_profile" value="$(find-pkg-share tag_based_pnp_calibrator)/rviz/default_profile.rviz"/>
<let name="lidar_model" value="velodyne_vls128"/>

<let name="lidar_frame" value="left_upper/lidar" if="$(eval &quot;'$(var camera_name)' == 'camera5' &quot;)"/>
<let name="lidar_frame" value="right_upper/lidar" if="$(eval &quot;'$(var camera_name)' == 'camera7' &quot;)"/>
<let name="lidar_frame" value="right_lower/lidar" if="$(eval &quot;'$(var camera_name)' == 'camera2' &quot;)"/>
<let name="lidar_frame" value="left_lower/lidar" if="$(eval &quot;'$(var camera_name)' == 'camera1' &quot;)"/>
<let name="lidar_frame" value="left_upper/lidar" if="$(eval &quot;'$(var camera_name)' == 'camera6' &quot;)"/>
<let name="lidar_frame" value="left_upper/lidar" if="$(eval &quot;'$(var camera_name)' == 'camera4' &quot;)"/>
<let name="lidar_frame" value="rear_upper/lidar" if="$(eval &quot;'$(var camera_name)' == 'camera0' &quot;)"/>
<let name="lidar_frame" value="front_upper/lidar" if="$(eval &quot;'$(var camera_name)' == 'camera3' &quot;)"/>

<let name="use_rectified_image" value="false"/>

<group>
<!-- image decompressor -->
<node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="decompressor" output="screen">
<remap from="decompressor/input/compressed_image" to="$(var image_compressed_topic)"/>
<remap from="decompressor/output/raw_image" to="$(var image_decompressed_topic)"/>

<param from="$(find-pkg-share image_transport_decompressor)/config/image_transport_decompressor.param.yaml"/>
</node>

<!-- tag based calibrator -->
<include file="$(find-pkg-share tag_based_pnp_calibrator)/launch/calibrator.launch.xml">
<arg name="image_topic" value="$(var image_decompressed_topic)"/>
<arg name="camera_info_topic" value="$(var camera_info_topic)"/>
<arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
<arg name="pointcloud_topic_ex" value="$(var camera_info_topic)"/>
<arg name="lidar_model" value="$(var lidar_model)"/>
<arg name="calibration_service_name" value="calibrate_camera_lidar"/>

<arg name="use_rectified_image" value="$(var use_rectified_image)"/>
<arg name="calibration_pairs" value="$(var calibration_pairs)"/>
<arg name="calibration_pairs_min_distance" value="$(var calibration_pairs_min_distance)"/>
</include>

<!-- interactive calibrator -->
<node pkg="interactive_camera_lidar_calibrator" exec="interactive_calibrator" name="interactive_calibrator" output="screen" if="$(eval &quot;'$(var view_only_ui)' == 'true' &quot;)">
<remap from="pointcloud" to="$(var pointcloud_topic)"/>
<remap from="image" to="$(var image_compressed_topic)"/>
<remap from="camera_info" to="$(var camera_info_topic)"/>
<remap from="calibration_points_input" to="calibration_points"/>

<param name="camera_frame" value="$(var camera_frame)"/>
<param name="use_calibration_api" value="false"/>
<param name="can_publish_tf" value="false"/>
</node>

<!-- camera view -->
<node pkg="tier4_calibration_views" exec="image_view_node.py" name="image_view_node_py" output="screen" if="$(eval &quot;'$(var view_only_ui)' == 'false' &quot;)">
<remap from="pointcloud" to="$(var pointcloud_topic)"/>
<remap from="image" to="$(var image_compressed_topic)"/>
<remap from="camera_info" to="$(var camera_info_topic)"/>
<remap from="calibration_points_input" to="calibration_points"/>
</node>
<!-- create a placeholder lidar frame to make the rviz profile generic -->
<node pkg="tf2_ros" exec="static_transform_publisher" name="tf_broadcaster" output="screen" args="0 0 0 0 0 0 $(var lidar_frame) lidar_frame"/>

<!-- remap the pointcloud topic to make the rviz profile generic -->
<node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_profile)" if="$(var rviz)">
<remap from="pointcloud_topic_placeholder" to="$(var pointcloud_topic)"/>
</node>
</group>
</launch>
Loading
Loading