Enemy filter

mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/object_associator.hpp

@@ -21,7 +21,7 @@ class Associator
   void update_config(Config const& config);
   /// Associate old objects with newly identified clusters. @prev_objects is updated + appended in place for new
   /// associations
-  void associate(ObjectMap& prev_objects, point_cloud const& pc, clusters_t clusters);
+  void associate(ObjectMap& prev_objects, point_cloud const& pc, clusters_t clusters, bool moving_back);
 
 private:
   double max_distance_;

mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp

@@ -7,6 +7,7 @@
 #include <nav_msgs/Odometry.h>
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
+#include <std_msgs/Float32.h>
 #include <std_srvs/Trigger.h>
 #include <tf2/convert.h>
 #include <tf2_eigen/tf2_eigen.h>
@@ -88,6 +89,9 @@ class Node : public NodeBase
 
   void velodyne_cb(const sensor_msgs::PointCloud2ConstPtr& pcloud);
 
+  void thrust_fl_cb(const std_msgs::Float32& thrust);
+  void thrust_fr_cb(const std_msgs::Float32& thrust);
+
   void initialize() override;
 
 private:
@@ -97,10 +101,17 @@ class Node : public NodeBase
   bool Reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) override;
 
 private:
+  // Switch that tells pcodar that Navigator is moving backwards
+  bool fr_back = false;
+  bool fl_back = false;
+  bool thrust_back = false;
+
   ros::Publisher pub_pcl_;
 
-  // Subscriber
+  // Subscribers
   ros::Subscriber pc_sub;
+  ros::Subscriber fl_sub;
+  ros::Subscriber fr_sub;
 
   // Model (It eventually will be object tracker, but for now just detections)
   InputCloudFilter input_cloud_filter_;

mil_common/perception/point_cloud_object_detection_and_recognition/src/object_associator.cpp

