From 568133a27b4f12ed6fd3fb6ece8e1c6ca8c0d899 Mon Sep 17 00:00:00 2001 From: Robots Date: Tue, 20 Jun 2023 16:33:05 +0200 Subject: [PATCH 1/2] Add parameter to os_cloud_node to control the publidhing of the sensor frames --- ouster-ros/config/parameters.yaml | 23 ++++++++++++----------- ouster-ros/src/os_cloud_node.cpp | 10 +++++++++- 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/ouster-ros/config/parameters.yaml b/ouster-ros/config/parameters.yaml index 7ea15975..b38572be 100644 --- a/ouster-ros/config/parameters.yaml +++ b/ouster-ros/config/parameters.yaml @@ -1,18 +1,19 @@ ouster/os_sensor: ros__parameters: - sensor_hostname: '' - udp_dest: '' - mtp_dest: '' + sensor_hostname: "" + udp_dest: "" + mtp_dest: "" mtp_main: false - lidar_mode: '' - timestamp_mode: '' - udp_profile_lidar: '' - metadata: '' + lidar_mode: "" + timestamp_mode: "" + udp_profile_lidar: "" + metadata: "" lidar_port: 0 imu_port: 0 ouster/os_cloud: ros__parameters: - sensor_frame: 'os_sensor' - lidar_frame: 'os_lidar' - imu_frame: 'os_imu' - timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode + sensor_frame: "os_sensor" + lidar_frame: "os_lidar" + imu_frame: "os_imu" + timestamp_mode: "" # this value needs to match os_sensor/timestamp_mode + publish_sensor_frames: true diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 8d4af4e2..6c146f0b 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -95,6 +95,7 @@ class OusterCloud : public OusterProcessingNodeBase { declare_parameter("lidar_frame", "os_lidar"); declare_parameter("imu_frame", "os_imu"); declare_parameter("timestamp_mode", ""); + declare_parameter("publish_sensor_frames", true); } void parse_parameters() { @@ -104,6 +105,8 @@ class OusterCloud : public OusterProcessingNodeBase { auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; + + publish_sensor_frames = get_parameter("publish_sensor_frames").as_bool(); } static double compute_scan_col_ts_spacing_ns(sensor::lidar_mode ld_mode) { @@ -118,7 +121,10 @@ class OusterCloud : public OusterProcessingNodeBase { RCLCPP_INFO(get_logger(), "OusterCloud: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); - send_static_transforms(); + if (publish_sensor_frames) + { + send_static_transforms(); + } n_returns = get_n_returns(); create_lidarscan_objects(); compute_scan_ts = [this](const auto& ts_v) { @@ -349,6 +355,8 @@ class OusterCloud : public OusterProcessingNodeBase { std::string imu_frame; std::string lidar_frame; + bool publish_sensor_frames; + tf2_ros::StaticTransformBroadcaster tf_bcast; bool use_ros_time; From f42ba92172a0d5b4d35ca4cba79a470e54de70d5 Mon Sep 17 00:00:00 2001 From: antoni-benavent Date: Mon, 14 Oct 2024 12:39:06 +0200 Subject: [PATCH 2/2] Add launch file for single ouster lidar --- ouster-ros/launch/ouster.launch.py | 103 +++++++++++++++++++++++++++++ 1 file changed, 103 insertions(+) create mode 100644 ouster-ros/launch/ouster.launch.py diff --git a/ouster-ros/launch/ouster.launch.py b/ouster-ros/launch/ouster.launch.py new file mode 100644 index 00000000..d273bc45 --- /dev/null +++ b/ouster-ros/launch/ouster.launch.py @@ -0,0 +1,103 @@ +#!/usr/bin/python3 +# Copyright 2023 Ouster, Inc. + +import lifecycle_msgs.msg +from launch import LaunchDescription +from launch.actions import EmitEvent, LogInfo, RegisterEventHandler +from launch.events import matches_action +from launch_ros.actions import LifecycleNode, Node +from launch_ros.event_handlers import OnStateTransition +from launch_ros.events.lifecycle import ChangeState +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument + +def generate_launch_description(): + + NAMESPACE_ARG = DeclareLaunchArgument( + "namespace", + description="(str) namespace that will contain the pipeline nodes and topics", + default_value="none", + ) + + NAMESPACE = LaunchConfiguration("namespace", default="none") + + PARAMS_ARG = DeclareLaunchArgument( + "params", + description="(str) Path to the yaml file containing the parameters for the nodes in " + "the launch file.", + default_value="", + ) + + PARAMS = LaunchConfiguration("params", default="") + + os_sensor = LifecycleNode( + package="ouster_ros", + executable="os_sensor", + name="os_sensor", + namespace=NAMESPACE, + parameters=[PARAMS], + respawn=True, + respawn_delay=4, + ) + + sensor_configure_event = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(os_sensor), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + ) + ) + + sensor_activate_event = RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=os_sensor, + goal_state="inactive", + entities=[ + LogInfo(msg="os_sensor activating..."), + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(os_sensor), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ) + ), + ], + handle_once=True, + ) + ) + + os_cloud = Node( + package="ouster_ros", + executable="os_cloud", + name="os_cloud", + namespace=NAMESPACE, + parameters=[PARAMS], + respawn=True, + respawn_delay=4, + ) + + os_image = Node( + package="ouster_ros", + executable="os_image", + name="os_image", + namespace=NAMESPACE, + parameters=[PARAMS], + remappings=[ + ([NAMESPACE, "/signal_image"], [NAMESPACE, "/intensity_image"]), + ([NAMESPACE, "/signal_image2"], [NAMESPACE, "/intensity_image2"]), + ([NAMESPACE, "/nearir_image"], [NAMESPACE, "/ambient_image"]), + ([NAMESPACE, "/nearir_image2"], [NAMESPACE, "/ambient_image2"]), + ], + respawn=True, + respawn_delay=4, + ) + + return LaunchDescription( + [ + NAMESPACE_ARG, + PARAMS_ARG, + os_sensor, + os_cloud, + os_image, + sensor_configure_event, + sensor_activate_event, + ] + )