def test_abort(self):
client = ActionClient('reference_action', TestAction)
self.assert_(client.wait_for_server(rospy.Duration(2.0)),
'Could not connect to the action server')
goal_work = TestGoal(4)
goal_abort = TestGoal(6)
goal_feedback = TestGoal(8)
g1=client.send_goal(goal_work)
g2=client.send_goal(goal_work)
g3=client.send_goal(goal_work)
g4=client.send_goal(goal_work)
rospy.sleep(0.5);
self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE) #,"Should be active")
self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE,"Shoule be active")
self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
g5=client.send_goal(goal_abort)
rospy.sleep(0.5);
self.assertEqual(g5.get_goal_status(),GoalStatus.SUCCEEDED,"Should be done")
self.assertEqual(g1.get_goal_status(),GoalStatus.ABORTED,"Should be aborted")
self.assertEqual(g2.get_goal_status(),GoalStatus.ABORTED,"Should be aborted")
self.assertEqual(g3.get_goal_status(),GoalStatus.ABORTED,"Shoule be aborted")
self.assertEqual(g4.get_goal_status(),GoalStatus.ABORTED,"Should be aborted")
def test_abort(self):
client = ActionClient('reference_action', TestAction)
self.assert_(client.wait_for_server(rospy.Duration(2.0)),
'Could not connect to the action server')
goal_work = TestGoal(4)
goal_abort = TestGoal(6)
goal_feedback = TestGoal(8)
g1=client.send_goal(goal_work)
g2=client.send_goal(goal_work)
g3=client.send_goal(goal_work)
g4=client.send_goal(goal_work)
rospy.sleep(0.5);
self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE) #,"Should be active")
self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE,"Shoule be active")
self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
g5=client.send_goal(goal_abort)
rospy.sleep(0.5);
self.assertEqual(g5.get_goal_status(),GoalStatus.SUCCEEDED,"Should be done")
self.assertEqual(g1.get_goal_status(),GoalStatus.ABORTED,"Should be aborted")
self.assertEqual(g2.get_goal_status(),GoalStatus.ABORTED,"Should be aborted")
self.assertEqual(g3.get_goal_status(),GoalStatus.ABORTED,"Shoule be aborted")
self.assertEqual(g4.get_goal_status(),GoalStatus.ABORTED,"Should be aborted")
async def test_goal_canceled_from_server(self):
queue = janus.Queue()
async def goal_coro(goal_handle):
queue.sync_q.put_nowait(goal_handle)
try:
goal_handle.set_accepted()
await asyncio.sleep(1000000)
except asyncio.CancelledError:
goal_handle.set_canceled()
raise
goal_handle.set_succeeded()
client = SyncActionClient("test_goal_canceled_from_server", TestAction)
server = AsyncActionServer(client.ns, TestAction, coro=goal_coro)
server_task = asyncio.ensure_future(server.start())
await asyncio.get_event_loop().run_in_executor(None, client.wait_for_server)
goal_handle = client.send_goal(TestGoal())
await self.wait_for_status(goal_handle, GoalStatus.ACTIVE)
server_goal_handle = await queue.async_q.get()
await server.cancel(server_goal_handle)
await self.wait_for_status(goal_handle, GoalStatus.PREEMPTED)
self.assertEquals(goal_handle.get_goal_status(), GoalStatus.PREEMPTED)
server_task.cancel()
await deflector_shield(server_task)
class MultipleActionClient(object):
def __init__(self):
# TODO(lucasw) make an object hold the goal and goal handle and anything else needed
self.goal_handles = []
self.action_client = ActionClient("multi", TwoIntsAction)
for i in range(4):
goal = TwoIntsGoal()
gh = self.action_client.send_goal(goal, self.handle_transition,
self.handle_feedback)
self.goal_handles.append(gh)
rospy.sleep(1.0)
# self.update_timer = rospy.Timer(rospy.Duration(0.1), self.update)
# self.action_client.start()
def handle_transition(self, gh):
rospy.loginfo('transition {}'.format(gh))
def handle_feedback(self, gh, feedback):
rospy.loginfo('feedback {} {}'.format(gh, feedback))
def update(self, event):
for goal in self.goals:
pass
class FollowJointTrajectoryClient(object):
def __init__(self, ns):
""" Constructs a client that executes JointTrajectory messages.
@param ns: namespace for the FollowJointTrajectoryAction server
@type ns: str
"""
from actionlib import ActionClient
from control_msgs.msg import FollowJointTrajectoryAction
self._client = ActionClient(ns, FollowJointTrajectoryAction)
self._client.wait_for_server()
def execute(self, traj_msg):
""" Execute a JointTrajectory message and return a TrajectoryFuture.
@param traj_msg: requested trajectory
@type traj_msg: trajectory_msgs.msg.JointTrajectory
@return future representing the execution of the trajectory
@rtype TrajectoryFuture
"""
import rospy
from control_msgs.msg import FollowJointTrajectoryGoal
goal_msg = FollowJointTrajectoryGoal()
goal_msg.trajectory = traj_msg
traj_future = TrajectoryFuture(traj_msg)
traj_future._handle = self._client.send_goal(
goal_msg,
transition_cb=traj_future.on_transition,
feedback_cb=traj_future.on_feedback
)
return traj_future
class TriggerClient(object):
def __init__(self, ns, controller_name, timeout=0.0):
"""Consructs a client that sends pr_control_msgs/Trigger actions
@param ns str: namespace for the ActionServer
@param controller_name str: name of the controller
"""
from actionlib import ActionClient
from pr_control_msgs.msg import TriggerAction
from rospy import Duration
self._client = ActionClient(ns + '/' + controller_name, TriggerAction)
if not self._client.wait_for_server(Duration(timeout)):
raise Exception('Could not connect to action server %s' % ns)
def execute(self):
"""Trigger the action and return a TriggerFuture
@return future pending on the completion of the action
@type TriggerFuture
"""
from pr_control_msgs.msg import TriggerActionGoal
goal_msg = TriggerActionGoal()
action_future = TriggerFuture()
action_future._handle = self._client.send_goal(
goal_msg,
transition_cb=action_future.on_transition,
feedback_cb=action_future.on_feedback
)
return action_future
def test_goal_canceled(self):
async def goal_coro(goal_handle):
with self.assertRaises(asyncio.CancelledError) as cm:
try:
goal_handle.set_accepted()
await asyncio.sleep(100)
except asyncio.CancelledError:
goal_handle.set_canceled()
raise
raise cm.exception
client = SyncActionClient("test_goal_canceled", TestAction)
server = AsyncActionServer(client.ns,
TestAction,
coro=goal_coro,
loop=self.loop)
client.wait_for_server()
goal_handle = client.send_goal(TestGoal())
self.loop.run_until_complete(
asyncio.wait_for(self.wait_for_status(goal_handle,
GoalStatus.ACTIVE),
timeout=1))
goal_handle.cancel()
self.loop.run_until_complete(
asyncio.wait_for(self.wait_for_status(goal_handle,
GoalStatus.PREEMPTED),
timeout=1))
self.assertEquals(goal_handle.get_goal_status(), GoalStatus.PREEMPTED)
class AsyncActionClient:
""" Async wrapper around the action client API. """
def __init__(self, name, action_spec, loop=None):
self.name = name
self._loop = loop if loop is not None else asyncio.get_running_loop()
self._client = ActionClient(name, action_spec)
self._status_sub = AsyncSubscriber(name + "/status", GoalStatusArray, loop=self._loop, queue_size=1)
def send_goal(self, goal):
""" Send a goal to an action server. As in rospy, if you have not made sure the server is up and listening to
the client, the goal will be swallowed.
"""
async_handle = _AsyncGoalHandle(name=self.name, loop=self._loop)
sync_handle = self._client.send_goal(
goal,
transition_cb=async_handle._transition_cb,
feedback_cb=async_handle._feedback_cb,
)
async_handle.cancel = sync_handle.cancel
return async_handle
async def ensure_goal(self, goal, resend_timeout):
""" Send a goal to an action server. If the goal is not processed by the action server within resend_timeout,
resend the goal.
"""
while True:
await self.wait_for_server()
handle = self.send_goal(goal)
try:
await asyncio.wait_for(handle.reach_status(GoalStatus.PENDING), timeout=resend_timeout)
except asyncio.TimeoutError:
continue
return handle
async def wait_for_server(self):
""" Reserve judgement, this replicates the behavior in actionlib.ActionClient.wait_for_server """
async for status_message in self._status_sub.subscribe():
server_id = status_message._connection_header["callerid"]
if self._client.pub_goal.impl.has_connection(server_id) and \
self._client.pub_cancel.impl.has_connection(server_id):
status_num_pubs = 0
for stat in self._client.status_sub.impl.get_stats()[1]:
if stat[4]:
status_num_pubs += 1
result_num_pubs = 0
for stat in self._client.result_sub.impl.get_stats()[1]:
if stat[4]:
result_num_pubs += 1
feedback_num_pubs = 0
for stat in self._client.feedback_sub.impl.get_stats()[1]:
if stat[4]:
feedback_num_pubs += 1
if status_num_pubs > 0 and result_num_pubs > 0 and feedback_num_pubs > 0:
return
async def test_server_simple(self):
event = asyncio.Event()
async def goal_coro(goal_handle):
delay = goal_handle.get_goal().goal
try:
if event.is_set():
raise RuntimeError(
f"Event wasn't cleared by another goal, bail!")
event.set()
goal_handle.set_accepted()
await asyncio.sleep(delay)
except asyncio.CancelledError:
event.clear()
goal_handle.set_canceled()
raise
event.clear()
goal_handle.set_succeeded(result=TestResult(delay))
client = SyncActionClient("test_server_simple", TestAction)
server = AsyncActionServer(client.ns,
TestAction,
coro=goal_coro,
simple=True)
server_task = asyncio.ensure_future(server.start())
await asyncio.get_event_loop().run_in_executor(None,
client.wait_for_server)
handles = []
for i in range(10):
handle = client.send_goal(TestGoal(1000000))
await self.wait_for_status(handle, GoalStatus.ACTIVE)
handles.append(handle)
last_handle = client.send_goal(TestGoal(0))
await self.wait_for_status(last_handle, GoalStatus.SUCCEEDED)
for handle in handles:
self.assertEqual(handle.get_goal_status(), GoalStatus.PREEMPTED)
self.assertEqual(last_handle.get_goal_status(), GoalStatus.SUCCEEDED)
server_task.cancel()
await deflector_shield(server_task)
class SynchronizedActionClient(object):
def __init__(self, action_namespace, action_spec):
self.goals = []
self.client = ActionClient(action_namespace, action_spec)
self.status_topic = rospy.remap_name(action_namespace) + '/status'
self.status_sub = rospy.Subscriber(self.status_topic, GoalStatusArray,
self.add_missing_goals)
# self.status_sub = rospy.Subscriber(
# rospy.remap_name(action_namespace) + '/goal',
# self.client.ActionGoal,
# self.add_new_goal)
self.client.wait_for_server()
rospy.wait_for_service(action_namespace + '/get_goal_from_id')
self.goal_from_id = rospy.ServiceProxy(
action_namespace + '/get_goal_from_id', GetTaskFromID)
def send_goal(self, goal, transition_cb=None, feedback_cb=None):
gh = self.client.send_goal(goal,
transition_cb=transition_cb,
feedback_cb=feedback_cb)
self.goals.append(gh)
return gh
def get_gh_from_task(self, task):
for gh in self.goals:
if task == get_task_from_gh(gh):
return gh
return None
def add_missing_goals(self, status_array):
tracked_goal_ids = [get_id_from_gh(gh) for gh in self.goals]
for goal_status in status_array.status_list:
if goal_status.status not in [2, 3, 4, 5, 8]:
if goal_status.goal_id.id not in tracked_goal_ids:
goal = self.goal_from_id(goal_status.goal_id.id).goal
if not null_task(goal.task):
self.start_tracking_goal_id(goal_status.goal_id, goal)
else:
msg = 'Null goal received, not tracking {} with status {}'
rospy.logwarn(
msg.format(goal_status.goal_id.id, goal_status))
def add_new_goal(self, msg):
self.start_tracking_goal_id(msg.goal_id, msg.goal)
pass
def start_tracking_goal_id(self, goal_id, goal):
action_goal = self.client.manager.ActionGoal(header=Header(),
goal_id=goal_id,
goal=goal)
csm = CommStateMachine(action_goal, None, None, None, None)
with self.client.manager.list_mutex:
self.client.manager.statuses.append(weakref.ref(csm))
self.goals.append(actionlib.ClientGoalHandle(csm))
def testsimple(self):
return
client = ActionClient('reference_action', TestAction)
self.assert_(client.wait_for_action_server_to_start(rospy.Duration(2.0)),
'Could not connect to the action server')
goal = TestGoal(1)
client.send_goal(goal)
self.assert_(client.wait_for_goal_to_finish(rospy.Duration(2.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.SUCCEEDED, client.get_terminal_state())
self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
goal = TestGoal(2)
client.send_goal(goal)
self.assert_(client.wait_for_goal_to_finish(rospy.Duration(10.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.ABORTED, client.get_terminal_state())
self.assertEqual(GoalStatus.ABORTED, client.get_state())
def testsimple(self):
return
client = ActionClient('reference_action', TestAction)
self.assert_(client.wait_for_action_server_to_start(rospy.Duration(2.0)),
'Could not connect to the action server')
goal = TestGoal(1)
client.send_goal(goal)
self.assert_(client.wait_for_goal_to_finish(rospy.Duration(2.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.SUCCEEDED, client.get_terminal_state())
self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
goal = TestGoal(2)
client.send_goal(goal)
self.assert_(client.wait_for_goal_to_finish(rospy.Duration(10.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.ABORTED, client.get_terminal_state())
self.assertEqual(GoalStatus.ABORTED, client.get_state())
def test_result(self):
client = ActionClient('reference_action', TestAction)
self.assert_(client.wait_for_server(rospy.Duration(2.0)),
'Could not connect to the action server')
goal_work = TestGoal(4)
goal_abort = TestGoal(6)
goal_result = TestGoal(8)
rospy.logwarn("This is a hacky way to associate goals with feedback")
g1 = client.send_goal(goal_work)
g2 = client.send_goal(goal_work)
g3 = client.send_goal(goal_work)
g4 = client.send_goal(goal_work)
rospy.sleep(0.5)
self.assertEqual(g1.get_goal_status(), GoalStatus.ACTIVE,
"Should be active")
self.assertEqual(g2.get_goal_status(), GoalStatus.ACTIVE,
"Should be active")
self.assertEqual(g3.get_goal_status(), GoalStatus.ACTIVE,
"Shoule be active")
self.assertEqual(g4.get_goal_status(), GoalStatus.ACTIVE,
"Should be active")
g5 = client.send_goal(goal_result)
rospy.sleep(0.5)
self.assertEqual(g5.get_goal_status(), GoalStatus.SUCCEEDED,
"Should be done")
self.assertEqual(g1.get_goal_status(), GoalStatus.SUCCEEDED)
self.assertEqual(g1.get_result().result, 4)
self.assertEqual(g2.get_goal_status(), GoalStatus.ABORTED)
self.assertEqual(g2.get_result().result, 3)
self.assertEqual(g3.get_goal_status(), GoalStatus.SUCCEEDED)
self.assertEqual(g3.get_result().result, 2)
self.assertEqual(g4.get_goal_status(), GoalStatus.ABORTED)
self.assertEqual(g4.get_result().result, 1)
g6 = client.send_goal(goal_abort)
rospy.sleep(0.5)
def test_feedback(self):
client = ActionClient('reference_action', TestAction)
self.assert_(client.wait_for_server(rospy.Duration(2.0)),
'Could not connect to the action server')
goal_work = TestGoal(4)
goal_abort = TestGoal(6)
goal_feedback = TestGoal(7)
rospy.logwarn("This is a hacky way to associate goals with feedback");
feedback={};
def update_feedback(id,g,f):
feedback[id]=f;
g1=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(0,g,f))
g2=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(1,g,f))
g3=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(2,g,f))
g4=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(3,g,f))
rospy.sleep(0.5);
self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE,"Shoule be active")
self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
g5=client.send_goal(goal_feedback)
rospy.sleep(0.5);
self.assertEqual(g5.get_goal_status(),GoalStatus.SUCCEEDED,"Should be done")
self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE)
self.assertEqual(feedback[0].feedback,4)
self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE)
self.assertEqual(feedback[1].feedback,3)
self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE)
self.assertEqual(feedback[2].feedback,2)
self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE)
self.assertEqual(feedback[3].feedback,1)
g6=client.send_goal(goal_abort)
rospy.sleep(0.5);
def test_feedback(self):
client = ActionClient('reference_action', TestAction)
self.assert_(client.wait_for_server(rospy.Duration(2.0)),
'Could not connect to the action server')
goal_work = TestGoal(4)
goal_abort = TestGoal(6)
goal_feedback = TestGoal(7)
rospy.logwarn("This is a hacky way to associate goals with feedback");
feedback={};
def update_feedback(id,g,f):
feedback[id]=f;
g1=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(0,g,f))
g2=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(1,g,f))
g3=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(2,g,f))
g4=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(3,g,f))
rospy.sleep(0.5);
self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE,"Shoule be active")
self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
g5=client.send_goal(goal_feedback)
rospy.sleep(0.5);
self.assertEqual(g5.get_goal_status(),GoalStatus.SUCCEEDED,"Should be done")
self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE)
self.assertEqual(feedback[0].feedback,4)
self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE)
self.assertEqual(feedback[1].feedback,3)
self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE)
self.assertEqual(feedback[2].feedback,2)
self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE)
self.assertEqual(feedback[3].feedback,1)
g6=client.send_goal(goal_abort)
rospy.sleep(0.5);
def test_result(self):
client = ActionClient('reference_action', TestAction)
self.assert_(client.wait_for_server(rospy.Duration(2.0)),
'Could not connect to the action server')
goal_work = TestGoal(4)
goal_abort = TestGoal(6)
goal_result = TestGoal(8)
rospy.logwarn("This is a hacky way to associate goals with feedback");
g1=client.send_goal(goal_work)
g2=client.send_goal(goal_work)
g3=client.send_goal(goal_work)
g4=client.send_goal(goal_work)
rospy.sleep(0.5);
self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE,"Shoule be active")
self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
g5=client.send_goal(goal_result)
rospy.sleep(0.5);
self.assertEqual(g5.get_goal_status(),GoalStatus.SUCCEEDED,"Should be done")
self.assertEqual(g1.get_goal_status(),GoalStatus.SUCCEEDED)
self.assertEqual(g1.get_result().result,4)
self.assertEqual(g2.get_goal_status(),GoalStatus.ABORTED)
self.assertEqual(g2.get_result().result,3)
self.assertEqual(g3.get_goal_status(),GoalStatus.SUCCEEDED)
self.assertEqual(g3.get_result().result,2)
self.assertEqual(g4.get_goal_status(),GoalStatus.ABORTED)
self.assertEqual(g4.get_result().result,1)
g6=client.send_goal(goal_abort)
rospy.sleep(0.5);
async def test_goal_exception(self):
async def goal_coro(goal_handle):
goal_handle.set_accepted()
raise RuntimeError()
client = SyncActionClient("test_goal_aborted", TestAction)
server = AsyncActionServer(client.ns, TestAction, coro=goal_coro)
server_task = asyncio.ensure_future(server.start())
await asyncio.get_event_loop().run_in_executor(None, client.wait_for_server)
goal_handle = client.send_goal(TestGoal())
await self.wait_for_status(goal_handle, GoalStatus.ABORTED)
self.assertEquals(goal_handle.get_goal_status(), GoalStatus.ABORTED)
with self.assertRaises(RuntimeError):
await deflector_shield(server_task)
class SetPositionClient(object):
def __init__(self, ns, controller_name, timeout=0.0):
"""Consructs a client that sends pr_control_msgs/SetPosition actions
@param ns: namespace for the ActionServer
@type ns: str
@param controller_name: name of the controller
@type controller_name: str
"""
from actionlib import ActionClient
from pr_control_msgs.msg import SetPositionAction
self.log = logging.getLogger(__name__)
as_name = ns + '/' + controller_name + '/set_position'
self._client = ActionClient(as_name, SetPositionAction)
if not self._client.wait_for_server(rospy.Duration(timeout)):
raise Exception('Could not connect to action server {}'.format(
as_name))
def execute(self, joint_state):
"""Execute a SetPosition action and return a SetPositionFuture
@param positional_joint_state: requested position
@type positional_joint_state: sensor_msgs.JointState
@return future pending on the completion of the action
@type SetPositionFuture
"""
from pr_control_msgs.msg import SetPositionGoal
goal_msg = SetPositionGoal()
goal_msg.command.header.stamp = rospy.Time.now()
goal_msg.command.position = joint_state
self.log.info('Sending SetPositionGoal: {}'.format(goal_msg))
action_future = SetPositionFuture(joint_state)
action_future._handle = self._client.send_goal(
goal_msg,
transition_cb=action_future.on_transition,
feedback_cb=action_future.on_feedback
)
return action_future
async def test_goal_succeeded(self):
magic_value = 5
async def goal_coro(goal_handle):
goal_handle.set_accepted()
goal_handle.set_succeeded(result=TestResult(goal_handle.get_goal().goal))
client = SyncActionClient("test_goal_succeeded", TestAction)
server = AsyncActionServer(client.ns, TestAction, coro=goal_coro)
server_task = asyncio.ensure_future(server.start())
await asyncio.get_event_loop().run_in_executor(None, client.wait_for_server)
goal_handle = client.send_goal(TestGoal(magic_value))
await self.wait_for_result(goal_handle)
self.assertEqual(goal_handle.get_goal_status(), GoalStatus.SUCCEEDED)
self.assertEqual(goal_handle.get_result().result, magic_value)
server_task.cancel()
await deflector_shield(server_task)
class FollowJointTrajectoryClient(object):
def __init__(self, ns, controller_name, timeout=0.0):
""" Constructs a client that executes JointTrajectory messages.
@param ns: namespace for the FollowJointTrajectoryAction server
@type ns: str
@param controller_name: name of controller under ns
@type controller_name: str
"""
from actionlib import ActionClient
from control_msgs.msg import FollowJointTrajectoryAction
from rospy import Duration
as_name = ns + '/' + controller_name + '/follow_joint_trajectory'
self._client = ActionClient(as_name, FollowJointTrajectoryAction)
if not self._client.wait_for_server(Duration(timeout)):
raise Exception('Could not connect to action server {}'.format(
as_name))
def execute(self, traj_msg):
""" Execute a JointTrajectory message and return a TrajectoryFuture.
@param traj_msg: requested trajectory
@type traj_msg: trajectory_msgs.msg.JointTrajectory
@return future representing the execution of the trajectory
@rtype TrajectoryFuture
"""
from control_msgs.msg import FollowJointTrajectoryGoal
goal_msg = FollowJointTrajectoryGoal()
goal_msg.trajectory = traj_msg
traj_future = TrajectoryFuture(traj_msg)
traj_future._handle = self._client.send_goal(
goal_msg,
transition_cb=traj_future.on_transition,
feedback_cb=traj_future.on_feedback
)
return traj_future
def test_goal_succeeded(self):
magic_value = 5
async def goal_coro(goal_handle):
goal_handle.set_accepted()
goal_handle.set_succeeded(
result=TestResult(goal_handle.get_goal().goal))
client = SyncActionClient("test_goal_succeeded", TestAction)
server = AsyncActionServer(client.ns,
TestAction,
coro=goal_coro,
loop=self.loop)
client.wait_for_server()
goal_handle = client.send_goal(TestGoal(magic_value))
self.loop.run_until_complete(
asyncio.wait_for(self.wait_for_status(goal_handle,
GoalStatus.SUCCEEDED),
timeout=1))
self.assertEqual(goal_handle.get_goal_status(), GoalStatus.SUCCEEDED)
self.assertEqual(goal_handle.get_result().result, magic_value)
else:
rospy.loginfo("RECALLED %s" % state) # 其他
def feedback_cb(handle, feedback):
if not isinstance(feedback, CountNumberFeedback):
return
rospy.loginfo("feedback_cb %f %%" % (feedback.rate * 100))
if __name__ == '__main__':
nodeName = "client_node"
rospy.init_node(nodeName, anonymous=True)
actionName = "/zcb01/action"
client = ActionClient(actionName, CountNumberAction)
client.wait_for_server()
goal = CountNumberGoal()
goal.max = 88
goal.duration = 0.1
# 这里一定要接受返回值
handle = client.send_goal(goal, transition_cb, feedback_cb)
# d = rospy.Duration(3)
# rospy.sleep(d)
# handle.cancel()
rospy.spin()
from actionlib import ActionClient
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
rospy.init_node('initial_joint_state_setter', anonymous=True)
ac = ActionClient('/whole_body_controller/body/follow_joint_trajectory',
FollowJointTrajectoryAction)
ac.wait_for_server()
goal = FollowJointTrajectoryGoal()
traj = JointTrajectory()
traj.header.stamp = rospy.get_rostime()
traj.joint_names = [
'torso_lift_joint', 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint',
'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint',
'r_wrist_flex_joint', 'r_wrist_roll_joint', 'l_upper_arm_roll_joint',
'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint',
'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint',
'head_pan_joint', 'head_tilt_joint'
]
p = JointTrajectoryPoint()
p.positions = [
0.1687062500000004, 0, 0, 0, 0, -1.1400000000000001, -1.0499999999999998,
0, 0, 0, 0, 0, -1.1399999999999995, -1.0499999999999998, 0, 0, 0
]
p.velocities = [0] * len(p.positions)
p.time_from_start = rospy.Duration(1)
traj.points.append(p)
goal.trajectory = traj
ac.send_goal(goal)
class Supply(SupplyChain):
def __init__(self):
SupplyChain.__init__(self)
self.addr = rospy.get_param('~plant_addr')
self.plant_node = rospy.get_param('~plant_node')
self.plant = ActionClient(self.plant_node, PlantAction)
self.plant.wait_for_server()
rospy.wait_for_service(self.plant_node + '/unload')
self.unload = rospy.ServiceProxy(self.plant_node + '/unload',
PlantUnload)
rospy.wait_for_service('/liability/finish')
self.finish = rospy.ServiceProxy('/liability/finish', Empty)
rospy.logdebug('Supply chain node ready')
def spin(self):
'''
Waiting for the new messages.
'''
rospy.spin()
def prepare(self, objective):
rospy.logdebug('Supply.prepare: ' + objective)
# ask for raw material
# self.ask(10, 1, supplier_market, supplier[objective][self.addr-1], 1, 50)
msg = Ask()
msg.model = supplier_market
msg.objective = supplier[objective][self.addr - 1]
msg.cost = 42
msg.count = 1
msg.fee = 3
self.signing_ask.publish(msg)
def task(self, objective):
rospy.logdebug('Supply.task: ' + objective)
self.start_order(objective) # returns after order complete
def start_order(self, order):
rospy.logdebug('New order: ' + order + ', to plant: ' +
self.plant_node)
plant_gh = self.plant.send_goal(PlantGoal(specification=order))
while not rospy.is_shutdown(): # wait until plant complete its job
if plant_gh.get_goal_status() < 2:
rospy.logdebug('Goal status: ' +
str(plant_gh.get_goal_status()))
rospy.sleep(1)
else:
break
rospy.logdebug('Goal complete, status: ' +
str(plant_gh.get_goal_status()))
plant_gh = None
def finalize(self, objective):
rospy.logdebug('Supply.finalize: ' + objective)
# ask for goods storage
# self.ask(10, 1, storage_market, storage[objective][self.addr-1], 1, 50)
msg = Ask()
msg.model = storage_market
msg.objective = storage[objective][self.addr - 1]
msg.cost = 10
msg.count = 1
msg.fee = 1
self.signing_ask.publish(msg)
self.unload()
rospy.sleep(5)
rospy.logdebug('Order complete')
self.finish()
self.make_bids() # publish offer for next launch
class ROSAtomicAction(AtomicAction):
def __init__(self, name, params=None, recovery=None):
super(ROSAtomicAction, self).__init__(name, params, recovery)
self.lock = Lock()
self.g = None
self.monitor_thread = None
self.server_thread = None
def get_goal_type(self, action_name):
topic_type = rostopic._get_topic_type("/%s/goal" % action_name)[0]
# remove "Action" string from goal type
assert ("Action" in topic_type)
return roslib.message.get_message_class(topic_type[:-10] + "Goal")
def get_action_type(self, action_name):
topic_type = rostopic._get_topic_type("/%s/goal" % action_name)[0]
# remove "Goal" string from action type
assert ("Goal" in topic_type)
return roslib.message.get_message_class(topic_type[:-4])
def call_server(self, kb):
with self.lock:
self.client = ActionClient(self.name,
self.get_action_type(self.name))
goal = self.get_goal_type(self.name)()
for slot in set(goal.__slots__) - set(self.params.keys()):
self.params[slot] = kb.query(slot)
for slot, value in self.params.items():
setattr(goal, slot, type(getattr(goal, slot))(value))
self.client.wait_for_server()
self.g = self.client.send_goal(goal)
def start(self, kb):
if self.server_thread is None or not self.server_thread.is_alive():
self.server_thread = Thread(target=self.call_server, args=(kb, ))
self.server_thread.start()
def wait_for_action(self, kb):
if self.g is not None:
with self.lock:
while self.g.get_goal_status() in (GoalStatus.ACTIVE,
GoalStatus.PENDING):
rospy.sleep(.01)
result = self.g.get_result()
if result != None and result:
for slot in result.__slots__:
res = getattr(result, slot)
kb.update(slot, res)
def monitor(self, kb):
if self.monitor_thread is None or not self.monitor_thread.is_alive():
self.monitor_thread = Thread(target=self.wait_for_action,
args=(kb, ))
self.monitor_thread.start()
def get_state(self):
if self.lock.acquire(False):
try:
return self.g.get_goal_status()
except:
return -1
finally:
self.lock.release()
return -1
@property
def succeeded(self):
return self.get_state() == GoalStatus.SUCCEEDED
@property
def preempted(self):
return self.get_state() in (GoalStatus.PREEMPTED,
GoalStatus.PREEMPTING, GoalStatus.RECALLED,
GoalStatus.RECALLING)
@property
def failed(self):
return self.get_state() in (GoalStatus.LOST, GoalStatus.ABORTED)
def get_result(self):
if self.g is not None:
with self.lock:
return self.g.get_result()
def __str__(self):
return "Action({}): [{}]".format(
self.name,
', '.join([str(k)+": "+str(v) for k,v in self.params.items()]) \
if self.params is not None else ""
)
class TaskExecutor:
"""
Class responsible for completing all tasks in the robot's agenda.
Agenda is provided by an AgendaManager.
Communication with move_base is done through its actionlib interface.
Parameters:
- roaming_mode: one of the following (default: SMART)
FIXED_POINTS: use predefined goals for up to 4 robots (see above)
SMART: use externally provided roaming goals
(eg. based on the costmaps to determine regions with high task probability)
requires service to get roam goal at /get_roaming_task
NO_ROAM: robots don't move at all if they don't have a task
Topics:
- subscribed:
agendaUpdated: AgendaUpdated - Receive full agenda updates
- publishes:
taskCompleted: String - Notify upon task completion by agenda_item id
"""
action_namespace = rospy.get_namespace() + "move_base"
def __init__(self):
self.schedule: List[ScheduleItem] = list()
self.action_client = ActionClient(self.action_namespace,
MoveBaseAction)
self.current_goal: ClientGoalHandle = None
self.roam_mode = rospy.get_param(f'{rospy.get_name()}/roaming_mode',
'FIXED_POSITIONS')
self.robot_prefix = rospy.get_namespace().replace('/', '')
self.current_task_id = 'NONE'
self.agenda_updated_sub = None
self.roaming_point_srv = None
self.task_completed_pub = None
self.__init_subscriptions()
self.__init_publishers()
if not self.action_client.wait_for_server(timeout=rospy.Duration(10)):
rospy.logerr(
f"Could not connect to action server at {self.action_namespace}"
)
raise rospy.ROSException(
f"[{self.robot_prefix}][{NODE_NAME}] Could not connect to action server"
)
rospy.loginfo(
f"[{self.robot_prefix}][{NODE_NAME}] node is ready - "
f"publishing task completion on '{self.task_completed_pub.resolved_name}', "
f"listening for schedule updates on '{self.schedule_updated_sub.resolved_name}', "
f"connected to move_base at {self.action_client.ns}")
if self.roam_mode not in ["FIXED_POSITIONS", "NO_ROAM"]:
rospy.wait_for_service('/get_roaming_task')
self.current_task_id = "NONE"
self.send_next()
rospy.spin()
def __init_subscriptions(self):
self.schedule_updated_sub = rospy.Subscriber(SCHEDULE_UPDATED_TOPIC,
ScheduleUpdated,
self.schedule_updated_cb)
self.roaming_point_srv = rospy.ServiceProxy('/get_roaming_task',
GetRoamingTask)
def __init_publishers(self):
self.task_completed_pub = rospy.Publisher(TASK_COMPLETED_TOPIC,
String,
queue_size=10)
def mark_current_task_completed(self) -> None:
"""
Notify subscribers that the current task is completed.
Task is identified by its schedule_item id (as set by Task Manager)
"""
item_id = self.schedule.pop(0).id
rospy.loginfo(
f"[{self.robot_prefix}][{NODE_NAME}] confirm completion of task {item_id}"
)
self.task_completed_pub.publish(item_id)
def schedule_updated_cb(self,
schedule_updated_message: ScheduleUpdated) -> None:
"""
Callback for the UPDATED_TOPIC subscription.
If the new agenda has a different first task than the current agenda,
the current task - if any - is preempted and the new first task is sent as the new goal
else just the list is updated.
:param schedule_updated_message: Full update of agenda, sent by AgendaManager
"""
rospy.logdebug(
f"[{self.robot_prefix}][{NODE_NAME}]"
f" got agenda of length {len(schedule_updated_message.schedule)}")
self.schedule = schedule_updated_message.schedule
if len(self.schedule
) == 0 or self.current_task_id != self.schedule[0].id:
rospy.logdebug(
f"[{self.robot_prefix}][{NODE_NAME}] New first node - updating goal"
)
if self.current_goal is not None:
self.current_goal.cancel()
self.send_next()
def action_client_completed_callback(
self, goal_handle: ClientGoalHandle) -> None:
"""
Callback to be called for all goal state tranisitions.
All transitions except to the DONE state are ignored.
If the task was not aborted or preempted it is marked as completed and the robot moves on to the next task.
If the task was completed with a non-succeeding state, it will still be marked as completed and a warning is
printed.
See also: http://docs.ros.org/api/actionlib/html/classactionlib_1_1action__client_1_1CommStateMachine.html
:param goal_handle: Actionlib GoalHandle for the task
"""
rospy.logdebug(
f"[{self.robot_prefix}][{NODE_NAME}] task {self.current_task_id}: stat change of task to "
f"{goal_handle.get_goal_status()} - {goal_handle.get_goal_status_text()}"
)
if goal_handle.get_comm_state() == CommState.DONE:
if goal_handle.get_goal_status() != GoalStatus.PREEMPTED:
if goal_handle.get_goal_status() != GoalStatus.SUCCEEDED:
rospy.logwarn(
f"[{self.robot_prefix}][{NODE_NAME}]"
f"Task completed with non-succeeding state: {goal_handle.get_goal_status_text()}"
)
if self.current_task_id != ROAMING_TASK_ID and self.current_task_id != "NONE":
self.mark_current_task_completed()
self.send_next()
else:
rospy.logwarn(f"[{self.robot_prefix}][{NODE_NAME}]"
f"Task preempted")
def send_next(self, timer_context=None) -> None:
"""
Send the next task or a roaming target - if any - as a goal to the move_base
"""
# if called from timer only execute if robot is still waiting for a goal
if timer_context is None or (len(self.schedule) == 0 and
self.current_goal.get_goal_status() == 3):
if len(self.schedule):
goal_point = Point(self.schedule[0].task.x,
self.schedule[0].task.y, 0)
else:
goal_point = self.get_roaming_point()
if goal_point is not None:
goal = MoveBaseGoal(
PoseStamped(Header(frame_id='map'),
Pose(goal_point, Quaternion(0, 0, 0, 1))))
self.current_task_id = self.schedule[0].id if len(
self.schedule) > 0 else ROAMING_TASK_ID
rospy.loginfo(
f"[{self.robot_prefix}][{NODE_NAME}] task {self.current_task_id}: Sending goal to move_base"
)
rospy.logdebug(
f"[{self.robot_prefix}][{NODE_NAME}] Goal was: {goal}")
self.current_goal = self.action_client.send_goal(
goal, self.action_client_completed_callback)
else:
# if no task or roaming goal is available try again in 5 seconds.
self.current_task_id = "NONE"
rospy.Timer(rospy.Duration(5), self.send_next)
def get_roaming_point(self):
if self.roam_mode == "FIXED_POSITIONS":
global r_counter
r_counter += 1
return roaming_goals[self.robot_prefix][r_counter % 2]
elif self.roam_mode == "NO_ROAM":
return None
else:
start: PoseWithCovarianceStamped = rospy.wait_for_message(
'amcl_pose', PoseWithCovarianceStamped)
rospy.logwarn(f'[{self.robot_prefix}]get roam point!')
resp: GetRoamingTaskResponse = self.roaming_point_srv(
Goal(None, start.pose.pose.position.x,
start.pose.pose.position.y, True))
if resp.success:
roam_task = resp.task
return Point(roam_task.x, roam_task.y, 0)
else:
return None
class AsyncActionClient:
""" Async wrapper around the action client API. """
def __init__(self, name, action_spec):
self._loop = asyncio.get_event_loop()
self.name = name
self.action_spec = action_spec
self._exception_monitor = ExceptionMonitor()
self._started_event = asyncio.Event()
async def start(self):
""" Start the action client. """
self._client = ActionClient(self.name, self.action_spec)
try:
self._started_event.set()
await self._exception_monitor.start()
finally:
# TODO(pbovbel) depends on https://github.com/ros/actionlib/pull/142
self._started_event.clear()
self._client.stop()
self._client = None
async def _started(self, log_period=None):
await log_during(self._started_event.wait(),
f"Waiting for {self.name} client to be started...",
period=5.0)
async def wait_for_server(self, log_period=None):
""" Wait for the action server to connect to this client. """
await self._started(log_period=log_period)
await log_during(self._wait_for_server(),
f"Waiting for {self.name} server...",
period=log_period)
async def _wait_for_server(self):
while True:
# Use a small timeout so that the execution can be cancelled if necessary
connected = await self._loop.run_in_executor(
None, self._client.wait_for_server, rospy.Duration(0.1))
if connected:
return connected
async def send_goal(self, goal):
""" Send a goal to an action server. As in rospy, if you have not made sure the server is up and listening to
the client, the goal will be swallowed.
"""
await self._started(log_period=5.0)
async_handle = _AsyncGoalHandle(
name=self.name,
exception_monitor=self._exception_monitor,
loop=self._loop)
sync_handle = self._client.send_goal(
goal,
transition_cb=async_handle._transition_cb,
feedback_cb=async_handle._feedback_cb,
)
async_handle.goal_id = sync_handle.comm_state_machine.action_goal.goal_id.id
async_handle.cancel = sync_handle.cancel
return async_handle
async def ensure_goal(self, goal, resend_timeout):
""" Send a goal to an action server. If the goal is not processed by the action server within resend_timeout,
resend the goal.
"""
while True:
await self.wait_for_server(log_period=5.0)
handle = await self.send_goal(goal)
try:
await asyncio.wait_for(handle.reach_status(GoalStatus.PENDING),
timeout=resend_timeout)
except asyncio.TimeoutError:
logger.warn(
f"Action goal for {self.name} was not processed within timeout, resending"
)
handle.cancel()
continue
except asyncio.CancelledError:
handle.cancel()
raise
return handle