From aa5194f074b96e138500f488d3e9faa3d885e5af Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Mon, 1 Jul 2024 12:01:01 +0200 Subject: [PATCH] Added 'use_first_detection' param (need testing) Signed-off-by: Alberto Tudela --- .../scitos2_charging_dock/charging_dock.hpp | 2 +- .../scitos2_charging_dock/perception.hpp | 4 ++ scitos2_charging_dock/src/perception.cpp | 26 +++++++- scitos2_charging_dock/test/CMakeLists.txt | 60 +++++-------------- .../test/test_perception.cpp | 4 +- 5 files changed, 46 insertions(+), 50 deletions(-) diff --git a/scitos2_charging_dock/include/scitos2_charging_dock/charging_dock.hpp b/scitos2_charging_dock/include/scitos2_charging_dock/charging_dock.hpp index 7341c25..224445c 100644 --- a/scitos2_charging_dock/include/scitos2_charging_dock/charging_dock.hpp +++ b/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 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_; diff --git a/scitos2_charging_dock/include/scitos2_charging_dock/perception.hpp b/scitos2_charging_dock/include/scitos2_charging_dock/perception.hpp index b3ca344..fb8b579 100644 --- a/scitos2_charging_dock/include/scitos2_charging_dock/perception.hpp +++ b/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_; diff --git a/scitos2_charging_dock/src/perception.cpp b/scitos2_charging_dock/src/perception.cpp index f232be4..3ac11e0 100644 --- a/scitos2_charging_dock/src/perception.cpp +++ b/scitos2_charging_dock/src/perception.cpp @@ -56,6 +56,8 @@ 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_); @@ -63,6 +65,7 @@ Perception::Perception( 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(); } } } diff --git a/scitos2_charging_dock/test/CMakeLists.txt b/scitos2_charging_dock/test/CMakeLists.txt index 226e173..1daf3bc 100644 --- a/scitos2_charging_dock/test/CMakeLists.txt +++ b/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} -) \ No newline at end of file +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}) \ No newline at end of file diff --git a/scitos2_charging_dock/test/test_perception.cpp b/scitos2_charging_dock/test/test_perception.cpp index 9911118..92a8de9 100644 --- a/scitos2_charging_dock/test/test_perception.cpp +++ b/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();