Added 'use_first_detection' param (need testing)

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

scitos2_charging_dock/include/scitos2_charging_dock/charging_dock.hpp

@@ -129,7 +129,7 @@ class ChargingDock : public opennav_docking_core::ChargingDock
   // Filtering of detected poses
   std::shared_ptr<opennav_docking::PoseFilter> filter_;
 
-  // external pose reference, this is the distance threshold
+  // External pose reference, this is the distance threshold
   double docking_threshold_;
   // Offset for staging pose relative to dock pose
   double staging_x_offset_;

scitos2_charging_dock/include/scitos2_charging_dock/perception.hpp

@@ -180,6 +180,10 @@ class Perception
   geometry_msgs::msg::PoseStamped detected_dock_pose_;
   // Dock template pointcloud
   Pcloud::Ptr dock_template_;
+  // Dock found
+  bool dock_found_;
+  // Only use the first detection
+  bool use_first_detection_{false};
 
   std::string name_;
   rclcpp::Clock::SharedPtr clock_;

scitos2_charging_dock/src/perception.cpp

@@ -56,13 +56,16 @@ Perception::Perception(
     node, name_ + ".perception.enable_debug", rclcpp::ParameterValue(false));
   nav2_util::declare_parameter_if_not_declared(
     node, name_ + ".perception.dock_template", rclcpp::ParameterValue(""));
+  nav2_util::declare_parameter_if_not_declared(
+    node, name_ + ".perception.use_first_detection", rclcpp::ParameterValue(false));
 
   node->get_parameter(name_ + ".perception.icp_min_score", icp_min_score_);
   node->get_parameter(name_ + ".perception.icp_max_iter", icp_max_iter_);
   node->get_parameter(name_ + ".perception.icp_max_corr_dis", icp_max_corr_dis_);
   node->get_parameter(name_ + ".perception.icp_max_trans_eps", icp_max_trans_eps_);
   node->get_parameter(name_ + ".perception.icp_max_eucl_fit_eps", icp_max_eucl_fit_eps_);
   node->get_parameter(name_ + ".perception.enable_debug", debug_);
+  node->get_parameter(name_ + ".perception.use_first_detection", use_first_detection_);
 
   // Load the dock template
   std::string dock_template;
@@ -102,11 +105,25 @@ geometry_msgs::msg::PoseStamped Perception::getDockPose(const sensor_msgs::msg::
   // Set the header of the template to the scan header
   dock_template_->header.frame_id = scan.header.frame_id;
 
-  // Refine the pose of each cluster to get the dock pose
+  // Refine the pose of each cluster to get the dock pose the first time
   geometry_msgs::msg::PoseStamped dock_pose;
-  if (refineAllClustersPoses(clusters, dock_template_, dock_pose)) {
-    detected_dock_pose_ = dock_pose;
+  if (!dock_found_) {
+    if (refineAllClustersPoses(clusters, dock_template_, dock_pose)) {
+      dock_found_ = true;
+      detected_dock_pose_ = dock_pose;
+    }
   }
+
+  // If we use the first detection, update the timestamp
+  // Otherwise, refine the pose of the clusters to get the new dock pose
+  if (use_first_detection_) {
+    detected_dock_pose_.header.stamp = clock_->now();
+  } else {
+    if (refineAllClustersPoses(clusters, dock_template_, dock_pose)) {
+      detected_dock_pose_ = dock_pose;
+    }
+  }
+
   return detected_dock_pose_;
 }
 
@@ -116,6 +133,7 @@ void Perception::setInitialEstimate(
   initial_estimate_pose_.pose = pose;
   initial_estimate_pose_.header.frame_id = frame;
   initial_estimate_pose_.header.stamp = clock_->now();
+  dock_found_ = false;
 }
 
 bool Perception::loadDockPointcloud(std::string filepath, Pcloud & dock)
@@ -373,6 +391,8 @@ rcl_interfaces::msg::SetParametersResult Perception::dynamicParametersCallback(
     } else if (type == rclcpp::ParameterType::PARAMETER_BOOL) {
       if (name == name_ + ".perception.enable_debug") {
         debug_ = parameter.as_bool();
+      } else if (name == name_ + ".perception.use_first_detection") {
+        use_first_detection_ = parameter.as_bool();
       }
     }
   }

