How do I send a signal when my robot is within a given range of a goal?
I want to send my robot multiple goals (waypoints really) to pass through. Right now it stops at each goal that is sent. I thought of sending a new goal once it comes within range of the previous goal (assuming the sending of a new goal will mean it skips the old goal, as with sending 2D_nav_goal in Rviz).
This is my current code for sending two goals.
import time import rospy import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal from std_msgs.msg import Int8
def movebase_client(): rate = rospy.Rate(1)
client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.746
goal.target_pose.pose.position.y = 1.44
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
goal.target_pose.pose.orientation.z = 1
goal.target_pose.pose.orientation.w = -0.9
client.send_goal(goal)
wait = client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
#goal2
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.586
goal.target_pose.pose.position.y = 0.851
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
goal.target_pose.pose.orientation.z = 1
goal.target_pose.pose.orientation.w = -0.9
client.send_goal(goal)
wait = client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
return client.get_result()
if __name__ == '__main__': try: rospy.init_node('movebase_client_py') result = movebase_client() if result: rospy.loginfo("Goal execution done!")
except rospy.ROSInterruptException:
rospy.loginfo("Navigation test finished.")
I imagine that I would monitor the amcl_pose topic and then use the location of the robot from that, but I dont know how to access the x, y, from the topic.
I'm using ROS melodic. Any help would be greatly appreciated. Thanks!