diff --git a/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp index 61cdac8c..2633b5f1 100644 --- a/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp +++ b/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp @@ -475,8 +475,9 @@ bool AreaRecordingBehavior::getDockingPosition(geometry_msgs::Pose &pos) { pos.position = odom_ptr->pose.pose.position; + tf2::Quaternion docking_orientation; double yaw = atan2(pos.position.y - first_docking_pos.position.y, pos.position.x - first_docking_pos.position.x); - tf2::Quaternion docking_orientation(0.0, 0.0, yaw); + docking_orientation.setRPY(0.0, 0.0, yaw); pos.orientation = tf2::toMsg(docking_orientation); update_actions();