Return number of samples in service

Use multithreaded spinner to enable servicing in parallel.

src/cam_lidar_panel.cpp

@@ -15,6 +15,7 @@ CamLidarPanel::CamLidarPanel(QWidget* parent) : rviz::Panel(parent)
   discard_button_ = new QPushButton("Discard last sample");
   connect(discard_button_, SIGNAL(clicked()), this, SLOT(discardSample()));
   optimise_button_ = new QPushButton("Optimise");
+  optimise_button_->setEnabled(false);
   connect(optimise_button_, SIGNAL(clicked()), this, SLOT(optimise()));
 
   button_layout->addWidget(capture_button_);
@@ -29,6 +30,14 @@ void CamLidarPanel::captureSample()
   Optimise srv;
   srv.request.operation = Optimise::Request::CAPTURE;
   optimise_client_.call(srv);
+  if (srv.response.samples < 3)
+  {
+    optimise_button_->setEnabled(false);
+  }
+  else
+  {
+    optimise_button_->setEnabled(true);
+  }
 }
 
 void CamLidarPanel::discardSample()
@@ -36,6 +45,14 @@ void CamLidarPanel::discardSample()
   Optimise srv;
   srv.request.operation = Optimise::Request::DISCARD;
   optimise_client_.call(srv);
+  if (srv.response.samples < 3)
+  {
+    optimise_button_->setEnabled(false);
+  }
+  else
+  {
+    optimise_button_->setEnabled(true);
+  }
 }
 
 void CamLidarPanel::optimise()

src/feature_extraction_node.cpp

@@ -9,10 +9,12 @@ int main(int argc, char** argv)
   cam_lidar_calibration::FeatureExtractor feature_extractor;
   ros::Rate loop_rate(10);
 
+  ros::AsyncSpinner spinner(4);
+  spinner.start();
+
   while (ros::ok())
   {
     feature_extractor.visualiseSamples();
-    ros::spinOnce();
     loop_rate.sleep();
   }
   return 0;

src/feature_extractor.cpp

@@ -92,6 +92,11 @@ bool FeatureExtractor::serviceCB(Optimise::Request& req, Optimise::Response& res
   }
   publishBoardPointCloud();
   flag = req.operation;  // read flag published by rviz calibration panel
+  // Wait for operation to complete
+  while (flag == Optimise::Request::CAPTURE)
+  {
+  }
+  res.samples = optimiser_->samples_.size();
   return true;
 }
 
@@ -443,8 +448,6 @@ void FeatureExtractor::extractRegionOfInterest(const sensor_msgs::Image::ConstPt
 
   if (flag == Optimise::Request::CAPTURE)
   {
-    flag = 0;  // Reset the capture flag
-
     ROS_INFO("Processing sample");
 
     auto [corner_vectors, chessboard_normal] = locateChessboard(image);
@@ -524,6 +527,8 @@ void FeatureExtractor::extractRegionOfInterest(const sensor_msgs::Image::ConstPt
     }
     // Push this sample to the optimiser
     optimiser_->samples_.push_back(sample);
+    flag = Optimise::Request::READY;  // Reset the capture flag
+
   }  // if (flag == Optimise::Request::CAPTURE)
 }  // End of extractRegionOfInterest
 

srv/Optimise.srv

 int8 operation
 
+int8 READY=0
 int8 CAPTURE=1
 int8 OPTIMISE=2
 int8 DISCARD=3
 int8 USE=4
 ---
+uint8 samples
+geometry_msgs/Transform transform