willow_tour.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: Bhaskara Marthi
#***********************************************************

# Runs a tour of Willow map
# Note will not work right now; you just have to remove the dependency
# on continuous_ops_msgs

PKG='semanticmodel'
import roslib
roslib.load_manifest(PKG)
import rospy
import time
import actionlib
import math

from move_base_msgs.msg import *
from geometry_msgs.msg import *
import pr2_common_action_msgs.msg as act
import actionlib_msgs.msg as am
import continuous_ops_msgs.msg as cont_ops
import std_msgs.msg as sm


# Original goals; who knows where from.
#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)]
#]
places = {\
"whiteboard": ("green room looking towards whiteboard from across", (17.52, 18.11, 1.25)),
"magazines": ("looking at magazines in green room", (18.30, 25.81, 2.82)),
"bigscreen": ("looking at big screen", (17.81, 22.15, 2.74)),
"cafe1": ("at entrance to cafeteria", (36.11, 21.77, 1.94)),
"coffee": ("looking diagonally at coffee machine", (34.56, 16.11, 1.22)),
"cafe2": ("looking out side entrance", (27.83, 10.84, 2.78)),
"glass": ("looking out glass door", (26.75, 15.04, 1.86)),
"whitelab": ("looking towards white lab", (33.39, 30.88, 1.20)),
"frontdoor": ("looking towards front door", (41.45, 20.78, 1.15)),
"pooltable": ("looking at pool table from narrow hallway", (19.90, 32.74, 1.67)),
"bike": ("looking out bike exit", (17.12, 55.21, 2.69)),
"research": ("looking out research exit", (05.00, 21.40, 1.75)),
"fishbowl": ("fishbowl, looking at pool table", (12.18, 32.55, 0.55)),
"boondocks": ("boondocks, looking at white lab", (41.23, 49.47, 0.31)),
"looking-at-fridge": ("pool room, looking at fridge", (20.4, 36.32, 0.448)),
}


# Mac's goals.
goals = [
    [(20.11, 25.77, 0.0), (0.0, 0.0, -0.83, 0.55)],
    [(17.85, 19.21, 0.0), (0.0, 0.0, -0.19, 0.98)],
    [(27.06, 15.57, 0.0), (0.0, 0.0, 0.56, 0.82)],
    [(35.37, 20.52, 0.0), (0, 0, -0.96, -0.25)],
    [(31.26, 25.06, 0.0), (0, 0, -0.829, 0.55)],
    [(19.78, 33.32, 0.0), (0, 0, 0.6, 0.79)],
    [(20.76, 37,84, 0.0), (0, 0, 0.98, 0.17)],
    [(19.22, 24.29, 0.0), (0, 0, -0.71, 0.69)]
]

def convert_2d_pose(p):
  return [(p[0], p[1], 0), (0, 0, math.sin(p[2]/2), math.cos(p[2]/2))]

# Bhaskara goals (test)
wp = ["whiteboard", "bigscreen", "magazines", "pooltable", "looking-at-fridge", "research", "bike", "boondocks",
      "cafe1", "coffee", "cafe2", "glass", "frontdoor", "whitelab", "whiteboard"
      ]


goals = [convert_2d_pose(places[p][1]) for p in wp]
print "The goals are ", [places[p] for p in wp]
    


def mb_goal(goal):
    return MoveBaseGoal(PoseStamped(sm.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]))))
                                           

ACTION_SERVER=None






def do_the_tour(msg=None):
    global ACTION_SERVER
    tuck_client = actionlib.SimpleActionClient('tuck_arms', act.TuckArmsAction)
    rospy.loginfo("Waiting for tuck_arms")
    tuck_client.wait_for_server()
    while not rospy.is_shutdown():
        rospy.loginfo("Sending tuck goal")
        res = tuck_client.send_goal_and_wait(act.TuckArmsGoal(True, True),
                execute_timeout=rospy.Duration(20.0))
        rospy.loginfo("Tuck result was {0}".format(res))
        if res == am.GoalStatus.SUCCEEDED:
           break

      


    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    rospy.loginfo("Waiting for move_base")
    client.wait_for_server()
    rospy.loginfo("Sending goals")

    for idx, goal in list(enumerate(goals))[:]:
        mb = mb_goal(goal)
        rospy.loginfo("Sending goal {1} at {0}".format(goal, wp[idx]))
        client.send_goal(mb)
        while not client.wait_for_result(rospy.Duration(1.0)):
            if rospy.is_shutdown():
                rospy.loginfo("Tour node shutting down, cancelling nav goals")
                client.cancel_all_goals()
                return
            if ACTION_SERVER.is_preempt_requested():
                rospy.loginfo("Tour has been preempted.  Cancelling")
                client.cancel_all_goals()
                ACTION_SERVER.set_preempted()
                return 
        rospy.sleep(3.0)
     
    rospy.loginfo('Done with tour')
    ACTION_SERVER.set_succeeded()



if __name__ == '__main__':
    rospy.init_node('willow_tour_node')
    ACTION_SERVER = actionlib.SimpleActionServer("semantic_passive", cont_ops.TaskAction,
            execute_cb = do_the_tour)
    ACTION_SERVER.start()