class ScanningService(object): '''Handle the motion of the rolling laser for taking a single 3D scan. ''' ## Instantiate the ScanningService class def __init__(self): ## scanning speed command publisher self.scanning_speed_pub = rospy.Publisher('/scanning_speed_cmd', Float64) ## laser center command publisher self.laser_center_pub = rospy.Publisher('/laser_center', Bool) ## robot status subscriber (to get the current speed of the laser) rospy.Subscriber('/robot_status', RobotStatusStamped, self.status_cb) ## scanning_once subscriber rospy.Subscriber('/scanning_once', Float64, self.scanning_once_cb) ## point cloud control publisher self.ptcld_ctrl_pub = rospy.Publisher('/pointcloud_control', Bool) ## scanning state self.scanning_state = ScanningFeedback.READY ## last goal time set, to avoid race condition self.last_goal_time = get_time() ## action server self.goal = ServerGoalHandle() self.action_server = actionlib.ActionServer('ScanningOnceAS', ScanningAction, self.goal_cb, auto_start=False) self.action_server.start() ## action client self.action_client = actionlib.SimpleActionClient('ScanningOnceAS', ScanningAction) self.last_angle = 0 self.last_direction = 0 rospy.Subscriber('/joint_states', JointState, self.joint_states_cb) # tf listener for laser scanner angle #self.tf_listener = tf.TransformListener() ## joint state callback to get the current position of the laser def joint_states_cb(self, joint_states): try: laser_idx = joint_states.name.index('laser_j') except ValueError: rospy.logwarn("NTH - laser joint angle not found in joint_state \ message.") angle = joint_states.position[laser_idx] if angle>self.last_angle: direction = 1 elif angle<self.last_angle: direction = -1 else: direction = 0 if self.scanning_speed*direction*self.last_direction < 0 and\ abs(angle)>pi/3: # TODO might be problematic # End of swipe event rospy.logdebug("NTH - End of swipe") if self.scanning_state == ScanningFeedback.WAITING_FOR_FIRST_SWIPE: # no end of swipe received before rospy.logdebug("NTH - Detected first end of swipe: waiting for a \ second one and activating point cloud publication.") self.ptcld_ctrl_pub.publish(True) self.scanning_state = ScanningFeedback.WAITING_FOR_COMPLETE_SWIPE self.goal.publish_feedback(ScanningFeedback(self.scanning_state)) elif self.scanning_state == ScanningFeedback.WAITING_FOR_COMPLETE_SWIPE: # first end of swipe received before rospy.logdebug("NTH - Detected second end of swipe: stopping laser and \ centering it.") self.scanning_state = ScanningFeedback.READY #self.scanning_speed_pub.publish(0.0) # may be unnecessary self.laser_center_pub.publish(True) self.goal.publish_feedback(ScanningFeedback(self.scanning_state)) self.goal.set_succeeded(ScanningResult(ScanningResult.SUCCESS), "Scan succeeded") rospy.loginfo("NTH - Scan ended") else: rospy.logdebug("NTH - end of swipe received and ignored.") self.last_direction = direction self.last_angle = angle ## robot status callback to get the current speed of the laser def status_cb(self, robot_status): self.scanning_speed = robot_status.scanning_speed if (self.scanning_speed == 0.0) and \ (get_time() - self.last_goal_time>0.5): # if not moving, updating state rospy.logdebug('NTH - Laser speed 0: setting state to "Not scanning".') if self.scanning_state != ScanningFeedback.READY: self.scanning_state = ScanningFeedback.READY self.goal.set_succeeded(ScanningResult(ScanningResult.ERROR), "Aborting as laser is not moving") self.goal.publish_feedback(ScanningFeedback(self.scanning_state)) def cancel_cb(self, goal): if goal==self.goal: self.stop_scanning() # Temporary ## callback when a goal is received def goal_cb(self, goal): if self.goal.get_goal() and \ self.goal.get_goal_status().status == GoalStatus.ACTIVE: goal.set_rejected(ScanningResult(ScanningResult.WARNING), "Can only do one scan at a time") return scanning_goal = goal.get_goal() if scanning_goal.action == ScanningGoal.START_SCANNING: if self.scanning_state == ScanningFeedback.READY and\ self.scanning_speed==0: self.goal = goal self.goal.set_accepted("Scan accepted") self.start_scanning(scanning_goal.speed) else: goal.set_rejected(ScanningResult(ScanningResult.ERROR), "Already scanning") elif scanning_goal.action == ScanningGoal.STOP_SCANNING: self.goal = goal self.goal.set_accepted("Stop accepted") self.stop_scanning() else: goal.set_rejected(ScanningResult(ScanningResult.ERROR), "Unknown action") def stop_scanning(self): rospy.loginfo("NTH - Stopping and centering laser") self.laser_center_pub.publish(True) if self.goal.get_goal().action==ScanningGoal.STOP_SCANNING: goal.set_succeeded(ScanningResult(ScanningResult.SUCCESS), "Success in stopping laser") else: goal.set_succeeded(ScanningResult(ScanningResult.WARNING), "Scan aborted") self.scanning_state = ScanningFeedback.READY goal.publish_feedback(ScanningFeedback(self.scanning_state)) ## forwarding scanning_once topic to a new goal def scanning_once_cb(self, speed): goal = ScanningGoal() goal.action = ScanningGoal.START_SCANNING goal.speed = speed.data self.action_client.send_goal(goal) def start_scanning(self, speed): # clip speed speed = max(min(speed, 0.1), 1.2) # send command rospy.loginfo("NTH - Starting scan") rospy.logdebug("NTH - Sending scanning speed command: %f and disabling \ publication of the messy point cloud."%speed) self.ptcld_ctrl_pub.publish(False) self.scanning_speed_pub.publish(speed) self.last_goal_time = get_time() # update state self.scanning_state = ScanningFeedback.WAITING_FOR_FIRST_SWIPE self.goal.publish_feedback(ScanningFeedback(self.scanning_state))
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 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 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 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 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 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 ScanningService(object): '''Handle the motion of the rolling laser for taking a single 3D scan. ''' ## Instantiate the ScanningService class def __init__(self): ## scanning speed command publisher self.scanning_speed_pub = rospy.Publisher('scanning_speed_cmd', Float64, queue_size=1) ## laser center command publisher self.laser_center_pub = rospy.Publisher('laser_center', Bool, queue_size=1) ## robot status subscriber (to get the current speed of the laser) rospy.Subscriber('robot_status', RobotStatusStamped, self.status_cb, queue_size=10) ## scanning_once subscriber rospy.Subscriber('scanning_once', Float64, self.scanning_once_cb, queue_size=1) ## point cloud control publisher self.ptcld_ctrl_pub = rospy.Publisher('pointcloud_control', Bool, queue_size=1) ## scanning state self.scanning_state = ScanningFeedback.READY ## last goal time set, to avoid race condition self.last_goal_time = get_time() ## action server self.goal = ServerGoalHandle() self.action_server = actionlib.ActionServer('ScanningOnceAS', ScanningAction, self.goal_cb, auto_start=False) self.action_server.start() ## action client self.action_client = actionlib.SimpleActionClient( 'ScanningOnceAS', ScanningAction) self.last_angle = 0 self.last_direction = 0 rospy.Subscriber('joint_states', JointState, self.joint_states_cb, queue_size=10) # tf listener for laser scanner angle #self.tf_listener = tf.TransformListener() ## joint state callback to get the current position of the laser def joint_states_cb(self, joint_states): if not "laser_j" in joint_states.name: return laser_idx = joint_states.name.index('laser_j') angle = joint_states.position[laser_idx] if angle > self.last_angle: direction = 1 elif angle < self.last_angle: direction = -1 else: direction = 0 if self.scanning_speed*direction*self.last_direction < 0 and\ abs(angle)>pi/3: # TODO might be problematic # End of swipe event rospy.logdebug("NTH - End of swipe") if self.scanning_state == ScanningFeedback.WAITING_FOR_FIRST_SWIPE: # no end of swipe received before rospy.logdebug( "NTH - Detected first end of swipe: waiting for a \ second one and activating point cloud publication.") self.ptcld_ctrl_pub.publish(True) self.scanning_state = ScanningFeedback.WAITING_FOR_COMPLETE_SWIPE self.goal.publish_feedback( ScanningFeedback(self.scanning_state)) elif self.scanning_state == ScanningFeedback.WAITING_FOR_COMPLETE_SWIPE: # first end of swipe received before rospy.logdebug( "NTH - Detected second end of swipe: stopping laser and \ centering it.") self.scanning_state = ScanningFeedback.READY #self.scanning_speed_pub.publish(0.0) # may be unnecessary self.laser_center_pub.publish(True) self.goal.publish_feedback( ScanningFeedback(self.scanning_state)) self.goal.set_succeeded(ScanningResult(ScanningResult.SUCCESS), "Scan succeeded") rospy.loginfo("NTH - Scan ended") else: rospy.logdebug("NTH - end of swipe received and ignored.") self.last_direction = direction self.last_angle = angle ## robot status callback to get the current speed of the laser def status_cb(self, robot_status): self.scanning_speed = robot_status.scanning_speed if (self.scanning_speed == 0.0) and \ (get_time() - self.last_goal_time>0.5): # if not moving, updating state rospy.logdebug( 'NTH - Laser speed 0: setting state to "Not scanning".') if self.scanning_state != ScanningFeedback.READY: self.scanning_state = ScanningFeedback.READY self.goal.set_succeeded(ScanningResult(ScanningResult.ERROR), "Aborting as laser is not moving") self.goal.publish_feedback( ScanningFeedback(self.scanning_state)) def cancel_cb(self, goal): if goal == self.goal: self.stop_scanning() # Temporary ## callback when a goal is received def goal_cb(self, goal): if self.goal.get_goal() and \ self.goal.get_goal_status().status == GoalStatus.ACTIVE: goal.set_rejected(ScanningResult(ScanningResult.WARNING), "Can only do one scan at a time") return scanning_goal = goal.get_goal() if scanning_goal.action == ScanningGoal.START_SCANNING: if self.scanning_state == ScanningFeedback.READY and\ self.scanning_speed==0: self.goal = goal self.goal.set_accepted("Scan accepted") self.start_scanning(scanning_goal.speed) else: goal.set_rejected(ScanningResult(ScanningResult.ERROR), "Already scanning") elif scanning_goal.action == ScanningGoal.STOP_SCANNING: self.goal = goal self.goal.set_accepted("Stop accepted") self.stop_scanning() else: goal.set_rejected(ScanningResult(ScanningResult.ERROR), "Unknown action") def stop_scanning(self): rospy.loginfo("NTH - Stopping and centering laser") self.laser_center_pub.publish(True) if self.goal.get_goal().action == ScanningGoal.STOP_SCANNING: goal.set_succeeded(ScanningResult(ScanningResult.SUCCESS), "Success in stopping laser") else: goal.set_succeeded(ScanningResult(ScanningResult.WARNING), "Scan aborted") self.scanning_state = ScanningFeedback.READY goal.publish_feedback(ScanningFeedback(self.scanning_state)) ## forwarding scanning_once topic to a new goal def scanning_once_cb(self, speed): goal = ScanningGoal() goal.action = ScanningGoal.START_SCANNING goal.speed = speed.data self.action_client.send_goal(goal) def start_scanning(self, speed): # clip speed speed = max(min(speed, 0.1), 1.2) # send command rospy.loginfo("NTH - Starting scan") rospy.logdebug( "NTH - Sending scanning speed command: %f and disabling \ publication of the messy point cloud." % speed) self.ptcld_ctrl_pub.publish(False) self.scanning_speed_pub.publish(speed) self.last_goal_time = get_time() # update state self.scanning_state = ScanningFeedback.WAITING_FOR_FIRST_SWIPE self.goal.publish_feedback(ScanningFeedback(self.scanning_state))