publisher.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 # Software License Agreement (BSD)
5 #
6 # file @publisher.py
7 # authors Mike Purvis <mpurvis@clearpathrobotics.com>
8 # NovAtel <novatel.com/support>
9 # copyright Copyright (c) 2012, Clearpath Robotics, Inc., All rights reserved.
10 # Copyright (c) 2014, NovAtel Inc., All rights reserved.
11 #
12 # Redistribution and use in source and binary forms, with or without modification, are permitted provided that
13 # the following conditions are met:
14 # * Redistributions of source code must retain the above copyright notice, this list of conditions and the
15 # following disclaimer.
16 # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
17 # following disclaimer in the documentation and/or other materials provided with the distribution.
18 # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
19 # products derived from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
22 # RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23 # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
24 # DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
25 # OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
26 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 
29 import rospy
30 import tf
31 import geodesy.utm
32 
33 from novatel_msgs.msg import BESTPOS, CORRIMUDATA, INSCOV, INSPVAX
34 from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus
35 from nav_msgs.msg import Odometry
36 from geometry_msgs.msg import Quaternion, Point, Pose, Twist
37 
38 from math import radians, pow
39 
40 # FIXED COVARIANCES
41 # TODO: Work these out...
42 IMU_ORIENT_COVAR = [1e-3, 0, 0,
43  0, 1e-3, 0,
44  0, 0, 1e-3]
45 
46 IMU_VEL_COVAR = [1e-3, 0, 0,
47  0, 1e-3, 0,
48  0, 0, 1e-3]
49 
50 IMU_ACCEL_COVAR = [1e-3, 0, 0,
51  0, 1e-3, 0,
52  0, 0, 1e-3]
53 
54 NAVSAT_COVAR = [1, 0, 0,
55  0, 1, 0,
56  0, 0, 1]
57 
58 POSE_COVAR = [1, 0, 0, 0, 0, 0,
59  0, 1, 0, 0, 0, 0,
60  0, 0, 1, 0, 0, 0,
61  0, 0, 0, 0.1, 0, 0,
62  0, 0, 0, 0, 0.1, 0,
63  0, 0, 0, 0, 0, 0.1]
64 
65 TWIST_COVAR = [1, 0, 0, 0, 0, 0,
66  0, 1, 0, 0, 0, 0,
67  0, 0, 1, 0, 0, 0,
68  0, 0, 0, 0.1, 0, 0,
69  0, 0, 0, 0, 0.1, 0,
70  0, 0, 0, 0, 0, 0.1]
71 
72 
73 class NovatelPublisher(object):
74  """ Subscribes to the directly-translated messages from the SPAN system
75  and repackages the resultant data as standard ROS messages. """
76 
77  def __init__(self):
78  # Parameters
79  self.publish_tf = rospy.get_param('~publish_tf', False)
80  self.odom_frame = rospy.get_param('~odom_frame', 'odom_combined')
81  self.base_frame = rospy.get_param('~base_frame', 'base_link')
82 
83  # When True, UTM odom x, y pose will be published with respect to the
84  # first coordinate received.
85  self.zero_start = rospy.get_param('~zero_start', False)
86 
87  self.imu_rate = rospy.get_param('~rate', 100)
88 
89  # Topic publishers
90  self.pub_imu = rospy.Publisher('imu/data', Imu, queue_size=1)
91  self.pub_odom = rospy.Publisher('navsat/odom', Odometry, queue_size=1)
92  self.pub_origin = rospy.Publisher('navsat/origin', Pose, queue_size=1, latch=True)
93  self.pub_navsatfix = rospy.Publisher('navsat/fix', NavSatFix, queue_size=1)
94 
95  if self.publish_tf:
97 
98  self.init = False # If we've been initialized
99  self.origin = Point() # Where we've started
100  self.orientation = [0] * 4 # Empty quaternion until we hear otherwise
101  self.orientation_covariance = IMU_ORIENT_COVAR
102 
103  # Subscribed topics
104  rospy.Subscriber('novatel_data/bestpos', BESTPOS, self.bestpos_handler)
105  rospy.Subscriber('novatel_data/corrimudata', CORRIMUDATA, self.corrimudata_handler)
106  rospy.Subscriber('novatel_data/inscov', INSCOV, self.inscov_handler)
107  rospy.Subscriber('novatel_data/inspvax', INSPVAX, self.inspvax_handler)
108 
109  def bestpos_handler(self, bestpos):
110  navsat = NavSatFix()
111 
112  # TODO: The timestamp here should come from SPAN, not the ROS system time.
113  navsat.header.stamp = rospy.Time.now()
114  navsat.header.frame_id = self.odom_frame
115 
116  # Assume GPS - this isn't exposed
117  navsat.status.service = NavSatStatus.SERVICE_GPS
118 
119  position_type_to_status = {
120  BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX,
121  BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX,
122  BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX,
123  BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX,
124  BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX,
125  BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX,
126  BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX,
127  BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX,
128  BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
129  BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX,
130  BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX,
131  BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX,
132  BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
133  BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
134  BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
135  BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX,
136  BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX,
137  BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX,
138  BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX,
139  BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX,
140  BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX,
141  BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
142  BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX,
143  BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX,
144  BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX,
145  BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX,
146  BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX,
147  BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX,
148  BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX,
149  BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
150  BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX,
151  BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
152  BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX,
153  }
154  navsat.status.status = position_type_to_status.get(bestpos.position_type,
155  NavSatStatus.STATUS_NO_FIX)
156 
157  # Position in degrees.
158  navsat.latitude = bestpos.latitude
159  navsat.longitude = bestpos.longitude
160 
161  # Altitude in metres.
162  navsat.altitude = bestpos.altitude + bestpos.undulation
163  navsat.position_covariance[0] = pow(2, bestpos.latitude_std)
164  navsat.position_covariance[4] = pow(2, bestpos.longitude_std)
165  navsat.position_covariance[8] = pow(2, bestpos.altitude_std)
166  navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
167 
168  # Ship ito
169  self.pub_navsatfix.publish(navsat)
170 
171  def inspvax_handler(self, inspvax):
172  # Convert the latlong to x,y coordinates and publish an Odometry
173  try:
174  utm_pos = geodesy.utm.fromLatLong(inspvax.latitude, inspvax.longitude)
175  except ValueError:
176  # Probably coordinates out of range for UTM conversion.
177  return
178 
179  if not self.init and self.zero_start:
180  self.origin.x = utm_pos.easting
181  self.origin.y = utm_pos.northing
182  self.origin.z = inspvax.altitude
183  self.pub_origin.publish(position=self.origin)
184 
185  odom = Odometry()
186  odom.header.stamp = rospy.Time.now()
187  odom.header.frame_id = self.odom_frame
188  odom.child_frame_id = self.base_frame
189  odom.pose.pose.position.x = utm_pos.easting - self.origin.x
190  odom.pose.pose.position.y = utm_pos.northing - self.origin.y
191  odom.pose.pose.position.z = inspvax.altitude - self.origin.z
192 
193  # Orientation
194  # Save this on an instance variable, so that it can be published
195  # with the IMU message as well.
196  self.orientation = tf.transformations.quaternion_from_euler(
197  radians(inspvax.roll),
198  radians(inspvax.pitch),
199  -radians(inspvax.azimuth), 'syxz')
200  odom.pose.pose.orientation = Quaternion(*self.orientation)
201  odom.pose.covariance[21] = self.orientation_covariance[0] = pow(inspvax.pitch_std, 2)
202  odom.pose.covariance[28] = self.orientation_covariance[4] = pow(inspvax.roll_std, 2)
203  odom.pose.covariance[35] = self.orientation_covariance[8] = pow(inspvax.azimuth_std, 2)
204 
205  # Twist is relative to vehicle frame
206  odom.twist.twist.linear.x = inspvax.east_velocity
207  odom.twist.twist.linear.y = inspvax.north_velocity
208  odom.twist.twist.linear.z = inspvax.up_velocity
209  TWIST_COVAR[0] = pow(2, inspvax.east_velocity_std)
210  TWIST_COVAR[7] = pow(2, inspvax.north_velocity_std)
211  TWIST_COVAR[14] = pow(2, inspvax.up_velocity_std)
212  odom.twist.covariance = TWIST_COVAR
213 
214  self.pub_odom.publish(odom)
215 
216  # Odometry transform (if required)
217  if self.publish_tf:
218  self.tf_broadcast.sendTransform(
219  (odom.pose.pose.position.x, odom.pose.pose.position.y,
220  odom.pose.pose.position.z),
221  self.orientation,
222  odom.header.stamp, odom.child_frame_id, odom.header.frame_id)
223 
224  # Mark that we've received our first fix, and set origin if necessary.
225  self.init = True
226 
227  def corrimudata_handler(self, corrimudata):
228  # TODO: Work out these covariances properly. Logs provide covariances in local frame, not body
229  imu = Imu()
230  imu.header.stamp = rospy.Time.now()
231  imu.header.frame_id = self.base_frame
232 
233  # Populate orientation field with one from inspvax message.
234  imu.orientation = Quaternion(*self.orientation)
235  imu.orientation_covariance = self.orientation_covariance
236 
237  # Angular rates (rad/s)
238  # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz
239  imu.angular_velocity.x = corrimudata.pitch_rate * self.imu_rate
240  imu.angular_velocity.y = corrimudata.roll_rate * self.imu_rate
241  imu.angular_velocity.z = corrimudata.yaw_rate * self.imu_rate
242  imu.angular_velocity_covariance = IMU_VEL_COVAR
243 
244  # Linear acceleration (m/s^2)
245  imu.linear_acceleration.x = corrimudata.x_accel * self.imu_rate
246  imu.linear_acceleration.y = corrimudata.y_accel * self.imu_rate
247  imu.linear_acceleration.z = corrimudata.z_accel * self.imu_rate
248  imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
249 
250  self.pub_imu.publish(imu)
251 
252  def inscov_handler(self, inscov):
253  # TODO: Supply this data in the IMU and Odometry messages.
254  pass


novatel_span_driver
Author(s): NovAtel Support
autogenerated on Mon Sep 2 2019 03:58:19