make the ROS odometry (yaw) consistent with non-ROS

facebookresearch · May 27, 2022 · 962db84 · 962db84
1 parent 55348ca
commit 962db84
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py
@@ -67,7 +67,8 @@ def get_slam_pose(self):
  ]
  )
  euler = euler_from_quaternion(quat)
- return (pose.pose.pose.position.x, pose.pose.pose.position.y, euler[2])
+ yaw = euler[2] + math.pi
+ return (pose.pose.pose.position.x, pose.pose.pose.position.y, yaw)
  else:
  return (0.0, 0.0, 0.0)
 
@@ -89,7 +90,8 @@ def get_odom(self):
  ]
  )
  euler = euler_from_quaternion(quat)
- return (pose.pose.position.x, pose.pose.position.y, euler[2])
+ yaw = euler[2] + math.pi
+ return (pose.pose.position.x, pose.pose.position.y, yaw)
 
  def get_joint_state(self, name=None):
  with self._lock: