Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Adding a GPS sensor to the Robot Pose EKF Filter

Available topics

The Robot Pose EKF node listens for ROS messages on the following topic names:

• /odom for the nav_msgs::Odometry message as a 2D pose

• /imu_data for the sensor_msgs::Imu message as a 3D orientation

• /vo for the nav_msgs::Odometry message as a 3D pose

To use the Robot Pose EKF node with your own sensor, you need to publish sensor measurements on one of these three topics. The sensor needs to publish the position and orientation of the base_footprint frame of the robot, in a world-fixed frame. Any world-fixed frame is fine, and each sensor can use a different world-fixed frame. Note that each topic can only take the input of one single sensor. In future versions, we plan the ability to attach n sensor signals to the robot pose ekf (see the Roadmap section). Currently, the most generic input message is the Odometry, sent on the /vo topic. It contains a 3D pose and 3D twist, each with a covariance. So this would be the best topic to use when adding your own sensor.

Building a GPS sensor message

A GPS sensor measures the robot 3d position, but not its orientation. The Odometry message sent by the GPS sensor could look something like this:

•  msg.header.stamp = gps_time                   // time of gps measurement
   msg.header.frame_id = base_footprint          // the tracked robot frame
   msg.pose.pose.position.x = gps_x              // x measurement GPS.
   msg.pose.pose.position.y = gps_y              // y measurement GPS.
   msg.pose.pose.position.z = gps_z              // z measurement GPS.
   msg.pose.pose.orientation.x = 1               // identity quaternion
   msg.pose.pose.orientation.y = 0               // identity quaternion
   msg.pose.pose.orientation.z = 0               // identity quaternion
   msg.pose.pose.orientation.w = 0               // identity quaternion
   msg.pose.covariance = {cox_x, 0, 0, 0, 0, 0,  // covariance on gps_x
                          0, cov_y, 0, 0, 0, 0,  // covariance on gps_y
                          0, 0, cov_z, 0, 0, 0,  // covariance on gps_z
                          0, 0, 0, 99999, 0, 0,  // large covariance on rot x
                          0, 0, 0, 0, 99999, 0,  // large covariance on rot y
                          0, 0, 0, 0, 0, 99999}  // large covariance on rot z

Using a GPS driver that publishes NavSatFix

New in Diamondback

The utm_odometry_node in the gps_common package may be used to convert GPS measurements from sensor_msgs/NavSatFix to sensor_msgs/Odometry.

• <node name="gps_conv" pkg="gps_common" type="utm_odometry_node">
    <remap from="odom" to="vo"/>
    <remap from="fix" to="/gps/fix" />
    <param name="rot_covariance" value="99999" />
    <param name="frame_id" value="base_footprint" />
  </node>

Remapping the topic name

The GPS sensor sends its measurements on the topic name gps_meas, but the Robot Pose EKF node expects messages of type Odometry on the topic name odom. To connect the GPS sensor with the filter node, we need to remap the topic name the node listens on. Therefore, add the following line to the launch file of the node, within the <node> ... </node> scope:

•   <remap from="vo" to="gps_meas" />

The output of the filter (the estimated 3D robot pose) is broadcast on the /robot_pose_ekf/odom_combined topic. The node also broadcasts the transform from 'odom_combined' to 'base_link' to the transform (tf) topic /tf_message.