Merge pull request moveit#91 from JStech/debug-missing-intrinsics

fix missing intrinsics on target type change
cschindlbeck · Jun 8, 2021 · 021d04e · 021d04e
2 parents b58edf6 + a7fe8c4
commit 021d04e
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 23 deletions.
diff --git a/...ration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h b/...ration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h
@@ -184,7 +184,7 @@ private Q_SLOTS:
 
   std::string optical_frame_;
 
-  sensor_msgs::CameraInfoPtr camera_info_;
+  sensor_msgs::CameraInfoConstPtr camera_info_;
 
   // **************************************************************
   // Ros components

diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp
@@ -151,9 +151,6 @@ TargetTabWidget::TargetTabWidget(QWidget* parent)
   // Initialize image publisher
   image_pub_ = it_.advertise("/handeye_calibration/target_detection", 1);
 
-  // Initialize camera info dada
-  camera_info_.reset(new sensor_msgs::CameraInfo());
-
   // Register custom types
   qRegisterMetaType<sensor_msgs::CameraInfo>();
   qRegisterMetaType<std::string>();
@@ -434,25 +431,13 @@ void TargetTabWidget::imageCallback(const sensor_msgs::ImageConstPtr& msg)
 
 void TargetTabWidget::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg)
 {
-  if (target_)
+  if (target_ && msg->height > 0 && msg->width > 0 && !msg->K.empty() && !msg->D.empty() &&
+      (msg->K != camera_info_->K || msg->P != camera_info_->P))
   {
-    if (msg->height > 0 && msg->width > 0 && !msg->K.empty() && !msg->D.empty())
-    {
-      if (msg->K != camera_info_->K || msg->P != camera_info_->P)
-      {
-        ROS_DEBUG("Received camera info.");
-        camera_info_->header = msg->header;
-        camera_info_->height = msg->height;
-        camera_info_->width = msg->width;
-        camera_info_->distortion_model = msg->distortion_model;
-        camera_info_->D = msg->D;
-        camera_info_->K = msg->K;
-        camera_info_->R = msg->R;
-        camera_info_->P = msg->P;
-        target_->setCameraIntrinsicParams(camera_info_);
-        Q_EMIT cameraInfoChanged(*camera_info_);
-      }
-    }
+    ROS_DEBUG_NAMED(LOGNAME, "Received camera info.");
+    camera_info_ = msg;
+    target_->setCameraIntrinsicParams(camera_info_);
+    Q_EMIT cameraInfoChanged(*camera_info_);
   }
 }
 
@@ -461,6 +446,10 @@ void TargetTabWidget::targetTypeComboboxChanged(const QString& text)
   if (!text.isEmpty())
   {
     loadInputWidgetsForTargetType(text.toStdString());
+    if (target_)
+    {
+      target_->setCameraIntrinsicParams(camera_info_);
+    }
   }
 }
 

diff --git a/...andeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h b/...andeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
@@ -210,7 +210,7 @@ class HandEyeTargetBase
    * @param msg Input camera info message.
    * @return True if the input camera info format is correct, false otherwise.
    */
-  virtual bool setCameraIntrinsicParams(const sensor_msgs::CameraInfoPtr& msg)
+  virtual bool setCameraIntrinsicParams(const sensor_msgs::CameraInfoConstPtr& msg)
   {
     if (!msg)
     {