I made my GPS navigation package that takes in GPS , IMU readings, calculates the required bearing, current heading and commands the robot to move to a defined goal.

To find out the initial direction in which the robot is heading. I record the mean of source (src) lat/ long (by standing still for 10 sec), make the robot move straight for 20 sec, record the mean of new (dst) lat/ long (by standing still for 10 sec), then apply this formula to find the heading

init_delta_long = mean_lon_dst - mean_lon_src
X = math.cos(mean_lat_dst) * math.sin(init_delta_long)
Y = (math.cos(mean_lat_src) * math.sin(mean_lat_dst)) - (math.sin(mean_lat_src) * math.cos(mean_lat_dst) * math.cos(init_delta_long))
self.init_heading = math.atan2(X, Y)

I also record the IMU yaw value at this time (let's call it imu_heading_offset) which basically doesn't change so far) so that I can update the robot's heading as it moves.

Robot's current/ true heading is calculated as self.true_heading = self.init_heading - (self.roll - self.imu_heading_offset) whenever a message on the IMU is read. Not to confuse but roll here is actually yaw (idk why does roll value update instead of yaw when i rotate the 4-wheeled robot).

I bring down the true heading in -180 to 180 degree range using below where gps_nav is just the instantiation of the class

if gps_nav.true_heading < math.radians(-180):
            gps_nav.true_heading = math.radians(180 - (abs(math.degrees(gps_nav.true_heading)) - 180))
        elif gps_nav.true_heading > math.radians(180):
            gps_nav.true_heading = -(math.radians(180) - (gps_nav.true_heading - math.radians(180)))

Then, I just compare the bearing with true heading and determine which direction to move or rotate. Now, if the world is centered at 0 lat, 0 long, everything works well but if it is centered at some other coordinate, the bearing calculation is incorrect (it doesn't point in the direction of the destination point). It works however, if I do atan2(Y,X) but that's not the right way.

This is the part from sdf file that ignition gazebo uses

      <!-- Works with atan2(y,x) but not otherwise -->
      <!-- <latitude_deg>-22.986687</latitude_deg>
      <longitude_deg>-43.202501</longitude_deg> -->
      <!-- works with atan2(x,y) but not otherwise -->
      <!-- <latitude_deg>0.0</latitude_deg>
      <longitude_deg>0.0</longitude_deg> -->


I am using ROS2 humble with Ignition Gazebo Fortress 6.16.0 with Clearpath Husky simulation Any idea if my math formula is incorrect or what's wrong here?

I got the math formulas from here: https://www.lifewire.com/what-is-bearing-in-gps-1683320 and https://mapscaping.com/how-to-calculate-bearing-between-two-coordinates/

This code is the same math but just structured differently from the snippets that I pasted. https://github.com/devvaibhav455/husky_foxy/blob/main/custom_nav_stack_pkg/scripts/mode_control.py



Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.