utm_odometry_to_navsatfix_node.cpp
Go to the documentation of this file.
1 /*
2  * Translates nav_msgs/Odometry in UTM coordinates back into sensor_msgs/NavSat{Fix,Status}
3  * Useful for visualizing UTM data on a map or comparing with raw GPS data
4  * Added by Dheera Venkatraman (dheera@dheera.net)
5  */
6 
7 #include <ros/ros.h>
10 #include <sensor_msgs/NavSatStatus.h>
11 #include <sensor_msgs/NavSatFix.h>
12 #include <gps_common/conversions.h>
13 #include <nav_msgs/Odometry.h>
14 
15 using namespace gps_common;
16 
18 std::string frame_id, child_frame_id;
19 std::string zone_param;
20 double rot_cov;
21 
22 void callback(const nav_msgs::OdometryConstPtr& odom) {
23 
24  if (odom->header.stamp == ros::Time(0)) {
25  return;
26  }
27 
28  if (!fix_pub) {
29  return;
30  }
31 
32  double northing, easting, latitude, longitude;
33  std::string zone;
34  sensor_msgs::NavSatFix fix;
35 
36  northing = odom->pose.pose.position.y;
37  easting = odom->pose.pose.position.x;
38 
39  if(zone_param.length() > 0) {
40  // utm zone was supplied as a ROS parameter
41  zone = zone_param;
42  fix.header.frame_id = odom->header.frame_id;
43  } else {
44  // look for the utm zone in the frame_id
45  std::size_t pos = odom->header.frame_id.find("/utm_");
46  if(pos==std::string::npos) {
47  ROS_WARN("UTM zone not found in frame_id");
48  return;
49  }
50  zone = odom->header.frame_id.substr(pos + 5, 3);
51  fix.header.frame_id = odom->header.frame_id.substr(0, pos);
52  }
53 
54  ROS_INFO("zone: %s", zone.c_str());
55 
56  fix.header.stamp = odom->header.stamp;
57 
58  UTMtoLL(northing, easting, zone, latitude, longitude);
59 
60  fix.latitude = latitude;
61  fix.longitude = longitude;
62  fix.altitude = odom->pose.pose.position.z;
63 
64  fix.position_covariance[0] = odom->pose.covariance[0];
65  fix.position_covariance[1] = odom->pose.covariance[1];
66  fix.position_covariance[2] = odom->pose.covariance[2];
67  fix.position_covariance[3] = odom->pose.covariance[6];
68  fix.position_covariance[4] = odom->pose.covariance[7];
69  fix.position_covariance[5] = odom->pose.covariance[8];
70  fix.position_covariance[6] = odom->pose.covariance[12];
71  fix.position_covariance[7] = odom->pose.covariance[13];
72  fix.position_covariance[8] = odom->pose.covariance[14];
73 
74  fix.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
75 
76  fix_pub.publish(fix);
77 }
78 
79 int main (int argc, char **argv) {
80  ros::init(argc, argv, "utm_odometry_to_navsatfix_node");
81  ros::NodeHandle node;
82  ros::NodeHandle priv_node("~");
83 
84  priv_node.param<std::string>("frame_id", frame_id, "");
85  priv_node.param<std::string>("zone", zone_param, "");
86 
87  fix_pub = node.advertise<sensor_msgs::NavSatFix>("odom_fix", 10);
88 
89  ros::Subscriber odom_sub = node.subscribe("odom", 10, callback);
90 
91  ros::spin();
92 }
93 
std::string child_frame_id
int main(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::string zone_param
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_WARN(...)
static void UTMtoLL(const double UTMNorthing, const double UTMEasting, const char *UTMZone, double &Lat, double &Long)
Definition: conversions.h:230
ROSCPP_DECL void spin(Spinner &spinner)
static ros::Publisher fix_pub
Universal Transverse Mercator transforms.
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string frame_id
void callback(const nav_msgs::OdometryConstPtr &odom)


gps_common
Author(s):
autogenerated on Thu May 28 2020 03:13:33