Hello, I'm using a Turtlebot with Kobuki base and running Ros Hydro on Ubuntu 12.04.
I need the Turtlebot to move in the shape of a square and guiding myself by this codes: albany , turtlebot motion and turtlebot tutorials, I put together a code that only sends the appropriate cmd_vel messages to make the robot move at a certain speed for a chosen time and another one that uses tf information from odom->base_footprint to make the robot move a chosen distance at a certain speed.
I was first testing the codes on turtlesim and they seemed to work well, but when I tested them on the gazebo turtlebot simulator and on the real turtlebot this was the outcome (obtained from the /odom topic on rviz):
Gazebo Simulation - From the code that doesn't use tf:
Gazebo Simulation - From the code that uses the odom->base_footprint tf:
Real Turtlebot - From the code that doesn't use tf:
Real Turtlebot - From the code that uses the odom->base_footprint tf:
From what I've read the odom->base_footprint tf uses the wheel odometry and IMU data and the kobuki base already comes with a calibrated gyro, so I don't think the problem is in the odometry tf. Also, the movement of the turtlebot with the code that doesn't use the tf looks similar to the one the /odom topic shows.
The result from the real turtlebot is terrible, but I was surprised that even in the simulation the code doesn't work well. I don't really know how to improve this results, because I expected that the way to improve the first code was to use the one that implements the tf and that one is the one that works the worst.
Am I misusing the tfs or am I missing something particular about the turtlebot with the messages I'm sending? at least in the first code it looks as if the robot doesn't move as much as the speed would indicate but the tf should be indifferent to that. I read the similar questions in the FAQs but none of them help me solve this.
Thank you very much in advance and sorry for the rviz plots, I didn't find another way to get the trajectory of the robot.
The two codes are below:
Code that only sends the appropriate cmd_vel messages to make the robot move at a certain speed for a chosen time:
#!/usr/bin/env python
# http://library.isr.ist.utl.pt/docs/roswiki/rospy%282f%29Overview%282f%29Time.html
""" Example code of how to move a robot around the shape of a square. """
import roslib
import rospy
import math
import time
from geometry_msgs.msg import Twist
class square: """ This example is in the form of a class. """
def __init__(self):
""" This is the constructor of our class. """
# register this function to be called on shutdown
rospy.on_shutdown(self.cleanup)
# publish to cmd_vel
self.p = rospy.Publisher('cmd_vel', Twist)
#twist = Twist()
# give our node/publisher a bit of time to connect
rospy.sleep(1)
# slow rate, 2 seconds sleep
r = rospy.Rate(0.5)
for i in range(4):
# create a Twist message, fill it in to drive forward
twist = Twist()
start = rospy.get_time()
print(rospy.get_time())
while rospy.get_time()-start<4:
twist.linear.x = 0.25; #move 0.25 m/s
twist.linear.y = 0;
twist.linear.z = 0;
twist.angular.x = 0;
twist.angular.y = 0;
twist.angular.z = 0;
self.p.publish(twist)
rospy.loginfo("Moving the robot forward.")
r.sleep()
twist = Twist()
start = rospy.get_time()
print(rospy.get_time())
while rospy.get_time()-start<2:
twist.linear.x = 0;
twist.linear.y = 0;
twist.linear.z = 0;
twist.angular.x = 0;
twist.angular.y = 0;
twist.angular.z = math.pi/4.0; #move pi/4 rad/sec
self.p.publish(twist)
rospy.loginfo("Turning the robot")
r.sleep()
def cleanup(self):
# stop the robot!
twist = Twist()
self.p.publish(twist)
if __name__=="__main__":
rospy.init_node('control', anonymous=True)
square()
This code uses tf information from odom->base_footprint to make the robot move a chosen distance at a certain speed:
#!/usr/bin/env python
""" Example code of how to move a robot around the shape of a square, using tf. """
# we always import these
import roslib
import rospy
import tf
import math
# recall: robots generally take base movement commands on a topic
# called "cmd_vel" using a message type "geometry_msgs/Twist"
from geometry_msgs.msg import Twist
class square:
""" This example is in the form of a class. """
def __init__(self):
""" This is the constructor of our class. """
print 'inwut'
# register this function to be called on shutdown
rospy.on_shutdown(self.cleanup)
self.listener = tf.TransformListener()
print 'k1'
# publish to cmd_vel
self.p = rospy.Publisher('cmd_vel', Twist)
# give our node/publisher a bit of time to connect
rospy.sleep(1.0)
twist = Twist()
#first = True
for i in range(2):
print 'hey'
done = False
first = 1
print first
while not done:
# create a Twist message, fill it in to drive forward
twist.linear.x = 0.2;
twist.angular.z = 0;
self.p.publish(twist)
rospy.loginfo("moving forward")
try:
((new_x,y,z), rot) = self.listener.lookupTransform('odom', 'base_footprint', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
rospy.logerr("AHHHH")
#print 'mid!'
if first:
orig_x=new_x
first=0
print 'first'
done=bool(abs(new_x-orig_x)>0.8)
print done
print new_x-orig_x
done = False
first = 1
while not done:
twist.angular.z = math.pi/4.0; #turns at pi/4 degrees/sec
twist.linear.x=0;
self.p.publish(twist)
rospy.loginfo("turning")
try:
((x,y,z), rot) = self.listener.lookupTransform('odom', 'base_footprint', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
rospy.logerr("AHHHH")
(phi, psi, theta) = tf.transformations.euler_from_quaternion(rot)
if first:
orig_ang=theta
first=0
done = bool(abs(theta-orig_ang)>=math.pi/2.0)
done = False
first = 1
print first
while not done:
# create a Twist message, fill it in to drive forward
twist.linear.x = 0.2;
twist.angular.z = 0;
self.p.publish(twist)
rospy.loginfo("moving forward")
try:
((x,new_y,z), rot) = self.listener.lookupTransform('odom', 'base_footprint', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
rospy.logerr("AHHHH")
#print 'mid!'
if first:
orig_y=new_y
first=0
print 'first'
done=bool(abs(new_y-orig_y)>0.8)
print done
print new_x-orig_x
done = False
first = 1
while not done:
twist.angular.z = math.pi/16.0;
twist.linear.x=0;
self.p.publish(twist)
rospy.loginfo("turning")
try:
((x,y,z), rot) = self.listener.lookupTransform('odom', 'base_footprint', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
rospy.logerr("AHHHH")
(phi, psi, theta) = tf.transformations.euler_from_quaternion(rot)
if first:
orig_ang=theta
first=0
done = bool(abs(theta-orig_ang)>=math.pi/2.0)
def cleanup(self):
# stop the robot!
twist = Twist()
self.p.publish(twist)
if __name__=="__main__":
rospy.init_node('square')
try:
square()
except:
pass
Originally posted by Athria on ROS Answers with karma: 78 on 2015-05-27
Post score: 0
