From df411b1c1805527ac97797156f4ace694a83f089 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 26 Sep 2023 22:47:44 +0530 Subject: [PATCH] [Frame Latency] Support for all topics --- .../launch/rs_intra_process_demo_launch.py | 51 +++++++++------ .../tools/frame_latency/frame_latency.cpp | 65 +++++++++++++++---- .../tools/frame_latency/frame_latency.h | 27 +++++++- 3 files changed, 108 insertions(+), 35 deletions(-) diff --git a/realsense2_camera/launch/rs_intra_process_demo_launch.py b/realsense2_camera/launch/rs_intra_process_demo_launch.py index 9bdf75111f..a4e54391a0 100644 --- a/realsense2_camera/launch/rs_intra_process_demo_launch.py +++ b/realsense2_camera/launch/rs_intra_process_demo_launch.py @@ -41,24 +41,32 @@ '\nplease make sure you run "colcon build --cmake-args \'-DBUILD_TOOLS=ON\'" command before running this launch file') -configurable_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'}, - {'name': 'serial_no', 'default': "''", 'description': 'choose device by serial number'}, - {'name': 'usb_port_id', 'default': "''", 'description': 'choose device by usb port id'}, - {'name': 'device_type', 'default': "''", 'description': 'choose device by type'}, - {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'}, - {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, - {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, - {'name': 'enable_depth', 'default': 'false', 'description': 'enable depth stream'}, - {'name': 'enable_infra', 'default': 'false', 'description': 'enable infra stream'}, - {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, - {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, - {'name': 'enable_gyro', 'default': 'false', 'description': "enable gyro stream"}, - {'name': 'enable_accel', 'default': 'false', 'description': "enable accel stream"}, - {'name': 'intra_process_comms', 'default': 'true', 'description': "enable intra-process communication"}, - {'name': 'publish_tf', 'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'}, - {'name': 'tf_publish_rate', 'default': '0.0', 'description': '[double] rate in HZ for publishing dynamic TF'}, - {'name': 'topic_name', 'default': '/color/image_raw', 'description': 'topic to which latency calculated'}, - ] +realsense_node_params = [{'name': 'serial_no', 'default': "''", 'description': 'choose device by serial number'}, + {'name': 'usb_port_id', 'default': "''", 'description': 'choose device by usb port id'}, + {'name': 'device_type', 'default': "''", 'description': 'choose device by type'}, + {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'}, + {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, + {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, + {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'enable_infra', 'default': 'false', 'description': 'enable infra stream'}, + {'name': 'enable_infra1', 'default': 'true', 'description': 'enable infra1 stream'}, + {'name': 'enable_infra2', 'default': 'true', 'description': 'enable infra2 stream'}, + {'name': 'enable_gyro', 'default': 'true', 'description': "enable gyro stream"}, + {'name': 'enable_accel', 'default': 'true', 'description': "enable accel stream"}, + {'name': 'unite_imu_method', 'default': "1", 'description': '[0-None, 1-copy, 2-linear_interpolation]'}, + {'name': 'intra_process_comms', 'default': 'true', 'description': "enable intra-process communication"}, + {'name': 'enable_sync', 'default': 'true', 'description': "'enable sync mode'"}, + {'name': 'pointcloud.enable', 'default': 'true', 'description': ''}, + {'name': 'enable_rgbd', 'default': 'true', 'description': "'enable rgbd topic'"}, + {'name': 'align_depth.enable', 'default': 'true', 'description': "'enable align depth filter'"}, + {'name': 'publish_tf', 'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'}, + {'name': 'tf_publish_rate', 'default': '1.0', 'description': '[double] rate in HZ for publishing dynamic TF'}, + ] + +frame_latency_node_params = [{'name': 'topic_name', 'default': '/camera/color/image_raw', 'description': 'topic to which latency calculated'}, + {'name': 'topic_type', 'default': 'image', 'description': 'topic type [image|points|imu|metadata|camera_info|rgbd|imu_info|tf]'}, + ] + def declare_configurable_parameters(parameters): return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters] @@ -70,7 +78,8 @@ def set_configurable_parameters(parameters): def generate_launch_description(): - return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [ + return LaunchDescription(declare_configurable_parameters(realsense_node_params) + + declare_configurable_parameters(frame_latency_node_params) +[ ComposableNodeContainer( name='my_container', namespace='', @@ -82,14 +91,14 @@ def generate_launch_description(): namespace='', plugin='realsense2_camera::' + rs_node_class, name="camera", - parameters=[set_configurable_parameters(configurable_parameters)], + parameters=[set_configurable_parameters(realsense_node_params)], extra_arguments=[{'use_intra_process_comms': LaunchConfiguration("intra_process_comms")}]) , ComposableNode( package='realsense2_camera', namespace='', plugin='rs2_ros::tools::frame_latency::' + rs_latency_tool_class, name='frame_latency', - parameters=[set_configurable_parameters(configurable_parameters)], + parameters=[set_configurable_parameters(frame_latency_node_params)], extra_arguments=[{'use_intra_process_comms': LaunchConfiguration("intra_process_comms")}]) , ], output='screen', diff --git a/realsense2_camera/tools/frame_latency/frame_latency.cpp b/realsense2_camera/tools/frame_latency/frame_latency.cpp index 59d37484a1..149da670ed 100644 --- a/realsense2_camera/tools/frame_latency/frame_latency.cpp +++ b/realsense2_camera/tools/frame_latency/frame_latency.cpp @@ -28,7 +28,39 @@ FrameLatencyNode::FrameLatencyNode( const std::string & node_name, { } -std::string topic_name = "/color/image_raw"; +std::string topic_name = "/camera/color/image_raw"; +std::string topic_type = "image"; + +template +void FrameLatencyNode::createListener(std::string topicName, const rmw_qos_profile_t qos_profile) +{ + _sub = this->create_subscription( + topicName, + rclcpp::QoS( rclcpp::QoSInitialization::from_rmw( qos_profile ), + qos_profile ), + [&, this]( const std::shared_ptr< MsgType> msg ) { + rclcpp::Time curr_time = this->get_clock()->now(); + auto latency = ( curr_time - msg->header.stamp ).seconds(); + ROS_INFO_STREAM( "Got msg with "<< msg->header.frame_id <<" frame id at address 0x" + << std::hex << reinterpret_cast< std::uintptr_t >( msg.get() ) + << std::dec << " with latency of " << latency << " [sec]" ); + } ); +} + +void FrameLatencyNode::createTFListener(std::string topicName, const rmw_qos_profile_t qos_profile) +{ + _sub = this->create_subscription( + topicName, + rclcpp::QoS( rclcpp::QoSInitialization::from_rmw( qos_profile ), + qos_profile ), + [&, this]( const std::shared_ptr msg ) { + rclcpp::Time curr_time = this->get_clock()->now(); + auto latency = ( curr_time - msg->transforms.back().header.stamp ).seconds(); + ROS_INFO_STREAM( "Got msg with "<< msg->transforms.back().header.frame_id <<" frame id at address 0x" + << std::hex << reinterpret_cast< std::uintptr_t >( msg.get() ) + << std::dec << " with latency of " << latency << " [sec]" ); + } ); +} FrameLatencyNode::FrameLatencyNode( const rclcpp::NodeOptions & node_options ) : Node( "frame_latency", "/", node_options ) @@ -39,21 +71,28 @@ FrameLatencyNode::FrameLatencyNode( const rclcpp::NodeOptions & node_options ) << ( this->get_node_options().use_intra_process_comms() ? "ON" : "OFF" ) ); topic_name = this->declare_parameter("topic_name", topic_name); + topic_type = this->declare_parameter("topic_type", topic_type); ROS_INFO_STREAM( "Subscribing to Topic: " << topic_name); - // Create a subscription on the input topic. - _sub = this->create_subscription< sensor_msgs::msg::Image >( - topic_name, - rclcpp::QoS( rclcpp::QoSInitialization::from_rmw( rmw_qos_profile_default ), - rmw_qos_profile_default ), - [&, this]( const sensor_msgs::msg::Image::SharedPtr msg ) { - rclcpp::Time curr_time = this->get_clock()->now(); - auto latency = ( curr_time - msg->header.stamp ).seconds(); - ROS_INFO_STREAM( "Got msg with "<< msg->header.frame_id <<" frame id at address 0x" - << std::hex << reinterpret_cast< std::uintptr_t >( msg.get() ) - << std::dec << " with latency of " << latency << " [sec]" ); - } ); + if (topic_type == "image") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "points") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "imu") + createListener(topic_name, rmw_qos_profile_sensor_data); + else if (topic_type == "metadata") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "camera_info") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "rgbd") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "imu_info") + createListener(topic_name, rmw_qos_profile_default); + else if (topic_type == "tf") + createTFListener(topic_name, rmw_qos_profile_default); + else + ROS_ERROR_STREAM("Specified message type '" << topic_type << "' is not supported"); } #include "rclcpp_components/register_node_macro.hpp" diff --git a/realsense2_camera/tools/frame_latency/frame_latency.h b/realsense2_camera/tools/frame_latency/frame_latency.h index 653c648578..aad8c81c0e 100644 --- a/realsense2_camera/tools/frame_latency/frame_latency.h +++ b/realsense2_camera/tools/frame_latency/frame_latency.h @@ -16,6 +16,25 @@ #include #include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include +#include +#include "realsense2_camera_msgs/msg/imu_info.hpp" +#include "realsense2_camera_msgs/msg/extrinsics.hpp" +#include "realsense2_camera_msgs/msg/metadata.hpp" +#include "realsense2_camera_msgs/msg/rgbd.hpp" +#include "realsense2_camera_msgs/srv/device_info.hpp" +#include +#include + +#include +#include +#include +#include +#include + namespace rs2_ros { namespace tools { @@ -31,8 +50,14 @@ class FrameLatencyNode : public rclcpp::Node const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().use_intra_process_comms( true ) ); + template + void createListener(std::string topicName, const rmw_qos_profile_t qos_profile); + + void createTFListener(std::string topicName, const rmw_qos_profile_t qos_profile); + private: - rclcpp::Subscription< sensor_msgs::msg::Image >::SharedPtr _sub; + std::shared_ptr _sub; + rclcpp::Logger _logger; }; } // namespace frame_latency