class ActionDispatcher(object): def __init__(self, name): rospy.loginfo("Starting %s" % name) self.client = None action_list = rospy.get_param("han_action_dispatcher")["han_actions"] self.default_action = rospy.get_param("~default_action") self.dyn_srv = DynServer(HanActionDispatcherConfig, self.dyn_callback) self.servers = {} for a in action_list.items(): name = a[0] self.servers[name] = SimpleActionServer( name, MoveBaseAction, execute_cb=(lambda x: lambda msg: self.execute_cb( msg, x[0], x[1]["action"]))(a), auto_start=False) self.servers[name].register_preempt_callback(self.preempt_cb) self.servers[name].start() rospy.loginfo("done") def execute_cb(self, msg, name, action): a = action if not self.use_default else self.default_action self.client = SimpleActionClient(a, MoveBaseAction) rospy.logdebug("Waiting for action server:" + a) self.client.wait_for_server() rospy.logdebug("Sending goal to:" + a) self.client.send_goal_and_wait(msg) self.servers[name].set_succeeded() self.client = None def preempt_cb(self): if self.client != None: self.client.cancel_all_goals() self.client = None def dyn_callback(self, config, level): self.use_default = config["use_default"] return config
def test_server_online(self): """Check that server is online In this simple setup, we don't really want to generate a plan. Instread, we can ask for a plan on our planner (gpp_gp), and verify, that the generated error-message is not INVALID_PLUGIN See https://github.com/magazino/move_base_flex/blob/596ed881bfcbd847e9d296c6d38e4d3fa3b74a4d/mbf_msgs/action/GetPath.action for reference. """ # setup the client get_path = SimpleActionClient('move_base_flex/get_path', GetPathAction) self.assertTrue(get_path.wait_for_server(rospy.Duration(60)), "{} server offline".format(get_path.action_client.ns)) # send a dummy goal with the right planner goal = GetPathGoal() goal.planner = 'gpp_gp' get_path.send_goal_and_wait(goal, rospy.Duration(1)) result = get_path.get_result() # we are happy as long the plugin is known self.assertNotEqual(result.outcome, GetPathResult.INVALID_PLUGIN)