ObstacleAwarePlatformUnit: using new interface

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

@@ -354,18 +354,17 @@ 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__;
+
+    const Eigen::Vector2f extents = Eigen::Affine3f(m_robot->getRobotNode("PlatformCornerFrontLeft")->getPoseInRootFrame()).translation().head<2>();
+
     // Apply obstacle avoidance and apply post processing to get the modulated velocity.
-    const Eigen::Vector2f modulated_vel = [this, &target_vel]
+    const Eigen::Vector2f modulated_vel = [this, &target_vel, &extents]
     {
-        const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos, m_control_data.target_pos);
+        const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos, extents, m_control_data.agent_ori, m_control_data.target_pos);
 
         ARMARX_DEBUG << "Distance to obstacle: " << distance;