publisher.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #     _____
00004 #    /  _  \
00005 #   / _/ \  \
00006 #  / / \_/   \
00007 # /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
00008 # \  / \_/ \  / /  _\| |  | __| / _ \ | ┌┐ \ | ┌┐ \ / _ \ |_   _|| | | |
00009 #  \ \_/ \_/ /  | |  | |  | └─┐| |_| || └┘ / | └┘_/| |_| |  | |  | └─┘ |
00010 #   \  \_/  /   | |_ | |_ | ┌─┘|  _  || |\ \ | |   |  _  |  | |  | ┌─┐ |
00011 #    \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
00012 #            ROBOTICS™
00013 #
00014 #
00015 #  Copyright © 2012 Clearpath Robotics, Inc. 
00016 #  All Rights Reserved
00017 #  
00018 # Redistribution and use in source and binary forms, with or without
00019 # modification, are permitted provided that the following conditions are met:
00020 #     * Redistributions of source code must retain the above copyright
00021 #       notice, this list of conditions and the following disclaimer.
00022 #     * Redistributions in binary form must reproduce the above copyright
00023 #       notice, this list of conditions and the following disclaimer in the
00024 #       documentation and/or other materials provided with the distribution.
00025 #     * Neither the name of Clearpath Robotics, Inc. nor the
00026 #       names of its contributors may be used to endorse or promote products
00027 #       derived from this software without specific prior written permission.
00028 # 
00029 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00030 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00031 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00032 # DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
00033 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00034 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00035 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00036 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00037 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00038 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00039 #
00040 # Please send comments, questions, or patches to skynet@clearpathrobotics.com
00041 #
00042 
00043 # ROS
00044 import roslib; roslib.load_manifest('applanix_publisher')
00045 import rospy
00046 import tf
00047 import PyKDL
00048 
00049 # Applanix node internal messages & modules
00050 from applanix_msgs.msg import NavigationSolution, GNSSStatus, IMUData
00051 from gps_utm import LLtoUTM
00052 
00053 # ROS standard messages
00054 from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus
00055 from nav_msgs.msg import Odometry
00056 from geometry_msgs.msg import Quaternion, Point, Pose
00057 
00058 # Other
00059 from math import radians as RAD
00060 
00061 # FIXED COVARIANCES
00062 # TODO: Work these out...
00063 IMU_ORIENT_COVAR = [1e-3, 0, 0,
00064                     0, 1e-3, 0,
00065                     0, 0, 1e-3]
00066 
00067 IMU_VEL_COVAR = [1e-3, 0, 0,
00068                  0, 1e-3, 0,
00069                  0, 0, 1e-3]
00070 
00071 IMU_ACCEL_COVAR = [1e-3, 0, 0,
00072                    0, 1e-3, 0,
00073                    0, 0, 1e-3]
00074 
00075 NAVSAT_COVAR = [1, 0, 0,
00076                 0, 1, 0,
00077                 0, 0, 1]
00078 
00079 POSE_COVAR = [1, 0, 0, 0, 0, 0,
00080               0, 1, 0, 0, 0, 0,
00081               0, 0, 1, 0, 0, 0,
00082               0, 0, 0, 0.1, 0, 0,
00083               0, 0, 0, 0, 0.1, 0,
00084               0, 0, 0, 0, 0, 0.1]
00085 
00086 TWIST_COVAR = [1, 0, 0, 0, 0, 0,
00087                0, 1, 0, 0, 0, 0,
00088                0, 0, 1, 0, 0, 0,
00089                0, 0, 0, 0.1, 0, 0,
00090                0, 0, 0, 0, 0.1, 0,
00091                0, 0, 0, 0, 0, 0.1]
00092 
00093 class ApplanixPublisher(object):
00094 
00095     def __init__(self):
00096         rospy.init_node('applanix_publisher')
00097         # Parameters
00098         self.publish_tf = rospy.get_param('~publish_tf', False)
00099         self.odom_frame = rospy.get_param('~odom_frame', 'odom_combined')
00100         self.base_frame = rospy.get_param('~base_frame', 'base_footprint')
00101         self.zero_start = rospy.get_param('~zero_start', False) # If this is True, UTM will be pub'd wrt. our first recv'd coordinate
00102 
00103         # Topic publishers
00104         self.pub_imu = rospy.Publisher('imu_data', Imu)
00105         self.pub_odom = rospy.Publisher('gps_odom', Odometry)
00106         self.pub_origin = rospy.Publisher('origin', Pose)
00107         self.pub_navsatfix = rospy.Publisher('gps_fix', NavSatFix)
00108         self.pub_navsatstatus = rospy.Publisher('gps_status', NavSatStatus)
00109         if self.publish_tf:
00110             self.tf_broadcast = tf.TransfromBroadcaster()
00111 
00112         # Init nav status
00113         self.nav_status = NavSatStatus()    # We need this for the NavSatFix broadcaster
00114         self.nav_status.status = NavSatStatus.STATUS_NO_FIX
00115         self.nav_status.service = NavSatStatus.SERVICE_GPS
00116 
00117         self.init = False       # If we've been initialized
00118         self.origin = Point()   # Where we've started
00119         
00120         # Subscribed topics
00121         rospy.Subscriber('nav', NavigationSolution, self.navigation_handler)
00122         rospy.Subscriber('status/gnss/primary', GNSSStatus, self.status_handler)
00123        
00124     def navigation_handler(self, data):
00125         """ Rebroadcasts navigation data in the following formats:
00126         1) /odom => /base footprint transform (if enabled, as per REP 105)
00127         2) Odometry message, with parent/child IDs as in #1
00128         3) NavSatFix message, for systems which are knowledgeable about GPS stuff
00129         4) IMU messages
00130         """
00131         rospy.logdebug("Navigation received")
00132         # If we don't have a fix, don't publish anything...
00133         if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
00134             return
00135         
00136         orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(data.pitch), RAD(data.heading)).GetQuaternion()
00137 
00138         # UTM conversion
00139         (zone, easting, northing) = LLtoUTM(23, data.latitude, data.longitude)
00140         # Initialize starting point if we haven't yet
00141         # TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid?
00142         if not self.init and self.zero_start:
00143             self.origin.x = easting
00144             self.origin.y = northing
00145             self.init = True
00146 
00147         # Publish origin reference for others to know about
00148         p = Pose()
00149         p.position.x = self.origin.x
00150         p.position.y = self.origin.y
00151         self.pub_origin.publish(p)
00152 
00153         #
00154         # Odometry 
00155         # TODO: Work out these covariances properly from DOP
00156         #
00157         odom = Odometry()
00158         odom.header.stamp = rospy.Time.now()
00159         odom.header.frame_id = self.odom_frame
00160         odom.child_frame_id = self.base_frame
00161         odom.pose.pose.position.x = easting - self.origin.x
00162         odom.pose.pose.position.y = northing - self.origin.y
00163         odom.pose.pose.position.z = data.altitude
00164         odom.pose.pose.orientation = Quaternion(*orient)
00165         odom.pose.covariance = POSE_COVAR
00166         # Twist is relative to /vehicle frame
00167         odom.twist.twist.linear.x = data.speed
00168         odom.twist.twist.linear.y = 0
00169         odom.twist.twist.linear.z = -data.down_vel
00170         odom.twist.twist.angular.x = RAD(data.ang_rate_long)
00171         odom.twist.twist.angular.y = RAD(-data.ang_rate_trans)
00172         odom.twist.twist.angular.z = RAD(-data.ang_rate_down)
00173         odom.twist.covariance = TWIST_COVAR
00174 
00175         self.pub_odom.publish(odom)
00176 
00177         #
00178         # Odometry transform (if required)
00179         #
00180         if self.publish_tf:
00181             self.tf_broadcast.sendTransform(
00182                 (odom.pose.pose.position.x, odom.pose.pose.position.y,
00183                  odom.pose.pose.position.z), Quaternion(*orient),
00184                  odom.header.stamp,odom.child_frame_id, odom.frame_id)
00185 
00186         # 
00187         # NavSatFix
00188         # TODO: Work out these covariances properly from DOP
00189         #
00190         navsat = NavSatFix()
00191         navsat.header.stamp = rospy.Time.now()
00192         navsat.header.frame_id = self.odom_frame
00193         navsat.status = self.nav_status
00194 
00195         navsat.latitude = data.latitude
00196         navsat.longitude = data.longitude
00197         navsat.altitude = data.altitude
00198 
00199         navsat.position_covariance = NAVSAT_COVAR
00200         navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
00201         
00202         self.pub_navsatfix.publish(navsat)
00203         
00204         #
00205         # IMU
00206         # TODO: Work out these covariances properly
00207         #
00208         imu = Imu()
00209         imu.header.stamp == rospy.Time.now()
00210         imu.header.frame_id = self.base_frame
00211       
00212         # Orientation
00213         imu.orientation = Quaternion(*orient)
00214         imu.orientation_covariance = IMU_ORIENT_COVAR
00215  
00216         # Angular rates
00217         imu.angular_velocity.x = RAD(data.ang_rate_long)
00218         imu.angular_velocity.y = RAD(-data.ang_rate_trans)
00219         imu.angular_velocity.y = RAD(-data.ang_rate_down)
00220         imu.angular_velocity_covariance = IMU_VEL_COVAR
00221 
00222         # Linear acceleration
00223         imu.linear_acceleration.x = data.long_accel
00224         imu.linear_acceleration.y = data.trans_accel
00225         imu.linear_acceleration.z = data.down_accel
00226         imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
00227 
00228         self.pub_imu.publish(imu)
00229         
00230          
00231         pass
00232 
00233     def status_handler(self, data):
00234         """ Rebroadcasts GNSS status as a standard NavSatStatus message """
00235         # In the below, not sure about mapping the "DGPS" status to SBAS instead of GBAS
00236         solution_map = {
00237             GNSSStatus.SOLUTION_UNKNOWN: NavSatStatus.STATUS_NO_FIX,
00238             GNSSStatus.SOLUTION_NO_DATA: NavSatStatus.STATUS_NO_FIX,
00239             GNSSStatus.SOLUTION_HORIZONTAL_CA: NavSatStatus.STATUS_FIX,
00240             GNSSStatus.SOLUTION_3D_CA: NavSatStatus.STATUS_FIX,
00241             GNSSStatus.SOLUTION_HORIZONTAL_DGPS: NavSatStatus.STATUS_SBAS_FIX,
00242             GNSSStatus.SOLUTION_3D_DGPS: NavSatStatus.STATUS_SBAS_FIX,
00243             GNSSStatus.SOLUTION_FLOAT_RTK: NavSatStatus.STATUS_GBAS_FIX,
00244             GNSSStatus.SOLUTION_WIDE_LANE_RTK: NavSatStatus.STATUS_GBAS_FIX,
00245             GNSSStatus.SOLUTION_NARROW_LANE_RTK: NavSatStatus.STATUS_GBAS_FIX,
00246             GNSSStatus.SOLUTION_P_CODE: NavSatStatus.STATUS_FIX,
00247             GNSSStatus.SOLUTION_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX,
00248             GNSSStatus.SOLUTION_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX,
00249             GNSSStatus.SOLUTION_OMNISTAR_VBS: NavSatStatus.STATUS_SBAS_FIX,
00250             }
00251         self.nav_status.status = solution_map.get(data.solution_status,NavSatStatus.STATUS_NO_FIX)
00252 
00253         # Assume GPS - this isn't exposed
00254         self.nav_status.service = NavSatStatus.SERVICE_GPS
00255             
00256         self.pub_navsatstatus.publish(self.nav_status)
00257 
00258 
00259 def main():
00260     node = ApplanixPublisher()
00261     rospy.spin()


applanix_publisher
Author(s): Mike Purvis
autogenerated on Thu Jan 2 2014 11:05:28