ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

navigate to NEAREST goal pos (if goal pos is blocked)

asked 2018-09-25 06:17:31 -0600

RoboRos gravatar image

Hello all,

I am working on a project where my robot needs to navigate to the goal position (and it is easy and working fine). But sometimes the goal position may be blocked and there is a possibilty that the goal planners fail hence resulting in no movement at all. In this case i want my robot to move to the nearest point of the goal position (this can be done by taking a radius around the goal position and assign it a new position and then navigate again but it might take alot of time). Please help me that how can i make my algorithms more robust to implement these changes. (using amcl)

Below is my code for navigation to the goal position. Please share your thoughts:

    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': 13.3, 'y' : 3.9}
        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")

#### code by mark sulliman
edit retag flag offensive close merge delete


Which global planner are you using ? The carrot_planner does it but your robot only move straight forward. There is also the navfn which accept a tolerance on a goal point.

Delb gravatar image Delb  ( 2018-09-25 06:47:05 -0600 )edit

Thanks for your help. Can you post it as an answer so i can accept it and close it. @Delb Thanks for your help too @Choco93

RoboRos gravatar image RoboRos  ( 2018-09-26 04:45:05 -0600 )edit

You can answer yourself to tell exactly what solved your problem/which solution you used, and then accept it (and don't close the question, only accept it). Glad your problem is solved !

Delb gravatar image Delb  ( 2018-09-26 04:50:30 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted

answered 2018-09-25 07:05:45 -0600

Choco93 gravatar image

You can set your tolerence param to be high, that should solve your problem.

edit flag offensive delete link more

answered 2020-05-11 08:49:20 -0600

felipeduque gravatar image

If you want anything more elaborate than carrot_planner, you can try emielke's global_replanner, which is a remake of global_planner. Vanilla global_planner does not take into account default_tolerance parameter, which probably does what you're asking for: if the goal is blocked or inside an obstacle, it looks for possible goals within a square of side equal to default_tolerance meters.

(I myself have been using this planner for some time now, it's working great.)

More info in this question.

edit flag offensive delete link more

Question Tools



Asked: 2018-09-25 06:17:31 -0600

Seen: 670 times

Last updated: May 11 '20