fix: Local fixes.

source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp

@@ -218,7 +218,7 @@ namespace armarx
         std::lock_guard l(m_managed_obstacles_mutex);
 
         ARMARX_DEBUG << "Remove Obstacle " << name << " from obstacle map and from obstacledetection";
-        for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
+        for (ManagedObstaclePtr managed_obstacle : m_managed_obstacles)
         {
             if (managed_obstacle->m_obstacle.name == name)
             {
@@ -244,15 +244,15 @@ namespace armarx
     {
         std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
 
-        const float sample_step = 50; // in [mm], towards goal.
+        const float sample_step = 5; // in [mm], towards goal.
         const float distance_to_goal = (goal - position).norm();
         float current_distance = sample_step;
 
-        while (sample_step < distance_to_goal)
+        while (current_distance < distance_to_goal)
         {
-            for (const auto& man_obstacle : m_managed_obstacles)
+            for (const auto man_obstacle : m_managed_obstacles)
             {
-                Eigen::Vector2f sample = position + ((goal - position).normalized() * sample_step);
+                Eigen::Vector2f sample = position + ((goal - position).normalized() * current_distance);
                 obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
                 Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
 
@@ -265,6 +265,7 @@ namespace armarx
             current_distance += sample_step;
         }
 
+
         return std::numeric_limits<float>::infinity();
     }
 

source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp

@@ -354,20 +354,26 @@ armarx::ObstacleAwarePlatformUnit::get_velocities()
 
     // Acquire control_data mutex to ensure data remains consistent.
     std::scoped_lock l{m_control_data.mutex};
-
+    ARMARX_LOG << __LINE__;
     // Update agent and get target velocities.
     update_agent_dependent_values();
+    ARMARX_LOG << __LINE__;
     const Eigen::Vector2f target_vel = get_target_velocity();
+    ARMARX_LOG << __LINE__;
     const float target_rot_vel = get_target_rotational_velocity();
-
+    ARMARX_LOG << __LINE__;
     // Apply obstacle avoidance and apply post processing to get the modulated velocity.
     const Eigen::Vector2f modulated_vel = [this, &target_vel]
     {
-        float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos, m_control_data.target_pos);
-        // https://www.wolframalpha.com/input/?i=line+through+%28750%2C+0%29+and+%282000%2C+1%29
-        float modifier = std::clamp((distance / 1250) - 0.6, 0.0, 1.0);
+        const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos, m_control_data.target_pos);
+
+        ARMARX_DEBUG << "Distance to obstacle: " << distance;
+
+        // https://www.wolframalpha.com/input/?i=line+through+%281000%2C+0%29+and+%282000%2C+1%29
+        float modifier = std::clamp((distance / 1000) - 1., 0.0, 1.0);
 
-        return modifier * target_vel;
+        const Eigen::Vector2f vel = modifier * target_vel;
+        return vel.norm() > 20 ? vel : Eigen::Vector2f{0, 0};
     }();
 
     ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";