Skip to content

Commit

Permalink
PR #2381 from Yadunund: Publish static transforms when intra porocess…
Browse files Browse the repository at this point in the history
… communication is enabled
  • Loading branch information
Nir-Az authored Jan 17, 2023
2 parents 333f4fb + e1a65bb commit 85e3d71
Show file tree
Hide file tree
Showing 2 changed files with 15 additions and 13 deletions.
8 changes: 0 additions & 8 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -288,14 +288,6 @@ Further details on efficient intra-process communication can be found [here](htt
### Limitations

* Node components are currently not supported on RCLPY
* Transformations: `/static_tf` topic will be disabled
* To get the transformations published:
* Set `tf_publish_rate` to `1.0` in the launch file (or on the command-line, using `-p tf_publish_rate:=1.0`)
* Activate and read `/tf` and `/extrinsic/<stream>_to_<stream>` topics
* To echo the `/extrinsic/<stream>_to_<stream>` topic you will need to change the default CLI QoS to match the new QoS that the `intra-process` flow uses. E.g.:
```bash
ros2 topic echo /extrinsics/depth_to_color --qos-durability=volatile --qos-reliability=reliable
```
* Compressed images using `image_transport` will be disabled as this isn't supported with intra-process communication
### Latency test tool and launch file
Expand Down
20 changes: 15 additions & 5 deletions realsense2_camera/src/base_realsense_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,13 @@
#include <fstream>
#include <image_publisher.h>

// Header files for disabling intra-process comms for static broadcaster.
#include <rclcpp/publisher_options.hpp>
// This header file is not available in ROS 2 Dashing.
#ifndef DASHING
#include <tf2_ros/qos.hpp>
#endif

using namespace realsense2_camera;

SyncedImuPublisher::SyncedImuPublisher(rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher,
Expand Down Expand Up @@ -91,11 +98,14 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
{
ROS_INFO("Intra-Process communication enabled");
}
else
{
// intra-process requirment of QoS.durability=Volatile cannot be fulfilled with `StaticTransformBroadcaster` as it only support `TransientLocal` durability.
_static_tf_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node);
}

rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
#ifndef DASHING
_static_tf_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node, tf2_ros::StaticBroadcasterQoS(), std::move(options));
#else
_static_tf_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node, rclcpp::QoS(100), std::move(options));
#endif

_image_format[1] = CV_8UC1; // CVBridge type
_image_format[2] = CV_16UC1; // CVBridge type
Expand Down

0 comments on commit 85e3d71

Please sign in to comment.