Go to the documentation of this file.00001 import rospy
00002 import time
00003 from geometry_msgs.msg import Twist
00004 from sensor_msgs.msg import JointState
00005 from std_msgs.msg import Float64
00006
00007
00008 class PhantomX:
00009 """Client ROS class for manipulating PhantomX in Gazebo"""
00010
00011 def __init__(self, ns='/phantomx/'):
00012 self.ns = ns
00013 self.joints = None
00014 self.angles = None
00015
00016 self._sub_joints = rospy.Subscriber(
00017 ns + 'joint_states', JointState, self._cb_joints, queue_size=1)
00018 rospy.loginfo('Waiting for joints to be populated...')
00019 while not rospy.is_shutdown():
00020 if self.joints is not None:
00021 break
00022 rospy.sleep(0.1)
00023 rospy.loginfo('Waiting for joints to be populated...')
00024 rospy.loginfo('Joints populated')
00025
00026 rospy.loginfo('Creating joint command publishers')
00027 self._pub_joints = {}
00028 for j in self.joints:
00029 p = rospy.Publisher(
00030 ns + j + '_position_controller/command', Float64, queue_size=1)
00031 self._pub_joints[j] = p
00032
00033 rospy.sleep(1)
00034
00035 self._pub_cmd_vel = rospy.Publisher(ns + 'cmd_vel', Twist, queue_size=1)
00036
00037 def set_walk_velocity(self, x, y, t):
00038 msg = Twist()
00039 msg.linear.x = x
00040 msg.linear.y = y
00041 msg.angular.z = t
00042 self._pub_cmd_vel.publish(msg)
00043
00044 def _cb_joints(self, msg):
00045 if self.joints is None:
00046 self.joints = msg.name
00047 self.angles = msg.position
00048
00049 def get_angles(self):
00050 if self.joints is None:
00051 return None
00052 if self.angles is None:
00053 return None
00054 return dict(zip(self.joints, self.angles))
00055
00056 def set_angles(self, angles):
00057 for j, v in angles.items():
00058 if j not in self.joints:
00059 rospy.logerror('Invalid joint name "' + j + '"')
00060 continue
00061 self._pub_joints[j].publish(v)
00062
00063 def set_angles_slow(self, stop_angles, delay=2):
00064 start_angles = self.get_angles()
00065 start = time.time()
00066 stop = start + delay
00067 r = rospy.Rate(100)
00068 while not rospy.is_shutdown():
00069 t = time.time()
00070 if t > stop:
00071 break
00072 ratio = (t - start) / delay
00073 angles = interpolate(stop_angles, start_angles, ratio)
00074 self.set_angles(angles)
00075 r.sleep()
00076
00077
00078 def interpolate(anglesa, anglesb, coefa):
00079 z = {}
00080 joints = anglesa.keys()
00081 for j in joints:
00082 z[j] = anglesa[j] * coefa + anglesb[j] * (1 - coefa)
00083 return z
00084
00085
00086 def get_distance(anglesa, anglesb):
00087 d = 0
00088 joints = anglesa.keys()
00089 if len(joints) == 0:
00090 return 0
00091 for j in joints:
00092 d += abs(anglesb[j] - anglesa[j])
00093 d /= len(joints)
00094 return d