Turtlebot driving to specifiy point with Twist
Hello guys,
first of all i must excuse i messed up by posting this question wrong and it got to confusing so i delete it and post it correct again. A special excuse to jayess i messed up with copying my code correct he is fully right that at this point:
if abs(angle_to_goal - theta) > 0.1:
speed.linear.x = 0.0
speed.angular.z = 0.1
Here needs to be bigger then 0.1 before here stands less...
So my question is how i can drive with my Turtlebot to a specific point or an array of points i give the robot. I am using Turtlebot package for ros kinetic and i run this in gazebo 7.0.
i tried this code:
#! /usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Point, Twist
from math import atan2
x = 0.0
y= 0.0
theta = 0.0
def newOdom(msg):
global x
global y
global theta
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(roll,pitch,theta) = euler_from_quaternion ([rot_q.x,rot_q.y,rot_q.z,rot_q.w])
rospy.init_node("speed_controller")
sub = rospy.Subscriber("/odom",Odometry,newOdom)
pub = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=1)
speed = Twist()
r = rospy.Rate(4)
goal = Point()
goal.x = 2
goal.y = 2
while not rospy.is_shutdown():
inc_x = goal.x - x
inc_y = goal.y - y
angle_to_goal = atan2(inc_y,inc_x)
if abs(angle_to_goal - theta) > 0.1:
speed.linear.x = 0.0
speed.angular.z = 0.1
else:
speed.linear.x = 0.5
speed.angular.z = 0.0
pub.publish(speed)
r.sleep()
So the program works for a few rotations but the turtlebot don´t reach the correct angle he drives most time before he reachs the correct angle and lands left of the target point.
I used this tutorial :
tutorial to drive to specific point with twist
He used same controller and the robot drives perfectly as he should... Only difference i use a turtlebot for it
In the future, please don't delete a question because "it got messy"
You can always edit the question to clean it up.
Okay i am sorry... In the future I will do . So I hope now the problem is better to understand ?