scitos2_charging_dock/test/CMakeLists.txt

@@ -1,54 +1,24 @@
 # Test cluster
-ament_add_gtest(test_scitos2_cluster
-  test_cluster.cpp
-)
-ament_target_dependencies(test_scitos2_cluster
-  ${dependencies}
-)
-target_link_libraries(test_scitos2_cluster
-  ${library_name}
-)
+ament_add_gtest(test_scitos2_cluster test_cluster.cpp)
+ament_target_dependencies(test_scitos2_cluster ${dependencies})
+target_link_libraries(test_scitos2_cluster ${library_name})
 
 # Test segmentation
-ament_add_gtest(test_scitos2_segmentation
-  test_segmentation.cpp
-)
-ament_target_dependencies(test_scitos2_segmentation
-  ${dependencies}
-)
-target_link_libraries(test_scitos2_segmentation
-  ${library_name}
-)
+ament_add_gtest(test_scitos2_segmentation test_segmentation.cpp)
+ament_target_dependencies(test_scitos2_segmentation ${dependencies})
+target_link_libraries(test_scitos2_segmentation ${library_name})
 
 # Test perception
-ament_add_gtest(test_scitos2_perception
-  test_perception.cpp
-)
-ament_target_dependencies(test_scitos2_perception
-  ${dependencies}
-)
-target_link_libraries(test_scitos2_perception
-  ${library_name}
-)
+ament_add_gtest(test_scitos2_perception test_perception.cpp)
+ament_target_dependencies(test_scitos2_perception ${dependencies})
+target_link_libraries(test_scitos2_perception ${library_name})
 
 # Test charging dock
-ament_add_gtest(test_scitos2_charging_dock
-  test_charging_dock.cpp
-)
-ament_target_dependencies(test_scitos2_charging_dock
-  ${dependencies}
-)
-target_link_libraries(test_scitos2_charging_dock
-  ${library_name}
-)
+ament_add_gtest(test_scitos2_charging_dock test_charging_dock.cpp)
+ament_target_dependencies(test_scitos2_charging_dock ${dependencies})
+target_link_libraries(test_scitos2_charging_dock ${library_name})
 
 # Test dock saver
-ament_add_gtest(test_dock_saver
-  test_dock_saver.cpp
-)
-ament_target_dependencies(test_dock_saver
-  ${dependencies}
-)
-target_link_libraries(test_dock_saver
-  ${library_name}
-)
+ament_add_gtest(test_dock_saver test_dock_saver.cpp)
+ament_target_dependencies(test_dock_saver ${dependencies})
+target_link_libraries(test_dock_saver ${library_name})

scitos2_charging_dock/test/test_perception.cpp

@@ -103,7 +103,8 @@ TEST(ScitosDockingPerception, dynamicParameters) {
       rclcpp::Parameter("test.perception.icp_max_corr_dis", 0.5),
       rclcpp::Parameter("test.perception.icp_max_trans_eps", 0.5),
       rclcpp::Parameter("test.perception.icp_max_eucl_fit_eps", 0.5),
-      rclcpp::Parameter("test.perception.enable_debug", false)});
+      rclcpp::Parameter("test.perception.enable_debug", false),
+      rclcpp::Parameter("test.perception.use_first_detection", true)});
 
   // Spin
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
@@ -115,6 +116,7 @@ TEST(ScitosDockingPerception, dynamicParameters) {
   EXPECT_DOUBLE_EQ(node->get_parameter("test.perception.icp_max_trans_eps").as_double(), 0.5);
   EXPECT_DOUBLE_EQ(node->get_parameter("test.perception.icp_max_eucl_fit_eps").as_double(), 0.5);
   EXPECT_EQ(node->get_parameter("test.perception.enable_debug").as_bool(), false);
+  EXPECT_EQ(node->get_parameter("test.perception.use_first_detection").as_bool(), true);
 
   // Cleaning up
   node->deactivate();