How to know that the goal is on the map or not?
My code is using the goal as input from the sensor but sometime some values is out the map because sensor's noise.
How to figure it out and put a condition if the value is on map (send goal), otherwise (no send goal)?
you can see the values in the code is position = {'x': 1.22, 'y': 2.56}
this full my code:
#!/usr/bin/env python
import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion
class GoToPose():
def __init__(self):
self.goal_sent = False
# What to do if shut down (e.g. Ctrl-C or failure)
rospy.on_shutdown(self.shutdown)
# Tell the action client that we want to spin a thread by default
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Wait for the action server to come up")
# Allow up to 5 seconds for the action server to come up
self.move_base.wait_for_server(rospy.Duration(5))
def goto(self, pos, quat):
# Send a goal
self.goal_sent = True
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000),
Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4']))
# Start moving
self.move_base.send_goal(goal)
# Allow TurtleBot up to 60 seconds to complete task
success = self.move_base.wait_for_result(rospy.Duration(60))
state = self.move_base.get_state()
result = False
if success and state == GoalStatus.SUCCEEDED:
# We made it!
result = True
else:
self.move_base.cancel_goal()
self.goal_sent = False
return result
def shutdown(self):
if self.goal_sent:
self.move_base.cancel_goal()
rospy.loginfo("Stop")
rospy.sleep(1)
if __name__ == '__main__':
try:
rospy.init_node('nav_test', anonymous=False)
navigator = GoToPose()
# Customize the following values so they are appropriate for your location
position = {'x': 1.22, 'y': 2.56}
quaternion = {'r1': 0.000, 'r2': 0.000, 'r3': 0.000, 'r4': 1.000}
rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y'])
success = navigator.goto(position, quaternion)
if success:
rospy.loginfo("Hooray, reached the desired pose")
else:
rospy.loginfo("The base failed to reach the desired pose")
# Sleep to give the last log messages time to be sent
rospy.sleep(1)
except rospy.ROSInterruptException:
rospy.loginfo("Ctrl-C caught. Quitting")
Thank you in advance!
You can probably check its costmap value as suggested by 942, but that does not tell you if the position is actually reachable. I would try the ~make_plan service of move_base with parameter make_plan_clear_costmap=false