#18 add double camera node

Author: KingRichord

example/NetCam/main_ros2.cpp

@@ -12,21 +12,27 @@ class CamDriver final : public rclcpp::Node {
  public:
   CamDriver()
       : Node("gige_driver"), node_handle_(std::shared_ptr<CamDriver>(this, [](auto *) {})), transport_(node_handle_) {
-    std::string camera_name = "cam_1";
+    std::string camera_name_1 = "cam_1";
+    std::string camera_name_2 = "cam_2";
     const std::string imu_name = "imu_1";
     // synchronizer_.SetUsbLink("/dev/ttyACM0", 921600);
     synchronizer_.SetNetLink("192.168.1.188", 8888);
     const auto mv_cam = std::make_shared<infinite_sense::MvCam>();
-    mv_cam->SetParams({{camera_name, infinite_sense::CAM_1}});
+    mv_cam->SetParams({{camera_name_1, infinite_sense::CAM_1},
+                          {camera_name_2, infinite_sense::CAM_2},
+    });
     synchronizer_.UseSensor(mv_cam);
     synchronizer_.Start();
     imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_name, 10);
-    img_pub_ = transport_.advertise(camera_name, 10);
+    img1_pub_ = transport_.advertise(camera_name_1, 10);
+    img2_pub_ = transport_.advertise(camera_name_2, 10);
     {
       using namespace std::placeholders;
       infinite_sense::Messenger::GetInstance().SubStruct(imu_name, std::bind(&CamDriver::ImuCallback, this, _1, _2));
-      infinite_sense::Messenger::GetInstance().SubStruct(camera_name,
-                                                         std::bind(&CamDriver::ImageCallback, this, _1, _2));
+      infinite_sense::Messenger::GetInstance().SubStruct(camera_name_1,
+                                                         std::bind(&CamDriver::ImageCallback1, this, _1, _2));
+      infinite_sense::Messenger::GetInstance().SubStruct(camera_name_2,
+                                                         std::bind(&CamDriver::ImageCallback2, this, _1, _2));
     }
   }
 
@@ -48,21 +54,32 @@ class CamDriver final : public rclcpp::Node {
     imu_pub_->publish(imu_msg);
   }
 
-  void ImageCallback(const void *msg, size_t) const {
+  void ImageCallback1(const void *msg, size_t) const {
     const auto *cam_data = static_cast<const infinite_sense::CamData *>(msg);
     std_msgs::msg::Header header;
     header.stamp = rclcpp::Time(cam_data->time_stamp_us * 1000);
     header.frame_id = "map";
     const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC1, cam_data->image.data);
     const sensor_msgs::msg::Image::SharedPtr image_msg = cv_bridge::CvImage(header, "mono8", image_mat).toImageMsg();
-    img_pub_.publish(image_msg);
+    img1_pub_.publish(image_msg);
   }
 
+  void ImageCallback2(const void *msg, size_t) const {
+    const auto *cam_data = static_cast<const infinite_sense::CamData *>(msg);
+    std_msgs::msg::Header header;
+    header.stamp = rclcpp::Time(cam_data->time_stamp_us * 1000);
+    header.frame_id = "map";
+    const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC1, cam_data->image.data);
+    const sensor_msgs::msg::Image::SharedPtr image_msg = cv_bridge::CvImage(header, "mono8", image_mat).toImageMsg();
+    img2_pub_.publish(image_msg);
+  }
+
+
  private:
   infinite_sense::Synchronizer synchronizer_;
   SharedPtr node_handle_;
   image_transport::ImageTransport transport_;
-  image_transport::Publisher img_pub_;
+  image_transport::Publisher img1_pub_,img2_pub_;
   rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
 };