added tracking conf
This commit is contained in:
SHERPA
104
src/sherpa/launch/calibration.launch.py
Normal file
104
src/sherpa/launch/calibration.launch.py
Normal file
@@ -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
|
||||
159
src/sherpa/launch/tracking.launch.py
Normal file
159
src/sherpa/launch/tracking.launch.py
Normal file
@@ -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
|
||||
Reference in New Issue
Block a user