Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Frame latency for the '/topic' provided by user #2853

Merged
merged 5 commits into from
Sep 27, 2023
Merged

Frame latency for the '/topic' provided by user #2853

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
00621dc
Get the topic to subscribe from user
Arun-Prasad-V Aug 22, 2023
642236c
Merge branch 'IntelRealSense:ros2-development' into frame_latency
Arun-Prasad-V Sep 11, 2023
df411b1
[Frame Latency] Support for all topics
Arun-Prasad-V Sep 26, 2023
16721e1
Merge branch 'IntelRealSense:ros2-development' into frame_latency
Arun-Prasad-V Sep 27, 2023
9679a9b
[Frame latency Tool] Added description
Arun-Prasad-V Sep 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 30 additions & 19 deletions realsense2_camera/launch/rs_intra_process_demo_launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,22 +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_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'},
]
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 @@ -68,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 @@ -80,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
86 changes: 72 additions & 14 deletions realsense2_camera/tools/frame_latency/frame_latency.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,25 @@
// See the License for the specific language governing permissions and
// limitations under the License.

// DESCRIPTION: #
// ------------ #
// This tool created a node which can be used to calulate the specified topic's latency.
// Input parameters:
// - topic_name : <String>
// - topic to which latency need to be calculated
// - topic_type : <String>
// - Message type of the topic.
// - Valid inputs: {'image','points','imu','metadata','camera_info','rgbd','imu_info','tf'}
// Note:
// - This tool doesn't support calulating latency for extrinsic topics.
// Because, those topics doesn't have timestamp in it and this tool uses
// that timestamp as an input to calculate the latency.
//

#include <sstream>
#include <string>
#include <frame_latency/frame_latency.h>
#include <constants.h>
// Node which receives sensor_msgs/Image messages and prints the image latency.

using namespace rs2_ros::tools::frame_latency;

Expand All @@ -28,6 +42,39 @@ FrameLatencyNode::FrameLatencyNode( const std::string & node_name,
{
}

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 @@ -36,19 +83,30 @@ FrameLatencyNode::FrameLatencyNode( const rclcpp::NodeOptions & node_options )
ROS_INFO_STREAM( "frame_latency node is UP!" );
ROS_INFO_STREAM( "Intra-Process is "
<< ( this->get_node_options().use_intra_process_comms() ? "ON" : "OFF" ) );
// Create a subscription on the input topic.
_sub = this->create_subscription< sensor_msgs::msg::Image >(
"/color/image_raw", // TODO Currently color only, we can declare and accept the required
// streams as ros parameters
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 address 0x"
<< std::hex << reinterpret_cast< std::uintptr_t >( msg.get() )
<< std::dec << " with latency of " << latency << " [sec]" );
} );

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);

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
Open in desktop
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
Loading