Calculate target position for planning (wip)

source/armarx/navigation/local_planning/TimedElasticBands.cpp

 #include "TimedElasticBands.h"
 
+#include <VirtualRobot/MathTools.h>
 #include <VirtualRobot/Robot.h>
 
 #include <ArmarXCore/core/logging/Logging.h>
@@ -79,15 +80,16 @@ namespace armarx::navigation::loc_plan
     std::optional<LocalPlannerResult>
     TimedElasticBands::plan(const core::Trajectory& goal)
     {
-        teb_local_planner::TimedElasticBand globalPath = conv::toRos(goal);
+        const teb_local_planner::TimedElasticBand globalPath = conv::toRos(goal);
         teb_globalPath = globalPath.poses();
         hcp_->setGlobalPath(&teb_globalPath);
 
-        //TODO (SALt): calculate target pose
-        const teb_local_planner::PoseSE2 start =
-            conv::toRos(static_cast<core::Pose>(scene.robot->getGlobalPose()));
-        const teb_local_planner::PoseSE2 end = conv::toRos(goal.points().end()->waypoint.pose);
-        bool planToDestination = true;
+        const core::Pose currentPose{scene.robot->getGlobalPose()};
+
+        const teb_local_planner::PoseSE2 start = conv::toRos(currentPose);
+
+        const FindTargetResult target = findTarget(currentPose, goal);
+        const teb_local_planner::PoseSE2 end = conv::toRos(target.target);
 
         core::Twist currentVelocity = core::Twist::Zero();
         if (scene.platformVelocity.has_value())
@@ -100,7 +102,7 @@ namespace armarx::navigation::loc_plan
 
         try
         {
-            hcp_->plan(start, end, &velocity_start, !planToDestination);
+            hcp_->plan(start, end, &velocity_start, !planToDest);
         }
         catch (std::exception& e)
         {
@@ -116,4 +118,70 @@ namespace armarx::navigation::loc_plan
     }
 
 
+    TimedElasticBands::FindTargetResult
+    TimedElasticBands::findTarget(const core::Pose& currentPose, const core::Trajectory& globalPath)
+    {
+        float distance = std::numeric_limits<float>::max();
+
+        const core::Position point = currentPose.translation();
+        core::Position projection;
+
+        size_t closestSegment = -1;
+        for (size_t i = 0; i < globalPath.points().size() - 1; i++)
+        {
+            const auto& wpBefore = globalPath.points().at(i);
+            const auto& wpAfter = globalPath.points().at(i + 1);
+
+            // FIXME remove after finding the bug
+            if ((wpBefore.waypoint.pose.translation() - wpAfter.waypoint.pose.translation())
+                    .norm() < 1.F)
+            {
+                // ARMARX_WARNING << "Trajectory segment " << i << " too small ...";
+                continue;
+            }
+
+            const auto closestPoint =
+                VirtualRobot::MathTools::nearestPointOnSegment<core::Position>(
+                    wpBefore.waypoint.pose.translation(),
+                    wpAfter.waypoint.pose.translation(),
+                    point);
+
+            const float currentDistance = (closestPoint - point).norm();
+
+            // 'less equal' to accept following segment if the closest point is the waypoint
+            if (currentDistance <= distance)
+            {
+                closestSegment = i;
+                projection = closestPoint;
+                distance = currentDistance;
+            }
+        }
+
+        //TODO (SALt): parameter to set planning distance
+        float remaining = 5.F;
+        core::Position closestPoint = projection;
+        for (size_t i = closestSegment; i < globalPath.points().size() - 1; i++)
+        {
+            const core::Position& segEnd =
+                globalPath.points().at(i + 1).waypoint.pose.translation();
+
+            const float segmentDistance = (segEnd - closestPoint).norm();
+
+            if (segmentDistance < remaining)
+            {
+                closestPoint = segEnd;
+                remaining -= segmentDistance;
+            }
+            else
+            {
+                const core::Position dest =
+                    closestPoint + remaining * (segEnd - closestPoint).normalized();
+
+                return {projection, dest, false};
+            }
+        }
+
+        return {projection, globalPath.points().end()[0].waypoint.pose.translation(), true};
+    }
+
 } // namespace armarx::navigation::loc_plan

source/armarx/navigation/local_planning/TimedElasticBands.h

@@ -52,7 +52,16 @@ namespace armarx::navigation::loc_plan
         std::optional<LocalPlannerResult> plan(const core::Trajectory& goal) override;
 
     private:
+        struct FindTargetResult
+        {
+            core::Pose projection;
+            core::Pose target;
+            bool planToDestination;
+        };
+
         void initConfig();
+        FindTargetResult findTarget(const core::Pose& currentPose,
+                                    const core::Trajectory& globalPath);
 
     protected:
         const Params params;