Ask Your Question
0

sending multiple goals to navigation stack

asked 2016-09-21 09:49:28 -0600

gleb gravatar image

updated 2016-09-22 04:48:35 -0600

Hi everyone,

I want to send multiple goals to the navigation stack, that the robot should pass successively. I want to use a node similar to the node from this tutorial http://wiki.ros.org/navigation/Tutori... . How can I use this node to send multiple goals?

Thank you in advance.

Update:

That is the code I am trying to use, but I am not sure what should be in the execute_cb-part:

#!/usr/bin/env python

import roslib
import rospy
import smach
import smach_ros
import actionlib

from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

# Create a action server
class WPServer:
    def __init__(self,name):
        self._sas = SimpleActionServer(name,
                Waypoints,
                execute_cb=self.execute_cb)

    def execute_cb(self, msg):
        if msg.goal == 0:
            self._sas.set_succeeded()
        elif msg.goal == 1:
            self._sas.set_aborted()
        elif msg.goal == 2:
            self._sas.set_preempted()

def main():
    rospy.init_node('waypoint_machine')

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])

    # Open the container
    with sm:
        # Add states to the container

    wp1 = MoveBaseGoal()
    wp1.target_pose.header.frame_id = '/map'
    wp1.target_pose.pose.position.x = 13
    wp1.target_pose.pose.position.y = 0
    wp1.target_pose.pose.orientation.w = 1.0
    StateMachine.add('Waypoint1',
                      SimpleActionState('WPServer',
                                        Waypoints,
                                        goal=wp1),
                      transitions={'succeeded':'Waypoint2'})

    wp2 = MoveBaseGoal()
    wp2.target_pose.header.frame_id = '/map'
    wp2.target_pose.pose.position.x = 18
    wp2.target_pose.pose.position.y = 0
    wp2.target_pose.pose.orientation.z = 0.70710678118
    wp2.target_pose.pose.orientation.w = 0.70710678118
    StateMachine.add('Waypoint2',
                      SimpleActionState('WPServer',
                                        Waypoints,
                                        goal=wp2),
                      transitions={'succeeded':'Waypoint3'})

    # Execute SMACH plan
    outcome = sm.execute()

    rospy.signal_shutdown('All done.')


if __name__ == '__main__':
    main()
edit retag flag offensive close merge delete

Comments

Your code should implement an action client, not a server. Then your client will send goals to the move_base action server.

spmaniato gravatar imagespmaniato ( 2016-09-22 16:38:40 -0600 )edit

3 Answers

Sort by ยป oldest newest most voted
1

answered 2016-09-21 11:06:58 -0600

spmaniato gravatar image

In brief, you can write a custom node that instantiates a simple action server which sends a sequence of MoveBaseActionGoals to move_base

edit flag offensive delete link more
1

answered 2016-09-21 10:40:24 -0600

alienmon gravatar image

updated 2016-09-22 20:35:47 -0600

"I want to send multiple goals to the navigation stack, that the robot should pass successively"

I'm not really sure what you want to do ultimately, but you might want to use smach . It is a state machine package in ROS.

Quoting from the page:

When should I use SMACH?

SMACH is useful when you want a robot to execute some complex plan, where all possible states and state transitions can be described explicitly. This basically takes the hacking out of hacking together different modules to make systems like mobile robotic manipulators do interesting things.

You can also control the goal/behavior of the robot depending on the result of the previous goal.

Check out the tutorial

------.

Here is an example from "Programming Robots with ROS" book:

#!/usr/bin/env python

import rospy
import actionlib
from smach import State,StateMachine
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

waypoints = [
    ['one', (2.1, 2.2), (0.0, 0.0, 0.0, 1.0)],
    ['two', (6.5, 4.43), (0.0, 0.0, -0.984047240305, 0.177907360295)]
]

