beacon_pkg

This package creates a ROS2 node called BeaconGoalPose that converts incoming GPS coordinates to a target coordinate, or waypoint, for the robot. The node subscribes to a stream of NMEA GGA messages from the beacon and extracts the latitude, longitude, and altitude from each message. These three values are then translated to the global frame and published as a PoseStamped message to the /goal_pose topic.

The three main files for the package are:

  • beacon_goalpose.cpp

    • Subscribes to the topic the data is coming in

    • Converts to latitude/longitude to UTM

    • Publishes converted to a PoseStamped message

  • beacon_goalpose.launch.py

    • Launches beacon_pkg with defined default parameters

  • beacon_receiver.launch.py

    • Remaps the beacon’s publication topic to /gps/beacon/fix

    • Launches nmea_navsat_driver with defined default parameters

System Design

The GPS beacon transmits NMEA GGA sentences that contain latitude and longitude coordinates through the HC-12 connection. The resulting sentences will be automatically read on the /fix topic, so to avoid data conflicts between the robot and beacon, they are remapped to publish to with /gps/robot/fix and /gps/beacon/fix, respectively.

Since Nav2 uses Cartesian map coordinates (x,y), this node convert the geographic coordinates into local map coordinates relative to a fixed origin using the geodesy package (see the geographic_info GitHub repository). The Cartesion coordinates are then published to the /goal_pose topic used for waypoint following.

High-Level Data Flow

GPS Beacon
    │
    ▼
nmea_serial_driver
    │
    ▼
/gps/beacon/fix   (NavSatFix message)
    │
    ▼
BeaconGoalPose Node
    │
    ▼
Convert Lat/Lon → UTM
    │
    ▼
Compute Local X/Y Relative to Origin
    │
    ▼
/goal_pose        (PoseStamped message)
    │
    ▼
Nav2 Navigation Stack

class BeaconGoalPose : public rclcpp::Node

This node consists of both a publisher and a subscriber. The publisher, as mentioned above, publishes a PoseStamped message to the /goal_pose topic. The subscriber subscribes to the /gps/beacon/fix topic that hosts a NavSatFix message.

There are two characteristics of the subscriber that are worth noting:

  1. rclcpp::QoS(10) - a ROS2 Quality of Service setting that tells the node to keep a history of the 10 most-recent messages

  2. std::bind(&BeaconGoalPose::callback, this, std::placeholders::_1)) - the node’s overall functionality is entirely dependent on this subscription, so the callback is bound to the subscriber to prevent repeated publications of stale data

The callback takes the message data and converts it to UTM coordinates that are compared to an origin point. This origin is set when the very first subscripted message is received. The difference between these two points is the corresponding Cartesion coordinate in the global frame.