implement mining eval

include/modules/impl/traversibility_gen_impl.hpp

@@ -142,6 +142,16 @@ typename TraversibilityGenerator<P, M>::MetaDataList&
     return swap_list;
 }
 
+template<typename P, typename M>
+void TraversibilityGenerator<P, M>::extractTravPoints(
+    TravPointCloud& trav_points) const
+{
+    trav_points.clear();
+
+    this->mtx.lock();
+    util::pc_copy_selection(this->points, this->trav_selection, trav_points);
+    this->mtx.unlock();
+}
 template<typename P, typename M>
 void TraversibilityGenerator<P, M>::extractTravElements(
     TravPointCloud& trav_points,
@@ -158,6 +168,20 @@ void TraversibilityGenerator<P, M>::extractTravElements(
         trav_meta_data);
     this->mtx.unlock();
 }
+
+template<typename P, typename M>
+void TraversibilityGenerator<P, M>::extractExtTravPoints(
+    TravPointCloud& ext_trav_points) const
+{
+    ext_trav_points.clear();
+
+    this->mtx.lock();
+    util::pc_copy_selection(
+        this->points,
+        this->ext_trav_selection,
+        ext_trav_points);
+    this->mtx.unlock();
+}
 template<typename P, typename M>
 void TraversibilityGenerator<P, M>::extractExtTravElements(
     TravPointCloud& ext_trav_points,
@@ -177,6 +201,20 @@ void TraversibilityGenerator<P, M>::extractExtTravElements(
         ext_trav_meta_data);
     this->mtx.unlock();
 }
