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

Jump to bottom

fix: update edge auto #28

Merged
merged 3 commits into from
Sep 11, 2024
Merged

fix: update edge auto #28

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5572599
fix(calibration): Update calibration related launch to use the new tool
manato Sep 10, 2024
a5d7824
fix: update XT32 related perception pipeline
manato Sep 10, 2024
33cb6c5
fix: udpate packages.xml to meet dependencies
manato Sep 10, 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
16 changes: 16 additions & 0 deletions edge_auto_launch/config/roi_cluster_fusion.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
/**:
ros__parameters:
fusion_distance: 100.0
trust_object_distance: 100.0
trust_object_iou_mode: "iou"
non_trust_object_iou_mode: "iou"
use_cluster_semantic_type: false
# only_allow_inside_cluster: true
only_allow_inside_cluster: false
roi_scale_factor: 1.1
iou_threshold: 0.3
unknown_iou_threshold: 0.1
remove_unknown: true
debug_mode: true
image_buffer_size: 1000

14 changes: 13 additions & 1 deletion edge_auto_launch/config/scan_ground_filter.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,18 @@
/**:
ros__parameters:
global_slope_max_angle_deg: 10.0
local_slope_max_angle_deg: 30.0
local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode
split_points_distance_tolerance: 0.2
use_virtual_ground_point: True
split_height_distance: 0.2
non_ground_height_threshold: 0.20
grid_size_m: 0.1
grid_mode_switch_radius: 20.0
gnd_grid_buffer_size: 4
detection_range_z_max: 2.5
elevation_grid_mode: true
low_priority_region_x: -20.0
center_pcl_shift: 0.0
radial_divider_angle_deg: 1.0
use_recheck_ground_cluster: true
use_lowest_point: true
59 changes: 20 additions & 39 deletions edge_auto_launch/launch/calibration_extrinsic_at128_sample.launch.xml
View file
Open in desktop
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
Open in desktop
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
Open in desktop

This file was deleted.

4 changes: 4 additions & 0 deletions edge_auto_launch/launch/component/hesai_pandar_xt32.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
<node_container pkg="rclcpp_components" exec="component_container" name="nebula_xt32_node" namespace="/">
<composable_node pkg="nebula_ros" plugin="HesaiDriverRosWrapper" name="hesai_driver_roswrapper_node" namespace="$(var namespace)">
<param name="sensor_model" value="$(var sensor_model)"/>
<param name="sensor_ip" value="$(var sensor_ip)"/>
<param name="return_mode" value="$(var return_mode)"/>
<param name="frame_id" value="$(var frame_id)"/>
<param name="scan_phase" value="$(var scan_phase)"/>
Expand Down Expand Up @@ -50,6 +51,9 @@
<param name="cloud_max_angle" value="$(var cloud_max_angle)"/>
<param name="dual_return_distance_threshold" value="$(var dual_return_distance_threshold)"/>
<param name="setup_sensor" value="$(var setup_sensor)"/>
<param name="ptp_profile" value="1588v2"/>
<param name="ptp_transport_type" value="udp"/>
<param name="ptp_switch_type" value="non_tsn"/>
<extra_arg name="use_intra_process_comms" value="true"/>
</composable_node>
<composable_node pkg="nebula_ros" plugin="HesaiHwMonitorRosWrapper" name="hesai_hw_monitor_ros_wrapper_node" namespace="$(var namespace)">
Expand Down
51 changes: 46 additions & 5 deletions edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py
View file
Open in desktop
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

Check warning on line 39 in edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (wxyz)

Check warning on line 39 in edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (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]}

Check warning on line 47 in edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (wxyz)

Check warning on line 47 in edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (wxyz)

Check warning on line 47 in edge_auto_launch/launch/component/lidar_camera_tf_publisher.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (wxyz)
}
}


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 @@
'--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 generate_launch_description():
return LaunchDescription(
[
Expand Down
Loading
Loading