@@ -2,17 +2,24 @@
 #include <pcl/registration/correspondence_estimation.h>
 #include <pcl/registration/correspondence_rejection_trimmed.h>
 #include <pcl/search/octree.h>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
 
 #include <point_cloud_object_detection_and_recognition/object_associator.hpp>
 #include <unordered_set>
 
 namespace pcodar
 {
-void Associator::associate(ObjectMap& prev_objects, point_cloud const& pc, clusters_t clusters)
+void Associator::associate(ObjectMap& prev_objects, point_cloud const& pc, clusters_t clusters, bool moving_back)
 {
   // Tracks which clusters have been seen
   std::unordered_set<uint> seen;
 
+  // Establish Area of Interest (AOI) that new points are not allowed to be published while Navigator is moving
+  // backwards
+  Eigen::Vector3f min_aoi(0.0f, -5.0f, -0.5f);  // Min bounds (x, y, z)
+  Eigen::Vector3f max_aoi(10.0f, 5.0f, 0.5f);   // Max bounds (x, y, z)
+
   // Iterate through each new cluster, finding which persistent cluster(s) it matches
   for (cluster_t const& cluster : clusters)
   {
@@ -40,19 +47,84 @@ void Associator::associate(ObjectMap& prev_objects, point_cloud const& pc, clust
         matches.push_back(pair);
     }
 
-    if (matches.size() == 0)
+    // If Navigator is moving back, new objects outside of the AOI are invalid
+    if (moving_back)
     {
-      // Add to object
-      auto id = prev_objects.add_object(cluster_pc, cluster_search_tree);
-      seen.insert(id);
+      // Compute the centroid of the cluster
+      Eigen::Vector4f centroid;
+      pcl::compute3DCentroid(*cluster_pc, centroid);
+
+      // Transform centroid from /enu to /wamv/baselink
+      tf::TransformListener listener;
+      tf::StampedTransform transform;
+
+      try
+      {
+        // Listen for the transform from /enu to /wamv/base_link
+        listener.waitForTransform("/wamv/base_link", "/enu", ros::Time(0), ros::Duration(1.0));
+        listener.lookupTransform("/wamv/base_link", "/enu", ros::Time(0), transform);
+
+        // Apply the transformation to the centroid
+        tf::Vector3 centroid_enu(centroid[0], centroid[1], centroid[2]);
+        tf::Vector3 centroid_baselink = transform * centroid_enu;
+
+        // Update centroid with the transformed values
+        centroid[0] = centroid_baselink.x();
+        centroid[1] = centroid_baselink.y();
+        centroid[2] = centroid_baselink.z();
+
+        // Check if the centroid is outside the AOI
+        bool outside_aoi = (centroid[0] < min_aoi[0] || centroid[0] > max_aoi[0] ||  // X bounds
+                            centroid[1] < min_aoi[1] || centroid[1] > max_aoi[1] ||  // Y bounds
+                            centroid[2] < min_aoi[2] || centroid[2] > max_aoi[2]);   // Z bounds
+
+        if (!outside_aoi)
+        {
+          std::cout << centroid[0] << ", " << centroid[1] << ", " << centroid[2] << "\n";
+          continue;
+        }
+        else
+        {
+          if (matches.size() == 0)
+          {
+            // Add to object
+            auto id = prev_objects.add_object(cluster_pc, cluster_search_tree);
+            seen.insert(id);
+          }
+          else
+          {
+            seen.insert((*matches.at(0)).first);
+            (*matches.at(0)).second.update_points(cluster_pc, cluster_search_tree);
+            for (size_t i = 1; i < matches.size(); ++i)
+            {
+              prev_objects.erase_object(matches.at(i));
+            }
+          }
+        }
+      }
+      catch (tf::TransformException& ex)
+      {
+        ROS_ERROR("%s", ex.what());
+        ros::Duration(1.0).sleep();
+        continue;  // Skip this iteration if the transform fails
+      }
     }
     else
     {
-      seen.insert((*matches.at(0)).first);
-      (*matches.at(0)).second.update_points(cluster_pc, cluster_search_tree);
-      for (size_t i = 1; i < matches.size(); ++i)
+      if (matches.size() == 0)
+      {
+        // Add to object
+        auto id = prev_objects.add_object(cluster_pc, cluster_search_tree);
+        seen.insert(id);
+      }
+      else
       {
-        prev_objects.erase_object(matches.at(i));
+        seen.insert((*matches.at(0)).first);
+        (*matches.at(0)).second.update_points(cluster_pc, cluster_search_tree);
+        for (size_t i = 1; i < matches.size(); ++i)
+        {
+          prev_objects.erase_object(matches.at(i));
+        }
       }
     }
   }

mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp

@@ -143,6 +143,9 @@ void Node::ConfigCallback(Config const& config, uint32_t level)
 void Node::initialize()
 {
   NodeBase::initialize();
+  // Subscribe to odom
+  fl_sub = nh_.subscribe("/wamv/thrusters/FL_thrust_cmd", 1, &Node::thrust_fl_cb, this);
+  fr_sub = nh_.subscribe("/wamv/thrusters/FR_thrust_cmd", 1, &Node::thrust_fr_cb, this);
 
   // Subscribe pointcloud
   pc_sub = nh_.subscribe("/velodyne_points", 1, &Node::velodyne_cb, this);
@@ -205,7 +208,7 @@ void Node::velodyne_cb(const sensor_msgs::PointCloud2ConstPtr& pcloud)
   clusters_t clusters = detector_.get_clusters(filtered_accrued);
 
   // Associate current clusters with old ones
-  ass.associate(*objects_, *filtered_accrued, clusters);
+  ass.associate(*objects_, *filtered_accrued, clusters, thrust_back);
 }
 
 bool Node::bounds_update_cb(const mil_bounds::BoundsConfig& config)
@@ -216,4 +219,16 @@ bool Node::bounds_update_cb(const mil_bounds::BoundsConfig& config)
   return true;
 }
 
+void Node::thrust_fl_cb(const std_msgs::Float32& thrust)
+{
+  fl_back = thrust.data < 0;
+  thrust_back = fr_back && fl_back;
+}
+
+void Node::thrust_fr_cb(const std_msgs::Float32& thrust)
+{
+  fr_back = thrust.data < 0;
+  thrust_back = fr_back && fl_back;
+}
+
 }  // namespace pcodar