darwin.py
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


darwin_gazebo
Author(s): Philippe Capdepuy
autogenerated on Fri Aug 28 2015 10:27:42