ROS : Robot moves to certain coordinate point but how to stop and set multiple goals?
HI
I followed the tutorial Construct ROS tutorial and this is referencing the question of ROS-Answers How to move to a certain point in space using Twist /cmd_vel
I am able to move the robot to a coordinate. But it keeps rotating when it reached the goal
CURRENT STATUS
Currently the robot can go to a single coordinate assigned by goal.x and goal.y when i run the program in the Pi. It keeps rotating once it reaches the goal.
QUESTION:
How do i stop the robot when it reaches my goal coordinate?
How do i add the next goal coordinate in the same code once the first goal is reached?
How do i make the robot face a particular direction (pose) once it reached a goal?
Please do help me anyone....
the code is as follows
import rospy
from nav_msgs.msg
import Odometry from tf.transformations import euler_from_quaternion from
geometry_msgs.msg import Point
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("/odometry/filtered",Odometry, newOdom)
pub =rospy.Publisher("/cmd_vel",Twist, quene_size=1)
speed = Twist()
goal = Point ()
goal.x = 5 // x coordinate for goal
goal.y = 5 // y coordinate for goal
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.3
else:
speed.linear.x = 0.5
speed.angular.z = 0.0
pub.publish(speed)
r.sleep()
Thanks in advance