diff --git a/src/sherpa/launch/calibration.launch.py b/src/sherpa/launch/calibration.launch.py new file mode 100644 index 0000000..f227c36 --- /dev/null +++ b/src/sherpa/launch/calibration.launch.py @@ -0,0 +1,104 @@ +import os.path + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch.conditions import IfCondition +import math + +def generate_launch_description(): + # ----- microros ----- + servo_uros_node = Node( + package='micro_ros_agent', + executable='micro_ros_agent', + arguments=["serial", "--dev", "/dev/ttyACM0", "--baud", "921600"], + ) + + servo_angle_tf_node = Node( + package='sherpa_lidar_utils', + executable='servo_angle_tf.py', + ) + + # ----- lidar position ----- + lidar_position_base_tf_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments=['0', '0', '0', '0', '0', '0', '1', 'velodyne', 'lidar_base'], + ) + + # ----- lidar topics ----- + vlp16_driver_node = Node( + package='velodyne_driver', + executable='velodyne_driver_node', + arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0", "-p", "device_ip:=10.42.200.201"], + remappings=[ + ('/velodyne_packets', '/lidar/vlp16/packets'), + ], + ) + + vlp16_pointcloud_node = Node( + package='velodyne_pointcloud', + executable='velodyne_transform_node', + arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"], + remappings=[ + ('/velodyne_packets', '/lidar/vlp16/packets'), + ('/velodyne_points', '/lidar/vlp16/points'), + ] + ) + + mid40_driver_node = Node( + package='livox_ros2_driver', + executable='livox_ros2_driver_node', + arguments=[], + remappings=[ + ('/livox/lidar', '/lidar/mid40/points'), + ], + ) + + # ----- pointcloud downsampling ----- + downsample_vlp16_node = Node( + package='target_tracking', + executable='cloud_preprocessing_node', + parameters=[ + {"topic_in": "/lidar/vlp16/points"}, + {"topic_out": "/lidar/vlp16/downsampled"}, + {"x_min": 0.0}, + {"x_max": 100.0}, + {"z_min": -5.0}, + {"z_max": 10.0}, + {"tan_h_fov": math.pi}, # 180° + {"tan_v_fov": math.pi / 5}, # ~36° + ], + ) + + downsample_mid40_node = Node( + package='target_tracking', + executable='cloud_preprocessing_node', + parameters=[ + {"topic_in": "/lidar/mid40/points"}, + {"topic_out": "/lidar/mid40/downsampled"}, + {"x_min": 0.0}, + {"x_max": 100.0}, + {"z_min": -5.0}, + {"z_max": 10.0}, + {"tan_h_fov": math.pi / 4}, # 45° + {"tan_v_fov": math.pi / 4}, # 45° + ], + ) + + # ----- create ld ----- + ld = LaunchDescription() + + ld.add_action(servo_uros_node) + ld.add_action(servo_angle_tf_node) + ld.add_action(lidar_position_base_tf_node) + ld.add_action(vlp16_driver_node) + ld.add_action(vlp16_pointcloud_node) + ld.add_action(mid40_driver_node) + ld.add_action(downsample_vlp16_node) + ld.add_action(downsample_mid40_node) + + return ld diff --git a/src/sherpa/launch/tracking.launch.py b/src/sherpa/launch/tracking.launch.py new file mode 100644 index 0000000..863182b --- /dev/null +++ b/src/sherpa/launch/tracking.launch.py @@ -0,0 +1,159 @@ +import os.path + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch.conditions import IfCondition +import math + +def generate_launch_description(): + # ----- microros ----- + servo_uros_node = Node( + package='micro_ros_agent', + executable='micro_ros_agent', + arguments=["serial", "--dev", "/dev/ttyACM0", "--baud", "921600"], + ) + + servo_angle_tf_node = Node( + package='sherpa_lidar_utils', + executable='servo_angle_tf.py', + ) + + # ----- lidar position ----- + lidar_position_tf_node = Node( + package='sherpa_lidar_utils', + executable='publish_calibration.py', + arguments=["--ros-args", "-p", "calibration:=/home/sherpa/lidar_calibration"], + ) + + lidar_position_base_tf_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments=['0', '0', '0', '0', '0', '0', '1', 'velodyne', 'lidar_base'], + ) + + # ----- lidar topics ----- + vlp16_driver_node = Node( + package='velodyne_driver', + executable='velodyne_driver_node', + arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0", "-p", "device_ip:=10.42.200.201"], + remappings=[ + ('/velodyne_packets', '/lidar/vlp16/packets'), + ], + ) + + vlp16_pointcloud_node = Node( + package='velodyne_pointcloud', + executable='velodyne_transform_node', + arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"], + remappings=[ + ('/velodyne_packets', '/lidar/vlp16/packets'), + ('/velodyne_points', '/lidar/vlp16/points'), + ] + ) + + mid40_driver_node = Node( + package='livox_ros2_driver', + executable='livox_ros2_driver_node', + arguments=[], + remappings=[ + ('/livox/lidar', '/lidar/mid40/points'), + ], + ) + + # ----- pointcloud downsampling ----- + downsample_vlp16_node = Node( + package='target_tracking', + executable='cloud_preprocessing_node', + parameters=[ + {"topic_in": "/lidar/vlp16/points"}, + {"topic_out": "/lidar/vlp16/downsampled"}, + {"x_min": 0.0}, + {"x_max": 100.0}, + {"z_min": -5.0}, + {"z_max": 10.0}, + {"tan_h_fov": math.pi}, # 180° + {"tan_v_fov": math.pi / 5}, # ~36° + ], + ) + + downsample_mid40_node = Node( + package='target_tracking', + executable='cloud_preprocessing_node', + parameters=[ + {"topic_in": "/lidar/mid40/points"}, + {"topic_out": "/lidar/mid40/downsampled"}, + {"x_min": 0.0}, + {"x_max": 100.0}, + {"z_min": -5.0}, + {"z_max": 10.0}, + {"tan_h_fov": math.pi / 4}, # 45° + {"tan_v_fov": math.pi / 4}, # 45° + ], + ) + + # ----- pointcloud merging ----- + pointcloud_merger_node = Node( + package='scans_merger', + executable='scans_merger', + parameters=[ + {"destination_frame": "lidar_base"}, + {"input_cloud_1": "/lidar/vlp16/downsampled"}, + {"input_cloud_2": "/lidar/mid40/downsampled"}, + {"merged_cloud": "/lidar/merged/points"}, + ], + ) + + # ----- tracking ----- + cloud_clustering_node = Node( + package='target_tracking', + executable='cloud_clustering_node', + parameters=[ + {"topic_in": "/lidar/merged/points"}, + {"frame_id": 'lidar_base'}, + {"z_dim_scale": 1.0}, + #{"cluster_tolerance": 0.065}, + #{"min_cluster_size": 450}, + #{"max_cluster_size": 1650}, + {"min_cluster_size": 125}, + {"max_cluster_size": 2250}, + {"cluster_tolerance": 0.15}, + ], + remappings=[ + ('/cluster5', '/tracking/cluster0'), + ('/cluster4', '/tracking/cluster1'), + ('/cluster3', '/tracking/cluster2'), + ('/cluster2', '/tracking/cluster3'), + ('/cluster1', '/tracking/cluster4'), + ('/cluster0', '/tracking/cluster5'), + ], + ) + + servo_tracking_node = Node( + package='sherpa_lidar_utils', + executable='track_cluster.py', + remappings=[ + ('/tracking/target', '/tracking/cluster0'), + ], + ) + + # ----- create ld ----- + ld = LaunchDescription() + + ld.add_action(servo_uros_node) + ld.add_action(servo_angle_tf_node) + ld.add_action(lidar_position_tf_node) + ld.add_action(lidar_position_base_tf_node) + ld.add_action(vlp16_driver_node) + ld.add_action(vlp16_pointcloud_node) + ld.add_action(mid40_driver_node) + ld.add_action(downsample_vlp16_node) + ld.add_action(downsample_mid40_node) + ld.add_action(pointcloud_merger_node) + ld.add_action(cloud_clustering_node) + ld.add_action(servo_tracking_node) + + return ld