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 __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()
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
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 __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)
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 __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 __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 __init__(self, do_register=True, silent=True):
self._name = rospy.get_name()
self._silent = silent
self._acknowledge_publisher = Publisher('/robot/state/server',
RobotModeMsg,
latch=True,
queue_size=5)
self._state_subscriber = Subscriber('/robot/state/clients',
RobotModeMsg,
self.server_state_information,
queue_size=10)
self._state_info = ServiceProxy('/robot/state/info', GetStateInfo)
self._state_changer = ActionClient('/robot/state/change',
RobotModeAction)
if do_register:
self.client_register()
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 __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()
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position,
graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.loginfo('Published ' + str(len(grasp_PA.poses)) + ' poses')
rospy.sleep(2)
if __name__ == '__main__':
name = 'grasp_object_server'
rospy.init_node(name, anonymous=False)
rospy.loginfo("Connecting to grasp generator AS")
grasps_ac = ActionClient('/moveit_grasps_server/generate',
GenerateGraspsAction)
grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
object_pose = geometry_msgs.msg.PoseStamped()
object_pose.pose.position.x = 1.0
object_pose.pose.position.y = 1.0
object_pose.pose.position.z = 1.0
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
grasp_list = generate_grasps(object_pose, 0.06)
rospy.loginfo('Generated ' + str(len(grasp_list)) + ' grasps.')
publish_grasps_as_poses(grasp_list)
rospy.sleep(10.0)
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)
#!/usr/bin/env python
import rospy
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)
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()
