From: Erik Andresen Date: Fri, 14 Aug 2015 04:12:35 +0000 (+0000) Subject: move_base: updated to new speed control, added odom code X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=0640fece763d17314bb1a9878c64309476fa1d4a;p=ros_wild_thumper.git 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 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 0de7703..4618895 100644 --- a/launch/teleop.launch +++ b/launch/teleop.launch @@ -1,8 +1,8 @@ - - + + diff --git a/scripts/move_base.py b/scripts/move_base.py index 5ece268..ae15b38 100755 --- 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 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))