Go to the documentation of this file.00001
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
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
00092 quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
00093
00094
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
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()