Merge branch 'ros2-development' into change_topics_in_readme

.github/workflows/main.yml

@@ -46,7 +46,7 @@ jobs:
       run: |
         mkdir -p ${{github.workspace}}/ros2/src
 
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4
       with:
         path: 'ros2/src/realsense-ros'
 
@@ -61,17 +61,17 @@ jobs:
     # so, seperating steps with if conditions
     - name: build ROS2 for foxy
       if: ${{ matrix.ros_distro == 'foxy' }}
-      uses: ros-tooling/setup-ros@v0.6
+      uses: ros-tooling/setup-ros@236ab287884fd5a314fc030e91b2017abb46719e #v0.6
       with:
         required-ros-distributions: ${{ matrix.ros_distro }}
     - name: build ROS2 for humble/iron/rolling/jazzy
       if: ${{ matrix.ros_distro != 'foxy' }}
-      uses: ros-tooling/setup-ros@v0.7
+      uses: ros-tooling/setup-ros@a6ce30ecca1e5dcc10ae5e6a44fe2169115bf852 #v0.7
       with:
         required-ros-distributions: ${{ matrix.ros_distro }}
 
     - name: Checkout librealsense/development
-      uses: actions/checkout@v4
+      uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4
       with:
         repository: IntelRealSense/librealsense
         path: librealsense

.github/workflows/pre-release.yml

@@ -45,6 +45,6 @@ jobs:
       BASEDIR: ${{ github.workspace }}/.work
 
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4
       - name: industrial_ci
-        uses: ros-industrial/industrial_ci@master
+        uses: ros-industrial/industrial_ci@d23b9ad2c63bfad638a2b1fe3df34b8df9a2f17b #Replace reference to 'master' with the latest commit hash

.github/workflows/static_analysis.yaml

@@ -13,7 +13,7 @@ jobs:
     name: cppcheck
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4
 
       - name: Install 
         shell: bash

README.md

@@ -302,6 +302,7 @@ User can set the camera name and camera namespace, to distinguish between camera
 
   > ros2 service list
   /robot1/D455_1/describe_parameters
+  /robot1/D455_1/hw_reset
   /robot1/D455_1/device_info
   /robot1/D455_1/get_parameter_types
   /robot1/D455_1/get_parameters
@@ -333,6 +334,7 @@ User can set the camera name and camera namespace, to distinguish between camera
 
 > ros2 service list
 /camera/camera/describe_parameters
+/camera/camera/hw_reset
 /camera/camera/device_info
 /camera/camera/get_parameter_types
 /camera/camera/get_parameters
@@ -657,6 +659,10 @@ Each of the above filters have it's own parameters, following the naming convent
 
 ## Available services
 
+### hw_reset:
+  - reset the device. The call stops all the streams too.
+  - Call example: `ros2 service call /camera/camera/hw_reset std_srvs/srv/Empty`
+
 ### device_info:
   - retrieve information about the device - serial_number, firmware_version etc.
   - Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list.

realsense2_camera/CMakeLists.txt

