Ask Your Question

How to know that the goal is on the map or not?

asked 2020-07-17 04:08:12 -0600

Redhwan gravatar image

updated 2020-07-17 04:09:05 -0600

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)

        # 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

    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 =
        goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000),
                                     Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4']))

        # Start moving

        # 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

        self.goal_sent = False
        return result

    def shutdown(self):
        if self.goal_sent:

if __name__ == '__main__':
        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")
            rospy.loginfo("The base failed to reach the desired pose")

        # Sleep to give the last log messages time to be sent

    except rospy.ROSInterruptException:
        rospy.loginfo("Ctrl-C caught. Quitting")

Thank you in advance!

edit retag flag offensive close merge delete


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

Humpelstilzchen gravatar image Humpelstilzchen  ( 2020-07-19 04:45:37 -0600 )edit

1 Answer

Sort by » oldest newest most voted

answered 2020-07-17 05:19:51 -0600 gravatar image

maybe we can get it from costmap code

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2020-07-17 04:08:12 -0600

Seen: 85 times

Last updated: Jul 17 '20