navigation_task.py

Go to the documentation of this file.

#! /usr/bin/python
#***********************************************************
#* Software License Agreement (BSD License)
#*
#*  Copyright (c) 2009, Willow Garage, Inc.
#*  All rights reserved.
#*
#*  Redistribution and use in source and binary forms, with or without
#*  modification, are permitted provided that the following conditions
#*  are met:
#*
#*   * Redistributions of source code must retain the above copyright
#*     notice, this list of conditions and the following disclaimer.
#*   * Redistributions in binary form must reproduce the above
#*     copyright notice, this list of conditions and the following
#*     disclaimer in the documentation and/or other materials provided
#*     with the distribution.
#*   * Neither the name of Willow Garage, Inc. nor the names of its
#*     contributors may be used to endorse or promote products derived
#*     from this software without specific prior written permission.
#*
#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#*  POSSIBILITY OF SUCH DAMAGE.
#* 
#* Author: Eitan Marder-Eppstein
#***********************************************************

import roslib; roslib.load_manifest('navigation_task')
import rospy
import smach
import smach_ros
import actionlib
import random

from smach_ros import SimpleActionState

from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from continuous_ops_msgs.msg import TaskAction
from geometry_msgs.msg import PoseStamped, Pose, Quaternion, Point
from pr2_common_action_msgs.msg import TuckArmsAction, TuckArmsGoal
from std_msgs.msg import Header

nav_goals = [
    [(19.380, 24.061, 0.000), (0.000, 0.000, 0.953, -0.304)],
    [(27.193, 15.448, 0.000), (0.000, 0.000, 0.526, 0.851)],
    [(29.872, 21.884, 0.000), (0.000, 0.000, 0.979, 0.206)],
    [(18.632, 26.155, 0.000), (0.000, 0.000, 0.807, 0.590)],
    [(20.772, 38.251, 0.000), (0.000, 0.000, 0.980, 0.201)],
    [(5.184, 22.094, 0.000), (0.000, 0.000, 0.559, 0.829)],
    [(17.346, 54.900, 0.000), (0.000, 0.000, 0.160, 0.987)],
    [(25.170, 52.124, 0.000), (0.000, 0.000, -0.099, 0.995)],
    [(20.409, 38.265, 0.000), (0.000, 0.000, -0.678, 0.735)],
    [(24.109, 20.406, 0.000), (0.000, 0.000, 0.973, 0.230)]
]

#define state navigate
class NavigateSM(smach.State):
    def __init__(self, goals, desired_num_goals):
        smach.State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'])
        self.goals = goals
        self.desired_num_goals = desired_num_goals
        self.goal_count = 0
        #make sure to randomize the order of the goals
        random.shuffle(self.goals)
        rospy.loginfo("Goals are: %s" % goals)
        self.client = actionlib.SimpleActionClient('pr2_move_base', MoveBaseAction)
        self.goal_timeout = rospy.Duration.from_sec(rospy.get_param('~goal_timeout', 300.0))

    def mb_goal(self, goal):
        return MoveBaseGoal(PoseStamped(Header(frame_id = 'map'),
                                      Pose(Point(goal[0][0], goal[0][1],
                                                 goal[0][2]),
                                           Quaternion(goal[1][0], goal[1][1],
                                                      goal[1][2], goal[1][3])))) 

    def execute(self, userdata):
        #randomize the goals for execution
        random.shuffle(self.goals)

        #reset the goal count
        self.goal_count = 0
        rospy.loginfo("Waiting for server")
        #make sure that the action server is up 
        self.client.wait_for_server()
        rospy.loginfo("Server is up")
        for goal in self.goals:
            #if we've been to enough goals... then we'll succeed
            if self.desired_num_goals != 0 and self.goal_count >= self.desired_num_goals:
                rospy.loginfo("We've been to %d goals... success." % self.goal_count)
                return 'succeeded'

            #send goal to move_base here
            rospy.loginfo("Sending goal %s" % goal)
            self.client.send_goal(self.mb_goal(goal))
            start_time = rospy.Time.now()

            r = rospy.Rate(10.0)

            #wait for the goal to finish (we don't care how, we'll just move on to the next goal)
            #for a very small amount of time the client should really just have an 
            #is_done method that wouldn't require this
            while not self.client.wait_for_result(rospy.Duration.from_sec(0.001)):
                if self.preempt_requested():
                    self.service_preempt()
                    #preempt move_base here
                    self.client.cancel_goal()
                    return 'preempted'

                #check to see if the goal has exceeded its timeout
                if start_time + self.goal_timeout < rospy.Time.now():
                    rospy.logwarn('Navigation task timed out trying to get to a goal after %.2f seconds, sending a new goal' % self.goal_timeout.to_sec())
                    break

                r.sleep()

            #increment the goal count
            self.goal_count += 1

        return 'succeeded'

def main():
    rospy.init_node('navigation_task')
    #Create a state machine with our one state, this state machine will never actually succeed or abort right now
    sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
    with sm:
        tuck_goal = TuckArmsGoal()
        tuck_goal.tuck_left=True
        tuck_goal.tuck_right=True
        tucking_succeeded = False
        smach.StateMachine.add('TUCK_ARMS', SimpleActionState('tuck_arms', TuckArmsAction,
                                                              goal=tuck_goal),
                               transitions={'succeeded':'NAVIGATE'})
        smach.StateMachine.add('NAVIGATE', NavigateSM(nav_goals, 0), transitions = {'succeeded':'NAVIGATE','preempted':'preempted'})

    asw = smach_ros.ActionServerWrapper(
        'navigation_task', TaskAction,
        wrapped_container = sm,
        succeeded_outcomes = ['succeeded'],
        aborted_outcomes = ['aborted'],
        preempted_outcomes = ['preempted'])

    asw.run_server()

    rospy.spin()

if __name__ == '__main__':
    main()