move_base: updated to new speed control, added odom code

diff --git a/data/test_speed.log b/data/test_speed.log
deleted file mode 100644 (file)
index 2df2737..0000000
--- a/data/test_speed.log
+++ /dev/null
@@ -1,100 +0,0 @@
-0.000: 1 -1 0 0
-0.012: 19 16 14 11
-0.024: 45 43 37 32
-0.036: 81 79 65 57
-0.048: 115 125 103 82
-0.060: 148 182 151 108
-0.072: 177 246 207 130
-0.084: 203 316 270 151
-0.096: 227 395 341 170
-0.108: 249 478 418 190
-0.121: 276 569 495 212
-0.137: 315 674 587 244
-0.151: 355 759 663 272
-0.165: 404 838 740 320
-0.179: 459 914 809 369
-0.192: 513 983 873 413
-0.205: 567 1053 943 462
-0.217: 623 1129 1013 513
-0.230: 678 1209 1086 561
-0.243: 737 1291 1159 611
-0.256: 797 1369 1230 666
-0.269: 862 1445 1301 725
-0.282: 931 1521 1370 785
-0.294: 1000 1595 1440 849
-0.307: 1069 1669 1511 915
-0.321: 1143 1750 1589 985
-0.333: 1215 1826 1667 1054
-0.347: 1288 1902 1744 1127
-0.360: 1375 1989 1831 1212
-0.375: 1453 2068 1909 1290
-0.388: 1529 2142 1985 1364
-0.400: 1608 2222 2064 1443
-0.413: 1688 2305 2145 1521
-0.426: 1769 2387 2225 1598
-0.438: 1851 2470 2307 1680
-0.451: 1934 2556 2391 1764
-0.464: 2023 2643 2477 1848
-0.477: 2111 2732 2565 1934
-0.490: 2199 2818 2655 2022
-0.503: 2300 2917 2754 2119
-0.517: 2390 3007 2843 2204
-0.530: 2480 3099 2931 2292
-0.543: 2569 3190 3020 2378
-0.556: 2659 3281 3108 2465
-0.571: 2768 3389 3217 2576
-0.584: 2859 3481 3307 2668
-0.597: 2956 3580 3411 2775
-0.611: 3061 3687 3506 2873
-0.625: 3156 3784 3601 2969
-0.638: 3255 3883 3698 3069
-0.652: 3359 3989 3802 3173
-0.665: 3455 4087 3898 3269
-0.677: 3554 4187 3998 3368
-0.691: 3655 4287 4098 3466
-0.703: 3753 4384 4196 3563
-0.716: 3854 4484 4297 3663
-0.730: 3958 4586 4398 3764
-0.742: 4058 4686 4496 3861
-0.755: 4156 4785 4594 3960
-0.767: 4255 4884 4692 4056
-0.780: 4354 4984 4790 4155
-0.793: 4454 5085 4888 4255
-0.805: 4554 5186 4987 4358
-0.818: 4656 5290 5088 4462
-0.831: 4757 5393 5190 4567
-0.844: 4861 5498 5292 4671
-0.857: 4962 5602 5394 4773
-0.870: 5065 5706 5496 4875
-0.882: 5168 5809 5599 4977
-0.895: 5271 5912 5703 5078
-0.908: 5376 6016 5807 5182
-0.920: 5480 6119 5910 5286
-0.933: 5584 6222 6014 5388
-0.946: 5689 6327 6117 5491
-0.958: 5795 6433 6223 5597
-0.971: 5900 6538 6326 5700
-0.984: 6008 6646 6434 5808
-0.997: 6115 6754 6538 5914
-1.010: 6217 6858 6640 6020
-1.023: 6320 6964 6743 6126
-1.036: 6427 7071 6848 6234
-1.048: 6530 7176 6952 6338
-1.061: 6634 7281 7057 6442
-1.073: 6739 7386 7161 6547
-1.092: 6893 7542 7315 6699
-1.105: 7001 7650 7423 6808
-1.118: 7110 7759 7534 6917
-1.131: 7218 7865 7639 7021
-1.143: 7324 7969 7744 7125
-1.156: 7431 8077 7851 7232
-1.169: 7537 8184 7956 7337
-1.181: 7642 8289 8059 7442
-1.194: 7746 8395 8162 7547
-1.207: 7850 8501 8267 7654
-1.219: 7956 8609 8372 7763
-1.232: 8063 8717 8478 7872
-1.245: 8176 8831 8591 7986
-1.258: 8282 8939 8697 8092
-1.271: 8390 9050 8806 8203
-1.286: 8516 9176 8932 8328

diff --git a/launch/teleop.launch b/launch/teleop.launch
index 0de7703beba0d5cdead9232bd3dd09696ca10334..46188952a3b284eb0496fe5e5c6f830c01149a2e 100644 (file)
--- a/launch/teleop.launch
+++ b/launch/teleop.launch
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <launch>
        <node pkg="turtlebot_teleop" type="turtlebot_teleop_joy" name="turtlebot_teleop_joystick">
