Go to the documentation of this file.00001
00002
00003 import rospy
00004
00005 from sr_robot_msgs.srv import PlanTrajectoryFromList as PlanFromList
00006 from sr_robot_msgs.srv import PlanTrajectoryFromPrefix as PlanFromPrefix
00007 from sr_robot_msgs.srv import ExecutePlannedTrajectory as ExecutePlan
00008 from sr_robot_msgs.srv import PlanNamedTrajectory as PlanNamed
00009 from sr_robot_msgs.srv import ListNamedTrajectories as ListNamed
00010 from functools import partial
00011 from copy import deepcopy
00012
00013
00014 class WaypointNamedServices(object):
00015 def __init__(self):
00016
00017 rospy.init_node('waypoint_named_services')
00018
00019 rospy.loginfo("Waiting for planning services...")
00020 rospy.wait_for_service('plan_trajectory_from_list')
00021 rospy.wait_for_service('plan_trajectory_from_prefix')
00022 rospy.wait_for_service('execute_planned_trajectory')
00023
00024 self.__from_list = rospy.ServiceProxy(
00025 'plan_trajectory_from_list', PlanFromList)
00026 self.__from_prefix = rospy.ServiceProxy(
00027 'plan_trajectory_from_prefix', PlanFromPrefix)
00028 self.__execute_plan = rospy.ServiceProxy(
00029 'execute_planned_trajectory', PlanFromPrefix)
00030 rospy.loginfo("Service proxies connected.")
00031
00032 self.service_mapping = rospy.get_param("~service_mapping")
00033 self.define_services()
00034
00035 def __plan_named_trajectory(self, req):
00036 mapping = [
00037 x for x in self.service_mapping if x["name"] ==
00038 req.name]
00039 if len(mapping) > 1:
00040 rospy.logfatal("Trajectory name is not unique")
00041 elif len(mapping) is 0:
00042 rospy.logfatal("Trajectory name does not exist")
00043 else:
00044 m = mapping[0]
00045
00046 if "list" not in m.keys() and "prefix" not in m.keys():
00047 rospy.logfatal("Service must specify either prefix " +
00048 "or list for choosing waypoints")
00049 elif "list" in m.keys() and "prefix" in m.keys():
00050 rospy.logfatal("Service mapping has both list and " +
00051 "prefix specified. Can only be one.")
00052 else:
00053 if "list" in m.keys():
00054 return self.__from_list(m["list"]).success
00055 else:
00056 return self.__from_prefix(m["prefix"]).success
00057
00058 return False
00059
00060 def __list_named_trajectories(self, req):
00061 return [map(lambda x:x["name"], self.service_mapping)]
00062
00063 def define_services(self):
00064 plan_service_name = rospy.get_param("~plan_named_trajectory_service")
00065 list_service_name = rospy.get_param("~list_named_trajectories_service")
00066
00067 self.__plan_server = rospy.Service(plan_service_name, PlanNamed,
00068 self.__plan_named_trajectory)
00069 self.__list_server = rospy.Service(list_service_name, ListNamed,
00070 self.__list_named_trajectories)
00071
00072 if __name__ == "__main__":
00073 sf = WaypointNamedServices()
00074 rospy.spin()