Find the cluster in front of the robot instead of near

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

scitos2_charging_dock/CMakeLists.txt

@@ -55,6 +55,7 @@ find_package(rclcpp_components REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(angles REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(pcl_ros REQUIRED)
 find_package(pcl_conversions REQUIRED)
@@ -78,6 +79,7 @@ set(dependencies
   pluginlib
   geometry_msgs
   sensor_msgs
+  angles
   nav2_util
   pcl_ros
   pcl_conversions

scitos2_charging_dock/package.xml

@@ -13,6 +13,7 @@
   <depend>pluginlib</depend>
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>angles</depend>
   <depend>nav2_util</depend>
   <depend>pcl_ros</depend>
   <depend>pcl_conversions</depend>

scitos2_charging_dock/src/dock_saver.cpp

@@ -15,6 +15,7 @@
 
 #include <mutex>
 
+#include "angles/angles.h"
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "scitos2_charging_dock/dock_saver.hpp"
 
@@ -159,14 +160,14 @@ bool DockSaver::saveDockCallback(
     return false;
   }
 
-  // Find closest cluster
-  std::vector<double> distances;
+  // Find the cluster in front of the robot
+  std::vector<double> angles;
   for (auto cluster : clusters) {
-    double distance = std::hypot(cluster.getCentroid().x, cluster.getCentroid().y);
-    distances.push_back(distance);
+    double angle = std::atan2(cluster.getCentroid().y, cluster.getCentroid().x);
+    angles.push_back(angles::normalize_angle_positive(angle));
   }
-  auto min_distance = std::min_element(distances.begin(), distances.end());
-  int idx = std::distance(distances.begin(), min_distance);
+  auto min_angle = std::min_element(angles.begin(), angles.end());
+  int idx = std::distance(angles.begin(), min_angle);
 
   // Store the dock pointcloud to a file
   scitos2_charging_dock::Pcloud::Ptr dock = clusters[idx].cloud;