2 # -*- coding: iso-8859-15 -*-
8 from geometry_msgs.msg import Twist
14 rospy.init_node('wild_thumper_move_base')
15 rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
17 rospy.loginfo("Init done")
21 rate = rospy.Rate(10.0)
22 while not rospy.is_shutdown():
25 def set_speed(self, left, right):
27 elif left < 0: left-=80
28 if right > 0: right+=80
29 elif right < 0: right-=80
31 if left > 255: left=255
32 elif left < -255: left=-255
33 if right > 255: right=255
34 elif right < -255: right=-255
37 s = struct.pack(">Bhh", 0x1, left, right)
41 def cmdVelReceived(self, msg):
43 rot = msg.angular.z # rad/s
45 right = rot*pi*WHEEL_DIST + trans
47 self.set_speed(left, right)
50 if __name__ == "__main__":