class WaitServer(object): def __init__(self, name): rospy.loginfo("Starting '%s'." % name) self._as = ActionServer(name, waitAction, goal_cb=self.goal_cb, cancel_cb=self.cancel_cb, auto_start=False) self._as.start() rospy.loginfo("Done.") def goal_cb(self, gh): t = Thread(target=self.execute, args=(gh,)) t.start() def cancel_cb(self, gh): gh.set_canceled(waitActionResult()) def execute(self, gh): gh.set_accepted() goal = gh.get_goal() print "Waiting for
def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True): self.new_goal = False self.preempt_request = False self.new_goal_preempt_request = False self.execute_callback = execute_cb self.goal_callback = None self.preempt_callback = None self.need_to_terminate = False self.terminate_mutex = threading.RLock() self.lock = threading.RLock() self.execute_condition = threading.Condition(self.lock) self.current_goal = ServerGoalHandle() self.next_goal = ServerGoalHandle() if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop) self.execute_thread.start() else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start)
def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True): self.new_goal = False self.preempt_request = False self.new_goal_preempt_request = False self.execute_callback = execute_cb self.goal_callback = None self.preempt_callback = None self.need_to_terminate = False self.terminate_mutex = threading.RLock() # since the internal_goal/preempt_callbacks are invoked from the # ActionServer while holding the self.action_server.lock # self.lock must always be locked after the action server lock # to avoid an inconsistent lock acquisition order self.lock = threading.RLock() self.execute_condition = threading.Condition(self.lock) self.current_goal = ServerGoalHandle() self.next_goal = ServerGoalHandle() if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop) self.execute_thread.start() else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start)
class WaitServer(object): def __init__(self, name): rospy.loginfo("Starting '%s'." % name) self._as = ActionServer(name, waitAction, goal_cb=self.goal_cb, cancel_cb=self.cancel_cb, auto_start=False) self._as.start() rospy.loginfo("Done.") def goal_cb(self, gh): t = Thread(target=self.execute, args=(gh, )) t.start() def cancel_cb(self, gh): gh.set_canceled(waitActionResult()) def execute(self, gh): gh.set_accepted() goal = gh.get_goal() print "Waiting for %s seconds" % goal.time end = rospy.Time.now().to_sec() + goal.time while rospy.Time.now().to_sec() < end and not rospy.is_shutdown() \ and not gh.status_tracker.status.status == GoalStatus.PREEMPTED: rospy.sleep(0.1) gh.publish_feedback( waitFeedback(remaining=end - rospy.Time.now().to_sec())) if not gh.status_tracker.status.status == GoalStatus.PREEMPTED: gh.set_succeeded(waitActionResult())
def create_server(self, ns, goal_cb, auto_start=True): action_server = ActionServer(ns, TestAction, goal_cb=goal_cb, auto_start=False) if auto_start: action_server.start() return action_server
def __init__(self): self.goals = [] self.action_server = ActionServer("multi", TwoIntsAction, self.goal_callback, self.cancel_callback, auto_start=False) self.update_timer = rospy.Timer(rospy.Duration(0.1), self.update) self.action_server.start()
def __init__(self, name): rospy.loginfo("Starting '%s'." % name) self._as = ActionServer(name, waitAction, goal_cb=self.goal_cb, cancel_cb=self.cancel_cb, auto_start=False) self._as.start() rospy.loginfo("Done.")
def __init__(self, name, action_spec, coro, loop=None): """ Initialize an action server. Incoming goals will be processed via the speficied coroutine. """ self.name = name self._loop = loop if loop is not None else asyncio.get_running_loop() self._coro = coro self._tasks = {} self._server = ActionServer( name, action_spec, goal_cb=self._goal_cb, cancel_cb=self._cancel_cb, auto_start=False) self._server.start()
def __init__(self): rospy.loginfo("Initializing UnifiedInterface...") self.joints_to_control = [] self.non_controlled_joints = [] self.controllers = [] self.configure() # Advertise ASs self.ns = rospy.get_name() self._as = ActionServer(self.ns + '/follow_joint_trajectory', FollowJointTrajectoryAction, goal_cb=self.exec_cb, auto_start=False) self._as.start() self._as_simple = ActionServer(self.ns + '/joints_to_pose', GoJointsToPositionAction, goal_cb=self.simple_exec_cb, auto_start=False) # Advertise topics self._sub = rospy.Subscriber(self.ns + '/command', JointTrajectory, self.topic_cb, queue_size=1) self._simple_sub = rospy.Subscriber(self.ns + '/joints_to_pose_topic', JointsToPosition, self.simple_topic_cb, queue_size=1) # Advertise incremental ASs self._as_incr = ActionServer(self.ns + '/incremental_follow_joint_trajectory', FollowJointTrajectoryAction, goal_cb=self.exec_incr_cb, auto_start=False) self._as_simple_incr = ActionServer(self.ns + '/incremental_joints_to_pose', GoJointsToPositionAction, goal_cb=self.simple_exec_incr_cb, auto_start=False) # Advertise incremental topics self._sub_incr = rospy.Subscriber(self.ns + '/incr_command', JointTrajectory, self.topic_cb, queue_size=1) self._simple_sub_incr = rospy.Subscriber(self.ns + '/incr_joints_to_pose_topic', JointsToPosition, self.simple_topic_cb, queue_size=1)
def __init__(self, name, ActionSpec, auto_start=True): self.preempt_request = False self.new_goal_preempt_request = False self.goal_callback = None self.preempt_callback = None self.goal_handle_lock = threading.RLock() self.goal_handles = {} self.preempt_requests = {} self.active_goals_pub = rospy.Publisher(name + "/active_goals", GoalList, queue_size=10) self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start);
def __init__(self, name): # stuff for grasp planning rospy.loginfo("Getting a TransformListener...") self.tf_listener = tf.TransformListener() rospy.loginfo("Getting a TransformBroadcaster...") self.tf_broadcaster = tf.TransformBroadcaster() rospy.loginfo("Initializing a ClusterBoundingBoxFinder...") self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link") self.last_clusters = None rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...") self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback) if DEBUG_MODE: self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped) rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...") self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...") self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction) self.place_ac.wait_for_server() rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...") self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction) self.grasps_ac.wait_for_server() rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...") self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty) self.depth_service.wait_for_service() rospy.loginfo("Getting a PlanningSceneInterface instance...") self.scene = PlanningSceneInterface() # blocking action server rospy.loginfo("Creating Action Server '" + name + "'...") self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False) self.as_feedback = ObjectManipulationFeedback() self.as_result = ObjectManipulationActionResult() self.current_goal = None # Take care of left and right arm grasped stuff self.right_hand_object = None self.left_hand_object = None self.current_side = 'right' rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!") self.grasp_obj_as.start()
def __init__(self, name, ActionSpec, execute_cb = None, auto_start = True): self.new_goal = False self.preempt_request = False self.new_goal_preempt_request = False self.execute_callback = execute_cb; self.goal_callback = None; self.preempt_callback = None; self.need_to_terminate = False self.terminate_mutex = threading.RLock(); # since the internal_goal/preempt_callbacks are invoked from the # ActionServer while holding the self.action_server.lock # self.lock must always be locked after the action server lock # to avoid an inconsistent lock acquisition order self.lock = threading.RLock(); self.execute_condition = threading.Condition(self.lock); self.current_goal = ServerGoalHandle(); self.next_goal = ServerGoalHandle(); if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop); self.execute_thread.start(); else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback,self.internal_preempt_callback,auto_start);
def __init__(self, name): # stuff for grasp planning self.tf_listener = tf.TransformListener() self.tf_broadcaster = tf.TransformBroadcaster() self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link") self.last_objects = RecognizedObjectArray() #rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback) self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback) self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray) rospy.loginfo("Connecting to pickup AS") self.pickup_ac = SimpleActionClient('/pickup', PickupAction) #pickup_ac.wait_for_server() # needed? rospy.loginfo("Connecting to grasp generator AS") self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction) #grasps_ac.wait_for_server() # needed? #planning scene for motion planning self.scene = PlanningSceneInterface() # blocking action server self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False) self.feedback = GraspObjectFeedback() self.result = GraspObjectResult() self.current_goal = None self.grasp_obj_as.start()
def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True): self.active_goals = GoalQueue() self.new_goals = GoalQueue() self.execute_callback = execute_cb self.need_to_terminate = False self.terminate_mutex = threading.RLock() self.lock = threading.RLock() self.execute_condition = threading.Condition(self.lock) if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop, name="ExecuterLoop") self.execute_thread.start() else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start)
def __init__(self, namespace, action_spec, goal_start_fn=None, goal_stop_fn=None): self.goals = {} self.goal_start_fn = goal_start_fn self.goal_stop_fn = goal_stop_fn self.goal_service = rospy.Service(namespace + '/get_goal_from_id', GetTaskFromID, self.task_id_cb) self.server = ActionServer(namespace, action_spec, self.receive_goal, self.stop_fn, auto_start=False) self.server.start()
def __init__(self, name, ActionSpec, execute_cb = None, auto_start = True): self.new_goal = False self.preempt_request = False self.new_goal_preempt_request = False self.execute_callback = execute_cb; self.goal_callback = None; self.preempt_callback = None; self.need_to_terminate = False self.terminate_mutex = threading.RLock(); self.lock = threading.RLock(); self.execute_condition = threading.Condition(self.lock); self.current_goal = ServerGoalHandle(); self.next_goal = ServerGoalHandle(); if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop); self.execute_thread.start(); else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback,self.internal_preempt_callback,auto_start);
def __init__(self, name, ActionSpec, execute_cb = None, auto_start = True): self.new_goal = False self.preempt_request = False self.new_goal_preempt_request = False self.maxlen = 5 #self.goals_buf = deque(maxlen=5) self.goals_buf = Queue.Queue(maxsize=self.maxlen) self.current_indexA = 0 self.current_indexP = 0 #self.maxlen = self.goals_buf.maxlen self.execute_callback = execute_cb self.goal_callback = None self.preempt_callback = None self.need_to_terminate = False self.terminate_mutex = threading.RLock() self.lock = threading.RLock() self.execute_condition = threading.Condition(self.lock) self.current_goal = ServerGoalHandle() self.next_goal = ServerGoalHandle() if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop) self.execute_thread.start() else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback,self.internal_preempt_callback,auto_start)
async def start(self): """ Start the action server. """ self._server = ActionServer( self.name, self.action_spec, auto_start=False, # Make sure to run callbacks on the main thread goal_cb=partial(self._loop.call_soon_threadsafe, self._goal_cb), cancel_cb=partial(self._loop.call_soon_threadsafe, self._cancel_cb), ) try: self._server.start() await self._exception_monitor.start() finally: # TODO(pbovbel) depends on https://github.com/ros/actionlib/pull/142 self._server.stop() self._server = None
def __init__(self, name, actionMsg, execute_cb=None, auto_start=True, call_on_elected=False): self.execute_callback = execute_cb self.goal_callback = None self.preempt_callback = None self.call_on_elected = call_on_elected self.queue = Queue() self.lock = threading.Lock() self.electionList = dict() self.ghDict = dict() # create the action server self.action_server = ActionServer(name, actionMsg, self.get_next_gh, self.internal_preempt_callback, auto_start) # create the scheduling daemon thread if name+"/schedThread" not in [t.name for t in threading.enumerate()]: workingThread = threading.Thread(name=name+"/schedThread", target=self.scheduling) workingThread.setDaemon(True) workingThread.start()
def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True): self.execute_callback = execute_cb self.goal_callback = None self.preempt_callback = None # since the internal_goal/preempt_callbacks are invoked from the # ActionServer while holding the self.action_server.lock # self.lock must always be locked after the action server lock # to avoid an inconsistent lock acquisition order #self.lock = threading.RLock() #self.execute_condition = threading.Condition(self.lock) self.current_goal = ServerGoalHandle() self.next_goal = ServerGoalHandle() #create the action server self.action_server = ActionServer(name, ActionSpec, self.get_next_gh, self.internal_preempt_callback, auto_start)
class AsyncActionServer: """ Async wrapper around the action server API. """ def __init__(self, name, action_spec, coro, loop=None): """ Initialize an action server. Incoming goals will be processed via the speficied coroutine. """ self.name = name self._loop = loop if loop is not None else asyncio.get_running_loop() self._coro = coro self._tasks = {} self._server = ActionServer( name, action_spec, goal_cb=self._goal_cb, cancel_cb=self._cancel_cb, auto_start=False) self._server.start() def _schedule_goal(self, goal_handle, goal_id): # TODO(pbovbel) consider wrapping the user's coroutine in a cancellation handler to set_canceled task = asyncio.create_task(self._coro(goal_handle)) self._tasks[goal_id] = task def untrack_task(wrapped, *args, **kwargs): del self._tasks[goal_id] wrapped(*args, **kwargs) # These methods lead to a terminal status and should cause the goal to become untracked for method in ['set_canceled', 'set_aborted', 'set_rejected', 'set_succeeded']: setattr(goal_handle, method, partial(untrack_task, wrapped=getattr(goal_handle, method))) def _preempt_goal(self, goal_id): try: self._tasks[goal_id].cancel() except KeyError: logger.error(f"Received cancellation for untracked goal_id {goal_id}") def _goal_cb(self, goal_handle): goal_id = goal_handle.get_goal_id().id # this is locking, should only be called from actionlib thread self._loop.call_soon_threadsafe(self._schedule_goal, goal_handle, goal_id) def _cancel_cb(self, goal_handle): goal_id = goal_handle.get_goal_id().id # this is locking, should only be called from actionlib thread self._loop.call_soon_threadsafe(self._preempt_goal, goal_id)
def __init__(self, name, topic, action_type, result_type): self.name = name self.result = result_type() self.topic = topic self.action_type = action_type self.timeout = 5 Subscriber('mock/' + name, String, self.receive_commands) self.server = ActionServer(self.topic, self.action_type, self.success, False) self.server.start() loginfo('>>> Starting ' + self.name)
class MultipleActionServer(object): def __init__(self): self.goals = [] self.action_server = ActionServer("multi", TwoIntsAction, self.goal_callback, self.cancel_callback, auto_start=False) self.update_timer = rospy.Timer(rospy.Duration(0.1), self.update) self.action_server.start() def goal_callback(self, goal): rospy.loginfo('new goal {}'.format(goal)) self.goals.append(goal) def cancel_callback(self, goal): rospy.loginfo('cancel {}'.format(goal)) def update(self, event): for goal in self.goals: pass
def __init__(self, name, topic): self._name = name self.action_result = None self._feedback = move_base_msgs.msg.MoveBaseFeedback() self._result = move_base_msgs.msg.MoveBaseResult() self.abort_with_feedback = False self._topic = topic Subscriber('mock/feedback_' + name, String, self.receive_commands) self._server = ActionServer(self._topic, MoveBaseAction, self.execute, False) self._server.start() loginfo('>>> Starting ' + self._name + ' with feedback.')
class SynchronizedActionServer(object): def __init__(self, namespace, action_spec, goal_start_fn=None, goal_stop_fn=None): self.goals = {} self.goal_start_fn = goal_start_fn self.goal_stop_fn = goal_stop_fn self.goal_service = rospy.Service(namespace + '/get_goal_from_id', GetTaskFromID, self.task_id_cb) self.server = ActionServer(namespace, action_spec, self.receive_goal, self.stop_fn, auto_start=False) self.server.start() def task_id_cb(self, request): key = request.task_id if key in self.goals: rospy.loginfo( 'task_id_cb: successfully found task {}.'.format(key)) gh = self.goals[key] return GetTaskFromIDResponse(gh.get_goal()) else: rospy.logwarn( 'task_id_cb: could not find requested goal {}'.format(key)) rospy.logwarn('task_id_cb: running goals: {}'.format( self.goals.keys())) return GetTaskFromIDResponse() def receive_goal(self, gh): self.goals[gh.get_goal_id().id] = gh print('Goals: {}'.format(self.goals.keys())) self.goal_start_fn(gh) def stop_fn(self, gh): self.goal_stop_fn(gh)
def __init__(self, name, topic, action_type): """ Creating a custom mock action server.""" self._topic = topic self._name = name self._action_type = action_type self.timeout = 5 self.action_result = None Subscriber('mock/' + name, String, self.receive_commands) Subscriber('mock/gui_result', Bool, self.set_gui_result) self._server = ActionServer(self._topic, self._action_type, self.success, False) self._server.start() loginfo('>>> Starting ' + self._name)
class AsyncActionServer: """ Async wrapper around the action server API. """ def __init__(self, name, action_spec, coro, simple=False): """ Initialize an action server. Incoming goals will be processed via the speficied coroutine. """ self.name = name self.action_spec = action_spec self.simple = simple self.tasks = {} self._loop = asyncio.get_event_loop() self._coro = coro self._exception_monitor = ExceptionMonitor() async def start(self): """ Start the action server. """ self._server = ActionServer( self.name, self.action_spec, auto_start=False, # Make sure to run callbacks on the main thread goal_cb=partial(self._loop.call_soon_threadsafe, self._goal_cb), cancel_cb=partial(self._loop.call_soon_threadsafe, self._cancel_cb), ) try: self._server.start() await self._exception_monitor.start() finally: # TODO(pbovbel) depends on https://github.com/ros/actionlib/pull/142 self._server.stop() self._server = None async def cancel(self, goal_handle): """ Cancel a particular goal's handler task. """ task = self._cancel_cb(goal_handle) if task: await deflector_shield(task) async def cancel_all(self): for task in self.tasks.values(): if not task.cancelled(): task.cancel() await deflector_shield(task) def _goal_cb(self, goal_handle): """ Process incoming goals by spinning off a new asynchronous task to handle the callback. """ goal_id = goal_handle.get_goal_id().id task = asyncio.ensure_future( self._wrapper_coro( goal_id=goal_id, goal_handle=goal_handle, preempt_tasks={**self.tasks} if self.simple else {}, )) task.add_done_callback( partial(self._task_done_callback, goal_handle=goal_handle)) self._exception_monitor.register_task(task) self.tasks[goal_id] = task async def _wrapper_coro(self, goal_id, goal_handle, preempt_tasks={}): """ Wrap the user-provided coroutine to allow the simple action mode to preempt any previously submitted goals. """ if preempt_tasks: logger.debug( f"Before goal {goal_id}, preempting {' '.join(self.tasks.keys())}" ) for other_task in preempt_tasks.values(): if not other_task.cancelled(): other_task.cancel() try: await deflector_shield(other_task) except asyncio.CancelledError: goal_handle.set_canceled( f"Goal {goal_id} was preempted before starting") raise logger.debug(f"Starting callback for goal {goal_id}") await self._coro(goal_handle) def _cancel_cb(self, goal_handle): """ Process incoming cancellations by finding the matching task and cancelling it. """ goal_id = goal_handle.get_goal_id().id try: task = self.tasks[goal_id] task.cancel() return task except KeyError: logger.debug( f"Received cancellation for untracked goal_id {goal_id}") return None PENDING_STATUS = {GoalStatus.PENDING, GoalStatus.RECALLING} ACTIVE_STATUS = {GoalStatus.ACTIVE, GoalStatus.PREEMPTING} NON_TERMINAL_STATUS = PENDING_STATUS | ACTIVE_STATUS def _task_done_callback(self, task, goal_handle): """ Process finished tasks and translate the result/exception into actionlib signals. """ goal_id = goal_handle.get_goal_id().id status = goal_handle.get_goal_status().status try: exc = task.exception() except asyncio.CancelledError: if status in self.NON_TERMINAL_STATUS: message = f"Task handler was cancelled, goal {goal_id} cancelled automatically" goal_handle.set_canceled(text=message) logger.warning( f"{message}. Please call set_canceled in the action server coroutine when CancelledError is raised" ) else: rejected_message = f"Goal {goal_id} rejected" aborted_message = f"Goal {goal_id} aborted" if exc is not None: reason = f"uncaught exception in actionserver handler: {exc}" if status in self.PENDING_STATUS: goal_handle.set_rejected( result=None, text=f"{rejected_message}, {reason}") elif status in self.ACTIVE_STATUS: goal_handle.set_aborted( result=None, text=f"{aborted_message}, {reason}") else: reason = f"never completed server-side" if status in self.PENDING_STATUS: goal_handle.set_rejected( result=None, text=f"{rejected_message}, {reason}") logger.warning(f"{rejected_message}, {reason}") elif status in self.ACTIVE_STATUS: goal_handle.set_aborted( result=None, text=f"{aborted_message}, {reason}") logger.warning(f"{aborted_message}, {reason}") del self.tasks[goal_id]
class UnifiedInterface(object): def __init__(self): rospy.loginfo("Initializing UnifiedInterface...") self.joints_to_control = [] self.non_controlled_joints = [] self.controllers = [] self.configure() # Advertise ASs self.ns = rospy.get_name() self._as = ActionServer(self.ns + '/follow_joint_trajectory', FollowJointTrajectoryAction, goal_cb=self.exec_cb, auto_start=False) self._as.start() self._as_simple = ActionServer(self.ns + '/joints_to_pose', GoJointsToPositionAction, goal_cb=self.simple_exec_cb, auto_start=False) # Advertise topics self._sub = rospy.Subscriber(self.ns + '/command', JointTrajectory, self.topic_cb, queue_size=1) self._simple_sub = rospy.Subscriber(self.ns + '/joints_to_pose_topic', JointsToPosition, self.simple_topic_cb, queue_size=1) # Advertise incremental ASs self._as_incr = ActionServer(self.ns + '/incremental_follow_joint_trajectory', FollowJointTrajectoryAction, goal_cb=self.exec_incr_cb, auto_start=False) self._as_simple_incr = ActionServer(self.ns + '/incremental_joints_to_pose', GoJointsToPositionAction, goal_cb=self.simple_exec_incr_cb, auto_start=False) # Advertise incremental topics self._sub_incr = rospy.Subscriber(self.ns + '/incr_command', JointTrajectory, self.topic_cb, queue_size=1) self._simple_sub_incr = rospy.Subscriber(self.ns + '/incr_joints_to_pose_topic', JointsToPosition, self.simple_topic_cb, queue_size=1) # Subscribe to joint states if necessary? def configure(self): """Configure the node from the param server""" # Read config from param server node_cfg = rospy.get_param('/unified_joint_trajectory_controller', None) if node_cfg is None: rospy.logerr("No '/unified_joint_trajectory_controller'" + " param found!") exit(0) # Parse every block controllers = node_cfg.get('controllers') if controllers is None: rospy.logerr( "No controllers in" + "'/unified_joint_trajectory_controller/controllers'," + " nothing to control") # Controllers should be a list of dictionaries # containing 'controller_type', from there # we deduct what other keys are needed for c in controllers: current_c = self.create_controller(c) self.controllers.append(current_c) def create_controller(self, controller_dict): c_type = controller_dict.get('controller_type') if c_type == 'pr2_follow_joint_trajectory_controller': # Import dynamically what we need from unified_joint_trajectory_controller.pr2_joint_trajectory_controller_manager import PR2JointTrajectoryControllerManager action_server_name = controller_dict.get('action_server') return PR2JointTrajectoryControllerManager(action_server_name) elif c_type == 'follow_joint_trajectory_controller': # TODO: return None elif c_type is None: rospy.lowarn("A controller configuration returned None on " + " params config: " + str(controller_dict)) return None def send_goal(self, goal): # send goals to all controllers for controller in self.controllers: controller.send_goal(goal) # publish feedback until they are done done_goals = [False] * len(self.controllers) while not rospy.is_shutdown() and not all(done_goals): # construct feedback message curr_fed = FollowJointTrajectoryFeedback() for cidx, controller in enumerate(self.controllers): if controller.is_goal_done(): done_goals[cidx] = True # add to feedback message this controller stuff fed = controller.get_feedback() # curr_fed add stuff # compose global result and publish for controller in self.controllers: res = controller.get_result() def cancel_all_goals(self): for c in self.controllers: c.cancel_all_goals() def exec_cb(self, goal): goal.set_accepted() rospy.loginfo("Goal is: " + str(goal.get_goal())) self.send_goal(goal.get_goal().trajectory) def simple_exec_cb(self, goal): pass def topic_cb(self, goal): pass def simple_topic_cb(self, goal): pass def exec_incr_cb(self, goal): pass def simple_exec_incr_cb(self, goal): pass def topic_incr_cb(self, goal): pass def simple_topic_incr_cb(self, goal): pass
class EnhancedActionServer(object): def __init__(self, name, actionMsg, execute_cb=None, auto_start=True, call_on_elected=False): self.execute_callback = execute_cb self.goal_callback = None self.preempt_callback = None self.call_on_elected = call_on_elected self.queue = Queue() self.lock = threading.Lock() self.electionList = dict() self.ghDict = dict() # create the action server self.action_server = ActionServer(name, actionMsg, self.get_next_gh, self.internal_preempt_callback, auto_start) # create the scheduling daemon thread if name+"/schedThread" not in [t.name for t in threading.enumerate()]: workingThread = threading.Thread(name=name+"/schedThread", target=self.scheduling) workingThread.setDaemon(True) workingThread.start() def start(self): """ start server if not already started """ self.action_server.start() def _set_gh_state(self, result=None, text="", succeeded=True): """ :param result: An optional result to send back to any clients of the goal :param text: An optionnal text associated to the SUCCESS|ABORTED status :param succeeded: the goal handle is set to SUCCEEDED if true, else ABORTED """ threadId = threading.current_thread().ident self.lock.acquire() if threadId not in self.ghDict: rospy.logwarn("set_succeeded called more than once in an actionlib server callback or outside one, ignoring call") return gh = self.ghDict[threadId] if result is None: result = self.get_default_result() rate = rospy.Rate(1000) self.lock.release() while not self.is_elected(gh): rate.sleep() sender, _, _ = gh.get_goal_id().id.split("-") self.electionList.get(sender).pop(0) if succeeded: gh.set_succeeded(result, text) else: gh.set_aborted(result, text) self.lock.acquire() self.ghDict.pop(threadId) self.lock.release() def set_succeeded(self, result=None, text=""): self._set_gh_state(result, text) def set_aborted(self, result=None, text=""): self._set_gh_state(result, text, succeeded=False) def get_default_result(self): """ :return: default content for result message """ return self.action_server.ActionResultType() def execute_callback_on_elected(self, goal_handle): rate = rospy.Rate(1000) while not self.is_elected(goal_handle): rate.sleep() self.execute_server_callback(goal_handle) def execute_server_callback(self, goal_handle): self.lock.acquire() self.ghDict[threading.current_thread().ident] = goal_handle self.lock.release() try: self.execute_callback(goal_handle.get_goal()) except (KeyboardInterrupt, SystemExit): raise except Exception as e: rospy.logerr("Error in the actionlib server callback: {}".format(traceback.format_exc())) finally: if threading.current_thread().ident in self.ghDict: rospy.logwarn("The actionlib server callback did not set the goal as succeeded, sending unsuccessful result") self.set_aborted() def get_next_gh(self, goal_handle): """ start new thread on new goal reception :type goal_handle: ServerGoalHandle :return: """ try: rospy.logdebug("A new goal %s has been recieved by the single goal action server", goal_handle.get_goal_id().id) if self.execute_callback: goal_handle.status_tracker.status.status = 6 self.queue.put(goal_handle.get_goal_id().id, block=False) try: if self.call_on_elected: t = threading.Thread(target=self.execute_callback_on_elected, args=(goal_handle,)) else: t = threading.Thread(target=self.execute_server_callback, args=(goal_handle,)) t.setDaemon(True) t.start() except threading.ThreadError: rospy.logerr("Error: unable to start thread") else: rospy.logerr("DEFINE an execute callback") except Exception as e: rospy.logerr("CustomActionServer.internal_goal_callback - exception %s", str(e)) # no preemption needed def internal_preempt_callback(self): pass def scheduling(self): """ method for the daemon thread to schedule the goal threads """ while True: goalId = self.queue.get() sender, _, stamp = goalId.split("-") stamp = float(stamp) self.lock.acquire() if sender in self.electionList: senderList = self.electionList.get(sender, []) senderList.append((stamp, goalId)) senderList.sort(key=lambda x : x[0]) else: self.electionList[sender] = [(stamp, goalId)] self.lock.release() def is_elected(self, goalHandle): """ :type goalHandle: ServerGoalHandle :param goalHandle: goal to check if allowed to be finalized :return: boolean value to check if the goal is elected """ isElected = False sender, _, _ = goalHandle.get_goal_id().id.split("-") self.lock.acquire() senderList = self.electionList.get(sender) if senderList: electedGoal = senderList[0][1] isElected = (goalHandle.get_goal_id().id == electedGoal) self.lock.release() return isElected def get_goal_handle(self): return self.ghDict[threading.current_thread().ident]
class ComplexActionServer: ## @brief Constructor for a ComplexActionServer ## @param name A name for the action server ## @param execute_cb Optional callback that gets called in a separate thread whenever ## a new goal is received, allowing users to have blocking callbacks. ## Adding an execute callback also deactivates the goalCallback. ## @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True): self.goals_received_ = 0 self.goal_queue_ = Queue.Queue() self.new_goal = False self.execute_callback = execute_cb self.goal_callback = None self.need_to_terminate = False self.terminate_mutex = threading.RLock() # since the internal_goal/preempt_callbacks are invoked from the # ActionServer while holding the self.action_server.lock # self.lock must always be locked after the action server lock # to avoid an inconsistent lock acquisition order self.lock = threading.RLock() self.execute_condition = threading.Condition(self.lock) self.current_goal = ServerGoalHandle() self.next_goal = ServerGoalHandle() if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop) self.execute_thread.start() else: self.execute_thread = None # create the action server self.action_server = ActionServer( name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start ) def __del__(self): if hasattr(self, "execute_callback") and self.execute_callback: with self.terminate_mutex: self.need_to_terminate = True assert self.execute_thread self.execute_thread.join() ## @brief Accepts a new goal when one is available The status of this ## goal is set to active upon acceptance, def accept_new_goal(self): with self.action_server.lock, self.lock: rospy.logdebug("Accepting a new goal") self.goals_received_ -= 1 # get from queue current_goal = self.goal_queue_.get() # set the status of the current goal to be active current_goal.set_accepted("This goal has been accepted by the simple action server") return current_goal # .get_goal(); ## @brief Allows polling implementations to query about the availability of a new goal ## @return True if a new goal is available, false otherwise def is_new_goal_available(self): return self.goals_received_ > 0 ## @brief Allows polling implementations to query about the status of the current goal ## @return True if a goal is active, false otherwise def is_active(self): if not self.current_goal.get_goal(): return False status = self.current_goal.get_goal_status().status return status == actionlib_msgs.msg.GoalStatus.ACTIVE # or status == actionlib_msgs.msg.GoalStatus.PREEMPTING; ## @brief Sets the status of the active goal to succeeded ## @param result An optional result to send back to any clients of the goal def set_succeeded(self, result=None, text="", goal_handle=None): with self.action_server.lock, self.lock: if not result: result = self.get_default_result() # self.current_goal.set_succeeded(result, text); goal_handle.set_succeeded(result, text) ## @brief Sets the status of the active goal to aborted ## @param result An optional result to send back to any clients of the goal def set_aborted(self, result=None, text="", goal_handle=None): with self.action_server.lock, self.lock: if not result: result = self.get_default_result() # self.current_goal.set_aborted(result, text); goal_handle.set_aborted(result, text) ## @brief Publishes feedback for a given goal ## @param feedback Shared pointer to the feedback to publish def publish_feedback(self, feedback): self.current_goal.publish_feedback(feedback) def get_default_result(self): return self.action_server.ActionResultType() ## @brief Allows users to register a callback to be invoked when a new goal is available ## @param cb The callback to be invoked def register_goal_callback(self, cb): if self.execute_callback: rospy.logwarn( "Cannot call ComplexActionServer.register_goal_callback() because an executeCallback exists. Not going to register it." ) else: self.goal_callback = cb ## @brief Explicitly start the action server, used it auto_start is set to false def start(self): self.action_server.start() ## @brief Callback for when the ActionServer receives a new goal and passes it on def internal_goal_callback(self, goal): self.execute_condition.acquire() try: rospy.logdebug("A new goal %shas been recieved by the single goal action server", goal.get_goal_id().id) print "got a goal" self.next_goal = goal self.new_goal = True self.goals_received_ += 1 # add goal to queue self.goal_queue_.put(goal) # Trigger runLoop to call execute() self.execute_condition.notify() self.execute_condition.release() except Exception, e: rospy.logerr("ComplexActionServer.internal_goal_callback - exception %s", str(e)) self.execute_condition.release()
class ObjectManipulationAS: def __init__(self, name): # stuff for grasp planning rospy.loginfo("Getting a TransformListener...") self.tf_listener = tf.TransformListener() rospy.loginfo("Getting a TransformBroadcaster...") self.tf_broadcaster = tf.TransformBroadcaster() rospy.loginfo("Initializing a ClusterBoundingBoxFinder...") self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link") self.last_clusters = None rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...") self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback) if DEBUG_MODE: self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped) rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...") self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...") self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction) self.place_ac.wait_for_server() rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...") self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction) self.grasps_ac.wait_for_server() rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...") self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty) self.depth_service.wait_for_service() rospy.loginfo("Getting a PlanningSceneInterface instance...") self.scene = PlanningSceneInterface() # blocking action server rospy.loginfo("Creating Action Server '" + name + "'...") self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False) self.as_feedback = ObjectManipulationFeedback() self.as_result = ObjectManipulationActionResult() self.current_goal = None # Take care of left and right arm grasped stuff self.right_hand_object = None self.left_hand_object = None self.current_side = 'right' rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!") self.grasp_obj_as.start() def objects_callback(self, data): rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects)) self.last_clusters = data def goal_callback(self, goal): if self.current_goal: goal.set_rejected() # "Server busy" return # TODO: Check if pose is not empty, if it is, reject # elif len(self.last_clusters.objects) - 1 < goal.get_goal().target_id: # goal.set_rejected() # "No objects to grasp were received on the objects topic." # return else: #store and accept new goal self.current_goal = goal self.current_goal.set_accepted() #run the corresponding operation if self.current_goal.get_goal().operation == ObjectManipulationGoal.PICK: rospy.loginfo("ObjectManipulation: PICK!") self.pick_operation() elif self.current_goal.get_goal().operation == ObjectManipulationGoal.PLACE: rospy.loginfo("ObjectManipulation: PLACE!") self.place_operation() else: rospy.logerr("ObjectManipulation: Erroneous operation!") #finished, get rid of goal self.current_goal = None def cancel_callback(self, goal): #TODO stop motions? self.current_goal.set_canceled() def pick_operation(self): """Execute pick operation""" if self.message_fields_ok(): self.as_result = ObjectManipulationResult() goal_message_field = self.current_goal.get_goal() self.update_feedback("Checking hand to use") # Check which arm group was requested and if it's currently free of objects if 'right' in goal_message_field.group: if self.right_hand_object: # Something already in the hand self.update_aborted("Right hand already contains an object.") return # necessary? self.current_side = 'right' elif 'left' in goal_message_field.group: if self.left_hand_object: self.update_aborted("Left hand already contains an object.") return self.current_side = 'left' # Publish pose of goal position if DEBUG_MODE: self.to_grasp_object_pose_pub.publish(goal_message_field.target_pose) self.update_feedback("Detecting clusters") if not self.wait_for_recognized_array(wait_time=5, timeout_time=10): # wait until we get clusters published self.update_aborted("Failed detecting clusters") # Search closer cluster # transform pose to base_link if needed if goal_message_field.target_pose.header.frame_id != "base_link": self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5)) object_to_grasp_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose) else: object_to_grasp_pose = goal_message_field.target_pose.pose self.update_feedback("Searching closer cluster while clustering") (closest_cluster_id, (object_points, obj_bbox_dims, object_bounding_box, object_pose)) = self.get_id_of_closest_cluster_to_pose(object_to_grasp_pose) rospy.logdebug("in AS: Closest cluster id is: " + str(closest_cluster_id)) #TODO visualize bbox #TODO publish filtered pointcloud? rospy.logdebug("BBOX: " + str(obj_bbox_dims)) ######## self.update_feedback("Check reachability") ######## self.update_feedback("Generating grasps") rospy.logdebug("Object pose before tf thing is: " + str(object_pose)) #transform pose to base_link, IS THIS NECESSARY?? should be a function in any case self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15)) trans_pose = self.tf_listener.transformPose("base_link", object_pose) object_pose = trans_pose #HACK remove orientation -> pose is aligned with parent(base_link) object_pose.pose.orientation.w = 1.0 object_pose.pose.orientation.x = 0.0 object_pose.pose.orientation.y = 0.0 object_pose.pose.orientation.z = 0.0 # shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects) # TODO remove this hack, fix it in table filtering object_pose.pose.position.z += obj_bbox_dims[2] / 2.0 grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x # check if there are grasps, if not, abort if len(grasp_list) == 0: self.update_aborted("No grasps received") return if DEBUG_MODE: # TODO: change to dynamic param publish_grasps_as_poses(grasp_list) self.current_goal.publish_feedback(self.as_feedback) ######## self.update_feedback("Setup planning scene") # Add object to grasp to planning scene self.scene.add_box(self.current_side + "_hand_object", object_pose, (obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2])) self.as_result.object_scene_name = self.current_side + "_hand_object" ######## self.update_feedback("Execute grasps") pug = createPickupGoal(self.current_side + "_hand_object", grasp_list, group=goal_message_field.group) rospy.loginfo("Sending pickup goal") self.pickup_ac.send_goal(pug) rospy.loginfo("Waiting for result...") self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val])) ######## self.update_feedback("finished") self.as_result.object_pose = object_pose self.as_result.error_code = result.error_code self.as_result.error_message = str(moveit_error_dict[result.error_code.val]) if result.error_code.val == MoveItErrorCodes.SUCCESS: if self.current_side == 'right': self.right_hand_object = self.current_side + "_hand_object" elif self.current_side == 'left': self.left_hand_object = self.current_side + "_hand_object" self.current_goal.set_succeeded(result=self.as_result) else: # Remove object as it has not been picked self.scene.remove_world_object(self.current_side + "_hand_object") self.update_aborted(text="MoveIt pick failed", manipulation_result=self.as_result) else: self.update_aborted("Goal fields not correctly fulfilled") def place_operation(self): """Execute the place operation""" if self.message_fields_ok(): self.as_result = ObjectManipulationResult() goal_message_field = self.current_goal.get_goal() self.update_feedback("Checking arm to use") # Check which arm group was requested and if it currently has an object if 'right' in goal_message_field.group: if not self.right_hand_object: # Something already in the hand self.update_aborted("Right hand does not contain an object.") return # necessary? self.current_side = 'right' current_target = self.right_hand_object elif 'left' in goal_message_field.group: if not self.left_hand_object: self.update_aborted("Left hand does not contain an object.") return self.current_side = 'left' current_target = self.left_hand_object # transform pose to base_link if needed if goal_message_field.target_pose.header.frame_id != "base_link": self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5)) placing_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose) else: placing_pose = goal_message_field.target_pose.pose #### TODO: ADD HERE LOGIC ABOUT SEARCHING GOOD PLACE POSE #### self.update_feedback("Sending place order to MoveIt!") placing_pose = PoseStamped(header=Header(frame_id="base_link"), pose=placing_pose) goal = createPlaceGoal(placing_pose, group=goal_message_field.group, target=current_target) rospy.loginfo("Sending place goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result...") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val])) self.as_result.object_pose = placing_pose self.as_result.error_code = result.error_code self.as_result.error_message = str(moveit_error_dict[result.error_code.val]) if result.error_code.val == MoveItErrorCodes.SUCCESS: # Emptying hand self.update_feedback("Emptying hand") if self.current_side == 'right': self.as_result.object_scene_name = current_target self.right_hand_object = None elif self.current_side == 'left': self.as_result.object_scene_name = current_target self.left_hand_object = None # Removing object from the collision world self.scene.remove_world_object(current_target) self.current_goal.set_succeeded(result=self.as_result) else: self.update_aborted(text="MoveIt place failed", manipulation_result=self.as_result) else: self.update_aborted("Goal fields not correctly fulfilled") def message_fields_ok(self): """Check if the minimal fields for the message are fulfilled""" if self.current_goal: goal_message_field = self.current_goal.get_goal() if len(goal_message_field.group) == 0: rospy.logwarn("Group field empty.") return False if goal_message_field.operation != ObjectManipulationGoal.PICK and goal_message_field.operation != ObjectManipulationGoal.PLACE: rospy.logwarn("Asked for an operation different from PICK or PLACE: " + str(goal_message_field.operation)) return False if len(goal_message_field.target_pose.header.frame_id) == 0: rospy.logwarn("Target pose frame_id is empty") return False return True def update_feedback(self, text): """Publish feedback with given text""" self.as_feedback.last_state = text self.current_goal.publish_feedback(self.as_feedback) def update_aborted(self, text="", manipulation_result=None): """Publish feedback and abort with the text give and optionally an ObjectManipulationResult already fulfilled""" self.update_feedback("aborted." + text) if manipulation_result == None: aborted_result = ObjectManipulationResult() aborted_result.error_message = text self.current_goal.set_aborted(result=aborted_result) else: self.current_goal.set_aborted(result=manipulation_result) def generate_grasps(self, pose, width): """Send request to block grasp generator service""" goal = GenerateGraspsGoal() goal.pose = pose.pose goal.width = width grasp_options = GraspGeneratorOptions() grasp_options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Y grasp_options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_DOWN grasp_options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_HALF goal.options.append(grasp_options) self.grasps_ac.send_goal(goal) if DEBUG_MODE: rospy.loginfo("Sent goal, waiting:\n" + str(goal)) self.grasps_ac.wait_for_result() grasp_list = self.grasps_ac.get_result().grasps return grasp_list def get_id_of_closest_cluster_to_pose(self, input_pose): """Returns the id of the closest cluster to the pose provided (in Pose or PoseStamped) and all the associated clustering information""" current_id = 0 closest_pose = None closest_object_points = None closest_id = 0 closest_bbox = None closest_bbox_dims = None closest_distance = 99999.9 for myobject in self.last_clusters.objects: (object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(myobject.point_clouds[0]) self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15)) trans_pose = self.tf_listener.transformPose("base_link", object_pose) object_pose = trans_pose rospy.loginfo("id: " + str(current_id) + "\n pose:\n" + str(object_pose)) if closest_pose == None: # first loop iteration closest_object_points = object_points closest_pose = object_pose closest_bbox = object_bounding_box closest_bbox_dims = obj_bbox_dims closest_distance = dist_between_poses(closest_pose, input_pose) else: if dist_between_poses(object_pose, input_pose) < closest_distance: closest_object_points = object_points closest_distance = dist_between_poses(object_pose, input_pose) closest_pose = object_pose closest_bbox = object_bounding_box closest_bbox_dims = obj_bbox_dims closest_id = current_id current_id += 1 rospy.loginfo("Closest id is: " + str(closest_id) + " at " + str(closest_pose)) return closest_id, (closest_object_points, closest_bbox_dims, closest_bbox_dims, closest_pose) def wait_for_recognized_array(self, wait_time=6, timeout_time=10): """Ask for depth images until we get a recognized array wait for wait_time between depth throtle calls stop if timeout_time is achieved If we dont find it in the correspondent time return false, true otherwise""" initial_time = rospy.Time.now() self.last_clusters = None count = 0 num_calls = 1 self.depth_service.call(EmptyRequest()) rospy.loginfo("Depth throtle server call #" + str(num_calls)) rospy.loginfo("Waiting for a recognized array...") while rospy.Time.now() - initial_time < rospy.Duration(timeout_time) and self.last_clusters == None: rospy.sleep(0.1) count += 1 if count >= wait_time / 10: self.depth_service.call(EmptyRequest()) num_calls += 1 rospy.loginfo("Depth throtle server call #" + str(num_calls)) if self.last_clusters == None: return False else: return True
class QueuedActionServer: ## @brief Constructor for a QueuedActionServer ## @param name A name for the action server ## @param execute_cb Optional callback that gets called in a separate thread whenever ## a new goal is received, allowing users to have blocking callbacks. ## Adding an execute callback also deactivates the goalCallback. ## @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True): self.new_goal = False self.preempt_request = False self.new_goal_preempt_request = False self.maxlen = 5 #self.goals_buf = deque(maxlen=5) self.goals_buf = Queue.Queue(maxsize=self.maxlen) self.current_indexA = 0 self.current_indexP = 0 #self.maxlen = self.goals_buf.maxlen self.execute_callback = execute_cb self.goal_callback = None self.preempt_callback = None self.need_to_terminate = False self.terminate_mutex = threading.RLock() self.lock = threading.RLock() self.execute_condition = threading.Condition(self.lock) self.current_goal = ServerGoalHandle() self.next_goal = ServerGoalHandle() if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop) self.execute_thread.start() else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start) def __del__(self): if hasattr(self, 'execute_callback') and self.execute_callback: with self.terminate_mutex: self.need_to_terminate = True assert (self.execute_thread) self.execute_thread.join() ## @brief Accepts a new goal when one is available The status of this ## goal is set to active upon acceptance, and the status of any ## previously active goal is set to preempted. Preempts received for the ## new goal between checking if isNewGoalAvailable or invokation of a ## goal callback and the acceptNewGoal call will not trigger a preempt ## callback. This means, isPreemptReqauested should be called after ## accepting the goal even for callback-based implementations to make ## sure the new goal does not have a pending preempt request. ## @return A shared_ptr to the new goal. def accept_new_goal(self): with self.lock: if not self.new_goal or not self.next_goal.get_goal(): rospy.logerr( "Attempting to accept the next goal when a new goal is not available" ) return None rospy.logdebug("Accepting a new goal") #accept the next goal self.current_goal = self.next_goal self.new_goal = False #set preempt to request to equal the preempt state of the new goal self.preempt_request = self.new_goal_preempt_request self.new_goal_preempt_request = False #set the status of the current goal to be active self.current_goal.set_accepted( "This goal has been accepted by the queued action server") return self.current_goal.get_goal() ## @brief Allows polling implementations to query about the availability of a new goal ## @return True if a new goal is available, false otherwise def is_new_goal_available(self): return self.new_goal ## @brief Allows polling implementations to query about preempt requests ## @return True if a preempt is requested, false otherwise def is_preempt_requested(self): return self.preempt_request ## @brief Allows polling implementations to query about the status of the current goal ## @return True if a goal is active, false otherwise def is_active(self): if not self.current_goal.get_goal(): return False status = self.current_goal.get_goal_status().status return status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING ## @brief Sets the status of the active goal to succeeded ## @param result An optional result to send back to any clients of the goal def set_succeeded(self, result=None, text=""): with self.lock: if not result: result = self.get_default_result() self.current_goal.set_succeeded(result, text) ## @brief Sets the status of the active goal to aborted ## @param result An optional result to send back to any clients of the goal def set_aborted(self, result=None, text=""): with self.lock: if not result: result = self.get_default_result() self.current_goal.set_aborted(result, text) ## @brief Publishes feedback for a given goal ## @param feedback Shared pointer to the feedback to publish def publish_feedback(self, feedback): self.current_goal.publish_feedback(feedback) def get_default_result(self): return self.action_server.ActionResultType() ## @brief Sets the status of the active goal to preempted ## @param result An optional result to send back to any clients of the goal def set_preempted(self, result=None, text=""): if not result: result = self.get_default_result() with self.lock: rospy.logdebug("Setting the current goal as canceled") self.current_goal.set_canceled(result, text) ## @brief Allows users to register a callback to be invoked when a new goal is available ## @param cb The callback to be invoked def register_goal_callback(self, cb): if self.execute_callback: rospy.logwarn( "Cannot call QueuedActionServer.register_goal_callback() because an executeCallback exists. Not going to register it." ) else: self.goal_callback = cb ## @brief Allows users to register a callback to be invoked when a new preempt request is available ## @param cb The callback to be invoked def register_preempt_callback(self, cb): self.preempt_callback = cb ## @brief Explicitly start the action server, used it auto_start is set to false def start(self): self.action_server.start() ## @brief Callback for when the ActionServer receives a new goal and passes it on def internal_goal_callback(self, goal): self.execute_condition.acquire() try: rospy.logdebug( "A new goal %shas been recieved by the Queued goal action server", goal.get_goal_id().id) if (self.goals_buf.empty()): self.new_goal = True self.next_goal = goal self.goals_buf.put(goal, timeout=1) else: self.goals_buf.put(goal, timeout=1) rospy.loginfo("Queued New Goal") if self.goal_callback: self.goal_callback() #rospy.loginfo("Goals List-----------------------------------------------") #for item in self.goals_buf: # rospy.loginfo("Goals Buffer%s" %item.get_goal_status()) #rospy.loginfo("End of the Goals List-------------------------------------") #if the user has defined a goal callback, we'll call it now #Trigger runLoop to call execute() self.execute_condition.notify() self.execute_condition.release() except Exception, e: rospy.logerr( "QueuedActionServer.internal_goal_callback - exception %s", str(e)) self.execute_condition.release()
class GraspObjectServer: def __init__(self, name): # stuff for grasp planning self.tf_listener = tf.TransformListener() self.tf_broadcaster = tf.TransformBroadcaster() self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link") self.last_objects = RecognizedObjectArray() #rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback) self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback) self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray) rospy.loginfo("Connecting to pickup AS") self.pickup_ac = SimpleActionClient('/pickup', PickupAction) #pickup_ac.wait_for_server() # needed? rospy.loginfo("Connecting to grasp generator AS") self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction) #grasps_ac.wait_for_server() # needed? #planning scene for motion planning self.scene = PlanningSceneInterface() # blocking action server self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False) self.feedback = GraspObjectFeedback() self.result = GraspObjectResult() self.current_goal = None self.grasp_obj_as.start() def objects_callback(self, data): rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects)) self.last_objects = data def goal_callback(self, goal): if self.current_goal: goal.set_rejected() # "Server busy" return elif len(self.last_objects.objects) - 1 < goal.get_goal().target_id: goal.set_rejected() # "No objects to grasp were received on the objects topic." return else: #store and accept new goal self.current_goal = goal self.current_goal.set_accepted() #run grasping state machine self.grasping_sm() #finished, get rid of goal self.current_goal = None def cancel_callback(self, goal): #TODO stop motions? self.current_goal.set_canceled() def grasping_sm(self): if self.current_goal: self.update_feedback("running clustering") (object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(self.last_objects.objects[self.current_goal.get_goal().target_id].point_clouds[0]) #TODO visualize bbox #TODO publish filtered pointcloud? print obj_bbox_dims ######## self.update_feedback("check reachability") ######## self.update_feedback("generate grasps") #transform pose to base_link self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(5)) trans_pose = self.tf_listener.transformPose("base_link", object_pose) object_pose = trans_pose #HACK remove orientation -> pose is aligned with parent(base_link) object_pose.pose.orientation.w = 1.0 object_pose.pose.orientation.x = 0.0 object_pose.pose.orientation.y = 0.0 object_pose.pose.orientation.z = 0.0 # shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects) # TODO remove this hack, fix it in table filtering object_pose.pose.position.z += obj_bbox_dims[2]/2.0 grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x # check if there are grasps, if not, abort if len(grasp_list) == 0: self.update_aborted("no grasps received") return self.publish_grasps_as_poses(grasp_list) self.feedback.grasps = grasp_list self.current_goal.publish_feedback(self.feedback) self.result.grasps = grasp_list ######## self.update_feedback("setup planning scene") #remove old objects self.scene.remove_world_object("object_to_grasp") # add object to grasp to planning scene self.scene.add_box("object_to_grasp", object_pose, (obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2])) self.result.object_scene_name = "object_to_grasp" ######## if self.current_goal.get_goal().execute_grasp: self.update_feedback("execute grasps") pug = self.createPickupGoal("object_to_grasp", grasp_list) rospy.loginfo("Sending goal") self.pickup_ac.send_goal(pug) rospy.loginfo("Waiting for result") self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() rospy.loginfo("Result is:") print result rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val])) ######## self.update_feedback("finished") self.result.object_pose = object_pose #bounding box in a point message self.result.bounding_box = Point() self.result.bounding_box.x = obj_bbox_dims[0] self.result.bounding_box.y = obj_bbox_dims[1] self.result.bounding_box.z = obj_bbox_dims[2] self.current_goal.set_succeeded(result = self.result) #self.current_goal.set_aborted() def update_feedback(self, text): self.feedback.last_state = text self.current_goal.publish_feedback(self.feedback) def update_aborted(self, text = ""): self.update_feedback("aborted." + text) self.current_goal.set_aborted() def generate_grasps(self, pose, width): #send request to block grasp generator service if not self.grasps_ac.wait_for_server(rospy.Duration(3.0)): return [] rospy.loginfo("Successfully connected.") goal = GenerateBlockGraspsGoal() goal.pose = pose.pose goal.width = width self.grasps_ac.send_goal(goal) rospy.loginfo("Sent goal, waiting:\n" + str(goal)) t_start = rospy.Time.now() self.grasps_ac.wait_for_result() t_end = rospy.Time.now() t_total = t_end - t_start rospy.loginfo("Result received in " + str(t_total.to_sec())) grasp_list = self.grasps_ac.get_result().grasps return grasp_list def publish_grasps_as_poses(self, grasps): rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose") graspmsg = Grasp() grasp_PA = PoseArray() header = Header() header.frame_id = "base_link" header.stamp = rospy.Time.now() grasp_PA.header = header for graspmsg in grasps: p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation) grasp_PA.poses.append(p) self.grasp_publisher.publish(grasp_PA) rospy.sleep(0.1) def createPickupGoal(self, target, possible_grasps, group="right_arm_torso_grasping"): """ Create a PickupGoal with the provided data""" pug = PickupGoal() pug.target_name = target pug.group_name = group pug.possible_grasps.extend(possible_grasps) pug.allowed_planning_time = 5.0 pug.planning_options.planning_scene_diff.is_diff = True pug.planning_options.planning_scene_diff.robot_state.is_diff = True pug.planning_options.plan_only = False pug.planning_options.replan = True pug.planning_options.replan_attempts = 10 pug.attached_object_touch_links = ['arm_right_5_link', "arm_right_grasping_frame"] pug.allowed_touch_objects.append(target) #pug.attached_object_touch_links.append('all') return pug
class SimpleActionServer: ## @brief Constructor for a SimpleActionServer ## @param name A name for the action server ## @param execute_cb Optional callback that gets called in a separate thread whenever ## a new goal is received, allowing users to have blocking callbacks. ## Adding an execute callback also deactivates the goalCallback. ## @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. def __init__(self, name, ActionSpec, execute_cb = None, auto_start = True): self.new_goal = False self.preempt_request = False self.new_goal_preempt_request = False self.execute_callback = execute_cb; self.goal_callback = None; self.preempt_callback = None; self.need_to_terminate = False self.terminate_mutex = threading.RLock(); # since the internal_goal/preempt_callbacks are invoked from the # ActionServer while holding the self.action_server.lock # self.lock must always be locked after the action server lock # to avoid an inconsistent lock acquisition order self.lock = threading.RLock(); self.execute_condition = threading.Condition(self.lock); self.current_goal = ServerGoalHandle(); self.next_goal = ServerGoalHandle(); if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop); self.execute_thread.start(); else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback,self.internal_preempt_callback,auto_start); def __del__(self): if hasattr(self, 'execute_callback') and self.execute_callback: with self.terminate_mutex: self.need_to_terminate = True; assert(self.execute_thread); self.execute_thread.join(); ## @brief Accepts a new goal when one is available The status of this ## goal is set to active upon acceptance, and the status of any ## previously active goal is set to preempted. Preempts received for the ## new goal between checking if isNewGoalAvailable or invokation of a ## goal callback and the acceptNewGoal call will not trigger a preempt ## callback. This means, isPreemptReqauested should be called after ## accepting the goal even for callback-based implementations to make ## sure the new goal does not have a pending preempt request. ## @return A shared_ptr to the new goal. def accept_new_goal(self): with self.action_server.lock, self.lock: if not self.new_goal or not self.next_goal.get_goal(): rospy.logerr("Attempting to accept the next goal when a new goal is not available"); return None; #check if we need to send a preempted message for the goal that we're currently pursuing if self.is_active() and self.current_goal.get_goal() and self.current_goal != self.next_goal: self.current_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server"); rospy.logdebug("Accepting a new goal"); #accept the next goal self.current_goal = self.next_goal; self.new_goal = False; #set preempt to request to equal the preempt state of the new goal self.preempt_request = self.new_goal_preempt_request; self.new_goal_preempt_request = False; #set the status of the current goal to be active self.current_goal.set_accepted("This goal has been accepted by the simple action server"); return self.current_goal.get_goal(); ## @brief Allows polling implementations to query about the availability of a new goal ## @return True if a new goal is available, false otherwise def is_new_goal_available(self): return self.new_goal; ## @brief Allows polling implementations to query about preempt requests ## @return True if a preempt is requested, false otherwise def is_preempt_requested(self): return self.preempt_request; ## @brief Allows polling implementations to query about the status of the current goal ## @return True if a goal is active, false otherwise def is_active(self): if not self.current_goal.get_goal(): return False; status = self.current_goal.get_goal_status().status; return status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING; ## @brief Sets the status of the active goal to succeeded ## @param result An optional result to send back to any clients of the goal def set_succeeded(self,result=None, text=""): with self.action_server.lock, self.lock: if not result: result=self.get_default_result(); self.current_goal.set_succeeded(result, text); ## @brief Sets the status of the active goal to aborted ## @param result An optional result to send back to any clients of the goal def set_aborted(self, result = None, text=""): with self.action_server.lock, self.lock: if not result: result=self.get_default_result(); self.current_goal.set_aborted(result, text); ## @brief Publishes feedback for a given goal ## @param feedback Shared pointer to the feedback to publish def publish_feedback(self,feedback): self.current_goal.publish_feedback(feedback); def get_default_result(self): return self.action_server.ActionResultType(); ## @brief Sets the status of the active goal to preempted ## @param result An optional result to send back to any clients of the goal def set_preempted(self,result=None, text=""): if not result: result=self.get_default_result(); with self.action_server.lock, self.lock: rospy.logdebug("Setting the current goal as canceled"); self.current_goal.set_canceled(result, text); ## @brief Allows users to register a callback to be invoked when a new goal is available ## @param cb The callback to be invoked def register_goal_callback(self,cb): if self.execute_callback: rospy.logwarn("Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it."); else: self.goal_callback = cb; ## @brief Allows users to register a callback to be invoked when a new preempt request is available ## @param cb The callback to be invoked def register_preempt_callback(self, cb): self.preempt_callback = cb; ## @brief Explicitly start the action server, used it auto_start is set to false def start(self): self.action_server.start(); ## @brief Callback for when the ActionServer receives a new goal and passes it on def internal_goal_callback(self, goal): self.execute_condition.acquire(); try: rospy.logdebug("A new goal %shas been recieved by the single goal action server",goal.get_goal_id().id); #check that the timestamp is past that of the current goal and the next goal if((not self.current_goal.get_goal() or goal.get_goal_id().stamp >= self.current_goal.get_goal_id().stamp) and (not self.next_goal.get_goal() or goal.get_goal_id().stamp >= self.next_goal.get_goal_id().stamp)): #if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting if(self.next_goal.get_goal() and (not self.current_goal.get_goal() or self.next_goal != self.current_goal)): self.next_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server"); self.next_goal = goal; self.new_goal = True; self.new_goal_preempt_request = False; #if the server is active, we'll want to call the preempt callback for the current goal if(self.is_active()): self.preempt_request = True; #if the user has registered a preempt callback, we'll call it now if(self.preempt_callback): self.preempt_callback(); #if the user has defined a goal callback, we'll call it now if self.goal_callback: self.goal_callback(); #Trigger runLoop to call execute() self.execute_condition.notify(); self.execute_condition.release(); else: #the goal requested has already been preempted by a different goal, so we're not going to execute it goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server"); self.execute_condition.release(); except Exception, e: rospy.logerr("SimpleActionServer.internal_goal_callback - exception %s",str(e)) self.execute_condition.release();
class MultiGoalActionServer(): def __init__(self, name, ActionSpec, auto_start=True): self.preempt_request = False self.new_goal_preempt_request = False self.goal_callback = None self.preempt_callback = None self.goal_handle_lock = threading.RLock() self.goal_handles = {} self.preempt_requests = {} self.active_goals_pub = rospy.Publisher(name + "/active_goals", GoalList, queue_size=10) self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start); def start(self): self.action_server.start() ## @brief Allows users to register a callback to be invoked when a new goal is available ## @param cb The callback to be invoked def register_goal_callback(self, goal_callback): self.goal_callback = goal_callback ## @brief Allows users to register a callback to be invoked when a new preempt request is available ## @param cb The callback to be invoked def register_preempt_callback(self, preempt_callback): self.preempt_callback = preempt_callback ## @brief Allows polling implementations to query about preempt requests ## @return True if a preempt is requested, false otherwise def is_preempt_requested(self, goal_handle): goal_id = goal_handle.get_goal_id().id return goal_id in self.preempt_requests ## @brief Allows polling implementations to query about the status of the current goal ## @return True if a goal is active, false otherwise def is_active(self, goal_handle): goal_id = goal_handle.get_goal_id().id if goal_id not in self.goal_handles: return False status = goal_handle.get_goal_status().status return status == GoalStatus.ACTIVE or status == GoalStatus.PREEMPTING def get_goal_handle(self, goal_handle): with self.goal_handle_lock: goal_id = goal_handle.get_goal_id().id if goal_id in self.goal_handles: return self.goal_handles[goal_id] return None def add_goal_handle(self, goal_handle): with self.goal_handle_lock: self.goal_handles[goal_handle.get_goal_id().id] = goal_handle def remove_goal_handle(self, goal_handle): with self.goal_handle_lock: goal_id = goal_handle.get_goal_id().id if goal_id in self.goal_handles: self.goal_handles.pop(goal_id) if goal_id in self.preempt_requests: self.preempt_requests.pop(goal_id) def publish_active_goals(self): with self.goal_handle_lock: active_goals = GoalList() print(str(self.goal_handles)) for key, goal_handle in self.goal_handles.iteritems(): active_goals.goal_list.append(goal_handle.get_goal_id()) self.active_goals_pub.publish(active_goals) ## @brief Callback for when the MultiGoalActionServer receives a new goal and passes it on def internal_goal_callback(self, goal_handle): rospy.loginfo("Goal received: %s", str(goal_handle.get_goal_id().id)) with self.goal_handle_lock: if self.goal_callback: self.set_accepted(goal_handle) self.goal_callback(goal_handle) else: raise Exception('goal_callback not registered') ## @brief Sets the status of the active goal to preempted ## @param result An optional result to send back to any clients of the goal def set_preempted(self, goal_handle, result=None, text=""): if not result: result = self.get_default_result() with self.goal_handle_lock: rospy.logdebug("Setting the current goal as canceled"); goal_handle.set_canceled(result, text) self.remove_goal_handle(goal_handle) ## @brief Callback for when the ActionServer receives a new preempt and passes it on def internal_preempt_callback(self, goal_handle): rospy.loginfo("Preempt received: %s", str(id(goal_handle))) with self.goal_handle_lock: goal_id = goal_handle.get_goal_id().id self.preempt_requests[goal_id] = True if self.preempt_callback: self.preempt_callback(goal_handle) ## @brief Accepts a new goal when one is available def set_accepted(self, goal_handle): with self.goal_handle_lock: rospy.loginfo("Accepting a new goal: %s", goal_handle) self.add_goal_handle(goal_handle) goal_handle.set_accepted("This goal has been accepted by the multi goal action server") self.publish_active_goals() ## @brief Sets the status of the active goal to succeeded ## @param result An optional result to send back to any clients of the goal def set_succeeded(self, goal_handle, result=None, text=""): with self.goal_handle_lock: if not result: result = self.get_default_result() goal_handle.set_succeeded(result, text) self.remove_goal_handle(goal_handle) self.publish_active_goals() ## @brief Sets the status of the active goal to aborted ## @param result An optional result to send back to any clients of the goal def set_aborted(self, goal_handle, result=None, text=""): with self.goal_handle_lock: if not result: result = self.get_default_result() goal_handle.set_aborted(result, text) self.remove_goal_handle(goal_handle) self.publish_active_goals() ## @brief Publishes feedback for a given goal ## @param feedback Shared pointer to the feedback to publish def publish_feedback(self, goal_handle, feedback): with self.goal_handle_lock: goal_handle.publish_feedback(feedback) def get_default_result(self): return self.action_server.ActionResultType();
class ComplexActionServer: ## @brief Constructor for a ComplexActionServer ## @param name A name for the action server ## @param execute_cb Optional callback that gets called in a separate thread whenever ## a new goal is received, allowing users to have blocking callbacks. ## Adding an execute callback also deactivates the goalCallback. ## @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. def __init__(self, name, ActionSpec, execute_cb = None, auto_start = True): self.goals_received_ = 0; self.goal_queue_ = six.moves.queue.Queue() self.new_goal = False self.execute_callback = execute_cb; self.goal_callback = None; self.need_to_terminate = False self.terminate_mutex = threading.RLock(); # since the internal_goal/preempt_callbacks are invoked from the # ActionServer while holding the self.action_server.lock # self.lock must always be locked after the action server lock # to avoid an inconsistent lock acquisition order self.lock = threading.RLock(); self.execute_condition = threading.Condition(self.lock); self.current_goal = ServerGoalHandle(); self.next_goal = ServerGoalHandle(); if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop); self.execute_thread.start(); else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback,self.internal_preempt_callback,auto_start); def __del__(self): if hasattr(self, 'execute_callback') and self.execute_callback: with self.terminate_mutex: self.need_to_terminate = True; assert(self.execute_thread); self.execute_thread.join(); ## @brief Accepts a new goal when one is available The status of this ## goal is set to active upon acceptance, def accept_new_goal(self): with self.action_server.lock, self.lock: rospy.logdebug("Accepting a new goal"); self.goals_received_ -= 1; #get from queue current_goal = self.goal_queue_.get() #set the status of the current goal to be active current_goal.set_accepted("This goal has been accepted by the simple action server"); return current_goal#.get_goal(); ## @brief Allows polling implementations to query about the availability of a new goal ## @return True if a new goal is available, false otherwise def is_new_goal_available(self): return self.goals_received_ > 0 ## @brief Allows polling implementations to query about the status of the current goal ## @return True if a goal is active, false otherwise def is_active(self): if not self.current_goal.get_goal(): return False; status = self.current_goal.get_goal_status().status; return status == actionlib_msgs.msg.GoalStatus.ACTIVE #or status == actionlib_msgs.msg.GoalStatus.PREEMPTING; ## @brief Sets the status of the active goal to succeeded ## @param result An optional result to send back to any clients of the goal def set_succeeded(self,result=None, text="", goal_handle=None): with self.action_server.lock, self.lock: if not result: result=self.get_default_result(); #self.current_goal.set_succeeded(result, text); goal_handle.set_succeeded(result,text) ## @brief Sets the status of the active goal to aborted ## @param result An optional result to send back to any clients of the goal def set_aborted(self, result = None, text="" , goal_handle=None): with self.action_server.lock, self.lock: if not result: result=self.get_default_result(); #self.current_goal.set_aborted(result, text); goal_handle.set_aborted(result,text) ## @brief Publishes feedback for a given goal ## @param feedback Shared pointer to the feedback to publish def publish_feedback(self,feedback): self.current_goal.publish_feedback(feedback); def get_default_result(self): return self.action_server.ActionResultType(); ## @brief Allows users to register a callback to be invoked when a new goal is available ## @param cb The callback to be invoked def register_goal_callback(self,cb): if self.execute_callback: rospy.logwarn("Cannot call ComplexActionServer.register_goal_callback() because an executeCallback exists. Not going to register it."); else: self.goal_callback = cb; ## @brief Explicitly start the action server, used it auto_start is set to false def start(self): self.action_server.start(); ## @brief Callback for when the ActionServer receives a new goal and passes it on def internal_goal_callback(self, goal): self.execute_condition.acquire(); try: rospy.logdebug("A new goal %shas been recieved by the single goal action server",goal.get_goal_id().id); print("got a goal") self.next_goal = goal; self.new_goal = True; self.goals_received_ += 1 #add goal to queue self.goal_queue_.put(goal) #Trigger runLoop to call execute() self.execute_condition.notify(); self.execute_condition.release(); except Exception as e: rospy.logerr("ComplexActionServer.internal_goal_callback - exception %s",str(e)) self.execute_condition.release(); ## @brief Callback for when the ActionServer receives a new preempt and passes it on def internal_preempt_callback(self,preempt): return ## @brief Called from a separate thread to call blocking execute calls def executeLoop(self): loop_duration = rospy.Duration.from_sec(.1); while (not rospy.is_shutdown()): rospy.logdebug("SAS: execute"); with self.terminate_mutex: if (self.need_to_terminate): break; if (self.is_new_goal_available()): # accept_new_goal() is performing its own locking goal_handle = self.accept_new_goal(); if not self.execute_callback: rospy.logerr("execute_callback_ must exist. This is a bug in ComplexActionServer"); return try: print("run new executecb") thread = threading.Thread(target=self.run_goal,args=(goal_handle.get_goal(),goal_handle)); thread.start() except Exception as ex: rospy.logerr("Exception in your execute callback: %s\n%s", str(ex), traceback.format_exc()) self.set_aborted(None, "Exception in execute callback: %s" % str(ex)) with self.execute_condition: self.execute_condition.wait(loop_duration.to_sec()); def run_goal(self,goal, goal_handle): print('new thread') self.execute_callback(goal,goal_handle);
class SimpleActionServer: ## @brief Constructor for a SimpleActionServer ## @param name A name for the action server ## @param execute_cb Optional callback that gets called in a separate thread whenever ## a new goal is received, allowing users to have blocking callbacks. ## Adding an execute callback also deactivates the goalCallback. ## @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True): self.new_goal = False self.preempt_request = False self.new_goal_preempt_request = False self.execute_callback = execute_cb self.goal_callback = None self.preempt_callback = None self.need_to_terminate = False self.terminate_mutex = threading.RLock() # since the internal_goal/preempt_callbacks are invoked from the # ActionServer while holding the self.action_server.lock # self.lock must always be locked after the action server lock # to avoid an inconsistent lock acquisition order self.lock = threading.RLock() self.execute_condition = threading.Condition(self.lock) self.current_goal = ServerGoalHandle() self.next_goal = ServerGoalHandle() if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop) self.execute_thread.start() else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start) def __del__(self): if hasattr(self, 'execute_callback') and self.execute_callback: with self.terminate_mutex: self.need_to_terminate = True assert (self.execute_thread) self.execute_thread.join() ## @brief Accepts a new goal when one is available The status of this ## goal is set to active upon acceptance, and the status of any ## previously active goal is set to preempted. Preempts received for the ## new goal between checking if isNewGoalAvailable or invokation of a ## goal callback and the acceptNewGoal call will not trigger a preempt ## callback. This means, isPreemptReqauested should be called after ## accepting the goal even for callback-based implementations to make ## sure the new goal does not have a pending preempt request. ## @return A shared_ptr to the new goal. def accept_new_goal(self): with self.action_server.lock, self.lock: if not self.new_goal or not self.next_goal.get_goal(): rospy.logerr( "Attempting to accept the next goal when a new goal is not available" ) return None # check if we need to send a preempted message for the goal that we're currently pursuing if self.is_active() and self.current_goal.get_goal( ) and self.current_goal != self.next_goal: self.current_goal.set_canceled( None, "This goal was canceled because another goal was received by the simple action server" ) rospy.logdebug("Accepting a new goal") # accept the next goal self.current_goal = self.next_goal self.new_goal = False # set preempt to request to equal the preempt state of the new goal self.preempt_request = self.new_goal_preempt_request self.new_goal_preempt_request = False # set the status of the current goal to be active self.current_goal.set_accepted( "This goal has been accepted by the simple action server") return self.current_goal.get_goal() ## @brief Allows polling implementations to query about the availability of a new goal ## @return True if a new goal is available, false otherwise def is_new_goal_available(self): return self.new_goal ## @brief Allows polling implementations to query about preempt requests ## @return True if a preempt is requested, false otherwise def is_preempt_requested(self): return self.preempt_request ## @brief Allows polling implementations to query about the status of the current goal ## @return True if a goal is active, false otherwise def is_active(self): if not self.current_goal.get_goal(): return False status = self.current_goal.get_goal_status().status return status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING ## @brief Sets the status of the active goal to succeeded ## @param result An optional result to send back to any clients of the goal def set_succeeded(self, result=None, text=""): with self.action_server.lock, self.lock: if not result: result = self.get_default_result() self.current_goal.set_succeeded(result, text) ## @brief Sets the status of the active goal to aborted ## @param result An optional result to send back to any clients of the goal def set_aborted(self, result=None, text=""): with self.action_server.lock, self.lock: if not result: result = self.get_default_result() self.current_goal.set_aborted(result, text) ## @brief Publishes feedback for a given goal ## @param feedback Shared pointer to the feedback to publish def publish_feedback(self, feedback): self.current_goal.publish_feedback(feedback) def get_default_result(self): return self.action_server.ActionResultType() ## @brief Sets the status of the active goal to preempted ## @param result An optional result to send back to any clients of the goal def set_preempted(self, result=None, text=""): if not result: result = self.get_default_result() with self.action_server.lock, self.lock: rospy.logdebug("Setting the current goal as canceled") self.current_goal.set_canceled(result, text) ## @brief Allows users to register a callback to be invoked when a new goal is available ## @param cb The callback to be invoked def register_goal_callback(self, cb): if self.execute_callback: rospy.logwarn( "Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it." ) else: self.goal_callback = cb ## @brief Allows users to register a callback to be invoked when a new preempt request is available ## @param cb The callback to be invoked def register_preempt_callback(self, cb): self.preempt_callback = cb ## @brief Explicitly start the action server, used it auto_start is set to false def start(self): self.action_server.start() ## @brief Callback for when the ActionServer receives a new goal and passes it on def internal_goal_callback(self, goal): self.execute_condition.acquire() try: rospy.logdebug( "A new goal %shas been recieved by the single goal action server", goal.get_goal_id().id) # check that the timestamp is past that of the current goal and the next goal if ((not self.current_goal.get_goal() or goal.get_goal_id().stamp >= self.current_goal.get_goal_id().stamp) and (not self.next_goal.get_goal() or goal.get_goal_id().stamp >= self.next_goal.get_goal_id().stamp)): # if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting if (self.next_goal.get_goal() and (not self.current_goal.get_goal() or self.next_goal != self.current_goal)): self.next_goal.set_canceled( None, "This goal was canceled because another goal was received by the simple action server" ) self.next_goal = goal self.new_goal = True self.new_goal_preempt_request = False # if the server is active, we'll want to call the preempt callback for the current goal if (self.is_active()): self.preempt_request = True # if the user has registered a preempt callback, we'll call it now if (self.preempt_callback): self.preempt_callback() # if the user has defined a goal callback, we'll call it now if self.goal_callback: self.goal_callback() # Trigger runLoop to call execute() self.execute_condition.notify() self.execute_condition.release() else: # the goal requested has already been preempted by a different goal, so we're not going to execute it goal.set_canceled( None, "This goal was canceled because another goal was received by the simple action server" ) self.execute_condition.release() except Exception as e: rospy.logerr( "SimpleActionServer.internal_goal_callback - exception %s", str(e)) self.execute_condition.release() ## @brief Callback for when the ActionServer receives a new preempt and passes it on def internal_preempt_callback(self, preempt): with self.lock: rospy.logdebug( "A preempt has been received by the SimpleActionServer") #if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback if (preempt == self.current_goal): rospy.logdebug( "Setting preempt_request bit for the current goal to TRUE and invoking callback" ) self.preempt_request = True #if the user has registered a preempt callback, we'll call it now if (self.preempt_callback): self.preempt_callback() #if the preempt applies to the next goal, we'll set the preempt bit for that elif (preempt == self.next_goal): rospy.logdebug( "Setting preempt request bit for the next goal to TRUE") self.new_goal_preempt_request = True ## @brief Called from a separate thread to call blocking execute calls def executeLoop(self): loop_duration = rospy.Duration.from_sec(.1) while (not rospy.is_shutdown()): with self.terminate_mutex: if (self.need_to_terminate): break # the following checks (is_active, is_new_goal_available) # are performed without locking # the worst thing that might happen in case of a race # condition is a warning/error message on the console if (self.is_active()): rospy.logerr( "Should never reach this code with an active goal") return if (self.is_new_goal_available()): # accept_new_goal() is performing its own locking goal = self.accept_new_goal() if not self.execute_callback: rospy.logerr( "execute_callback_ must exist. This is a bug in SimpleActionServer" ) return try: self.execute_callback(goal) if self.is_active(): rospy.logwarn( "Your executeCallback did not set the goal to a terminal status. " + "This is a bug in your ActionServer implementation. Fix your code! " + "For now, the ActionServer will set this goal to aborted" ) self.set_aborted(None, "No terminal state was set.") except Exception as ex: rospy.logerr("Exception in your execute callback: %s\n%s", str(ex), traceback.format_exc()) self.set_aborted( None, "Exception in execute callback: %s" % str(ex)) with self.execute_condition: self.execute_condition.wait(loop_duration.to_sec())
class QueuedActionServer: ## @brief Constructor for a QueuedActionServer ## @param name A name for the action server ## @param execute_cb Optional callback that gets called in a separate thread whenever ## a new goal is received, allowing users to have blocking callbacks. ## Adding an execute callback also deactivates the goalCallback. ## @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. def __init__(self, name, ActionSpec, execute_cb = None, auto_start = True): self.new_goal = False self.preempt_request = False self.new_goal_preempt_request = False self.maxlen = 5 #self.goals_buf = deque(maxlen=5) self.goals_buf = Queue.Queue(maxsize=self.maxlen) self.current_indexA = 0 self.current_indexP = 0 #self.maxlen = self.goals_buf.maxlen self.execute_callback = execute_cb self.goal_callback = None self.preempt_callback = None self.need_to_terminate = False self.terminate_mutex = threading.RLock() self.lock = threading.RLock() self.execute_condition = threading.Condition(self.lock) self.current_goal = ServerGoalHandle() self.next_goal = ServerGoalHandle() if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop) self.execute_thread.start() else: self.execute_thread = None #create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback,self.internal_preempt_callback,auto_start) def __del__(self): if hasattr(self, 'execute_callback') and self.execute_callback: with self.terminate_mutex: self.need_to_terminate = True assert(self.execute_thread) self.execute_thread.join() ## @brief Accepts a new goal when one is available The status of this ## goal is set to active upon acceptance, and the status of any ## previously active goal is set to preempted. Preempts received for the ## new goal between checking if isNewGoalAvailable or invokation of a ## goal callback and the acceptNewGoal call will not trigger a preempt ## callback. This means, isPreemptReqauested should be called after ## accepting the goal even for callback-based implementations to make ## sure the new goal does not have a pending preempt request. ## @return A shared_ptr to the new goal. def accept_new_goal(self): with self.lock: if not self.new_goal or not self.next_goal.get_goal(): rospy.logerr("Attempting to accept the next goal when a new goal is not available") return None rospy.logdebug("Accepting a new goal") #accept the next goal self.current_goal = self.next_goal self.new_goal = False #set preempt to request to equal the preempt state of the new goal self.preempt_request = self.new_goal_preempt_request self.new_goal_preempt_request = False #set the status of the current goal to be active self.current_goal.set_accepted("This goal has been accepted by the queued action server") return self.current_goal.get_goal() ## @brief Allows polling implementations to query about the availability of a new goal ## @return True if a new goal is available, false otherwise def is_new_goal_available(self): return self.new_goal ## @brief Allows polling implementations to query about preempt requests ## @return True if a preempt is requested, false otherwise def is_preempt_requested(self): return self.preempt_request ## @brief Allows polling implementations to query about the status of the current goal ## @return True if a goal is active, false otherwise def is_active(self): if not self.current_goal.get_goal(): return False status = self.current_goal.get_goal_status().status return status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING ## @brief Sets the status of the active goal to succeeded ## @param result An optional result to send back to any clients of the goal def set_succeeded(self,result=None, text=""): with self.lock: if not result: result=self.get_default_result() self.current_goal.set_succeeded(result, text) ## @brief Sets the status of the active goal to aborted ## @param result An optional result to send back to any clients of the goal def set_aborted(self, result = None, text=""): with self.lock: if not result: result=self.get_default_result() self.current_goal.set_aborted(result, text) ## @brief Publishes feedback for a given goal ## @param feedback Shared pointer to the feedback to publish def publish_feedback(self,feedback): self.current_goal.publish_feedback(feedback) def get_default_result(self): return self.action_server.ActionResultType() ## @brief Sets the status of the active goal to preempted ## @param result An optional result to send back to any clients of the goal def set_preempted(self,result=None, text=""): if not result: result=self.get_default_result() with self.lock: rospy.logdebug("Setting the current goal as canceled") self.current_goal.set_canceled(result, text) ## @brief Allows users to register a callback to be invoked when a new goal is available ## @param cb The callback to be invoked def register_goal_callback(self,cb): if self.execute_callback: rospy.logwarn("Cannot call QueuedActionServer.register_goal_callback() because an executeCallback exists. Not going to register it.") else: self.goal_callback = cb ## @brief Allows users to register a callback to be invoked when a new preempt request is available ## @param cb The callback to be invoked def register_preempt_callback(self, cb): self.preempt_callback = cb ## @brief Explicitly start the action server, used it auto_start is set to false def start(self): self.action_server.start() ## @brief Callback for when the ActionServer receives a new goal and passes it on def internal_goal_callback(self, goal): self.execute_condition.acquire() try: rospy.logdebug("A new goal %shas been recieved by the Queued goal action server",goal.get_goal_id().id) if(self.goals_buf.empty()): self.new_goal = True self.next_goal = goal self.goals_buf.put(goal, timeout=1) else: self.goals_buf.put(goal, timeout=1) rospy.loginfo("Queued New Goal") if self.goal_callback: self.goal_callback() #rospy.loginfo("Goals List-----------------------------------------------") #for item in self.goals_buf: # rospy.loginfo("Goals Buffer%s" %item.get_goal_status()) #rospy.loginfo("End of the Goals List-------------------------------------") #if the user has defined a goal callback, we'll call it now #Trigger runLoop to call execute() self.execute_condition.notify() self.execute_condition.release() except Exception, e: rospy.logerr("QueuedActionServer.internal_goal_callback - exception %s",str(e)) self.execute_condition.release()