Skip to content

Commit

Permalink
[Frame Latency] Support for all topics
Browse files Browse the repository at this point in the history
  • Loading branch information
@Arun-Prasad-V
Arun-Prasad-V committed Sep 26, 2023
1 parent 642236c commit df411b1
Show file tree
Hide file tree
Showing 3 changed files with 108 additions and 35 deletions.
51 changes: 30 additions & 21 deletions realsense2_camera/launch/rs_intra_process_demo_launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -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='',
Expand All @@ -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',
Expand Down
65 changes: 52 additions & 13 deletions realsense2_camera/tools/frame_latency/frame_latency.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <typename MsgType>
void FrameLatencyNode::createListener(std::string topicName, const rmw_qos_profile_t qos_profile)
{
_sub = this->create_subscription<MsgType>(
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<tf2_msgs::msg::TFMessage>(
topicName,
rclcpp::QoS( rclcpp::QoSInitialization::from_rmw( qos_profile ),
qos_profile ),
[&, this]( const std::shared_ptr<tf2_msgs::msg::TFMessage> 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 )
Expand All @@ -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<sensor_msgs::msg::Image>(topic_name, rmw_qos_profile_default);
else if (topic_type == "points")
createListener<sensor_msgs::msg::PointCloud2>(topic_name, rmw_qos_profile_default);
else if (topic_type == "imu")
createListener<sensor_msgs::msg::Imu>(topic_name, rmw_qos_profile_sensor_data);
else if (topic_type == "metadata")
createListener<realsense2_camera_msgs::msg::Metadata>(topic_name, rmw_qos_profile_default);
else if (topic_type == "camera_info")
createListener<sensor_msgs::msg::CameraInfo>(topic_name, rmw_qos_profile_default);
else if (topic_type == "rgbd")
createListener<realsense2_camera_msgs::msg::RGBD>(topic_name, rmw_qos_profile_default);
else if (topic_type == "imu_info")
createListener<realsense2_camera_msgs::msg::IMUInfo>(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"
Expand Down
27 changes: 26 additions & 1 deletion realsense2_camera/tools/frame_latency/frame_latency.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,25 @@

#include <rclcpp/rclcpp.hpp>
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"

#include <diagnostic_updater/diagnostic_updater.hpp>
#include <diagnostic_updater/publisher.hpp>
#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 <librealsense2/hpp/rs_processing.hpp>
#include <librealsense2/rs_advanced_mode.hpp>

#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <nav_msgs/msg/odometry.hpp>


namespace rs2_ros {
namespace tools {
Expand All @@ -31,8 +50,14 @@ class FrameLatencyNode : public rclcpp::Node
const rclcpp::NodeOptions & node_options
= rclcpp::NodeOptions().use_intra_process_comms( true ) );

template <typename MsgType>
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<void > _sub;

rclcpp::Logger _logger;
};
} // namespace frame_latency
Expand Down

0 comments on commit df411b1

Please sign in to comment.