From dbd022c722435c624843edf754c6a37a3e4f8195 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Fri, 21 Jan 2022 18:25:32 +0900 Subject: [PATCH] fix(behavior_velocity_planner): fix inversion of object orientation in intersection planner (#280) Signed-off-by: tomoya.kimura --- .../src/scene_module/intersection/scene_intersection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index d36c32d6ba9d4..fb9cb9071153c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -44,7 +44,7 @@ static geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( double yaw, pitch, roll; tf2::getEulerYPR(obj_pose.orientation, yaw, pitch, roll); tf2::Quaternion inv_q; - inv_q.setRPY(roll, pitch, -yaw); + inv_q.setRPY(roll, pitch, yaw + M_PI); obj_pose.orientation = tf2::toMsg(inv_q); return obj_pose; }