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)
