diff --git a/ateam_kenobi/src/core/defense_area_enforcement.cpp b/ateam_kenobi/src/core/defense_area_enforcement.cpp index e1830acf..6fc8f2e7 100644 --- a/ateam_kenobi/src/core/defense_area_enforcement.cpp +++ b/ateam_kenobi/src/core/defense_area_enforcement.cpp @@ -21,6 +21,7 @@ #include "defense_area_enforcement.hpp" #include #include +#include "core/motion/frame_conversions.hpp" namespace ateam_kenobi::defense_area_enforcement { @@ -71,9 +72,11 @@ bool WouldVelocityCauseCollision( const double delta_t = 0.01; - // TODO(barulicm): This ignores the velocity frame - const ateam_geometry::Vector velocity{motion_command.twist.linear.x, + ateam_geometry::Vector velocity{motion_command.twist.linear.x, motion_command.twist.linear.y}; + if (motion_command.twist_frame == ateam_msgs::msg::RobotMotionCommand::FRAME_BODY) { + velocity = ateam_kenobi::motion::LocalToWorldFrame(velocity, world.our_robots[robot_id]); + } const ateam_geometry::Vector displacement = velocity * delta_t; diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp index 79bfccd0..28792066 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.cpp +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -69,6 +69,9 @@ std::array, }, [&](const intents::linear::VelocityIntent & v) { body_velocity.linear = v.velocity; + if (v.frame == intents::linear::Frame::World) { + body_velocity.linear = WorldToLocalFrame(v.velocity, robot); + } use_controller_linvel = false; controller.reset_trajectory({robot.pos}); },