utm_odometry_node.cpp
Go to the documentation of this file.
1 /*
2  * Translates sensor_msgs/NavSat{Fix,Status} into nav_msgs/Odometry using UTM
3  */
4 
5 #include <ros/ros.h>
8 #include <sensor_msgs/NavSatStatus.h>
9 #include <sensor_msgs/NavSatFix.h>
10 #include <gps_common/conversions.h>
11 #include <nav_msgs/Odometry.h>
12 
13 using namespace gps_common;
14 
16 std::string frame_id, child_frame_id;
17 double rot_cov;
18 bool append_zone = false;
19 
20 void callback(const sensor_msgs::NavSatFixConstPtr& fix) {
21  if (fix->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
22  ROS_DEBUG_THROTTLE(60,"No fix.");
23  return;
24  }
25 
26  if (fix->header.stamp == ros::Time(0)) {
27  return;
28  }
29 
30  double northing, easting;
31  std::string zone;
32 
33  LLtoUTM(fix->latitude, fix->longitude, northing, easting, zone);
34 
35  if (odom_pub) {
36  nav_msgs::Odometry odom;
37  odom.header.stamp = fix->header.stamp;
38 
39  if (frame_id.empty()) {
40  if(append_zone) {
41  odom.header.frame_id = fix->header.frame_id + "/utm_" + zone;
42  } else {
43  odom.header.frame_id = fix->header.frame_id;
44  }
45  } else {
46  if(append_zone) {
47  odom.header.frame_id = frame_id + "/utm_" + zone;
48  } else {
49  odom.header.frame_id = frame_id;
50  }
51  }
52 
53  odom.child_frame_id = child_frame_id;
54 
55  odom.pose.pose.position.x = easting;
56  odom.pose.pose.position.y = northing;
57  odom.pose.pose.position.z = fix->altitude;
58 
59  odom.pose.pose.orientation.x = 0;
60  odom.pose.pose.orientation.y = 0;
61  odom.pose.pose.orientation.z = 0;
62  odom.pose.pose.orientation.w = 1;
63 
64  // Use ENU covariance to build XYZRPY covariance
65  boost::array<double, 36> covariance = {{
66  fix->position_covariance[0],
67  fix->position_covariance[1],
68  fix->position_covariance[2],
69  0, 0, 0,
70  fix->position_covariance[3],
71  fix->position_covariance[4],
72  fix->position_covariance[5],
73  0, 0, 0,
74  fix->position_covariance[6],
75  fix->position_covariance[7],
76  fix->position_covariance[8],
77  0, 0, 0,
78  0, 0, 0, rot_cov, 0, 0,
79  0, 0, 0, 0, rot_cov, 0,
80  0, 0, 0, 0, 0, rot_cov
81  }};
82 
83  odom.pose.covariance = covariance;
84 
85  odom_pub.publish(odom);
86  }
87 }
88 
89 int main (int argc, char **argv) {
90  ros::init(argc, argv, "utm_odometry_node");
91  ros::NodeHandle node;
92  ros::NodeHandle priv_node("~");
93 
94  priv_node.param<std::string>("frame_id", frame_id, "");
95  priv_node.param<std::string>("child_frame_id", child_frame_id, "");
96  priv_node.param<double>("rot_covariance", rot_cov, 99999.0);
97  priv_node.param<bool>("append_zone", append_zone, false);
98 
99  odom_pub = node.advertise<nav_msgs::Odometry>("odom", 10);
100 
101  ros::Subscriber fix_sub = node.subscribe("fix", 10, callback);
102 
103  ros::spin();
104 }
105 
bool append_zone
static ros::Publisher odom_pub
void publish(const boost::shared_ptr< M > &message) const
#define ROS_DEBUG_THROTTLE(rate,...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::string child_frame_id
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
static void LLtoUTM(const double Lat, const double Long, double &UTMNorthing, double &UTMEasting, char *UTMZone)
Definition: conversions.h:148
int main(int argc, char **argv)
Universal Transverse Mercator transforms.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
double rot_cov
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string frame_id
void callback(const sensor_msgs::NavSatFixConstPtr &fix)


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