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():
23 #self.get_dist_forward()
24 #self.get_dist_backward()
26 #self.get_dist_right()
29 def set_speed(self, left, right):
31 elif left < 0: left-=80
32 if right > 0: right+=80
33 elif right < 0: right-=80
35 if left > 255: left=255
36 elif left < -255: left=-255
37 if right > 255: right=255
38 elif right < -255: right=-255
41 s = struct.pack(">Bhh", 0x1, left, right)
45 def cmdVelReceived(self, msg):
47 rot = msg.angular.z # rad/s
49 right = rot*pi*WHEEL_DIST + trans
51 self.set_speed(left, right)
53 # http://rn-wissen.de/wiki/index.php/Sensorarten#Sharp_GP2D12
54 def get_dist_ir(self, num):
56 s = struct.pack("B", num)
66 val = struct.unpack(">H", s)[0]
67 return 15221/(val - -276.42)/100;
69 def get_dist_srf(self, num):
71 s = struct.pack("B", num)
81 return struct.unpack(">H", s)[0]/1000.0
83 def get_dist_left(self):
84 dist = self.get_dist_ir(0x1)
86 def get_dist_right(self):
87 dist = self.get_dist_ir(0x3)
89 def get_dist_forward(self):
90 dist = self.get_dist_srf(0x5)
92 def get_dist_backward(self):
93 dist = self.get_dist_srf(0x7)
96 if __name__ == "__main__":