def __init__(self, base_topic):
self._activity_manager = ThreadedActivityManager()
start_activity_topic = '%s/start_activity' % base_topic
self._start_activity_as = actionlib.SimpleActionServer(
start_activity_topic, StartActivityAction, self.start_activity_cb, False)
stop_activity_topic = '%s/stop_activity' % base_topic
self._stop_activity_as = actionlib.SimpleActionServer(
stop_activity_topic, StopActivityAction, self.stop_activity_cb, False)
list_activities_topic = '%s/list_running_activities' % base_topic
self._list_activities_as = actionlib.SimpleActionServer(
list_activities_topic, ListRunningActivitiesAction, self.list_running_activities_cb, False)
evaluate_query_topic = '%s/evaluate_query' % base_topic
self._evaluate_query_as = actionlib.SimpleActionServer(
evaluate_query_topic, EvaluateQueryAction, self.evaluate_query_cb, False)
self._start_activity_as.start()
self._stop_activity_as.start()
self._list_activities_as.start()
self._evaluate_query_as.start()
class ActivityServer:
def __init__(self, base_topic):
self._activity_manager = ThreadedActivityManager()
start_activity_topic = '%s/start_activity' % base_topic
self._start_activity_as = actionlib.SimpleActionServer(
start_activity_topic, StartActivityAction, self.start_activity_cb, False)
stop_activity_topic = '%s/stop_activity' % base_topic
self._stop_activity_as = actionlib.SimpleActionServer(
stop_activity_topic, StopActivityAction, self.stop_activity_cb, False)
list_activities_topic = '%s/list_running_activities' % base_topic
self._list_activities_as = actionlib.SimpleActionServer(
list_activities_topic, ListRunningActivitiesAction, self.list_running_activities_cb, False)
evaluate_query_topic = '%s/evaluate_query' % base_topic
self._evaluate_query_as = actionlib.SimpleActionServer(
evaluate_query_topic, EvaluateQueryAction, self.evaluate_query_cb, False)
self._start_activity_as.start()
self._stop_activity_as.start()
self._list_activities_as.start()
self._evaluate_query_as.start()
def start_activity_cb(self, goal):
rospy.loginfo('Starting activity: %s' % goal.activity_type)
# ask the activity manager to start the activity
activity_id = self._activity_manager.start_activity(goal.activity_type, goal.goal, goal.desired_activity_id)
# started successfully (even though the activity itself may end up failing)
result = StartActivityResult()
result.activity_id = activity_id
result.error_code.val = ActivityErrorCode.SUCCESS
self._start_activity_as.set_succeeded(result)
return
def stop_activity_cb(self, goal):
rospy.loginfo('Stopping activity')
# ask the activity manager to stop the activity
self._activity_manager.stop_activity(goal.activity_id)
result = StopActivityResult()
result.error_code.val = ActivityErrorCode.SUCCESS
self._stop_activity_as.set_succeeded(result)
return
def list_running_activities_cb(self, goal):
rospy.loginfo('Listing running activities')
result = ListRunningActivitiesResult()
result.running_activities = self._activity_manager.get_running_activities()
self._list_activities_as.set_succeeded(result)
return
def evaluate_query_cb(self, goal):
rospy.loginfo('Evaluating query')
res = self._activity_manager.evaluate_query(goal.query.query, goal.query.args)
result = EvaluateQueryResult()
result.result = res
self._evaluate_query_as.set_succeeded(result)
return
def run(self):
while not rospy.is_shutdown():
rospy.sleep(0.1)