Support SR300 camera in ROS2 Eloquent

realsense_ros/CMakeLists.txt

@@ -93,6 +93,7 @@ add_library(${PROJECT_NAME} SHARED
   src/rs_d435.cpp
   src/rs_d435i.cpp
   src/rs_t265.cpp
+  src/rs_sr300.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

realsense_ros/include/realsense/rs_constants.hpp

@@ -24,6 +24,7 @@ namespace realsense
   const uint16_t RS435i_RGB_PID   = 0x0B3A; // AWGC_MM
   const uint16_t RS_T265_PID      = 0x0b37; // T265
   const uint16_t RS_USB2_PID      = 0x0ad6; // USB2
+  const uint16_t RS_SR300_PID     = 0x0aa5; // SR300
   using stream_index_pair = std::pair<rs2_stream, int>;
 
   const stream_index_pair COLOR{RS2_STREAM_COLOR, 0};

realsense_ros/include/realsense/rs_sr300.hpp

@@ -0,0 +1,59 @@
+// Copyright (c) 2019 Intel Corporation. All Rights Reserved
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef REALSENSE__RS_SR300_HPP_
+#define REALSENSE__RS_SR300_HPP_
+
+#include "cv_bridge/cv_bridge.h"
+#include "sensor_msgs/msg/camera_info.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "realsense/rs_base.hpp"
+#include "realsense/rs_constants.hpp"
+
+using namespace rclcpp::node_interfaces;
+
+namespace realsense
+{
+class RealSenseSR300 : public RealSenseBase
+{
+public:
+  RealSenseSR300(rs2::context ctx, rs2::device dev, rclcpp::Node & node);
+  virtual ~RealSenseSR300() = default;
+  virtual void publishTopicsCallback(const rs2::frame & frame) override;
+  virtual Result paramChangeCallback(const std::vector<rclcpp::Parameter> & params) override;
+  void publishAlignedDepthTopic(const rs2::frame & frame, const rclcpp::Time & time);
+  void publishSparsePointCloud(const rs2::points & points, const rs2::video_frame & color_frame, const rclcpp::Time & time);
+  void publishDensePointCloud(const rs2::points & points, const rs2::video_frame & color_frame, const rclcpp::Time & time);
+  void updateStreamCalibData(const rs2::video_stream_profile & video_profile);
+
+protected:
+  bool align_depth_;
+  bool enable_pointcloud_;
+  bool dense_pc_;
+  bool initialized_ = false;
+  rs2::align align_to_color_ = rs2::align(RS2_STREAM_COLOR);
+  rs2::pointcloud pc_;
+  rs2::points points_;
+
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr aligned_depth_image_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr aligned_depth_info_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
+
+private:
+  const std::vector<stream_index_pair> IMAGE_STREAMS = {COLOR, DEPTH, INFRA1};
+};
+}  // namespace realsense
+#endif  // REALSENSE__RS_SR300_HPP_

realsense_ros/src/rs_factory.cpp

@@ -17,6 +17,7 @@
 #include "realsense/rs_d435.hpp"
 #include "realsense/rs_d435i.hpp"
 #include "realsense/rs_t265.hpp"
+#include "realsense/rs_sr300.hpp"
 
 namespace realsense
 {
@@ -92,9 +93,14 @@ void RealSenseNodeFactory::startDevice()
         RCLCPP_INFO(this->get_logger(), "Create a node for T265 Camera");
         rs_node_ = std::make_unique<RealSenseT265>(ctx_, dev_, *this);
         break;
+      case RS_SR300_PID:
+        RCLCPP_INFO(this->get_logger(), "Create a node for SR300 Camera");
+        rs_node_ = std::make_unique<RealSenseSR300>(ctx_, dev_, *this);
+        break;
       default:
         RCLCPP_ERROR(this->get_logger(), "Unsupported device! Product ID: 0x%s", pid_str);
         rclcpp::shutdown();
+
     }
   rs_node_->startPipeline();
 }
@@ -152,4 +158,4 @@ void RealSenseNodeFactory::getDevice(rs2::device_list & list)
 }  // namespace realsense
 
 #include "rclcpp_components/register_node_macro.hpp"
-RCLCPP_COMPONENTS_REGISTER_NODE(realsense::RealSenseNodeFactory)
+RCLCPP_COMPONENTS_REGISTER_NODE(realsense::RealSenseNodeFactory)