to_fix.cpp

Go to the documentation of this file.

#include <boost/bind.hpp>

#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"
#include "nav_msgs/Odometry.h"

#include "enu/enu.h"  // ROS wrapper for conversion functions

static void handle_enu(const nav_msgs::OdometryConstPtr odom_ptr,
                       const sensor_msgs::NavSatFix& datum,
                       const ros::Publisher& pub_fix) {
  // Prepare LLH from ENU coordinates, perform conversion
  // using predefined datum
  // Use input message frame_id and timestamp in output fix message
  sensor_msgs::NavSatFix fix;
  enu::point_to_fix(odom_ptr->pose.pose.position, datum, &fix);

  fix.header.frame_id = odom_ptr->child_frame_id;
  fix.header.stamp = odom_ptr->header.stamp;

  // We only need to populate the diagonals of the covariance matrix; the
  // rest initialize to zero automatically, which is correct as the
  // dimensions of the state are independent.
  fix.position_covariance[0] = odom_ptr->pose.covariance[0];
  fix.position_covariance[4] = odom_ptr->pose.covariance[7];
  fix.position_covariance[8] = odom_ptr->pose.covariance[14];

  pub_fix.publish(fix);
}

static void handle_datum(const sensor_msgs::NavSatFixConstPtr datum_ptr,
                         ros::NodeHandle& n) {
  // Pass datum into the data subscriber. Subscriber, Publisher, and the datum
  // array are static, so that they stick around when this function has exited.
  static sensor_msgs::NavSatFix datum(*datum_ptr);
  static ros::Publisher pub_fix = n.advertise<sensor_msgs::NavSatFix>("fix", 5);
  static ros::Subscriber sub_enu = n.subscribe<nav_msgs::Odometry>("enu", 5,
      boost::bind(handle_enu, _1, datum, boost::ref(pub_fix)));
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "to_fix");
  ros::NodeHandle n;

  // Initially just subscribe to the datum topic. Once we receive that, we'll
  // spin up the rest of the node's activities.
  ros::Subscriber sub_datum = n.subscribe<sensor_msgs::NavSatFix>("enu_datum", 5,
      boost::bind(handle_datum, _1, boost::ref(n)));

  ros::spin();
  return 0;
}