From e14ba753ef2b90218f92bbe4655011f54505bfcb Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 18 Aug 2023 22:09:43 +0530 Subject: [PATCH 1/3] Create /imu topic only when motion streams enabled --- .../include/base_realsense_node.h | 3 ++- realsense2_camera/src/base_realsense_node.cpp | 10 +++++++--- realsense2_camera/src/rs_node_setup.cpp | 19 +++++++++++++------ 3 files changed, 22 insertions(+), 10 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index d6f0a1b97c..865ff9d13a 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -252,7 +252,6 @@ namespace realsense2_camera void startDiagnosticsUpdater(); void monitoringProfileChanges(); void publish_temperature(); - void setupFiltersPublishers(); void setAvailableSensors(); void setCallbackFunctions(); void updateSensors(); @@ -311,6 +310,8 @@ namespace realsense2_camera bool _enable_rgbd; bool _is_color_enabled; bool _is_depth_enabled; + bool _is_accel_enabled; + bool _is_gyro_enabled; bool _pointcloud; bool _publish_odom_tf; imu_sync_method _imu_sync_method; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index ae6b50d33c..e079ff556d 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -112,6 +112,8 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _enable_rgbd(ENABLE_RGBD), _is_color_enabled(false), _is_depth_enabled(false), + _is_accel_enabled(false), + _is_gyro_enabled(false), _pointcloud(false), _publish_odom_tf(false), _imu_sync_method(imu_sync_method::NONE), @@ -381,7 +383,7 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain()); } - if (0 != _synced_imu_publisher->getNumSubscribers()) + if (_synced_imu_publisher && (0 != _synced_imu_publisher->getNumSubscribers())) { auto crnt_reading = *(reinterpret_cast(frame.get_data())); Eigen::Vector3d v(crnt_reading.x, crnt_reading.y, crnt_reading.z); @@ -464,7 +466,8 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) void BaseRealSenseNode::frame_callback(rs2::frame frame) { - _synced_imu_publisher->Pause(); + if (_synced_imu_publisher) + _synced_imu_publisher->Pause(); double frame_time = frame.get_timestamp(); // We compute a ROS timestamp which is based on an initial ROS time at point of first frame, @@ -576,7 +579,8 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) } publishFrame(frame, t, sip, _images, _info_publishers, _image_publishers); } - _synced_imu_publisher->Resume(); + if (_synced_imu_publisher) + _synced_imu_publisher->Resume(); } // frame_callback void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_method sync_method) diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index fcdb673e34..652eb63ca7 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -27,18 +27,12 @@ void BaseRealSenseNode::setup() setAvailableSensors(); SetBaseStream(); setupFilters(); - setupFiltersPublishers(); setCallbackFunctions(); monitoringProfileChanges(); updateSensors(); publishServices(); } -void BaseRealSenseNode::setupFiltersPublishers() -{ - _synced_imu_publisher = std::make_shared(_node.create_publisher("imu", 5)); -} - void BaseRealSenseNode::monitoringProfileChanges() { int time_interval(10000); @@ -190,6 +184,9 @@ void BaseRealSenseNode::stopPublishers(const std::vector& profil } else if (profile.is()) { + _is_accel_enabled = false; + _is_gyro_enabled = false; + _synced_imu_publisher.reset(); _imu_publishers.erase(sip); _imu_info_publishers.erase(sip); } @@ -270,6 +267,11 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi } else if (profile.is()) { + if(profile.stream_type() == RS2_STREAM_ACCEL) + _is_accel_enabled = true; + else if (profile.stream_type() == RS2_STREAM_GYRO) + _is_gyro_enabled = true; + std::stringstream data_topic_name, info_topic_name; data_topic_name << stream_name << "/sample"; _imu_publishers[sip] = _node.create_publisher(data_topic_name.str(), @@ -305,6 +307,11 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(extrinsics_qos), extrinsics_qos), std::move(options)); } } + if (_is_accel_enabled && _is_gyro_enabled && (_imu_sync_method > imu_sync_method::NONE)) + { + _synced_imu_publisher = std::make_shared(_node.create_publisher("imu", 5)); + } + } void BaseRealSenseNode::startRGBDPublisherIfNeeded() From 6fdfaa0c07562290e4643384c520afc3e3487d01 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 7 Sep 2023 06:02:21 +0530 Subject: [PATCH 2/3] updated readme --- README.md | 28 ++++++++++++++++------------ realsense2_camera/src/parameters.cpp | 2 ++ 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index e308b3b80b..58506045cc 100644 --- a/README.md +++ b/README.md @@ -304,6 +304,22 @@ User can set the camera name and camera namespace, to distinguish between camera - This param also depends on **publish_tf** param - If **publish_tf:=false**, then no TFs will be published, even if **tf_publish_rate** is >0.0 Hz - If **publish_tf:=true** and **tf_publish_rate** set to >0.0 Hz, then dynamic TFs will be published at the specified rate +- **unite_imu_method**: + - For the D400 cameras with built in IMU components, below 2 unrelated streams (each with it's own frequency) will be created: + - *gyro* - which shows angular velocity + - *accel* - which shows linear acceleration. + - Both streams will publish data to its corresponding topics: + - '/camera/camera/gyro/sample' & '/camera/camera/accel/sample' + - Though both topics are of same message type 'sensor_msgs::Imu', only their relevant fields are filled out. + - A new topic called **imu** will be created, when both *accel* and *gyro* streams are enabled and the param *unite_imu_method* set to > 0. + - Data from both accel and gyro are combined and published to this topic + - All the fields of the Imu message are filled out. + - It will be published at the rate of the gyro. + - `unite_imu_method` param supports below values: + - 0 -> **none**: no imu topic + - 1 -> **copy**: Every gyro message will be attached by the last accel message. + - 2 -> **linear_interpolation**: Every gyro message will be attached by an accel message which is interpolated to gyro's timestamp. + - Note: When the param *unite_imu_method* is dynamically updated, re-enable either gyro or accel stream for the change to take effect. #### Parameters that cannot be changed in runtime: - **serial_no**: @@ -339,18 +355,6 @@ User can set the camera name and camera namespace, to distinguish between camera - For example: `initial_reset:=true` - **base_frame_id**: defines the frame_id all static transformations refers to. - **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. - -- **unite_imu_method**: - - D400 cameras have built in IMU components which produce 2 unrelated streams, each with it's own frequency: - - *gyro* - which shows angular velocity - - *accel* which shows linear acceleration. - - By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. - - Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. - - The *imu* topic is published at the rate of the gyro. - - All the fields of the Imu message under the *imu* topic are filled out. - - `unite_imu_method` parameter supported values are [0-2] meaning: [0 -> None, 1 -> Copy, 2 -> Linear_ interpolation] when: - - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. - - **copy**: Every gyro message is attached by the last accel message. - **clip_distance**: - Remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - For example: `clip_distance:=1.5` diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 16965f27cf..c5d1abef3a 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -135,6 +135,8 @@ void BaseRealSenseNode::setDynamicParams() [this](const rclcpp::Parameter& parameter) { _imu_sync_method = imu_sync_method(parameter.get_value()); + ROS_WARN("For the 'unite_imu_method' param update to take effect, " + "re-enable either gyro or accel stream."); }, crnt_descriptor); _parameters_names.push_back(param_name); } From 41ae997c9ac6da139725a789679329aec156e36d Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 7 Sep 2023 06:41:51 +0530 Subject: [PATCH 3/3] updated QoS of IMU topic --- realsense2_camera/src/rs_node_setup.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 51aa070273..6e05d9ad85 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -311,7 +311,10 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi } if (_is_accel_enabled && _is_gyro_enabled && (_imu_sync_method > imu_sync_method::NONE)) { - _synced_imu_publisher = std::make_shared(_node.create_publisher("~/imu", 5)); + rmw_qos_profile_t qos = _use_intra_process ? qos_string_to_qos(DEFAULT_QOS) : qos_string_to_qos(HID_QOS); + + _synced_imu_publisher = std::make_shared(_node.create_publisher("~/imu", + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos))); } }