Go to the documentation of this file.00001
00002
00003 """
00004 Battery monitor for the Segway RMP platform.
00005
00006 Author: Chris Dunkers, Worcester Polytechnic Institute
00007 Version: June 23, 2014
00008 """
00009 from rmp_msgs.msg import RMPFeedback
00010 from python_ethernet_rmp.system_defines import *
00011 from std_msgs.msg import String
00012 from sensor_msgs.msg import JointState
00013 import rospy
00014 import math
00015 import time
00016 import os
00017
00018 class JointStateMonitor:
00019
00020 def __init__(self):
00021 """
00022 Initialize the subscriptions and publishers of the node.
00023 """
00024 self.jointStatePub = rospy.Publisher('rmp_joint_states', JointState, queue_size = 'None')
00025
00026 rospy.Subscriber("rmp_feedback", RMPFeedback, self.get_batt_state)
00027
00028 """
00029 Get the battery parameters
00030 """
00031 self.has_two_wheels = rospy.get_param('~has_two_wheels',True)
00032 self.has_caster_wheel = rospy.get_param('~has_caster_wheel',True)
00033 self.link_left_front = rospy.get_param('~link_left_front','base_link_left_wheel_joint')
00034 self.link_right_front = rospy.get_param('~link_right_front','base_link_right_wheel_joint')
00035 self.link_left_rear = rospy.get_param('~link_left_rear','base_link_left_rear_wheel_joint')
00036 self.link_right_rear = rospy.get_param('~link_right_rear','base_link_right_rear_wheel_joint')
00037 self.link_caster = rospy.get_param('~link_caster','caster_plate_link_caster_link_joint')
00038 tire_diameter = rospy.get_param('/ethernet_rmp/my_tire_diameter',DEFAULT_TIRE_DIAMETER_M)
00039 self.circumference = math.pi*tire_diameter;
00040 self.prev_caster_pos = 0
00041
00042 def get_batt_state(self, rmp):
00043 """
00044 Read in the current RMP feedback and publish the pose
00045 :param rmp: the RMP feedback message
00046 """
00047 rmp_items = rmp.sensor_items
00048 rmp_values = rmp.sensor_values
00049
00050 joint_state = JointState()
00051
00052 names = [self.link_left_front, self.link_right_front,
00053 self.link_left_rear, self.link_right_rear, self.link_caster]
00054 pos = [0,0,0,0,0]
00055 vel = [0,0,0,0,0]
00056
00057 """
00058 get the values for the feedback items needed
00059 """
00060 for x in range(0, len(rmp_items)):
00061 if rmp_items[x] == 'left_front_pos_m':
00062 pos[0] = -((rmp_values[x]/self.circumference) % 1.0)*(2*math.pi)
00063 elif rmp_items[x] == 'right_front_pos_m':
00064 pos[1] = ((rmp_values[x]/self.circumference) % 1.0)*(2*math.pi)
00065 elif rmp_items[x] == 'left_rear_pos_m':
00066 pos[2] = -((rmp_values[x]/self.circumference) % 1.0)*(2*math.pi)
00067 elif rmp_items[x] == 'right_rear_pos_m':
00068 pos[3] = ((rmp_values[x]/self.circumference) % 1.0)*(2*math.pi)
00069 elif rmp_items[x] == 'left_front_vel_mps':
00070 vel[0] = -rmp_values[x]
00071 elif rmp_items[x] == 'right_front_vel_mps':
00072 vel[1] = rmp_values[x]
00073 elif rmp_items[x] == 'left_rear_vel_mps':
00074 vel[2] = -rmp_values[x]
00075 elif rmp_items[x] == 'right_rear_vel_mps':
00076 vel[3] = rmp_values[x]
00077
00078 if self.has_two_wheels:
00079 num = 2
00080 else:
00081 num = 4
00082
00083 for x in range(0,num):
00084 joint_state.name.append(names[x])
00085 joint_state.position.append(pos[x])
00086 joint_state.velocity.append(vel[x])
00087
00088 """
00089 calculate (very rough) caster wheel position
00090 """
00091 if self.has_caster_wheel:
00092 left_vel = -vel[0]
00093 right_vel = vel[1]
00094 if abs(left_vel) < 0.01:
00095 left_vel = 0
00096 if abs(right_vel) < 0.01:
00097 right_vel = 0
00098 if left_vel != 0 or right_vel != 0:
00099 step = .04
00100 target = math.atan2(right_vel, left_vel) - math.pi/4
00101 if target > math.pi:
00102 target -= 2*math.pi
00103 if target < -math.pi:
00104 target += 2*math.pi
00105 pos[4] = self.prev_caster_pos
00106
00107 if abs(pos[4] - target) >= step:
00108 direction = 1
00109 if pos[4] > target:
00110 if abs(target - pos[4]) < (2*math.pi + target - pos[4]):
00111 direction = -1
00112 else:
00113 if (2*math.pi - (pos[4] - target)) < (pos[4] - target):
00114 direction = -1
00115
00116 pos[4] += direction*step
00117 while pos[4] <= -math.pi:
00118 pos[4] += 2*math.pi
00119 while pos[4] > math.pi:
00120 pos[4] -= 2*math.pi
00121 else:
00122 pos[4] = target
00123 self.prev_caster_pos = pos[4]
00124 else:
00125 pos[4] = self.prev_caster_pos
00126
00127 joint_state.name.append(names[4])
00128 joint_state.position.append(pos[4])
00129 joint_state.velocity.append(vel[4])
00130
00131 """
00132 Publish the state of the wheels/joints
00133 """
00134 joint_state.header.stamp = rospy.Time.now()
00135 self.jointStatePub.publish(joint_state)
00136
00137 if __name__ == "__main__":
00138 rospy.init_node("rmp_joint_state")
00139 jointState = JointStateMonitor()
00140 rospy.loginfo("RMP Joint State Started")
00141 rospy.spin()