fixed error msg 'Tried to advertise a service that is already adverti…

…sed in this node [/camera/camera/set_camera_info]'
orbbec · Jan 11, 2023 · af69db6 · af69db6
1 parent 506e525
commit af69db6
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 5 deletions.
diff --git a/src/ob_camera_info.cpp b/src/ob_camera_info.cpp
@@ -26,8 +26,8 @@ OBCameraParams OBCameraNode::getCameraParams() {
     camera_params_ = params;
     return params;
   } else if (pid == DABAI_MAX_PID) {
-      ROS_ERROR_STREAM("current SDK not support dabai max camera");
-      throw std::runtime_error("current SDK not support dabai max camera");
+    ROS_ERROR_STREAM("current SDK not support dabai max camera");
+    throw std::runtime_error("current SDK not support dabai max camera");
   } else {
     OBCameraParamsData camera_params_data;
     int data_size = sizeof(camera_params_data);
@@ -231,7 +231,7 @@ sensor_msgs::CameraInfo OBCameraNode::getDepthCameraInfo() {
 sensor_msgs::CameraInfo OBCameraNode::getColorCameraInfo() {
   int width = width_[COLOR];
   int height = height_[COLOR];
-  if (color_info_manager_->isCalibrated()) {
+  if (color_info_manager_ && color_info_manager_->isCalibrated()) {
     auto camera_info = color_info_manager_->getCameraInfo();
     if (camera_info.width != static_cast<uint32_t>(width) ||
         camera_info.height != static_cast<uint32_t>(height)) {

diff --git a/src/ob_camera_node.cpp b/src/ob_camera_node.cpp
@@ -94,8 +94,10 @@ void OBCameraNode::init() {
   auto serial_number = getSerialNumber();
   ir_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(
       nh_private_, "ir_camera", ir_info_uri_);
-  color_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(
-      nh_private_, "rgb_camera", color_info_uri_);
+  if(device_->hasSensor(openni::SENSOR_COLOR)) {
+    color_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(
+        nh_, "rgb_camera", color_info_uri_);
+  }
   if (keep_alive_) {
     keep_alive_timer_ =
         nh_.createTimer(ros::Duration(keep_alive_interval_), &OBCameraNode::sendKeepAlive, this);