def __init__(self):
rospy.loginfo("Initializing UnifiedInterface...")
self.joints_to_control = []
self.non_controlled_joints = []
self.controllers = []
self.configure()
# Advertise ASs
self.ns = rospy.get_name()
self._as = ActionServer(self.ns + '/follow_joint_trajectory',
FollowJointTrajectoryAction,
goal_cb=self.exec_cb,
auto_start=False)
self._as.start()
self._as_simple = ActionServer(self.ns + '/joints_to_pose',
GoJointsToPositionAction,
goal_cb=self.simple_exec_cb,
auto_start=False)
# Advertise topics
self._sub = rospy.Subscriber(self.ns + '/command',
JointTrajectory,
self.topic_cb,
queue_size=1)
self._simple_sub = rospy.Subscriber(self.ns + '/joints_to_pose_topic',
JointsToPosition,
self.simple_topic_cb,
queue_size=1)
# Advertise incremental ASs
self._as_incr = ActionServer(self.ns +
'/incremental_follow_joint_trajectory',
FollowJointTrajectoryAction,
goal_cb=self.exec_incr_cb,
auto_start=False)
self._as_simple_incr = ActionServer(self.ns +
'/incremental_joints_to_pose',
GoJointsToPositionAction,
goal_cb=self.simple_exec_incr_cb,
auto_start=False)
# Advertise incremental topics
self._sub_incr = rospy.Subscriber(self.ns + '/incr_command',
JointTrajectory,
self.topic_cb,
queue_size=1)
self._simple_sub_incr = rospy.Subscriber(self.ns +
'/incr_joints_to_pose_topic',
JointsToPosition,
self.simple_topic_cb,
queue_size=1)
def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True):
self.new_goal = False
self.preempt_request = False
self.new_goal_preempt_request = False
self.execute_callback = execute_cb
self.goal_callback = None
self.preempt_callback = None
self.need_to_terminate = False
self.terminate_mutex = threading.RLock()
self.lock = threading.RLock()
self.execute_condition = threading.Condition(self.lock)
self.current_goal = ServerGoalHandle()
self.next_goal = ServerGoalHandle()
if self.execute_callback:
self.execute_thread = threading.Thread(None, self.executeLoop)
self.execute_thread.start()
else:
self.execute_thread = None
#create the action server
self.action_server = ActionServer(name, ActionSpec,
self.internal_goal_callback,
self.internal_preempt_callback,
auto_start)
def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True):
self.new_goal = False
self.preempt_request = False
self.new_goal_preempt_request = False
self.execute_callback = execute_cb
self.goal_callback = None
self.preempt_callback = None
self.need_to_terminate = False
self.terminate_mutex = threading.RLock()
# since the internal_goal/preempt_callbacks are invoked from the
# ActionServer while holding the self.action_server.lock
# self.lock must always be locked after the action server lock
# to avoid an inconsistent lock acquisition order
self.lock = threading.RLock()
self.execute_condition = threading.Condition(self.lock)
self.current_goal = ServerGoalHandle()
self.next_goal = ServerGoalHandle()
if self.execute_callback:
self.execute_thread = threading.Thread(None, self.executeLoop)
self.execute_thread.start()
else:
self.execute_thread = None
#create the action server
self.action_server = ActionServer(name, ActionSpec,
self.internal_goal_callback,
self.internal_preempt_callback,
auto_start)
def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True):
self.active_goals = GoalQueue()
self.new_goals = GoalQueue()
self.execute_callback = execute_cb
self.need_to_terminate = False
self.terminate_mutex = threading.RLock()
self.lock = threading.RLock()
self.execute_condition = threading.Condition(self.lock)
if self.execute_callback:
self.execute_thread = threading.Thread(None,
self.executeLoop,
name="ExecuterLoop")
self.execute_thread.start()
else:
self.execute_thread = None
#create the action server
self.action_server = ActionServer(name, ActionSpec,
self.internal_goal_callback,
self.internal_preempt_callback,
auto_start)
def create_server(self, ns, goal_cb, auto_start=True):
action_server = ActionServer(ns,
TestAction,
goal_cb=goal_cb,
auto_start=False)
if auto_start:
action_server.start()
return action_server
def __init__(self):
self.goals = []
self.action_server = ActionServer("multi",
TwoIntsAction,
self.goal_callback,
self.cancel_callback,
auto_start=False)
self.update_timer = rospy.Timer(rospy.Duration(0.1), self.update)
self.action_server.start()
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._as = ActionServer(name,
waitAction,
goal_cb=self.goal_cb,
cancel_cb=self.cancel_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Done.")
def __init__(self, name, action_spec, coro, loop=None):
""" Initialize an action server. Incoming goals will be processed via the speficied coroutine. """
self.name = name
self._loop = loop if loop is not None else asyncio.get_running_loop()
self._coro = coro
self._tasks = {}
self._server = ActionServer(
name, action_spec, goal_cb=self._goal_cb, cancel_cb=self._cancel_cb, auto_start=False)
self._server.start()
def __init__(self, name, topic, action_type, result_type):
self.name = name
self.result = result_type()
self.topic = topic
self.action_type = action_type
self.timeout = 5
Subscriber('mock/' + name, String, self.receive_commands)
self.server = ActionServer(self.topic, self.action_type, self.success,
False)
self.server.start()
loginfo('>>> Starting ' + self.name)
def __init__(self, name, ActionSpec, auto_start=True):
self.preempt_request = False
self.new_goal_preempt_request = False
self.goal_callback = None
self.preempt_callback = None
self.goal_handle_lock = threading.RLock()
self.goal_handles = {}
self.preempt_requests = {}
self.active_goals_pub = rospy.Publisher(name + "/active_goals", GoalList, queue_size=10)
self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start);
def __init__(self, name, topic):
self._name = name
self.action_result = None
self._feedback = move_base_msgs.msg.MoveBaseFeedback()
self._result = move_base_msgs.msg.MoveBaseResult()
self.abort_with_feedback = False
self._topic = topic
Subscriber('mock/feedback_' + name, String, self.receive_commands)
self._server = ActionServer(self._topic, MoveBaseAction, self.execute,
False)
self._server.start()
loginfo('>>> Starting ' + self._name + ' with feedback.')
def __init__(self, name, topic, action_type):
""" Creating a custom mock action server."""
self._topic = topic
self._name = name
self._action_type = action_type
self.timeout = 5
self.action_result = None
Subscriber('mock/' + name, String, self.receive_commands)
Subscriber('mock/gui_result', Bool, self.set_gui_result)
self._server = ActionServer(self._topic, self._action_type,
self.success, False)
self._server.start()
loginfo('>>> Starting ' + self._name)
def __init__(self, name):
# stuff for grasp planning
rospy.loginfo("Getting a TransformListener...")
self.tf_listener = tf.TransformListener()
rospy.loginfo("Getting a TransformBroadcaster...")
self.tf_broadcaster = tf.TransformBroadcaster()
rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_clusters = None
rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)
if DEBUG_MODE:
self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)
rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
self.grasps_ac.wait_for_server()
rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
self.depth_service.wait_for_service()
rospy.loginfo("Getting a PlanningSceneInterface instance...")
self.scene = PlanningSceneInterface()
# blocking action server
rospy.loginfo("Creating Action Server '" + name + "'...")
self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
self.as_feedback = ObjectManipulationFeedback()
self.as_result = ObjectManipulationActionResult()
self.current_goal = None
# Take care of left and right arm grasped stuff
self.right_hand_object = None
self.left_hand_object = None
self.current_side = 'right'
rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
self.grasp_obj_as.start()
def __init__(self,
namespace,
action_spec,
goal_start_fn=None,
goal_stop_fn=None):
self.goals = {}
self.goal_start_fn = goal_start_fn
self.goal_stop_fn = goal_stop_fn
self.goal_service = rospy.Service(namespace + '/get_goal_from_id',
GetTaskFromID, self.task_id_cb)
self.server = ActionServer(namespace,
action_spec,
self.receive_goal,
self.stop_fn,
auto_start=False)
self.server.start()
def __init__(self, name, actionMsg, execute_cb=None, auto_start=True, call_on_elected=False):
self.execute_callback = execute_cb
self.goal_callback = None
self.preempt_callback = None
self.call_on_elected = call_on_elected
self.queue = Queue()
self.lock = threading.Lock()
self.electionList = dict()
self.ghDict = dict()
# create the action server
self.action_server = ActionServer(name, actionMsg, self.get_next_gh, self.internal_preempt_callback, auto_start)
# create the scheduling daemon thread
if name+"/schedThread" not in [t.name for t in threading.enumerate()]:
workingThread = threading.Thread(name=name+"/schedThread", target=self.scheduling)
workingThread.setDaemon(True)
workingThread.start()
async def start(self):
""" Start the action server. """
self._server = ActionServer(
self.name,
self.action_spec,
auto_start=False,
# Make sure to run callbacks on the main thread
goal_cb=partial(self._loop.call_soon_threadsafe, self._goal_cb),
cancel_cb=partial(self._loop.call_soon_threadsafe,
self._cancel_cb),
)
try:
self._server.start()
await self._exception_monitor.start()
finally:
# TODO(pbovbel) depends on https://github.com/ros/actionlib/pull/142
self._server.stop()
self._server = None
def __init__(self, name, 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)
handle.set_canceled(result, " client cancel[zcb]")
isCancel = False
# elif isAborted:
# handle.set_aborted(result," server aborted[zcb]")
# isAborted = False
else:
handle.set_succeeded(result, " success [zcb]")
def goal_cb(handle):
t = threading.Thread(target=do_goal, args=(handle, ))
t.start()
def cancel_cb(handle):
global isCancel
rospy.loginfo("cancel cb")
isCancel = True
if __name__ == '__main__':
nodeName = "server_node"
rospy.init_node(nodeName)
actionName = "/zcb01/action"
server = ActionServer(actionName, CountNumberAction, goal_cb, cancel_cb,
False)
server.start()
rospy.spin()
