move_base: updated to new speed control, added odom code
[ros_wild_thumper.git] / scripts / move_base.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import rospy
5 import tf
6 import struct
7 from i2c import *
8 from math import *
9 from geometry_msgs.msg import Twist
10 from nav_msgs.msg import Odometry
11
12 WHEEL_DIST = 0.248
13
14 class MoveBase:
15         def __init__(self):
16                 rospy.init_node('wild_thumper_move_base')
17                 rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
18                 self.tf_broadcaster = tf.broadcaster.TransformBroadcaster()
19                 self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=16)
20                 self.set_speed(0, 0)
21                 rospy.loginfo("Init done")
22                 i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction
23                 self.run()
24         
25         def run(self):
26                 rate = rospy.Rate(20.0)
27                 while not rospy.is_shutdown():
28                         #self.get_odom()
29                         #self.get_dist_forward()
30                         #self.get_dist_backward()
31                         #self.get_dist_left()
32                         #self.get_dist_right()
33                         rate.sleep()
34
35         def get_odom(self):
36                 posx, posy, angle = struct.unpack(">fff", i2c_read_reg(0x50, 0x40, 12))
37                 speed_trans, speed_rot = struct.unpack(">ff", i2c_read_reg(0x50, 0x38, 8))
38                 current_time = rospy.Time.now()
39
40                 # since all odometry is 6DOF we'll need a quaternion created from yaw
41                 odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle)
42
43                 # first, we'll publish the transform over tf
44                 self.tf_broadcaster.sendTransform((posx, posy, 0.0), odom_quat, current_time, "base_link", "odom")
45
46                 # next, we'll publish the odometry message over ROS
47                 odom = Odometry()
48                 odom.header.stamp = current_time
49                 odom.header.frame_id = "/odom"
50
51                 # set the position
52                 odom.pose.pose.position.x = posx
53                 odom.pose.pose.position.y = posy
54                 odom.pose.pose.position.z = 0.0
55                 odom.pose.pose.orientation.x = odom_quat[0]
56                 odom.pose.pose.orientation.y = odom_quat[1]
57                 odom.pose.pose.orientation.z = odom_quat[2]
58                 odom.pose.pose.orientation.w = odom_quat[3]
59
60                 # set the velocity
61                 odom.child_frame_id = "base_link"
62                 odom.twist.twist.linear.x = speed_trans
63                 odom.twist.twist.linear.y = 0.0
64                 odom.twist.twist.angular.z = speed_rot
65
66                 # publish the message
67                 self.pub_odom.publish(odom)
68
69         
70         def set_speed(self, trans, rot):
71                 i2c_write_reg(0x50, 0x50, struct.pack(">ff", trans, rot))
72
73         def cmdVelReceived(self, msg):
74                 trans = msg.linear.x
75                 rot = msg.angular.z # rad/s
76                 self.set_speed(trans, rot)
77
78         # http://rn-wissen.de/wiki/index.php/Sensorarten#Sharp_GP2D12
79         def get_dist_ir(self, num):
80                 dev = i2c(0x52)
81                 s = struct.pack("B", num)
82                 dev.write(s)
83                 dev.close()
84
85                 sleep(2e-6)
86
87                 dev = i2c(0x52)
88                 s = dev.read(2)
89                 dev.close()
90
91                 val = struct.unpack(">H", s)[0]
92                 return 15221/(val - -276.42)/100;
93         
94         def get_dist_srf(self, num):
95                 dev = i2c(0x52)
96                 s = struct.pack("B", num)
97                 dev.write(s)
98                 dev.close()
99
100                 sleep(50e-3)
101
102                 dev = i2c(0x52)
103                 s = dev.read(2)
104                 dev.close()
105
106                 return struct.unpack(">H", s)[0]/1000.0
107
108         def get_dist_left(self):
109                 dist = self.get_dist_ir(0x1)
110
111         def get_dist_right(self):
112                  dist = self.get_dist_ir(0x3)
113
114         def get_dist_forward(self):
115                 dist = self.get_dist_srf(0x5)
116
117         def get_dist_backward(self):
118                 dist = self.get_dist_srf(0x7)
119
120
121 if __name__ == "__main__":
122         MoveBase()