rmp_pose_updater.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004 Pose updater for the Segway RMP platform.
00005 
00006 Author:  Chris Dunkers, Worcester Polytechnic Institute
00007 Author:  Russell Toris, Worcester Polytechnic Institute
00008 Version: June 10, 2014
00009 """
00010 from rmp_msgs.msg import RMPFeedback
00011 from tf.msg import *
00012 from nav_msgs.msg import Odometry
00013 from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion
00014 from tf.transformations import *
00015 import tf
00016 import rospy
00017 import math
00018 import time
00019 
00020 class PoseUpdate:
00021         """
00022         Pose updater for the Segway RMP platform.
00023         """
00024 
00025         prev_lin_pos = 0.0
00026         prev_yaw = 0.0
00027         prev_x_pos = 0.0
00028         prev_y_pos = 0.0
00029         prev_time = 0.0
00030 
00031         def __init__(self):
00032                 """
00033                 Initialize the subscriptions and publishers of the node.
00034                 """
00035                 self.odomPub = rospy.Publisher('odom', Odometry, queue_size = 'None')
00036                 self.tfBroadCast = tf.TransformBroadcaster()
00037                 rospy.Subscriber("rmp_feedback", RMPFeedback, self.pose_update)
00038                 self.publish_tf = rospy.get_param('~publish_tf',True)
00039                 
00040         def pose_update(self, rmp):
00041                 """
00042                 Read in the current RMP feedback and publish the pose
00043                 :param rmp: the RMP feedback message
00044                 """
00045                 rmp_items = rmp.sensor_items
00046                 rmp_values = rmp.sensor_values
00047                 time_msg_received = rmp.header.stamp.secs + 10**-9 * rmp.header.stamp.nsecs
00048                 
00049                 """
00050                 get the values for the feedback items needed
00051                 
00052                 angles and angle rates are flipped because the RMP's do not 
00053                 follow the right hand rule convention
00054                 """
00055                 for x in range(0, len(rmp_items)):
00056                         if rmp_items[x] == 'linear_vel_mps':
00057                                 lin_vel = rmp_values[x]
00058                         elif rmp_items[x] == 'linear_pos_m':
00059                                 lin_pos = rmp_values[x] 
00060                         elif rmp_items[x] == 'differential_wheel_vel_rps':
00061                                 diff_rate = -1 * rmp_values[x]
00062                         elif rmp_items[x] == 'angle_target_deg':
00063                                 ang_targ = (rmp_values[x] - 90) * math.pi/180
00064                         elif rmp_items[x] == 'pse_pitch_deg':
00065                                 pitch = (-1 * rmp_values[x]) * math.pi/180
00066                         elif rmp_items[x] == 'pse_roll_deg':
00067                                 roll = (-1 * rmp_values[x]) * math.pi/180
00068                 
00069                 """
00070                 Segway RMP base tends to drift when stationary, basic 
00071                 filter to ignore low differences and reduce/stop drift
00072                 """     
00073                 if diff_rate >= -0.005 and diff_rate <= 0.005:
00074                         diff_rate = 0.0
00075 
00076                 """
00077                 Calculate the new pose based on the feedback from the Segway
00078                 and the time difference from the previous calculation
00079                 """
00080                 yaw = self.prev_yaw + diff_rate * (time_msg_received - self.prev_time)
00081                 x_pos = self.prev_x_pos + (lin_pos - self.prev_lin_pos) * math.cos(yaw)
00082                 y_pos = self.prev_y_pos + (lin_pos - self.prev_lin_pos) * math.sin(yaw)
00083 
00084                 # store the current values to be used in the next iteration
00085                 self.prev_lin_pos = lin_pos
00086                 self.prev_x_pos = x_pos
00087                 self.prev_y_pos = y_pos
00088                 self.prev_yaw = yaw
00089                 self.prev_time = time_msg_received
00090 
00091                 # create quaternion array from rmp and IMU data
00092                 quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
00093 
00094                 # make and publish the odometry message
00095                 odom = Odometry()
00096                 odom.header.stamp = rospy.Time.now()
00097                 odom.header.frame_id = "odom"
00098                 odom.child_frame_id = "base_footprint"
00099                 odom.pose.pose.position.x = x_pos
00100                 odom.pose.pose.position.y = y_pos
00101                 odom.pose.pose.position.z = 0.0
00102                 odom.pose.pose.orientation.x = quaternion[0]
00103                 odom.pose.pose.orientation.y = quaternion[1]
00104                 odom.pose.pose.orientation.z = quaternion[2]
00105                 odom.pose.pose.orientation.w = quaternion[3]
00106                 odom.twist.twist.linear.x = lin_vel * math.cos(ang_targ)
00107                 odom.twist.twist.linear.y = lin_vel * math.sin(ang_targ)
00108                 odom.twist.twist.angular.z = diff_rate
00109                 self.odomPub.publish(odom)
00110 
00111                 # publish the transform from odom to the base footprint
00112                 if self.publish_tf:
00113                         self.tfBroadCast.sendTransform((x_pos, y_pos, 0.0), quaternion, rospy.Time.now(), '/base_footprint', '/odom')
00114 
00115 if __name__ == "__main__":
00116         rospy.init_node("rmp_pose_updater")
00117         poseUpdate = PoseUpdate()
00118         rospy.loginfo("RMP Pose Updater Started")
00119         rospy.spin()


ros_ethernet_rmp
Author(s): SEGWAY Inc., Chris Dunkers , David Kent
autogenerated on Tue Mar 8 2016 01:14:58