Ask Your Question
0

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

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

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)
    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': 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")
        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")


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

Comments

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 -0500 )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 -0500 )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 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

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

Choco93 gravatar image

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

edit flag offensive delete link more
0

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

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

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

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

Seen: 415 times

Last updated: May 11 '20