move_base_rfn.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('move_base_rfn')
import rospy
from rfnserver import RFNServer
from geometry_msgs.msg import Pose
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from location_memorizer.srv import GetLocation, GetLocationRequest

from tf.transformations import quaternion_from_euler

from actionlib import SimpleActionClient, SimpleGoalState

class TNavRFN:
    def __init__(self):
        self.navclient = SimpleActionClient("move_base", MoveBaseAction)
        self.rfnserver = RFNServer("navigation", self.cb_rfn_start)
        self.rfnserver.register_with_frame("navigating_to_location")
        self.rfnserver.start()

    # map: /opt/ros/unstable/stacks/wg_common/willow_maps/willow-sans-whitelab-2010-02-18-0.025.pgm
    def location_to_pose(self, location):
        position = False
        orientation = False
        if location == "room": # green room
            position = (17.590, 21.611, 0.000)
            orientation = (0.000, 0.000, -0.232, 0.973)
        elif location == "office": # ken's office
            position = (19.243, 26.436, 0.000)
            orientation = (0.000, 0.000, 0.534, 0.846)
        elif location == "fridge":
            position = (21.458, 37.838, 0.000)
            orientation = (0.000, 0.000, -0.198, 0.980)
        elif location == "hangout":
            position = (15.828, 17.155, 0.000)
            orientation = (0.000, 0.000, -0.072, 0.997)
        # if location == "room": # pool room
        #     position = (18.639, 33.727, 0.000)
        #     orientation = (0.000, 0.000, 0.714, 0.700)
        else:
            return False
        goal_pose = Pose()
        goal_pose.position.x = position[0]
        goal_pose.position.y = position[1]
        goal_pose.position.z = position[2]
        goal_pose.orientation.x = orientation[0]
        goal_pose.orientation.y = orientation[1]
        goal_pose.orientation.z = orientation[2]
        goal_pose.orientation.w = orientation[3]
        return goal_pose


    def cb_rfn_start(self, filled_semantic_frame):
        # Check that the server is alive; give it a few seconds to respond.
        if not self.navclient.wait_for_server(timeout=rospy.Duration(5.0)):
            rospy.logwarn("Nav client unavailable!")
            self.rfnserver.set_aborted()
            return
        # Server responded!
        # Determine and fill in the goal pose.
        goal_location = RFNServer.frame_argument(filled_semantic_frame,
                                                 "location")
        goal_pose = self.location_to_pose(goal_location)
        if goal_pose:
            goal = MoveBaseGoal()
            goal.target_pose.pose = goal_pose
            goal.target_pose.header.frame_id = "/map"
            rospy.loginfo("Sending goal:\n" + str(goal))
            self.navclient.send_goal(goal)
        else:
            rospy.logerr("I don't know the location " + goal_location)
            self.rfnserver.set_aborted()
            return
        # Wait for the goal to give us a result, or until we're kicked off.
        r = rospy.Rate(10) #hz
        while(not self.rfnserver.is_preempt_requested() and
              # vv Polling equivalent to self.navclient.wait_for_result()
              not self.navclient.simple_state == SimpleGoalState.DONE and
              not rospy.is_shutdown()):
            r.sleep()
        if self.rfnserver.is_preempt_requested():
            self.navclient.cancel_all_goals()
            self.rfnserver.set_preempted()
            return
        if self.navclient.simple_state == SimpleGoalState.DONE:
            self.rfnserver.set_succeeded()
            return
        if rospy.is_shutdown():
            self.rfnserver.set_aborted()
            return
        self.rfnserver.set_aborted()
        return


    def cb_rfn_preempt(self):
        # Call preempt on the navigation action server.
        rospy.loginfo("move_base_rfn: Cancelling goals.")
        navclient.cancel_all_goals()
        self.rfnserver.set_preempted()
        return



def main():
    rospy.init_node('move_base_rfn')
    t = TNavRFN()
    rospy.spin()


if __name__ == '__main__':
    main()