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