Uncommited changes from testing obstacle awareness

on the robot

scenarios/dasd/config/DynamicObstacleManager.cfg

@@ -147,7 +147,7 @@ ArmarX.DynamicObstacleManager.MaxObstacleSize = 400
 #  - Default:            50
 #  - Case sensitivity:   yes
 #  - Required:           no
-ArmarX.DynamicObstacleManager.MinLengthOfLines = 200
+ArmarX.DynamicObstacleManager.MinLengthOfLines = 50
 
 
 # ArmarX.DynamicObstacleManager.MinObstacleSize:  The minimal obstacle size in mm.
@@ -163,7 +163,7 @@ ArmarX.DynamicObstacleManager.MinObstacleSize = 5
 #  - Default:            1000
 #  - Case sensitivity:   yes
 #  - Required:           no
-ArmarX.DynamicObstacleManager.MinObstacleValueForAccepting = 50
+ArmarX.DynamicObstacleManager.MinObstacleValueForAccepting = 5
 
 
 # ArmarX.DynamicObstacleManager.MinSampleRatioPerEllipsis:  Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle

source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp

@@ -259,8 +259,13 @@ namespace armarx
     {
         std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
 
+        const Eigen::Vector2f diff = goal - agentPosition;
+        const Eigen::Vector2f orthogonal_normalized = Eigen::Vector2f(Eigen::Rotation2Df(M_PI_2f32) * diff).normalized();
+
+
+
         const float sample_step = 5; // in [mm], sample step size towards goal.
-        const float distance_to_goal = (goal - agentPosition).norm() + safetyRadius;
+        const float distance_to_goal = diff.norm() + safetyRadius;
         float current_distance = safetyRadius;
 
         while (current_distance < distance_to_goal)
@@ -268,6 +273,8 @@ namespace armarx
             for (const auto man_obstacle : m_managed_obstacles)
             {
                 Eigen::Vector2f sample = agentPosition + ((goal - agentPosition).normalized() * current_distance);
+                Eigen::Vector2f sample_left = sample + (orthogonal_normalized * 250);
+                Eigen::Vector2f sample_right = sample - (orthogonal_normalized * 250);
                 obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
                 Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
 
@@ -275,6 +282,16 @@ namespace armarx
                 {
                     return current_distance - safetyRadius;
                 }
+
+                if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample_left))
+                {
+                    return current_distance - safetyRadius;
+                }
+
+                if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample_right))
+                {
+                    return current_distance - safetyRadius;
+                }
             }
 
             current_distance += sample_step;