Improve perception

Signed-off-by: Alberto Tudela <[email protected]>

.github/workflows/build.yml

@@ -39,7 +39,6 @@ jobs:
           package-name: |
             scitos2
             scitos2_behavior_tree
-            scitos2_charging_dock
             scitos2_common
             scitos2_core
             scitos2_mira

scitos2_charging_dock/README.md

@@ -36,6 +36,10 @@ A **save_dock** service is provided to save the current pointcloud of the chargi
 
 ### Parameters
 
+* **`docking_threshold`** (double, default: 0.05)
+
+	The pose threshold to the docking pose where `isDocked() = true`.
+
 * **`staging_x_offset`** (double, default: -0.7)
 
 	Staging pose offset forward (negative) of dock pose (m).

scitos2_charging_dock/include/scitos2_charging_dock/charging_dock.hpp

@@ -129,6 +129,8 @@ 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
+  double docking_threshold_;
   // Offset for staging pose relative to dock pose
   double staging_x_offset_;
   double staging_yaw_offset_;

scitos2_charging_dock/include/scitos2_charging_dock/cluster.hpp

@@ -37,14 +37,14 @@ namespace scitos2_charging_dock
  */
 struct Cluster
 {
+  // Identifier of the cluster
+  int id;
   // Original pointcloud of the cluster.
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
   // Pointcloud of the cluster after perform ICP.
   pcl::PointCloud<pcl::PointXYZ>::Ptr matched_cloud;
   // Score of the ICP.
   double icp_score;
-  // Distance from the initial pose.
-  double distance_from_initial_pose;
   // Pose of the dock.
   geometry_msgs::msg::PoseStamped icp_pose;
 
@@ -71,7 +71,9 @@ struct Cluster
    */
   double width() const
   {
-    if (!cloud) {return 0;}
+    if (!cloud) {
+      return 0.0;
+    }
 
     double dx = cloud->back().x - cloud->front().x;
     double dy = cloud->back().y - cloud->front().y;

scitos2_charging_dock/params/example.yaml

@@ -32,6 +32,7 @@ docking_server:
     dock_database: ''
     scitos_dock:
       plugin: "scitos2_charging_dock::ChargingDock"
+      docking_threshold: 0.05
       staging_x_offset: -0.75
       staging_yaw_offset: 0.0
       external_detection_timeout: 5.0

scitos2_charging_dock/src/charging_dock.cpp

@@ -51,6 +51,10 @@ void ChargingDock::configure(
   nav2_util::declare_parameter_if_not_declared(
     node_, name + ".filter_coef", rclcpp::ParameterValue(0.1));
 
+  // This is how close robot should get to pose
+  nav2_util::declare_parameter_if_not_declared(
+    node_, name + ".docking_threshold", rclcpp::ParameterValue(0.05));
+
   // Staging pose configuration
   nav2_util::declare_parameter_if_not_declared(
     node_, name + ".staging_x_offset", rclcpp::ParameterValue(-0.7));
@@ -67,6 +71,7 @@ void ChargingDock::configure(
   node_->get_parameter(name + ".external_detection_rotation_pitch", pitch);
   node_->get_parameter(name + ".external_detection_rotation_roll", roll);
   external_detection_rotation_.setEuler(pitch, roll, yaw);
+  node_->get_parameter(name + ".docking_threshold", docking_threshold_);
   node_->get_parameter(name + ".staging_x_offset", staging_x_offset_);
   node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_);
 
@@ -191,7 +196,27 @@ bool ChargingDock::isCharging()
 
 bool ChargingDock::isDocked()
 {
-  return is_charging_;
+  if (dock_pose_.header.frame_id.empty()) {
+    // Dock pose is not yet valid
+    return false;
+  }
+
+  // Find base pose in target frame
+  geometry_msgs::msg::PoseStamped base_pose;
+  base_pose.header.stamp = rclcpp::Time(0);
+  base_pose.header.frame_id = "base_link";
+  base_pose.pose.orientation.w = 1.0;
+  try {
+    tf2_buffer_->transform(base_pose, base_pose, dock_pose_.header.frame_id);
+  } catch (const tf2::TransformException & ex) {
+    return false;
+  }
+
+  // If we are close enough, pretend we are charging
+  double d = std::hypot(
+    base_pose.pose.position.x - dock_pose_.pose.position.x,
+    base_pose.pose.position.y - dock_pose_.pose.position.y);
+  return d < docking_threshold_;
 }
 
 bool ChargingDock::disableCharging()

scitos2_charging_dock/src/perception.cpp

@@ -161,8 +161,10 @@ Clusters Perception::extractClustersFromScan(const sensor_msgs::msg::LaserScan &
 Clusters Perception::segmentsToClusters(std::string frame, const Segments & segments)
 {
   Clusters clusters;
+  int count = 0;
   for (const auto & segment : segments) {
     Cluster cluster;
+    cluster.id = count++;
     cluster.matched_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
     cluster.cloud = segmentToPcloud(frame, segment);
     clusters.push_back(cluster);
@@ -222,15 +224,15 @@ bool Perception::refineClusterPose(Cluster & cluster, Pcloud::ConstPtr cloud_tem
     cluster.icp_pose.pose = pose_correct;
     cluster.matched_cloud = matched_cloud;
 
-    RCLCPP_DEBUG(logger_, "ICP converged, cloud matched!");
+    success = true;
+
+    RCLCPP_DEBUG(logger_, "ICP converged for cluster %i", cluster.id);
     RCLCPP_DEBUG(logger_, "ICP fitness score: %f", cluster.icp_score);
     RCLCPP_DEBUG(
       logger_, "ICP transformation: (%f, %f)",
       cluster.icp_pose.pose.position.x, cluster.icp_pose.pose.position.y);
-
-    success = true;
   } else {
-    RCLCPP_DEBUG(logger_, "ICP did not converge");
+    RCLCPP_DEBUG(logger_, "ICP did not converge for cluster %i", cluster.id);
   }
 
   return success;
@@ -239,11 +241,9 @@ bool Perception::refineClusterPose(Cluster & cluster, Pcloud::ConstPtr cloud_tem
 bool Perception::refineAllClustersPoses(
   Clusters & clusters, Pcloud::ConstPtr cloud_template, geometry_msgs::msg::PoseStamped & dock_pose)
 {
-  long double lowest_score = icp_min_score_;
-  int lowest_score_idx = -1;
   bool success = false;
+  Clusters potential_docks;
 
-  int count = 0;
   for (auto & cluster : clusters) {
     // Discard clusters not valid (i.e. clusters not similar to dock by size, ...)
     double dx = (*cloud_template).back().x - (*cloud_template).front().x;
@@ -308,38 +308,29 @@ bool Perception::refineAllClustersPoses(
       if (cluster.icp_score < icp_min_score_) {
         RCLCPP_DEBUG(
           logger_, "Dock potentially identified at cluster %i with score %f",
-          count, cluster.icp_score);
-        if (lowest_score_idx == -1) {
-          lowest_score_idx = count;
-        }
-
-        if (cluster.icp_score <= clusters[lowest_score_idx].icp_score) {
-          lowest_score_idx = count;
-          lowest_score = clusters[lowest_score_idx].icp_score;
-          RCLCPP_DEBUG(
-            logger_, "Dock identified at cluster %i with score %Lf", lowest_score_idx,
-            lowest_score);
-        }
+          cluster.id, cluster.icp_score);
+        potential_docks.push_back(cluster);
       }
     }
-    count++;
   }
 
   // Check if dock is found
-  if (lowest_score_idx != -1) {
-    dock_pose = clusters[lowest_score_idx].icp_pose;
+  if (!potential_docks.empty()) {
+    // Sort the clusters by ICP score
+    std::sort(potential_docks.begin(), potential_docks.end());
+    dock_pose = potential_docks.front().icp_pose;
     // Publish the dock cloud
     if (debug_) {
       sensor_msgs::msg::PointCloud2 cloud_msg;
-      pcl::toROSMsg(*(clusters[lowest_score_idx].matched_cloud), cloud_msg);
+      pcl::toROSMsg(*(clusters.front().matched_cloud), cloud_msg);
       cloud_msg.header.frame_id = initial_estimate_pose_.header.frame_id;
       cloud_msg.header.stamp = clock_->now();
       dock_cloud_pub_->publish(std::move(cloud_msg));
     }
-    RCLCPP_DEBUG(logger_, "Dock successfully identified at cluster %i", lowest_score_idx);
     success = true;
+    RCLCPP_DEBUG(logger_, "Dock successfully identified at cluster %i", potential_docks.front().id);
   } else {
-    RCLCPP_DEBUG(logger_, "Unable to identify the dock");
+    RCLCPP_WARN(logger_, "Unable to identify the dock");
   }
 
   return success;

scitos2_charging_dock/test/CMakeLists.txt

@@ -1,3 +1,14 @@
+# 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}
+)
+
 # Test segmentation
 ament_add_gtest(test_scitos2_segmentation
   test_segmentation.cpp
@@ -33,7 +44,7 @@ target_link_libraries(test_scitos2_charging_dock
 
 # Test dock saver
 ament_add_gtest(test_dock_saver
-test_dock_saver.cpp
+  test_dock_saver.cpp
 )
 ament_target_dependencies(test_dock_saver
   ${dependencies}

scitos2_charging_dock/test/test_charging_dock.cpp

@@ -140,6 +140,9 @@ TEST(ScitosChargingDock, refinedPoseTest)
   pub->on_activate();
   auto dock = std::make_unique<scitos2_charging_dock::ChargingDock>();
 
+  // Create the TF
+  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+
   // Update parameters to read the test dock template
   std::string pkg = ament_index_cpp::get_package_share_directory("scitos2_charging_dock");
   std::string path = pkg + "/test/dock_test.pcd";
@@ -154,7 +157,7 @@ TEST(ScitosChargingDock, refinedPoseTest)
   nav2_util::declare_parameter_if_not_declared(
     node, "my_dock.segmentation.min_distance", rclcpp::ParameterValue(0.0));
 
-  dock->configure(node, "my_dock", nullptr);
+  dock->configure(node, "my_dock", tf_buffer);
   dock->activate();
 
   geometry_msgs::msg::PoseStamped pose;
@@ -188,6 +191,7 @@ TEST(ScitosChargingDock, refinedPoseTest)
   // as they depend on the perception module and its already tested
   pose.header.frame_id = "my_frame";
   EXPECT_TRUE(dock->getRefinedPose(pose));
+  EXPECT_FALSE(dock->isDocked());
 
   dock->deactivate();
   dock->cleanup();

scitos2_charging_dock/test/test_cluster.cpp

@@ -0,0 +1,81 @@
+// Copyright (c) 2024 Alberto J. Tudela Roldán
+// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "gtest/gtest.h"
+#include "scitos2_charging_dock/cluster.hpp"
+
+TEST(ScitosDockingCluster, getCentroid) {
+  scitos2_charging_dock::Cluster cluster;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  cloud->push_back(pcl::PointXYZ(1.0, 2.0, 3.0));
+  cloud->push_back(pcl::PointXYZ(4.0, 5.0, 6.0));
+  cluster.cloud = cloud;
+
+  auto centroid = cluster.getCentroid();
+
+  EXPECT_DOUBLE_EQ(2.5, centroid.x);
+  EXPECT_DOUBLE_EQ(3.5, centroid.y);
+  EXPECT_DOUBLE_EQ(4.5, centroid.z);
+}
+
+TEST(ScitosDockingCluster, width) {
+  scitos2_charging_dock::Cluster cluster;
+
+  // No cloud
+  EXPECT_DOUBLE_EQ(cluster.width(), 0.0);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  cloud->push_back(pcl::PointXYZ(1.0, 2.0, 3.0));
+  cloud->push_back(pcl::PointXYZ(4.0, 5.0, 6.0));
+  cluster.cloud = cloud;
+  EXPECT_DOUBLE_EQ(cluster.width(), 4.2426406871192848);
+}
+
+TEST(ScitosDockingCluster, valid) {
+  scitos2_charging_dock::Cluster cluster;
+
+  // No cloud
+  EXPECT_FALSE(cluster.valid(0.0));
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  cloud->push_back(pcl::PointXYZ(1.0, 2.0, 3.0));
+  cloud->push_back(pcl::PointXYZ(4.0, 5.0, 6.0));
+  cluster.cloud = cloud;
+
+  EXPECT_FALSE(cluster.valid(0.0));
+  EXPECT_TRUE(cluster.valid(5.0));
+  EXPECT_FALSE(cluster.valid(10.0));
+}
+
+TEST(ScitosDockingCluster, operators) {
+  scitos2_charging_dock::Cluster cluster1;
+  cluster1.icp_score = 1.0;
+  scitos2_charging_dock::Cluster cluster2;
+  cluster2.icp_score = 2.0;
+  EXPECT_TRUE(cluster1 < cluster2);
+  EXPECT_FALSE(cluster1 > cluster2);
+
+  cluster1.icp_score = 3.0;
+  cluster2.icp_score = 2.0;
+  EXPECT_FALSE(cluster1 < cluster2);
+  EXPECT_TRUE(cluster1 > cluster2);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  bool success = RUN_ALL_TESTS();
+  return success;
+}