@@ -110,6 +110,7 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(realsense2_camera_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
@@ -245,6 +246,7 @@ set(dependencies
   rclcpp
   rclcpp_components
   realsense2_camera_msgs
+  std_srvs
   std_msgs
   sensor_msgs
   nav_msgs
@@ -318,6 +320,7 @@ if(BUILD_TESTING)
           $<INSTALL_INTERFACE:include>
        )
        ament_target_dependencies(${_test_name}
+          std_srvs
           std_msgs
        )
        #target_link_libraries(${_test_name} name_of_local_library)

realsense2_camera/examples/launch_params_from_file/README.md

@@ -24,7 +24,7 @@ param2: value
 Example:
 ```
 enable_color: true
-rgb_camera.profile: 1280x720x15
+rgb_camera.color_profile: 1280x720x15
 enable_depth: true
 align_depth.enable: true
 enable_sync: true

realsense2_camera/examples/launch_params_from_file/config/config.yaml

@@ -1,5 +1,5 @@
 enable_color: true
-rgb_camera.profile: 1280x720x15
+rgb_camera.color_profile: 1280x720x15
 enable_depth: true
 align_depth.enable: true
 enable_sync: true

realsense2_camera/include/base_realsense_node.h

@@ -27,6 +27,7 @@
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <diagnostic_updater/publisher.hpp>
+#include <std_srvs/srv/empty.hpp>
 #include "realsense2_camera_msgs/msg/imu_info.hpp"
 #include "realsense2_camera_msgs/msg/extrinsics.hpp"
 #include "realsense2_camera_msgs/msg/metadata.hpp"
@@ -155,6 +156,7 @@ namespace realsense2_camera
         std::string _camera_name;
         std::vector<rs2_option> _monitor_options;
         rclcpp::Logger _logger;
+        rclcpp::Service<std_srvs::srv::Empty>::SharedPtr _reset_srv;
         rclcpp::Service<realsense2_camera_msgs::srv::DeviceInfo>::SharedPtr _device_info_srv;
         rclcpp::Service<realsense2_camera_msgs::srv::CalibConfigRead>::SharedPtr _calib_config_read_srv;
         rclcpp::Service<realsense2_camera_msgs::srv::CalibConfigWrite>::SharedPtr _calib_config_write_srv;
@@ -166,6 +168,9 @@ namespace realsense2_camera
         void restartStaticTransformBroadcaster();
         void publishExtrinsicsTopic(const stream_index_pair& sip, const rs2_extrinsics& ex);
         void calcAndAppendTransformMsgs(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile);
+
+        void handleHWReset(const std_srvs::srv::Empty::Request::SharedPtr req,
+                                const std_srvs::srv::Empty::Response::SharedPtr res);
         void getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req,
                                  realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res);
         void CalibConfigReadService(const realsense2_camera_msgs::srv::CalibConfigRead::Request::SharedPtr req,

realsense2_camera/include/constants.h

@@ -65,7 +65,8 @@ namespace realsense2_camera
     const uint16_t RS430i_PID       = 0x0b4b; // D430i
     const uint16_t RS405_PID        = 0x0B5B; // DS5U
     const uint16_t RS455_PID        = 0x0B5C; // D455
-    const uint16_t RS457_PID        = 0xABCD; // D457   
+    const uint16_t RS457_PID        = 0xABCD; // D457
+    const uint16_t RS555_PID        = 0x0B56; // D555
 
     const bool ALLOW_NO_TEXTURE_POINTS = false;
     const bool ORDERED_PC     = false;

realsense2_camera/include/ros_utils.h

@@ -33,6 +33,7 @@ namespace realsense2_camera
     const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2};
     const stream_index_pair GYRO{RS2_STREAM_GYRO, 0};
     const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0};
+    const stream_index_pair MOTION{RS2_STREAM_MOTION, 0};
 
     bool isValidCharInName(char c);
 

realsense2_camera/launch/rs_launch.py

@@ -62,6 +62,7 @@
                            {'name': 'enable_gyro',                  'default': 'false', 'description': "'enable gyro stream'"},
                            {'name': 'enable_accel',                 'default': 'false', 'description': "'enable accel stream'"},
                            {'name': 'gyro_fps',                     'default': '0', 'description': "''"},
+                           {'name': 'enable_motion',                'default': 'false', 'description': "'enable motion stream (IMU) for DDS devices'"},
                            {'name': 'accel_fps',                    'default': '0', 'description': "''"},
                            {'name': 'unite_imu_method',             'default': "0", 'description': '[0-None, 1-copy, 2-linear_interpolation]'},
                            {'name': 'clip_distance',                'default': '-2.', 'description': "''"},

realsense2_camera/package.xml

@@ -23,6 +23,7 @@
   <depend>realsense2_camera_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>std_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>tf2</depend>

realsense2_camera/src/base_realsense_node.cpp

@@ -480,7 +480,26 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame)
                 frame.get_profile().stream_index(),
                 rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain()));
 
-    auto stream_index = (stream == GYRO.first)?GYRO:ACCEL;
+    stream_index_pair stream_index;
+
+    if(stream == GYRO.first)
+    {
+        stream_index = GYRO;
+    }
+    else if(stream == ACCEL.first)
+    {
+        stream_index = ACCEL;
+    }
+    else if(stream == MOTION.first)
+    {
+        stream_index = MOTION;
+    }
+    else 
+    {
+        ROS_ERROR("Unknown IMU stream type.");
+        return;
+    }
+
     rclcpp::Time t(frameSystemTimeSec(frame));
 
     if(_imu_publishers.find(stream_index) == _imu_publishers.end())
@@ -495,19 +514,41 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame)
         ImuMessage_AddDefaultValues(imu_msg);
         imu_msg.header.frame_id = OPTICAL_FRAME_ID(stream_index);
 
-        auto crnt_reading = *(reinterpret_cast<const float3*>(frame.get_data()));
-        if (GYRO == stream_index)
+        if (MOTION == stream_index)
         {
-            imu_msg.angular_velocity.x = crnt_reading.x;
-            imu_msg.angular_velocity.y = crnt_reading.y;
-            imu_msg.angular_velocity.z = crnt_reading.z;
+            auto combined_motion_data = frame.as<rs2::motion_frame>().get_combined_motion_data();
+
+            imu_msg.linear_acceleration.x = combined_motion_data.linear_acceleration.x;
+            imu_msg.linear_acceleration.y = combined_motion_data.linear_acceleration.y;
+            imu_msg.linear_acceleration.z = combined_motion_data.linear_acceleration.z;
+
+            imu_msg.angular_velocity.x = combined_motion_data.angular_velocity.x;
+            imu_msg.angular_velocity.y = combined_motion_data.angular_velocity.y;
+            imu_msg.angular_velocity.z = combined_motion_data.angular_velocity.z;
+
+            imu_msg.orientation.x = combined_motion_data.orientation.x;
+            imu_msg.orientation.y = combined_motion_data.orientation.y;
+            imu_msg.orientation.z = combined_motion_data.orientation.z;
+            imu_msg.orientation.w = combined_motion_data.orientation.w;
+
         }
-        else if (ACCEL == stream_index)
+        else
         {
-            imu_msg.linear_acceleration.x = crnt_reading.x;
-            imu_msg.linear_acceleration.y = crnt_reading.y;
-            imu_msg.linear_acceleration.z = crnt_reading.z;
+            auto motion_data = frame.as<rs2::motion_frame>().get_motion_data();
+            if (GYRO == stream_index)
+            {
+                imu_msg.angular_velocity.x = motion_data.x;
+                imu_msg.angular_velocity.y = motion_data.y;
+                imu_msg.angular_velocity.z = motion_data.z;
+            }
+            else // ACCEL == stream_index
+            {
+                imu_msg.linear_acceleration.x = motion_data.x;
+                imu_msg.linear_acceleration.y = motion_data.y;
+                imu_msg.linear_acceleration.z = motion_data.z;
+            }
         }
+
         imu_msg.header.stamp = t;
         _imu_publishers[stream_index]->publish(imu_msg);
         ROS_DEBUG("Publish %s stream", ros_stream_to_string(frame.get_profile().stream_type()).c_str());

realsense2_camera/src/realsense_node_factory.cpp

@@ -107,24 +107,31 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list)
                 std::vector<std::string> results;
                 ROS_INFO_STREAM("Device with name " << name << " was found.");
                 std::string port_id = parseUsbPort(pn);
-                if (port_id.empty())
+
+                std::string pid_str(dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
+                if(pid_str != "DDS")
                 {
-                    std::stringstream msg;
-                    msg << "Error extracting usb port from device with physical ID: " << pn << std::endl << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros";
-                    if (_usb_port_id.empty())
+                    if (port_id.empty())
                     {
-                        ROS_WARN_STREAM(msg.str());
+                        std::stringstream msg;
+                        msg << "Error extracting usb port from device with physical ID: " << pn << std::endl
+                            << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros";
+                        if (_usb_port_id.empty())
+                        {
+                            ROS_WARN_STREAM(msg.str());
+                        }
+                        else
+                        {
+                            ROS_ERROR_STREAM(msg.str());
+                            ROS_ERROR_STREAM("Please use serial number instead of usb port.");
+                        }
                     }
                     else
                     {
-                        ROS_ERROR_STREAM(msg.str());
-                        ROS_ERROR_STREAM("Please use serial number instead of usb port.");
+                        ROS_INFO_STREAM("Device with port number " << port_id << " was found.");
                     }
                 }
-                else
-                {
-                    ROS_INFO_STREAM("Device with port number " << port_id << " was found.");                    
-                }
+
                 bool found_device_type(true);
                 if (!_device_type.empty())
                 {
@@ -352,8 +359,20 @@ void RealSenseNodeFactory::init()
 void RealSenseNodeFactory::startDevice()
 {
     if (_realSenseNode) _realSenseNode.reset();
+    std::string device_name(_device.get_info(RS2_CAMERA_INFO_NAME));
     std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
-    uint16_t pid = std::stoi(pid_str, 0, 16);
+    uint16_t pid;
+
+    if (device_name == "Intel RealSense D555")
+    {
+        // currently the PID of DDS devices is hardcoded as "DDS"
+        // need to be fixed in librealsense
+        pid = RS555_PID;
+    }
+    else
+    {
+        pid = std::stoi(pid_str, 0, 16);
+    }
     try
     {
         switch(pid)
@@ -374,6 +393,7 @@ void RealSenseNodeFactory::startDevice()
         case RS435i_RGB_PID:
         case RS455_PID:
         case RS457_PID:
+        case RS555_PID:
         case RS_USB2_PID:
             _realSenseNode = std::unique_ptr<BaseRealSenseNode>(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms()));
             break;

realsense2_camera/src/rs_node_setup.cpp

@@ -295,8 +295,14 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
                 rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos));
             // Publish Intrinsics:
             info_topic_name << "~/" << stream_name << "/imu_info";
+
+            // IMU Info will have latched QoS, and it will publish its data only once during the ROS Node lifetime.
+            // intra-process do not support latched QoS, so we need to disable intra-process for this topic
+            rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+            options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
             _imu_info_publishers[sip] = _node.create_publisher<IMUInfo>(info_topic_name.str(),
-                                        rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));
+                                                                        rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_latched), rmw_qos_profile_latched),
+                                                                        std::move(options));
             IMUInfo info_msg = getImuInfo(profile);
             _imu_info_publishers[sip]->publish(info_msg);
         }
@@ -499,6 +505,12 @@ void BaseRealSenseNode::publishServices()
 {
     // adding "~/" to the service name will add node namespace and node name to the service
     // see "Private Namespace Substitution Character" section on https://design.ros2.org/articles/topic_and_service_names.html
+    _reset_srv = _node.create_service<std_srvs::srv::Empty>(
+            "~/hw_reset",
+            [&](const std_srvs::srv::Empty::Request::SharedPtr req,
+                        std_srvs::srv::Empty::Response::SharedPtr res)
+                        {handleHWReset(req, res);});
+
     _device_info_srv = _node.create_service<realsense2_camera_msgs::srv::DeviceInfo>(
             "~/device_info",
             [&](const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req,
@@ -535,6 +547,32 @@ void BaseRealSenseNode::publishActions()
 
 }
 
+void BaseRealSenseNode::handleHWReset(const std_srvs::srv::Empty::Request::SharedPtr req,
+                                const std_srvs::srv::Empty::Response::SharedPtr res)
+{
+    (void)req;
+    (void)res;
+    ROS_INFO_STREAM("Reset requested through service call");
+    if (_dev)
+    {
+        try
+        {
+            for(auto&& sensor : _available_ros_sensors)
+            {
+                std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME)));
+                ROS_INFO_STREAM("Stopping Sensor: " << module_name);
+                sensor->stop();
+            }
+            ROS_INFO("Resetting device...");
+            _dev.hardware_reset();
+        }
+        catch(const std::exception& ex)
+        {
+            ROS_WARN_STREAM("An exception has been thrown: " << __FILE__ << ":" << __LINE__ << ":" << ex.what());
+        }
+    }
+}
+
 void BaseRealSenseNode::getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr,
                                             realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res)
 {