+
+template<typename P, typename M>
+void TraversibilityGenerator<P, M>::extractNonTravPoints(
+    TravPointCloud& non_trav_points) const
+{
+    non_trav_points.clear();
+
+    this->mtx.lock();
+    util::pc_copy_selection(
+        this->points,
+        this->avoid_selection,
+        non_trav_points);
+    this->mtx.unlock();
+}
 template<typename P, typename M>
 void TraversibilityGenerator<P, M>::extractNonTravElements(
     TravPointCloud& non_trav_points,

include/modules/traversibility_gen.hpp

@@ -134,12 +134,15 @@ class TraversibilityGenerator
      * processed result! */
     MetaDataList& swapMetaDataList(MetaDataList& swap_list);
 
+    void extractTravPoints(TravPointCloud& trav_points) const;
     void extractTravElements(
         TravPointCloud& trav_points,
         MetaDataList& trav_meta_data) const;
+    void extractExtTravPoints(TravPointCloud& ext_trav_points) const;
     void extractExtTravElements(
         TravPointCloud& ext_trav_points,
         MetaDataList& ext_trav_meta_data) const;
+    void extractNonTravPoints(TravPointCloud& non_trav_points) const;
     void extractNonTravElements(
         TravPointCloud& non_trav_points,
         MetaDataList& non_trav_meta_data) const;
@@ -198,9 +201,9 @@ class TraversibilityGenerator
     typename TravPointCloud::Ptr points_ptr;
 
     pcl::Indices
-        trav_selection,   // set of points that are traversible (excluding spread)
-        ext_trav_selection, // extended set of points that may be traversible
-        avoid_selection;  // set of points that are not traversible ever
+        trav_selection,  // set of points that are traversible (excluding spread)
+        ext_trav_selection,  // extended set of points that may be traversible
+        avoid_selection;     // set of points that are not traversible ever
 
     mutable std::mutex mtx;
 

include/synchronization.hpp

@@ -77,7 +77,7 @@ class ResourcePipeline
 public:
     /* Aquire a reference to the current input buffer. Internally locks a control
      * mutex, so unlockInput() or unlockInputAndNotify() must be called when
-     * ruffer modification is complete on the current thread to unlock it! */
+     * buffer modification is complete on the current thread to unlock it! */
     T& lockInput()
     {
         this->swap_mtx.lock();

include/transform_sync.hpp

@@ -46,6 +46,7 @@
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
+#include <tf2_ros/buffer.hpp>
 #include <tf2_ros/transform_broadcaster.h>
 
 #include "util.hpp"
@@ -82,10 +83,12 @@ class TransformSynchronizer
 public:
     inline TransformSynchronizer(
         tf2_ros::TransformBroadcaster& tf_broadcaster,
+        tf2_ros::Buffer* tf_buffer = nullptr,
         std::string_view map_frame_id = "map",
         std::string_view odom_frame_id = "odom",
         std::string_view base_frame_id = "base_link") :
         tf_broadcaster{tf_broadcaster},
+        tf_buffer{tf_buffer},
         map_frame{map_frame_id},
         odom_frame{odom_frame_id},
         base_frame{base_frame_id}
@@ -134,6 +137,7 @@ class TransformSynchronizer
 
 protected:
     tf2_ros::TransformBroadcaster& tf_broadcaster;
+    tf2_ros::Buffer* tf_buffer{nullptr};
     TrajectoryFilterT trajectory_filter;
 
     std::string map_frame;
@@ -454,29 +458,37 @@ void TransformSynchronizer<MP, F>::publishMap()
 {
     using namespace util::geom::cvt::ops;
 
-    geometry_msgs::msg::TransformStamped _tf;
+    geometry_msgs::msg::TransformStamped tf_;
 
-    _tf.header.stamp = util::toTimeStamp(this->map_stamp);
-    _tf.header.frame_id = this->map_frame;
-    _tf.child_frame_id = this->odom_frame;
-    _tf.transform << this->map_tf.pose;
+    tf_.header.stamp = util::toTimeStamp(this->map_stamp);
+    tf_.header.frame_id = this->map_frame;
+    tf_.child_frame_id = this->odom_frame;
+    tf_.transform << this->map_tf.pose;
 
-    this->tf_broadcaster.sendTransform(_tf);
+    this->tf_broadcaster.sendTransform(tf_);
+    if(this->tf_buffer)
+    {
+        this->tf_buffer->setTransform(tf_, "cardinal_perception");
+    }
 }
 
 template<typename MP, typename F>
 void TransformSynchronizer<MP, F>::publishOdom()
 {
     using namespace util::geom::cvt::ops;
 
-    geometry_msgs::msg::TransformStamped _tf;
+    geometry_msgs::msg::TransformStamped tf_;
 
-    _tf.header.stamp = util::toTimeStamp(this->odom_stamp);
-    _tf.header.frame_id = this->odom_frame;
-    _tf.child_frame_id = this->base_frame;
-    _tf.transform << this->odom_tf.pose;
+    tf_.header.stamp = util::toTimeStamp(this->odom_stamp);
+    tf_.header.frame_id = this->odom_frame;
+    tf_.child_frame_id = this->base_frame;
+    tf_.transform << this->odom_tf.pose;
 
-    this->tf_broadcaster.sendTransform(_tf);
+    this->tf_broadcaster.sendTransform(tf_);
+    if(this->tf_buffer)
+    {
+        this->tf_buffer->setTransform(tf_, "cardinal_perception");
+    }
 }
 
 };  // namespace perception

src/core/threads/localization_worker.cpp

@@ -86,15 +86,15 @@ namespace perception
 
 LocalizationWorker::LocalizationWorker(
     RclNode& node,
-    const Tf2Buffer& tf_buffer,
+    Tf2Buffer& tf_buffer,
     const ImuIntegrator<>& imu_sampler) :
     node{node},
     tf_buffer{tf_buffer},
     imu_sampler{imu_sampler},
     tf_broadcaster{node},
     pub_map{&node, PERCEPTION_TOPIC(""), PERCEPTION_PUBSUB_QOS},
     lidar_odom{node},
-    transform_sync{tf_broadcaster}
+    transform_sync{tf_broadcaster, &tf_buffer}
 {
 }
 

src/core/threads/localization_worker.hpp

@@ -86,7 +86,7 @@ class LocalizationWorker
 public:
     LocalizationWorker(
         RclNode& node,
-        const Tf2Buffer& tf_buffer,
+        Tf2Buffer& tf_buffer,
         const ImuIntegrator<>& imu_sampler);
     ~LocalizationWorker();
 

src/core/threads/mining_eval_worker.cpp

@@ -39,22 +39,33 @@
 
 #include "mining_eval_worker.hpp"
 
+#include <limits>
+
+#include <Eigen/Core>
+
 #include <cardinal_perception/msg/mining_eval_results.hpp>
 
 #include <csm_metrics/profiling.hpp>
 
+#include <util.hpp>
+#include <geometry.hpp>
+
+
+using Vec3f = Eigen::Vector3f;
+using Iso3f = Eigen::Isometry3f;
+using Box3f = Eigen::AlignedBox3f;
 
 using MiningEvalResultsMsg = cardinal_perception::msg::MiningEvalResults;
 
+using namespace util::geom::cvt::ops;
+
 
 namespace csm
 {
 namespace perception
 {
 
-MiningEvalWorker::MiningEvalWorker(
-    RclNode& node,
-    const Tf2Buffer& tf_buffer) :
+MiningEvalWorker::MiningEvalWorker(RclNode& node, const Tf2Buffer& tf_buffer) :
     node{node},
     tf_buffer{tf_buffer},
     pub_map{&node, PERCEPTION_TOPIC(""), PERCEPTION_PUBSUB_QOS}
@@ -72,7 +83,7 @@ void MiningEvalWorker::accept(
     const UpdateMiningEvalSrv::Request::SharedPtr& req,
     const UpdateMiningEvalSrv::Response::SharedPtr& resp)
 {
-    if(req->completed)
+    if (req->completed)
     {
         this->srv_enable_state = false;
         resp->query_id = -1;
@@ -82,11 +93,13 @@ void MiningEvalWorker::accept(
         this->srv_enable_state = true;
         {
             auto& buff = this->query_notifier.lockInput();
-            if(!buff)
+            if (!buff)
             {
                 buff = std::make_shared<Query>();
             }
             buff->poses = req->queries;
+            buff->eval_width = req->eval_width;
+            buff->eval_height = req->eval_height;
             resp->query_id = buff->id = this->query_count.load();
             this->query_notifier.unlockInputAndNotify(buff);
         }
@@ -165,6 +178,52 @@ void MiningEvalWorker::mining_eval_callback(MiningEvalResources& buff)
     msg.query_id = query_ptr->id;
     msg.ranges.resize(query_ptr->poses.poses.size(), 0.f);
 
+    Iso3f odom_to_ref_tf = Iso3f::Identity();
+    const auto& header = query_ptr->poses.header;
+    if (header.frame_id != this->odom_frame)
+    {
+        try
+        {
+            odom_to_ref_tf << this->tf_buffer
+                                  .lookupTransform(
+                                      header.frame_id,
+                                      this->odom_frame,
+                                      util::toTf2TimePoint(header.stamp))
+                                  .transform;
+        }
+        catch (const std::exception& e)
+        {
+            return;
+        }
+    }
+
+    Box3f eval_bound;
+    eval_bound.min().x() = 0.f;
+    eval_bound.min().y() = query_ptr->eval_width * -0.5f;
+    eval_bound.min().z() = query_ptr->eval_height * -0.5f;
+    eval_bound.max().x() = std::numeric_limits<float>::infinity();
+    eval_bound.max().y() = query_ptr->eval_width * 0.5f;
+    eval_bound.max().z() = query_ptr->eval_height * 0.5f;
+
+    for (size_t i = 0; i < query_ptr->poses.poses.size(); i++)
+    {
+        Iso3f pose_tf;
+        // (ref to pose tf)[4x4] * (odom to ref tf)[4x4] --> (odom to pose tf)[4x4]
+        pose_tf =
+            (pose_tf << query_ptr->poses.poses[i]).inverse() * odom_to_ref_tf;
+
+        msg.ranges[i] = std::numeric_limits<float>::infinity();
+        for (const auto& pt : buff.avoid_points)
+        {
+            Vec3f pt_ = (pose_tf * pt.getVector4fMap()).template head<3>();
+
+            if(eval_bound.contains(pt_) && pt_.x() < msg.ranges[i])
+            {
+                msg.ranges[i] = pt_.x();
+            }
+        }
+    }
+
     this->pub_map.publish("mining_evaluation", msg);
 }
 

src/core/threads/mining_eval_worker.hpp

@@ -81,7 +81,7 @@ class MiningEvalWorker
     ~MiningEvalWorker();
 
 public:
-    void configure(const std::string& odom_frame);  // TODO
+    void configure(const std::string& odom_frame);
 
     void accept(
         const UpdateMiningEvalSrv::Request::SharedPtr& req,
@@ -102,6 +102,8 @@ class MiningEvalWorker
         using SharedPtr = std::shared_ptr<Query>;
 
         PoseArrayMsg poses;
+        float eval_width;
+        float eval_height;
         uint32_t id;
     };
 

src/core/threads/path_planning_worker.cpp

@@ -204,8 +204,8 @@ void PathPlanningWorker::path_planning_callback(PathPlanningResources& buff)
             odom_target,
             buff.bounds_min,
             buff.bounds_max,
-            *buff.points,
-            *buff.points_meta,
+            buff.points,
+            buff.points_meta,
             path))
     {
         return;

src/core/threads/shared_resources.hpp

@@ -89,17 +89,16 @@ struct PathPlanningResources
     Eigen::Vector3f bounds_max;
     util::geom::PoseTf3f base_to_odom;
     geometry_msgs::msg::PoseStamped target;
-    pcl::PointCloud<TraversibilityPointType>::Ptr points;
-    pcl::PointCloud<TraversibilityMetaType>::Ptr points_meta;
+    pcl::PointCloud<TraversibilityPointType> points;
+    pcl::PointCloud<TraversibilityMetaType> points_meta;
 };
 
 struct MiningEvalResources
 {
     double stamp;
     Eigen::Vector3f bounds_min;
     Eigen::Vector3f bounds_max;
-    pcl::PointCloud<TraversibilityPointType>::Ptr points;
-    pcl::PointCloud<TraversibilityMetaType>::Ptr points_meta;
+    pcl::PointCloud<TraversibilityPointType> avoid_points;
     std::shared_ptr<void> query;
 };