Skip to content

Commit

Permalink
changed units to match clean_slate type (utm)
Browse files Browse the repository at this point in the history
  • Loading branch information
TiaSinghania committed Dec 24, 2024
1 parent a537b88 commit a5a6ab5
Showing 1 changed file with 5 additions and 2 deletions.
7 changes: 5 additions & 2 deletions rb_ws/src/buggy/buggy/simulator/engine.py
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ def __init__(self):
self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1)

# simulate the INS's outputs (noise included)
# this is published as a BuggyState (UTM and radians)
self.pose_publisher = self.create_publisher(Odometry, "nav/odom", 1)

self.navsatfix_noisy_publisher = self.create_publisher(
Expand Down Expand Up @@ -154,13 +155,15 @@ def publish(self):
odom.header.stamp = time_stamp

odom_pose = Pose()
odom_pose.position.x = float(long_noisy)
odom_pose.position.y = float(lat_noisy)
east, north = utm.from_latlon(lat_noisy, long_noisy)
odom_pose.position.x = float(east)
odom_pose.position.y = float(north)
odom_pose.position.z = float(260)

odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2)
odom_pose.orientation.w = np.cos(np.deg2rad(self.heading) / 2)

# NOTE: autonsystem only cares about magnitude of velocity, so we don't need to split into components
odom_twist = Twist()
odom_twist.linear.x = float(velocity)

Expand Down

0 comments on commit a5a6ab5

Please sign in to comment.