Modify Depth Camera Driver sim code

- Reuse CameraDriverSim module

bringup/config/params.camera_driver.yaml

@@ -9,6 +9,7 @@ camera_driver:
 
     remapping_list: ['/tf', '/tf_static']
 
+    topic_name: "rgb"
     frame_id: "camera_link"
 
     transform: [0.344000, 0.0, 1.014100, 0.0, 0.0, 0.0]

bringup/config/params.depth_camera_driver.yaml

@@ -3,12 +3,13 @@ depth_camera_driver:
     use_sim_time: True
     sim:
       model: "cloi"
-      parts: "depthcamera"
+      parts: "realsense"
       manager_ip: "127.0.0.1"
       manager_port: 25554
 
     remapping_list: ['/tf', '/tf_static']
 
+    topic_name: "depth"
     frame_id: "link"
 
     transform: [0.344000, 0.0, 1.014100, 0.0, 0.0, 0.0]

camera_driver_sim/include/camera_driver_sim/CameraDriverSim.hpp

@@ -23,17 +23,17 @@
 class CameraDriverSim : public DriverSim
 {
 public:
-  CameraDriverSim();
+  explicit CameraDriverSim(const std::string node_name = "camera_driver_sim");
   virtual ~CameraDriverSim();
 
-private:
-  virtual void Initialize();
-  virtual void Deinitialize();
-  virtual void UpdateData();
+protected:
+  virtual void Initialize() override;
+  virtual void Deinitialize() override;
+  virtual void UpdateData() override;
 
   void InitializeCameraInfoMessage();
 
-private:
+protected:
   // key for connection
   std::string m_hashKeySub;
 

camera_driver_sim/src/CameraDriverSim.cpp

@@ -21,8 +21,8 @@
 using namespace std;
 using namespace chrono_literals;
 
-CameraDriverSim::CameraDriverSim()
-    : DriverSim("camera_driver_sim", 2)
+CameraDriverSim::CameraDriverSim(const std::string node_name)
+    : DriverSim(node_name, 2)
 {
   Start();
 }
@@ -36,14 +36,14 @@ CameraDriverSim::~CameraDriverSim()
 void CameraDriverSim::Initialize()
 {
   string frame_id_;
-  string camera_name_;
+  string topic_name_;
   vector<double> transform_;
 
+  get_parameter_or("topic_name", topic_name_, string("topic"));
   get_parameter_or("frame_id", frame_id_, string("camera_link"));
-  get_parameter_or("camera_name", camera_name_, string("camera"));
   get_parameter_or("transform", transform_, vector<double>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
 
-  const auto topic_base_name_ = camera_name_ + "/rgb";
+  const auto topic_base_name_ = GetPartsName() + "/" + topic_name_;
   m_hashKeySub = GetRobotName() + GetPartsName();
 
   DBG_SIM_INFO("[CONFIG] topic_name:%s", topic_base_name_.c_str());
@@ -75,7 +75,7 @@ void CameraDriverSim::Initialize()
   pubCameraInfo = create_publisher<sensor_msgs::msg::CameraInfo>(topic_base_name_ + "/camera_info", rclcpp::SensorDataQoS());
 
   // Initialize camera_info_manager
-  cameraInfoManager = std::make_shared<camera_info_manager::CameraInfoManager>(GetNode(), camera_name_);
+  cameraInfoManager = std::make_shared<camera_info_manager::CameraInfoManager>(GetNode(), GetPartsName());
 
   InitializeCameraInfoMessage();
 }

camera_driver_sim/src/main.cpp

@@ -22,7 +22,7 @@ int main(int argc, char** argv)
   rclcpp::sleep_for(100ms);
 
   auto node = std::make_shared<CameraDriverSim>();
-  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::spin(node);
   rclcpp::shutdown();
   return 0;
-}
+}

depth_camera_driver_sim/include/depth_camera_driver_sim/DepthCameraDriverSim.hpp

@@ -4,7 +4,7 @@
  *  @author Sungkyu Kang
  *  @author hyunseok Yang
  *  @brief
- *        ROS2 Depth                                                                    Camera Driver class for simulator
+ *        ROS2 Depth Camera Driver class for simulator
  *  @remark
  *  @warning
  *       LGE Advanced Robotics Laboratory
@@ -14,48 +14,27 @@
 #ifndef _DepthCameraDriverSim_H_
 #define _DepthCameraDriverSim_H_
 
-#include "driver_sim/driver_sim.hpp"
-#include <image_transport/image_transport.h>
-#include <camera_info_manager/camera_info_manager.h>
-#include <sensor_msgs/msg/camera_info.hpp>
+#include "camera_driver_sim/CameraDriverSim.hpp"
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <protobuf/image_stamped.pb.h>
 
-class DepthCameraDriverSim : public DriverSim
+class DepthCameraDriverSim : public CameraDriverSim
 {
 public:
   DepthCameraDriverSim();
   virtual ~DepthCameraDriverSim();
 
 private:
-  virtual void Initialize();
-  virtual void Deinitialize();  
-  virtual void UpdateData();
-
-  void InitializeCameraInfoManager();
+  virtual void Initialize() override;
+  virtual void Deinitialize() override;
+  virtual void UpdateData() override;
 
 private:
-  // key for connection
-  std::string m_hashKeySub;
-
-  // buffer from simulation
-  gazebo::msgs::ImageStamped m_pbBuf;
-
-  // message for ROS2 communictaion
-  sensor_msgs::msg::Image msg_img;
+  // Camera info publisher
+  // rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr pubDepthCameraInfo{nullptr};
 
   // Store current point cloud.
   // sensor_msgs::msg::PointCloud2 msg_pc2;
 
-  // Camera info publisher
-  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr pubDepthCameraInfo{nullptr};
-
-  // Depth Camera info manager
-  std::shared_ptr<camera_info_manager::CameraInfoManager> cameraInfoManager;
-
-  // Depth image publisher.
-  image_transport::Publisher pubDepthImage;
-
   // Point cloud publisher.
   // rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubPointCloud;
 };

depth_camera_driver_sim/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depth_camera_driver_sim</name>
-  <version>1.1.0</version>
+  <version>1.1.1</version>
   <description>virtual depth camera driver for simulator</description>
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
@@ -14,12 +14,14 @@
   <depend>camera_info_manager</depend>
   <depend>image_transport</depend>
 
-  <build_depend>driver_sim</build_depend>
   <build_depend>sim_bridge</build_depend>
+  <build_depend>driver_sim</build_depend>
+  <build_depend>camera_driver_sim</build_depend>
   <build_depend>rclcpp</build_depend>
 
   <exec_depend>sim_bridge</exec_depend>
   <exec_depend>driver_sim</exec_depend>
+  <exec_depend>camera_driver_sim</exec_depend>
   <exec_depend>rclcpp</exec_depend>
 
   <export>