thymio_navigation_driver.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #kate: replace-tabs off; tab-width 4; indent-width 4; indent-mode normal
00003 import roslib; roslib.load_manifest('thymio_navigation_driver')
00004 import rospy
00005 from asebaros.msg import AsebaAnonymousEvent
00006 from asebaros.msg import AsebaEvent
00007 from asebaros.srv import LoadScripts
00008 from asebaros.srv import GetNodeList
00009 from geometry_msgs.msg import Quaternion
00010 from geometry_msgs.msg import Twist
00011 from nav_msgs.msg import Odometry
00012 from tf.broadcaster import TransformBroadcaster
00013 from math import sin,cos,atan2
00014 import time
00015 
00016 BASE_WIDTH = 95     # millimeters
00017 MAX_SPEED = 500     # units
00018 SPEED_COEF = 2.93   # 1mm/sec corresponds to X units of real thymio speed
00019 
00020 class ThymioDriver():
00021         # ======== class initialization ======== 
00022         def __init__(self):
00023                 rospy.init_node('thymio')
00024                 # initialize parameters
00025                 self.x = 0
00026                 self.y = 0
00027                 self.th = 0
00028                 self.then = rospy.Time.now()
00029                 self.odom = Odometry(header=rospy.Header(frame_id='odom'),child_frame_id='base_link')
00030                 
00031                 # load script on the Thymio
00032                 rospy.wait_for_service('/aseba/load_script')
00033                 load_script = rospy.ServiceProxy('/aseba/load_script',LoadScripts)
00034                 script_filename = roslib.packages.get_pkg_dir('thymio_navigation_driver') + '/aseba/thymio_ros.aesl'
00035                 load_script(script_filename)
00036                 
00037                 # subscribe to topics
00038                 rospy.Subscriber("cmd_vel", Twist, self.on_cmd_vel)
00039                 rospy.Subscriber('/aseba/events/odometry', AsebaEvent, self.on_aseba_odometry_event)
00040                 self.aseba_pub = rospy.Publisher('/aseba/events/set_speed', AsebaEvent)
00041                 self.odom_pub = rospy.Publisher('odom',Odometry)
00042                 self.odom_broadcaster = TransformBroadcaster()
00043         
00044         # ======== we send the speed to the aseba running on the robot  ======== 
00045         def set_speed(self,values):
00046                 self.aseba_pub.publish(AsebaEvent(rospy.get_rostime(),0,values))
00047         
00048         # ======== stop the robot safely ======== 
00049         def shutdown(self):
00050                 self.set_speed([0,0])
00051         
00052         # ======== processing odometry events received from the robot ======== 
00053         def on_aseba_odometry_event(self,data): 
00054                 now = data.stamp
00055                 dt = (now-self.then).to_sec()
00056                 self.then = now
00057                 dsl = (data.data[0]*dt)/SPEED_COEF # left wheel delta in mm
00058                 dsr = (data.data[1]*dt)/SPEED_COEF # right wheel delta in mm
00059                 ds = ((dsl+dsr)/2.0)/1000.0      # robot traveled distance in meters
00060                 dth = atan2(dsr-dsl,BASE_WIDTH)  # turn angle
00061 
00062                 self.x += ds*cos(self.th+dth/2.0)
00063                 self.y += ds*sin(self.th+dth/2.0)
00064                 self.th+= dth
00065 
00066                 # prepare tf from base_link to odom 
00067                 quaternion = Quaternion()
00068                 quaternion.z = sin(self.th/2.0)
00069                 quaternion.w = cos(self.th/2.0)
00070 
00071                 # prepare odometry
00072                 self.odom.header.stamp = rospy.Time.now() # OR TO TAKE ONE FROM THE EVENT?
00073                 self.odom.pose.pose.position.x = self.x
00074                 self.odom.pose.pose.position.y = self.y
00075                 self.odom.pose.pose.position.z = 0
00076                 self.odom.pose.pose.orientation = quaternion
00077                 self.odom.twist.twist.linear.x = ds/dt
00078                 self.odom.twist.twist.angular.z = dth/dt
00079 
00080                 # publish odometry
00081                 self.odom_broadcaster.sendTransform((self.x,self.y,0),(quaternion.x,quaternion.y,quaternion.z,quaternion.w),self.then,"base_link","odom")
00082                 self.odom_pub.publish(self.odom)
00083         
00084         # ======== processing events received from the robot  ======== 
00085         def on_cmd_vel(self,data):
00086                 x = data.linear.x * 1000.0 # from meters to millimeters 
00087                 x = x * SPEED_COEF # to thymio units
00088                 th = data.angular.z * (BASE_WIDTH/2) # in mm
00089                 th = th * SPEED_COEF # in thymio units
00090                 k = max(abs(x-th),abs(x+th))
00091                 # sending commands higher than max speed will fail
00092                 if k > MAX_SPEED:
00093                         x = x*MAX_SPEED/k; th = th*MAX_SPEED/k
00094                 self.set_speed([int(x-th),int(x+th)])
00095 
00096         # ======== ======== ======== ======== ======== ======== ========      
00097         def control_loop(self): 
00098                 rospy.on_shutdown(self.shutdown) # on shutdown hook
00099                 while not rospy.is_shutdown():
00100                         rospy.spin()
00101 
00102 def main():
00103         try:
00104                 robot = ThymioDriver()
00105                 robot.control_loop()
00106         except rospy.ROSInterruptException:
00107                 pass
00108 
00109 if __name__ == '__main__':
00110         main()
00111         


thymio_navigation_driver
Author(s): Stéphane Magnenat and Alexey Gribovskiy
autogenerated on Sun Oct 5 2014 23:47:13