rmp_joint_state.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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                                 #calculate direction to turn
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                                         #turn to target
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()


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