Only released in EOL distros:   kinetic

Overview

The gps_goal ROS node will convert navigation goals in GPS coordinates to ROS frame coordinates. It uses the WGS84 ellipsoid and geographiclib python library for calculations.

The GPS goal can be set using a geometry_msgs/PoseStamped or sensor_msgs/NavSatFix message. The robot's desired yaw, pitch, and roll can be set in a PoseStamped message but when using a NavSatFix they will always be set to 0 degrees.

The goal is calculated in a ROS coordinate frame by comparing the goal GPS location to a known GPS location at the origin (0,0) of a ROS frame given by the local_xy_frame parameter (typically world). This initial origin GPS location is best published using a helper initialize_origin node (see the initialize_origin section below).

Quick Start

1. Install:

$ sudo apt-get install ros-kinetic-gps-goal

2. Launch:

$ roslaunch gps_goal gps_goal.launch

3. Initialize known starting GPS location:

This is needed to calculate the distance to the goal.

# use the next GPS coordinate (requires package ros-kinetic-swri-transform-util)
$ roslaunch gps_goal initialize_origin.launch origin:=auto
OR
$ rostopic pub /local_xy_origin geometry_msgs/PoseStamped '{ header: { frame_id: "/map" }, pose: { position: { x: 43.658, y: -79.379 } } }' -1

4. Set a goal using GPS

$ rostopic pub /gps_goal_fix sensor_msgs/NavSatFix "{latitude: 38.42, longitude: -110.79}" -1
OR
$ rostopic pub /gps_goal_pose geometry_msgs/PoseStamped '{ header: { frame_id: "/map" }, pose: { position: { x: 43.658, y: -79.379 } } }' -1
OR
$ roscd gps_goal
$ ./src/gps_goal/gps_goal.py --lat 43.658 --long -79.379 # decimal format
OR 
$ ./src/gps_goal/gps_goal.py --lat 43,39,31 --long -79,22,45 # DMS format

Command Line Interface (CLI)

Invoke once using the Command Line Interface (CLI).

$ roscd gps_goal
$ ./src/gps_goal/gps_goal.py --help

Usage: gps_goal.py [OPTIONS]

  Send goal to move_base given latitude and longitude

  Two usage formats:
  gps_goal.py --lat 43.658 --long -79.379 # decimal format
  gps_goal.py --lat 43,39,31 --long -79,22,45 # DMS format

Options:
  --lat TEXT         Latitude
  --long TEXT        Longitude
  -r, --roll FLOAT   Set target roll for goal
  -p, --pitch FLOAT  Set target pitch for goal
  -y, --yaw FLOAT    Set target yaw for goal
  --help             Show this message and exit.

Nodes

gps_goal

Actions Called

• The move_base action server performs navigation to the goal.

Subscribed Topics

• Set a GPS goal using a PoseStamped message. The x and y values will be considered latitude and longitude respectively. Z, roll, pitch, and yaw values will be respected.

• Set a GPS goal using a NavSatFix GPS message. The goal for roll, pitch, and yaw will be 0 degrees because they are not in this message type.

• The topic to get the origin GPS location from. This location is the origin (0,0) of the frame (typically world) given by the local_xy_frame parameter to the initialize_origin node. This location is used to calculate distances for goals. One message on this topic is consumed when the node starts only.

Parameters

• The tf frame for move_base goals.

initialize_origin

This node will continuously publish (actually in a latched manner) a geometry_msgs/PoseStamped on the local_xy_origin topic and this is better than manually publishing the origin GPS location with rostopic pub. This location is the origin (0,0) of the frame (typically world) given by the local_xy_frame parameter to the initialize_origin node. This location is used to calculate distances for goals. One message on this topic is consumed when the node starts only.

This node is provided by the swri_transform_util package (apt-get install ros-kinetic-swri-transform-util) and it is often launched as a helper node for mapviz, a top-down robot and world visualization tool.

There are two modes for initialize_origin: static or auto.

Static Mode

You can hard code a GPS location (useful for testing) for the origin (0,0). In the following example the coordinates for the Mars Desert Research Station (MDRS) are hard coded in initialize_origin.launch and selected on the command line with the option "origin:=MRDS".

$ roslaunch gps_goal initialize_origin.launch origin:=MRDS

Auto Mode

When using the "auto" mode, the origin will be to the first GPS fix that it recieves on the topic configured in the initialize_origin.launch file.

$ roslaunch gps_goal initialize_origin.launch origin:=auto

Launch example:

<node pkg="swri_transform_util" type="initialize_origin.py" name="initialize_origin" output="screen">
  <param name="local_xy_frame" value="/world"/>
  <param name="local_xy_origin" value="MDRS"/> <!-- setting "auto" here will set the origin to the first GPS fix that it recieves -->
  <remap from="gps" to="gps"/>
  <rosparam param="local_xy_origins">
    [{ name: MDRS,
       latitude: 38.40630,
       longitude: -110.79201,
       altitude: 0.0,
       heading: 0.0}]
  </rosparam>
</node>

Normal Output

$ roscd gps_goal
$ ./src/gps_goal/gps_goal.py --lat 43.658 --long -79.379

[INFO]: Connecting to move_base...
[INFO]: Connected.
[INFO]: Waiting for a message to initialize the origin GPS location...
[INFO]: Received origin: lat 43.642, long -79.380.
[INFO]: Given GPS goal: lat 43.658, long -79.379.
[INFO]: The distance from the origin to the goal is 97.3 m.
[INFO]: The azimuth from the origin to the goal is 169.513 degrees.
[INFO]: The translation from the origin to the goal is (x,y) 91.3, 13.6 m.
[INFO]: Executing move_base goal to position (x,y) 91.3, 13.6, with 138.14 degrees yaw.
[INFO]: To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'
[INFO]: Inital goal status: PENDING
[INFO]: Final goal status: COMPLETE

Improvement Roadmap

• Support using the current robot GPS location instead of an initial origin

Pull requests are welcome!