class Waypoint(State):
    def __init__(self, position, orientation):
        State.__init__(self, outcomes=['success'])

        # Get an action client
        self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        self.client.wait_for_server()

        # Define the goal
        self.goal = MoveBaseGoal()
        self.goal.target_pose.header.frame_id = 'map'
        self.goal.target_pose.pose.position.x = position[0]
        self.goal.target_pose.pose.position.y = position[1]
        self.goal.target_pose.pose.position.z = 0.0
        self.goal.target_pose.pose.orientation.x = orientation[0]
        self.goal.target_pose.pose.orientation.y = orientation[1]
        self.goal.target_pose.pose.orientation.z = orientation[2]
        self.goal.target_pose.pose.orientation.w = orientation[3]

    def execute(self, userdata):
        self.client.send_goal(self.goal)
        self.client.wait_for_result()
        return 'success'


if __name__ == '__main__':
    rospy.init_node('patrol')

    patrol = StateMachine('success')
    with patrol:
        for i,w in enumerate(waypoints):
            StateMachine.add(w[0],
                             Waypoint(w[1], w[2]),
                             transitions={'success':waypoints[(i + 1) % \
                             len(waypoints)][0]})

    patrol.execute()
edit flag offensive delete link more

Comments

Thank you for your answer, I think I can make it work with SMACH, but I am new to programming and not sure about my code, could you please check my code and help me fix it? I am gonna update my question with my code.

gleb gravatar imagegleb ( 2016-09-22 04:48:52 -0600 )edit

@gleb You need to make actionclient, and send goal to the actionserver in this case move_base. You don't need callback function, cause you are the one who'll be sending goals. The move_base will have the callback function which actually handles the goal and move the robot to the goal location.

alienmon gravatar imagealienmon ( 2016-09-22 20:32:14 -0600 )edit

@gleb I edited my answer with a code example. You can refer to it

alienmon gravatar imagealienmon ( 2016-09-22 20:36:15 -0600 )edit

@alienmon thank you very much for your help, this code works for me! I want to integrate following line:

if self.client.get_state() == actionlib.SimpleClientGoalState.SUCCESS:
    return 'succeded'
else:
    return 'aborted'
gleb gravatar imagegleb ( 2016-10-05 04:56:37 -0600 )edit

but I get this error : AttributeError: 'module' object has no attribute 'SimpleClientGoalState'! Do you have an idea how to implement it right?

gleb gravatar imagegleb ( 2016-10-05 04:57:19 -0600 )edit

The error is pretty clear actionlib doesn't have SimpleClientGoalState........................

alienmon gravatar imagealienmon ( 2016-10-05 20:20:12 -0600 )edit
-1

answered 2018-05-24 04:19:35 -0600

fiorellasibona gravatar image

updated 2019-01-15 10:50:14 -0600

Hi! Hoping it can be helpful to someone, I have wrote two posts about sending goals to the navigation stack with a Python node (with example code):

  • Sending a single goal (equivalent to the C++ tutorial), here.

  • Sending a sequence of goals (complete of simulation with a turtlebot robot), here.

Cheers!

edit flag offensive delete link more

Comments

2

Can you please update your answer with more information including code examples that way it will be self-contained? If those pages move or disappear then this answer will be useless.

jayess gravatar imagejayess ( 2018-05-26 01:30:57 -0600 )edit

Hi @jayess, I have updated my answer, thank you!

fiorellasibona gravatar imagefiorellasibona ( 2019-03-04 08:54:20 -0600 )edit

@fiorellasibona sorry for the confusion, but I meant putting the examples in the answer. The reason is is to keep answers self-contained because files/pages can and do disappear from other sites making the answers not helpful

jayess gravatar imagejayess ( 2019-03-04 12:54:35 -0600 )edit

Your Answer

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

Add Answer

Question Tools

4 followers

Stats

Asked: 2016-09-21 09:49:28 -0600

Seen: 2,958 times

Last updated: Jan 15 '19