-               <param name="scale_linear" value="25"/>
-               <param name="scale_angular" value="20"/>
+               <param name="scale_linear" value="0.2"/>
+               <param name="scale_angular" value="-0.2"/>
                <param name="axis_deadman" value="3"/>
                <param name="axis_linear" value="1"/>
                <param name="axis_angular" value="0"/>

diff --git a/scripts/move_base.py b/scripts/move_base.py
index 5ece2683f6d0ad31cee0dd0cebfd9efdb95b26e2..ae15b380ddd1cb3afc1ae05ad1f162b22fb9377a 100755 (executable)
--- a/scripts/move_base.py
+++ b/scripts/move_base.py
@@ -2,10 +2,12 @@
 # -*- coding: iso-8859-15 -*-
 
 import rospy
+import tf
 import struct
 from i2c import *
 from math import *
 from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
 
 WHEEL_DIST = 0.248
 
@@ -13,6 +15,8 @@ class MoveBase:
        def __init__(self):
                rospy.init_node('wild_thumper_move_base')
                rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
+               self.tf_broadcaster = tf.broadcaster.TransformBroadcaster()
+               self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=16)
                self.set_speed(0, 0)
                rospy.loginfo("Init done")
                i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction
@@ -21,38 +25,55 @@ class MoveBase:
        def run(self):
                rate = rospy.Rate(20.0)
                while not rospy.is_shutdown():
-                       self.get_pos()
+                       #self.get_odom()
                        #self.get_dist_forward()
                        #self.get_dist_backward()
                        #self.get_dist_left()
                        #self.get_dist_right()
                        rate.sleep()
 
-       def get_pos(self):
-               s = i2c_read_reg(0x50, 0x10, 8)
-               hall1, hall2, hall3, hall4 = struct.unpack(">hhhh", s)
-               print hall1, hall2, hall3, hall4
+       def get_odom(self):
+               posx, posy, angle = struct.unpack(">fff", i2c_read_reg(0x50, 0x40, 12))
+               speed_trans, speed_rot = struct.unpack(">ff", i2c_read_reg(0x50, 0x38, 8))
+               current_time = rospy.Time.now()
 
-       def set_speed(self, left, right):
-               if left > 0: left+=80
-               elif left < 0: left-=80
-               if right > 0: right+=80
-               elif right < 0: right-=80
+               # since all odometry is 6DOF we'll need a quaternion created from yaw
+               odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle)
 
-               if left > 255: left=255
-               elif left < -255: left=-255
-               if right > 255: right=255
-               elif right < -255: right=-255
+               # first, we'll publish the transform over tf
+               self.tf_broadcaster.sendTransform((posx, posy, 0.0), odom_quat, current_time, "base_link", "odom")
 
-               i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", right, right, left, left))
+               # next, we'll publish the odometry message over ROS
+               odom = Odometry()
+               odom.header.stamp = current_time
+               odom.header.frame_id = "/odom"
+
+               # set the position
+               odom.pose.pose.position.x = posx
+               odom.pose.pose.position.y = posy
+               odom.pose.pose.position.z = 0.0
+               odom.pose.pose.orientation.x = odom_quat[0]
+               odom.pose.pose.orientation.y = odom_quat[1]
+               odom.pose.pose.orientation.z = odom_quat[2]
+               odom.pose.pose.orientation.w = odom_quat[3]
+
+               # set the velocity
+               odom.child_frame_id = "base_link"
+               odom.twist.twist.linear.x = speed_trans
+               odom.twist.twist.linear.y = 0.0
+               odom.twist.twist.angular.z = speed_rot
+
+               # publish the message
+               self.pub_odom.publish(odom)
+
+       
+       def set_speed(self, trans, rot):
+               i2c_write_reg(0x50, 0x50, struct.pack(">ff", trans, rot))
 
        def cmdVelReceived(self, msg):
                trans = msg.linear.x
                rot = msg.angular.z # rad/s
-
-               right = rot*pi*WHEEL_DIST + trans
-               left = trans*2-right
-               self.set_speed(left, right)
+               self.set_speed(trans, rot)
 
        # http://rn-wissen.de/wiki/index.php/Sensorarten#Sharp_GP2D12
        def get_dist_ir(self, num):

diff --git a/scripts/test_speed.py b/scripts/test_speed.py
deleted file mode 100755 (executable)
index 022112e..0000000
--- a/scripts/test_speed.py
+++ /dev/null
@@ -1,25 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: iso-8859-15 -*-
-
-import struct
-from i2c import *
-from datetime import datetime
-from time import sleep
-
-def get_pos():
-       s = i2c_read_reg(0x50, 0x10, 8)
-       hall1, hall2, hall3, hall4 = struct.unpack(">hhhh", s)
-       return hall1, hall2, hall3, hall4
-
-if __name__ == "__main__":
-       i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction
-       speed = 255
-       i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", speed, speed, speed, speed))
-       start = datetime.now()
-       for i in range(100):
-               diff = datetime.now() - start
-               status = get_pos()
-               print "%d.%03d: %d %d %d %d" % ((diff.seconds, diff.microseconds/1000) + status)
-               sleep(0.01)
-       speed = 0
-       i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", speed, speed, speed, speed))