fix(behavior_velocity_planner): fix inversion of object orientation i…

…n intersection planner (#280) Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
tier4 · Jan 29, 2022 · dbd022c · dbd022c
1 parent 160c26f
commit dbd022c
Showing 1 changed file with 1 addition and 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
@@ -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;
 }