From 1faa9c9c175e5d67f944128a7959db2712f88f8e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 8 Jun 2021 10:13:49 +0200
Subject: [PATCH] ObstacleAwarePlatformUnit: using new interface
---
.../ObstacleAwarePlatformUnit.cpp | 11 +++++------
1 file changed, 5 insertions(+), 6 deletions(-)
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
index cbfb380d1..82f363703 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
+++ b/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;
--
GitLab