DynamicObstacleManager: considering robot's bounding box for distance computation

source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp

@@ -22,6 +22,8 @@
 
 #include "DynamicObstacleManager.h"
 
+#include <VirtualRobot/MathTools.h>
+
 // STD/STL
 #include <string>
 #include <vector>
@@ -240,19 +242,60 @@ namespace armarx
     }
 
     float
-    DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& position, const Eigen::Vector2f& goal, const Ice::Current&)
+    DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& agentPosition, const Eigen::Vector2f& agentExtents, const float yaw, const Eigen::Vector2f& goal, const Ice::Current&)
     {
         std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
 
+        Eigen::Vector3f minLocal;
+        Eigen::Vector3f maxLocal;
+        minLocal << -agentExtents.x(), -agentExtents.y(), 0.F;
+        maxLocal << agentExtents.x(), agentExtents.y(), 0.F;
+
+        Eigen::Affine3f pose = Eigen::Affine3f::Identity();
+        pose.linear() = VirtualRobot::MathTools::rpy2eigen3f(0, 0, yaw);
+        pose.translation().head<2>() = agentPosition;
+
+        const VirtualRobot::MathTools::OOBB oobb(minLocal, maxLocal, pose.matrix());
+
+        const Eigen::Vector2f movementDirection = (goal - agentPosition).normalized();
+        const Eigen::Vector2f normal = Eigen::Rotation2Df(M_PI_2f32) * movementDirection;
+
+        const auto to3D = [](const Eigen::Vector2f & pt) -> Eigen::Vector3f
+        {
+            return Eigen::Vector3f{pt.x(), pt.y(), 0.F};
+        };
+
+
+        const VirtualRobot::MathTools::Plane plane(to3D(agentPosition), to3D(normal));
+
+        std::vector<Eigen::Vector3f> intersectionPoints;
+        const auto res = VirtualRobot::MathTools::intersectOOBBPlane(oobb, plane, intersectionPoints);
+
+        ARMARX_CHECK(res == VirtualRobot::MathTools::IntersectionResult::eIntersection);
+
+        ARMARX_CHECK(not intersectionPoints.empty());
+
+        std::vector<float> distancesToGoal;
+        std::transform(intersectionPoints.begin(), intersectionPoints.end(), std::back_inserter(distancesToGoal), [&goal](const Eigen::Vector3f & ip) -> float
+        {
+            return (goal - ip.head<2>()).norm();
+        });
+
+        const size_t minElementIdx = std::distance(distancesToGoal.begin(), std::min_element(distancesToGoal.begin(), distancesToGoal.end()));
+
+        const Eigen::Vector2f desIntersectionPoint = intersectionPoints.at(minElementIdx).head<2>();
+
+        const float platformExtent = (desIntersectionPoint - agentPosition).norm();
+
         const float sample_step = 5; // in [mm], towards goal.
-        const float distance_to_goal = (goal - position).norm();
+        const float distance_to_goal = (goal - agentPosition).norm() + platformExtent;
         float current_distance = sample_step;
 
         while (current_distance < distance_to_goal)
         {
             for (const auto man_obstacle : m_managed_obstacles)
             {
-                Eigen::Vector2f sample = position + ((goal - position).normalized() * current_distance);
+                Eigen::Vector2f sample = agentPosition + ((goal - agentPosition).normalized() * current_distance);
                 obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
                 Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
 

source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h

@@ -73,7 +73,7 @@ namespace armarx
         void directly_update_obstacle(const std::string& name, const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
         void remove_obstacle(const std::string& name, const Ice::Current& = Ice::Current()) override;
         void wait_unitl_obstacles_are_ready(const Ice::Current& = Ice::Current()) override;
-        float distanceToObstacle(const Eigen::Vector2f& position, const Eigen::Vector2f& goal, const Ice::Current& = Ice::Current()) override;
+        float distanceToObstacle(const Eigen::Vector2f& agentPosition, const Eigen::Vector2f& agentExtents,  float yaw, const Eigen::Vector2f& goal, const Ice::Current& = Ice::Current()) override;
 
     protected:
         void onInitComponent() override;