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, 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()
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 internal_cancel_callback(self, goal_id):
with self.lock:
# if we're not started... then we're not actually going to do anything
if not self.started:
return
# we need to handle a cancel for the user
rospy.logdebug(
"The action server has received a new cancel request")
goal_id_found = False
for st in self.status_list[:]:
# check if the goal id is zero or if it is equal to the goal id of
# the iterator or if the time of the iterator warrants a cancel
cancel_everything = (
goal_id.id == "" and goal_id.stamp == rospy.Time()
) # rospy::Time()) #id and stamp 0 --> cancel everything
cancel_this_one = (goal_id.id == st.status.goal_id.id
) # ids match... cancel that goal
cancel_before_stamp = (
goal_id.stamp != rospy.Time()
and st.status.goal_id.stamp <= goal_id.stamp
) # //stamp != 0 --> cancel everything before stamp
if cancel_everything or cancel_this_one or cancel_before_stamp:
# we need to check if we need to store this cancel request for later
if goal_id.id == st.status.goal_id.id:
goal_id_found = True
# attempt to get the handle_tracker for the list item if it exists
handle_tracker = st.handle_tracker
if handle_tracker is None:
# if the handle tracker is expired, then we need to create a new one
handle_tracker = HandleTrackerDeleter(self, st)
st.handle_tracker = handle_tracker
# we also need to reset the time that the status is supposed to be removed from the list
st.handle_destruction_time = rospy.Time.now()
# set the status of the goal to PREEMPTING or RECALLING as approriate
# and check if the request should be passed on to the user
gh = ServerGoalHandle(st, self, handle_tracker)
if gh.set_cancel_requested():
# call the user's cancel callback on the relevant goal
self.cancel_callback(gh)
# if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
if goal_id.id != "" and not goal_id_found:
tracker = StatusTracker(
goal_id, actionlib_msgs.msg.GoalStatus.RECALLING)
self.status_list.append(tracker)
# start the timer for how long the status will live in the list without a goal handle to it
tracker.handle_destruction_time = rospy.Time.now()
# make sure to set last_cancel_ based on the stamp associated with this cancel request
if goal_id.stamp > self.last_cancel:
self.last_cancel = goal_id.stamp
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, 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 __init__(self, name):
ActionServer.__init__(self, name, DockAction, self.__goal_callback,
self.__cancel_callback, False)
self.__docked = False
self.__dock_speed = rospy.get_param('~dock/dock_speed', 0.05)
self.__dock_distance = rospy.get_param('~dock/dock_distance', 1.0)
self.__map_frame = rospy.get_param('~map_frame', 'map')
self.__odom_frame = rospy.get_param('~odom_frame', 'odom')
self.__base_frame = rospy.get_param('~base_frame', 'base_footprint')
self.__dock_ready_pose = Pose()
self.__dock_ready_pose.position.x = rospy.get_param('~dock/pose_x')
self.__dock_ready_pose.position.y = rospy.get_param('~dock/pose_y')
self.__dock_ready_pose.position.z = rospy.get_param('~dock/pose_z')
self.__dock_ready_pose.orientation.x = rospy.get_param(
'~dock/orientation_x')
self.__dock_ready_pose.orientation.y = rospy.get_param(
'~dock/orientation_y')
self.__dock_ready_pose.orientation.z = rospy.get_param(
'~dock/orientation_z')
self.__dock_ready_pose.orientation.w = rospy.get_param(
'~dock/orientation_w')
self.__dock_ready_pose_2 = PoseStamped()
rospy.loginfo('param: dock_spped, %s, dock_distance %s' %
(self.__dock_speed, self.__dock_distance))
rospy.loginfo('param: map_frame %s, odom_frame %s, base_frame %s' %
(self.__map_frame, self.__odom_frame, self.__base_frame))
rospy.loginfo('dock_pose:')
rospy.loginfo(self.__dock_ready_pose)
self.__movebase_client = SimpleActionClient('move_base',
MoveBaseAction)
rospy.loginfo('wait for movebase server...')
self.__movebase_client.wait_for_server()
rospy.loginfo('movebase server connected')
self.__cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rospy.Subscriber('dock_pose', PoseStamped, self.__dock_pose_callback)
self.__tf_listener = tf.TransformListener()
self.__docked = False
# self.__saved_goal = MoveBaseGoal()
self.__no_goal = True
self.__current_goal_handle = ServerGoalHandle()
self.__exec_condition = threading.Condition()
self.__exec_thread = threading.Thread(None, self.__exec_loop)
self.__exec_thread.start()
rospy.loginfo('Creating ActionServer [%s]\n', name)
def internal_cancel_callback(self, goal_id):
with self.lock:
#if we're not started... then we're not actually going to do anything
if not self.started:
return;
#we need to handle a cancel for the user
rospy.logdebug("The action server has received a new cancel request");
goal_id_found = False;
for st in self.status_list[:]:
#check if the goal id is zero or if it is equal to the goal id of
#the iterator or if the time of the iterator warrants a cancel
cancel_everything = (goal_id.id == "" and goal_id.stamp == rospy.Time() ) #rospy::Time()) #id and stamp 0 --> cancel everything
cancel_this_one = ( goal_id.id == st.status.goal_id.id) #ids match... cancel that goal
cancel_before_stamp = (goal_id.stamp != rospy.Time() and st.status.goal_id.stamp <= goal_id.stamp) #//stamp != 0 --> cancel everything before stamp
if cancel_everything or cancel_this_one or cancel_before_stamp:
#we need to check if we need to store this cancel request for later
if goal_id.id == st.status.goal_id.id:
goal_id_found = True;
#attempt to get the handle_tracker for the list item if it exists
handle_tracker = st.handle_tracker;
if handle_tracker is None:
#if the handle tracker is expired, then we need to create a new one
handle_tracker = HandleTrackerDeleter(self, st);
st.handle_tracker = handle_tracker;
#we also need to reset the time that the status is supposed to be removed from the list
st.handle_destruction_time = rospy.Time.now()
#set the status of the goal to PREEMPTING or RECALLING as approriate
#and check if the request should be passed on to the user
gh = ServerGoalHandle(st, self, handle_tracker);
if gh.set_cancel_requested():
#call the user's cancel callback on the relevant goal
self.cancel_callback(gh);
#if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
if goal_id.id != "" and not goal_id_found:
tracker= StatusTracker(goal_id, actionlib_msgs.msg.GoalStatus.RECALLING);
self.status_list.append(tracker)
#start the timer for how long the status will live in the list without a goal handle to it
tracker.handle_destruction_time = rospy.Time.now()
#make sure to set last_cancel_ based on the stamp associated with this cancel request
if goal_id.stamp > self.last_cancel:
self.last_cancel = goal_id.stamp;
def internal_goal_callback(self, goal):
with self.lock:
# if we're not started... then we're not actually going to do anything
if not self.started:
return
rospy.logdebug("The action server has received a new goal request")
# we need to check if this goal already lives in the status list
for st in self.status_list[:]:
if goal.goal_id.id == st.status.goal_id.id:
rospy.logdebug(
"Goal %s was already in the status list with status %i"
% (goal.goal_id.id, st.status.status))
# Goal could already be in recalling state if a cancel came in before the goal
if st.status.status == actionlib_msgs.msg.GoalStatus.RECALLING:
st.status.status = actionlib_msgs.msg.GoalStatus.RECALLED
self.publish_result(st.status, self.ActionResultType())
# if this is a request for a goal that has no active handles left,
# we'll bump how long it stays in the list
if st.handle_tracker is None:
st.handle_destruction_time = rospy.Time.now()
# make sure not to call any user callbacks or add duplicate status onto the list
return
# if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
st = StatusTracker(None, None, goal)
self.status_list.append(st)
# we need to create a handle tracker for the incoming goal and update the StatusTracker
handle_tracker = HandleTrackerDeleter(self, st)
st.handle_tracker = handle_tracker
# check if this goal has already been canceled based on its timestamp
gh = ServerGoalHandle(st, self, handle_tracker)
if goal.goal_id.stamp != rospy.Time(
) and goal.goal_id.stamp <= self.last_cancel:
# if it has... just create a GoalHandle for it and setCanceled
gh.set_canceled(
None,
"This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request"
)
else:
# now, we need to create a goal handle and call the user's callback
self.goal_callback(gh)
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)
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)
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)
def execute_cb(self, goal):
rospy.logdebug("Goal:\n" + str(goal))
result = TestRequestResult(goal.the_result, True)
if goal.pause_status > rospy.Duration(0.0):
rospy.loginfo("Locking the action server for %.3f seconds" %
goal.pause_status.to_sec())
status_continue_time = rospy.get_rostime() + goal.pause_status
# Takes the action server lock to prevent status from
# being published (simulates a freeze).
with self.action_server.lock:
while rospy.get_rostime() < status_continue_time:
time.sleep(0.02)
rospy.loginfo("Unlocking the action server")
terminate_time = rospy.get_rostime() + goal.delay_terminate
while rospy.get_rostime() < terminate_time:
time.sleep(0.02)
if not goal.ignore_cancel:
if self.is_preempt_requested():
self.set_preempted(result, goal.result_text)
return
rospy.logdebug("Terminating goal as: %i" % goal.terminate_status)
if goal.terminate_status == TestRequestGoal.TERMINATE_SUCCESS:
self.set_succeeded(result, goal.result_text)
elif goal.terminate_status == TestRequestGoal.TERMINATE_ABORTED:
self.set_aborted(result, goal.result_text)
elif goal.terminate_status == TestRequestGoal.TERMINATE_REJECTED:
rospy.logerr("Simple action server cannot reject goals")
self.set_aborted(None, "Simple action server cannot reject goals")
elif goal.terminate_status == TestRequestGoal.TERMINATE_DROP:
rospy.loginfo(
"About to drop the goal. This should produce an error message."
)
return
elif goal.terminate_status == TestRequestGoal.TERMINATE_EXCEPTION:
rospy.loginfo(
"About to throw an exception. This should produce an error message."
)
raise Exception("Terminating by throwing an exception")
elif goal.terminate_status == TestRequestGoal.TERMINATE_LOSE:
# Losing the goal requires messing about in the action server's innards
for i, s in enumerate(self.action_server.status_list):
if s.status.goal_id.id == self.current_goal.goal.goal_id.id:
del self.action_server.status_list[i]
break
self.current_goal = ServerGoalHandle()
else:
rospy.logerr("Don't know how to terminate as %d" %
goal.terminate_status)
self.set_aborted(
None,
"Don't know how to terminate as %d" % goal.terminate_status)
def internal_goal_callback(self, goal):
with self.lock:
#if we're not started... then we're not actually going to do anything
if not self.started:
return;
rospy.logdebug("The action server has received a new goal request");
#we need to check if this goal already lives in the status list
for st in self.status_list[:]:
if goal.goal_id.id == st.status.goal_id.id:
rospy.logdebug("Goal %s was already in the status list with status %i" % (goal.goal_id.id, st.status.status))
# Goal could already be in recalling state if a cancel came in before the goal
if st.status.status == actionlib_msgs.msg.GoalStatus.RECALLING:
st.status.status = actionlib_msgs.msg.GoalStatus.RECALLED
self.publish_result(st.status, self.ActionResultType())
#if this is a request for a goal that has no active handles left,
#we'll bump how long it stays in the list
if st.handle_tracker is None:
st.handle_destruction_time = rospy.Time.now()
#make sure not to call any user callbacks or add duplicate status onto the list
return;
#if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
st = StatusTracker(None,None,goal)
self.status_list.append(st);
#we need to create a handle tracker for the incoming goal and update the StatusTracker
handle_tracker = HandleTrackerDeleter(self, st);
st.handle_tracker = handle_tracker;
#check if this goal has already been canceled based on its timestamp
gh= ServerGoalHandle(st, self, handle_tracker);
if goal.goal_id.stamp != rospy.Time() and goal.goal_id.stamp <= self.last_cancel:
#if it has... just create a GoalHandle for it and setCanceled
gh.set_canceled(None, "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request");
else:
#now, we need to create a goal handle and call the user's callback
self.goal_callback(gh);
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))
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 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();
def create_goal(goal_id, secs, file_path):
gh = ServerGoalHandle(
StatusTracker(goal=SoundRequestActionGoal(
goal_id=GoalID(id=goal_id, stamp=rospy.Time(secs=secs)),
goal=SoundRequestGoal(stamp=rospy.Time(secs), file=file_path)), ))
return gh
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 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 DockActionServer(ActionServer):
def __init__(self, name):
ActionServer.__init__(self, name, DockAction, self.__goal_callback,
self.__cancel_callback, False)
self.__docked = False
self.__dock_speed = rospy.get_param('~dock/dock_speed', 0.05)
self.__dock_distance = rospy.get_param('~dock/dock_distance', 1.0)
self.__map_frame = rospy.get_param('~map_frame', 'map')
self.__odom_frame = rospy.get_param('~odom_frame', 'odom')
self.__base_frame = rospy.get_param('~base_frame', 'base_footprint')
self.__dock_ready_pose = Pose()
self.__dock_ready_pose.position.x = rospy.get_param('~dock/pose_x')
self.__dock_ready_pose.position.y = rospy.get_param('~dock/pose_y')
self.__dock_ready_pose.position.z = rospy.get_param('~dock/pose_z')
self.__dock_ready_pose.orientation.x = rospy.get_param(
'~dock/orientation_x')
self.__dock_ready_pose.orientation.y = rospy.get_param(
'~dock/orientation_y')
self.__dock_ready_pose.orientation.z = rospy.get_param(
'~dock/orientation_z')
self.__dock_ready_pose.orientation.w = rospy.get_param(
'~dock/orientation_w')
self.__dock_ready_pose_2 = PoseStamped()
rospy.loginfo('param: dock_spped, %s, dock_distance %s' %
(self.__dock_speed, self.__dock_distance))
rospy.loginfo('param: map_frame %s, odom_frame %s, base_frame %s' %
(self.__map_frame, self.__odom_frame, self.__base_frame))
rospy.loginfo('dock_pose:')
rospy.loginfo(self.__dock_ready_pose)
self.__movebase_client = SimpleActionClient('move_base',
MoveBaseAction)
rospy.loginfo('wait for movebase server...')
self.__movebase_client.wait_for_server()
rospy.loginfo('movebase server connected')
self.__cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rospy.Subscriber('dock_pose', PoseStamped, self.__dock_pose_callback)
self.__tf_listener = tf.TransformListener()
self.__docked = False
# self.__saved_goal = MoveBaseGoal()
self.__no_goal = True
self.__current_goal_handle = ServerGoalHandle()
self.__exec_condition = threading.Condition()
self.__exec_thread = threading.Thread(None, self.__exec_loop)
self.__exec_thread.start()
rospy.loginfo('Creating ActionServer [%s]\n', name)
def __del__(self):
self.__movebase_client.cancel_all_goals()
def __dock_pose_callback(self, data):
ps = PoseStamped()
# ps.header.stamp = rospy.Time.now()
ps.header.frame_id = 'dock'
ps.pose.position.x = -self.__dock_distance
try:
self.__dock_ready_pose_2 = self.__tf_listener.transformPose(
'map', ps)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
self.__dock_ready_pose_2.pose.position.z = -1.0
rospy.logwarn('tf error, %s' % e)
self.__dock_ready_pose_2.pose.position.z = 0.0
# rospy.loginfo('get dock pose')
def __goal_callback(self, gh):
rospy.loginfo('get new goal')
if not self.__no_goal:
gh.set_rejected(None, 'robot is busy, rejected')
rospy.logwarn('robot is busy, rejected')
return
self.__exec_condition.acquire()
self.__current_goal_handle = gh
self.__no_goal = False
self.__exec_condition.notify()
self.__exec_condition.release()
def __set_charge_relay(self, state):
pass
def __cancel_callback(self, gh):
self.__movebase_client.cancel_goal()
rospy.logwarn('cancel callback')
def __get_delta(self, pose, target):
if pose < 0:
pose = math.pi * 2 + pose
if target < 0:
target = math.pi * 2 + target
delta_a = target - pose
delta_b = math.pi * 2 - math.fabs(delta_a)
if delta_a > 0:
delta_b = delta_b * -1.0
if math.fabs(delta_a) < math.fabs(delta_b):
return delta_a
else:
return delta_b
def __rotate(self, delta):
try:
pose, quaternion = self.__tf_listener.lookupTransform(
'odom', self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
(roll, pitch, yaw) = euler_from_quaternion(quaternion)
target_yaw = yaw + delta
if target_yaw > math.pi:
target_yaw = target_yaw - (2.0 * math.pi)
elif target_yaw < -math.pi:
target_yaw = target_yaw + 2.0 * math.pi
rospy.loginfo('rotate %f to %f', delta, target_yaw)
cmd = Twist()
time = rospy.Time.now() + rospy.Duration(15)
while rospy.Time.now() < time and not rospy.is_shutdown(
) and math.fabs(self.__get_delta(yaw, target_yaw)) > 0.03:
try:
pose, quaternion = self.__tf_listener.lookupTransform(
'odom', self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
(roll, pitch, yaw) = euler_from_quaternion(quaternion)
a = self.__get_delta(yaw, target_yaw) * 0.4
if a > 0 and a < 0.08:
cmd.angular.z = 0.08
elif a < 0 and a > -0.08:
cmd.angular.z = -0.08
else:
cmd.angular.z = a
self.__cmd_pub.publish(cmd)
rospy.loginfo('rotate %f : %f, delta %f, speed %f, %f', target_yaw,
yaw, self.__get_delta(yaw,
target_yaw), a, cmd.angular.z)
return True
def __head_align(self):
cmd = Twist()
time = rospy.Time.now() + rospy.Duration(10)
while rospy.Time.now() < time:
try:
dock_pose, dock_quaternion = self.__tf_listener.lookupTransform(
self.__base_frame, 'dock', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
# when dock's x is close to zero, it means dock is just in the front of robot(base_footprint)
if dock_pose[1] < -0.002:
cmd.angular.z = -0.08
elif dock_pose[1] > 0.002:
cmd.angular.z = 0.08
else:
cmd.angular.z = 0.00
self.__cmd_pub.publish(cmd)
break
rospy.loginfo('algin %f, speed %f', dock_pose[1], cmd.angular.z)
self.__cmd_pub.publish(cmd)
self.__cmd_pub.publish(cmd)
rospy.Rate(0.5).sleep()
return True
def __moveto_dock(self):
cmd = Twist()
cmd.linear.x = -self.__dock_speed
ca_feedback = DockFeedback()
try:
last_pose, last_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
delta_distance = 0
while delta_distance < self.__dock_distance - 0.235 and not rospy.is_shutdown(
):
self.__cmd_pub.publish(cmd)
try:
current_pose, current_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
delta_distance = math.sqrt(
math.pow(current_pose[0] - last_pose[0], 2) +
math.pow(current_pose[1] - last_pose[1], 2) +
math.pow(current_pose[2] - last_pose[2], 2))
ca_feedback.dock_feedback = 'Moving to Dock, %fm left' % (
self.__dock_distance - delta_distance)
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('Moving to Dock, %fm left' %
(self.__dock_distance - delta_distance))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error aa')
rospy.Rate(20).sleep()
ca_feedback.dock_feedback = 'Stop on Dock'
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('stop robot')
# stop robot
cmd.linear.x = 0
self.__cmd_pub.publish(cmd)
# set charge relay on
self.__set_charge_relay(True)
return True
def __moveto_dock_ready(self):
# step 1
mb_goal = MoveBaseGoal()
mb_goal.target_pose.header.stamp = rospy.Time.now()
mb_goal.target_pose.header.frame_id = self.__map_frame
mb_goal.target_pose.header.seq = 1
mb_goal.target_pose.pose = self.__dock_ready_pose
self.__movebase_client.send_goal(mb_goal)
self.__movebase_client.wait_for_result()
rospy.loginfo('arrived dock_ready_pose')
rospy.Rate(2).sleep()
mb_goal = MoveBaseGoal()
mb_goal.target_pose.header.seq = 2
mb_goal.target_pose.header.stamp = rospy.Time.now()
mb_goal.target_pose.header.frame_id = self.__map_frame
if self.__dock_ready_pose_2.pose.position.z == -1.0:
rospy.logwarn('dock_ready_pose_2 failed')
return False
else:
rospy.loginfo('get dock ready pose 2 ()()')
t = self.__dock_ready_pose_2.pose
# t.position.z == 0.0
# t.position.x = -self.__dock_distance
mb_goal.target_pose.pose = t
rospy.loginfo('move to dock_ready_pose_2')
self.__movebase_client.cancel_all_goals()
self.__movebase_client.send_goal(mb_goal)
rospy.loginfo(self.__movebase_client.wait_for_result())
rospy.loginfo('arrived dock_ready_pose_2')
return True
def __dock(self):
if self.__moveto_dock_ready():
if self.__head_align():
if self.__rotate(math.pi):
if self.__moveto_dock():
self.__docked = True
return True
else:
rospy.logwarn("unable to move to dock")
else:
rospy.logwarn("unable to rotate 180")
else:
rospy.logwarn("unable to align head")
else:
rospy.logwarn("unable to move to dock ready")
self.__docked = False
return False
def __undock(self):
cmd = Twist()
cmd.linear.x = self.__dock_speed
ca_feedback = DockFeedback()
try:
last_pose, last_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
delta_distance = 0
while delta_distance < self.__dock_distance - 0.275 and not rospy.is_shutdown(
):
self.__cmd_pub.publish(cmd)
try:
current_pose, current_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
delta_distance = math.sqrt(
math.pow(current_pose[0] - last_pose[0], 2) +
math.pow(current_pose[1] - last_pose[1], 2) +
math.pow(current_pose[2] - last_pose[2], 2))
ca_feedback.dock_feedback = 'Undock, %fm left' % (
self.__dock_distance - delta_distance)
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('Undock, %fm left' %
(self.__dock_distance - delta_distance))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error aa')
rospy.Rate(20).sleep()
ca_feedback.dock_feedback = 'Stop on DockReady'
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('stop robot')
# stop robot
cmd.linear.x = 0.0
self.__cmd_pub.publish(cmd)
# set charge relay off
self.__set_charge_relay(False)
self.__docked = False
self.__current_goal_handle.set_succeeded(None, 'Undocked')
rospy.loginfo('UnDocked')
def __exec_loop(self):
rospy.loginfo('auto dock thread started')
while not rospy.is_shutdown():
with self.__exec_condition:
self.__exec_condition.wait(3)
if self.__no_goal:
continue
rospy.loginfo('processing new goal')
goal = self.__current_goal_handle.get_goal()
if goal.dock == True:
if self.__docked == True:
rospy.logwarn('rejected, robot has already docked')
self.__current_goal_handle.set_rejected(
None, 'already docked')
else:
rospy.loginfo('Docking')
self.__current_goal_handle.set_accepted('Docking')
self.__dock()
if self.__docked:
self.__current_goal_handle.set_succeeded(
None, 'Docked')
rospy.loginfo('Docked')
else:
self.__current_goal_handle.set_aborted(
None, 'Dock failed')
rospy.loginfo('Dock failed')
elif goal.dock == False:
if self.__docked == False:
rospy.logwarn('cancel_all_goals')
self.__movebase_client.cancel_all_goals()
rospy.logwarn('rejected, robot is not on charging')
self.__current_goal_handle.set_rejected(
None, 'robot is not on charging')
else:
rospy.loginfo('Start undock')
self.__current_goal_handle.set_accepted('Start undock')
self.__undock()
else:
rospy.logwarn(
'unknown dock data type, should be true or false')
# self.__current_goal_handle.set_succeeded(None, 'Docked')
rospy.loginfo('new goal finish')
self.__no_goal = True
rospy.loginfo('auto dock thread stop')
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 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);
