class FollowTargetActionClient(Node): def __init__(self, action_name='/robot1/FollowTargets', action_client_name='follow_target_action_client'): super().__init__(action_client_name) self._action_client = ActionClient(self, FollowTargets, action_name) def get_action_client(self): return self._action_client def send_targets(self, poses, loads): self.get_logger().info('Received Goal poses: {0}'.format(len(poses))) self.get_logger().info('Received Load instructions: {0}'.format( len(loads))) if len(poses) == len(loads): goal_msg = FollowTargets.Goal() goal_msg.poses = poses goal_msg.load = loads self.goal_length = len(poses) self._action_client.wait_for_server() self._action_client.send_goal_async( goal_msg, feedback_callback=self.feedback_callback) else: self.get_logger().warn('Unequal amount of Poses and loads!') def feedback_callback(self, feedback_msg): feedback = feedback_msg.feedback
def test_send_goal_async_with_feedback_for_another_goal(self): ac = ActionClient(self.node, Fibonacci, 'fibonacci') try: self.assertTrue(ac.wait_for_server(timeout_sec=2.0)) # Send a goal and then publish feedback first_goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) future = ac.send_goal_async( Fibonacci.Goal(), feedback_callback=self.feedback_callback, goal_uuid=first_goal_uuid) rclpy.spin_until_future_complete(self.node, future, self.executor) # Send another goal, but without a feedback callback second_goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) future = ac.send_goal_async(Fibonacci.Goal(), goal_uuid=second_goal_uuid) rclpy.spin_until_future_complete(self.node, future, self.executor) # Publish feedback for the second goal self.mock_action_server.publish_feedback(second_goal_uuid) self.timed_spin(1.0) self.assertEqual(self.feedback, None) # Publish feedback for the first goal (with callback) self.mock_action_server.publish_feedback(first_goal_uuid) self.timed_spin(1.0) self.assertNotEqual(self.feedback, None) finally: ac.destroy()
class KickCapsule(): last_feedback = None # type: KickFeedback last_feedback_received = None # type: rospy.Time last_goal = None # type: KickGoal last_goal_sent = None # type: rospy.Time last_result = None # type: KickActionResult last_result_received = None # type: rospy.Time is_currently_kicking = False # type: bool __connected = False # type: bool __action_client = None # type: actionlib.SimpleActionClient def __init__(self, blackboard): """ :param blackboard: Global blackboard instance """ self.__blackboard = blackboard self.connect() def connect(self): topic = self.__blackboard.config["dynamic_kick"]["topic"] self.get_logger().info("Connecting {}.KickCapsule to bitbots_dynamic_kick ({})" .format(str(self.__blackboard.__class__).split(".")[-1], topic)) self.__action_client = ActionClient(self, KickAction, topic) self.__connected = self.__action_client.wait_for_server( Duration(seconds=self.__blackboard.config["dynamic_kick"]["wait_time"])) if not self.__connected: self.get_logger().error("No dynamic_kick server running on {}".format(topic)) def kick(self, goal): """ :param goal: Goal to kick to :type goal: KickGoal :raises RuntimeError: when not connected to dynamic_kick server """ if not self.__connected: # try to connect again self.__connected = self.__action_client.wait_for_server( Duration(seconds=self.__blackboard.config["dynamic_kick"]["wait_time"])) if not self.__connected: raise RuntimeError("Not connected to any dynamic_kick server") self.__action_client.send_goal_async(goal, self.__done_cb, self.__active_cb, self.__feedback_cb) self.last_goal = goal self.last_goal_sent = self.get_clock().now() def __done_cb(self, state, result): self.last_result = KickActionResult(status=state, result=result) self.last_result_received = self.get_clock().now() self.is_currently_kicking = False def __feedback_cb(self, feedback): self.last_feedback = feedback self.last_feedback_received = self.get_clock().now() def __active_cb(self): self.is_currently_kicking = True
def test_different_type_raises(self): ac = ActionClient(self.node, Fibonacci, 'fibonacci') try: with self.assertRaises(TypeError): ac.send_goal('different goal type') with self.assertRaises(TypeError): ac.send_goal_async('different goal type') finally: ac.destroy()
class GoTo(Strategies): def __init__(self, id: float, x: float, y: float, theta: float) -> None: self.client_ = ActionClient(World().node_, Behavior, f"robot_{id}/behavior") self.client_.wait_for_server() self.client_.send_goal_async(create_move_to(x, y, theta)) def update(self): pass
class HeadTiltClient(Node): def __init__(self): super().__init__('head_tilt_client') # Action client to send goals to jaw controller self.eye_action_client_horizontal = ActionClient( self, FollowJointTrajectory, '/head_controller/follow_joint_trajectory') # Subscriber to get data from face tracker self.get_data = self.create_subscription( Float32MultiArray, 'tilt_head', self.send_goal, 10, ) def send_goal(self, position): # Message type in JointTrajectory goal_msg = JointTrajectory() # Joint trajectory points jtp = JointTrajectoryPoint() jtp.velocities = [0.0] jtp.time_from_start.sec = 0 jtp.time_from_start.nanosec = 0 jtp.positions = [position.data[0]] if position.data[1] == 1.0: joint = "head_tilt_right_joint" elif position.data[1] == 0.0: joint = "head_tilt_vertical_joint" elif position.data[1] == -1.0: joint = "head_tilt_left_joint" jointNames = [] jointNames.append(joint) # Build message goal_msg.points = [jtp] goal_msg.joint_names = jointNames # Assign goal goal = FollowJointTrajectory.Goal() goal.trajectory = goal_msg self.eye_action_client_horizontal.wait_for_server() self.get_logger().info('Goal: %f' % jtp.positions[0]) self.eye_action_client_horizontal.send_goal_async(goal)
class MinimalActionClient(Node): def __init__(self): super().__init__('minimal_action_client') self._action_client = ActionClient(self, Fibonacci, 'fibonacci') def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info('goal rejected') return self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def feedback_callback(self, feedback): self.get_logger().info('feedback:{}'.format( feedback.feedback.sequence)) def get_result_callback(self, future): status = future.result().action_status if status == GoalStatus.STATUS_SUCCEEDED: self.get_logger().info('result:{}'.format( future.result().requence)) rclpy.shutdown() def send_goal(self): self.get_logger().info('waiting...') self._action_client.wait_for_server() goal_msg = Fibonacci.Goal() goal_msg.order = 10 self._send_goal_future = self._action_client.send_goal_async( goal_msg, feedback_callback=self.feedback_callback) self._send_goal_future.add_done_callback(self.goal_response_callback)
class HydrophoneStreamerAClient(Node): def __init__(self): super().__init__('Hydrophone_streamer_aclient') self._action_client = ActionClient(self,Hydrophoneraw,'hydrophoneraw') def send_goal(self, streamseconds): goal_msg = Hydrophoneraw.Goal() goal_msg.streamseconds = streamseconds self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async(goal_msg,feedback_callback=self.feedback_callback) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self,future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info('Client goal has been rejected.') return self.get_logger().info('Client goal has been accepted.') self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def get_result_callback(self, future): result = future.result().result self.get_logger().info('Result: {0}'.format(result.rawdata)) rclpy.shutdown() def feedback_callback(self,feedback_msg): feedback = feedback_msg.feedback self.get_logger().info('Recieved feedback: {0}'.format(feedback.rawsnapshot))
class PlanTrajectoryClientMock(rclpy.node.Node): def __init__(self): super().__init__('plan_trajectory_client_mock') self._action_client = ActionClient(self, PlanTrajectory, 'plan_parking_trajectory') self._send_goal_future = None self._get_result_future = None self._result = None def send_goal(self, goal): self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async(goal) self._send_goal_future.add_done_callback(self.goal_response_callback) rclpy.spin_until_future_complete(self, self._send_goal_future) def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: return self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) rclpy.spin_until_future_complete(self, self._get_result_future) def get_result_callback(self, future): self._result = future.result().result
class FibonacciActionClient(Node): def __init__(self): super().__init__('fibonacci_action_client') self._action_client = ActionClient(self, Fibonacci, 'fibonacci') def send_goal(self, order): goal_msg = Fibonacci.Goal() goal_msg.order = order self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async( goal_msg, feedback_callback=self.feedback_callback) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info('Goal rejected:(') return self.get_logger().info('Goal accepted :)') self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def get_result_callback(self, future): result = future.result().result self.get_logger().info('Result: {0}'.format(result.sequence)) rclpy.shutdown() def feedback_callback(self, feedback_msg): feedback = feedback_msg.feedback self.get_logger().info('Received feedback:{0}'.format( feedback.partial_sequence))
class MoveLinearClient(Node): def __init__(self): super().__init__('MotionMoveLinear') self._action_client = ActionClient(self, MotionMoveLinear, 'Motion/MoveLinear') self._ret = 0 self._count = 0 def send_goal(self, goal_msg): self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async(goal_msg) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: return self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def get_result_callback(self, future): result = future.result().result self._ret = int(result.ret) self._count += 1
class JoyTeleopActionCommand(JoyTeleopCommand): def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None: super().__init__(name, config, 'buttons', 'axes') self.name = name action_type = get_interface_type(config['interface_type'], 'action') self.goal = action_type.Goal() if 'action_goal' in config: # Set the message fields for the goal in the constructor. This goal will be used # during runtime, and has hte side benefit of giving the user early feedback if the # config can't work. set_message_fields(self.goal, config['action_goal']) action_name = config['action_name'] self.action_client = ActionClient(node, action_type, action_name) self.client_ready = False def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None: # The logic for responding to this joystick press is: # 1. Save off the current state of active. # 2. Update the current state of active. # 3. If this command is currently not active, return without calling the action. # 4. Save off the current state of whether the action was ready. # 5. Update whether the action is ready. # 6. If the action is not currently ready, return without calling the action. # 7. If the action was already ready, and the state of the button is the same as before, # debounce and return without calling the action. # 8. In all other cases, call the action. This means that either this is a button # transition 0 -> 1, or that the action became ready since the last call. last_active = self.active self.update_active_from_buttons_and_axes(joy_state) if not self.active: return last_ready = self.client_ready self.client_ready = self.action_client.server_is_ready() if not self.client_ready: return if last_ready == self.client_ready and last_active == self.active: return self.action_client.send_goal_async(self.goal)
class FibonacciActionClient(Node): def __init__(self): super().__init__('fibonacci_action_client') self._action_client = ActionClient(self, Fibonacci, 'fibonacci') def send_goal(self, order): goal_msg = Fibonacci.Goal() goal_msg.order = order self._action_client.wait_for_server() self._action_client.send_goal_async(goal_msg) def feedback_callback(self, feedback_msg): feedback = feedback_msg.feedback self.get_logger().info('Received feedback: {0}'.format( feedback.partial_sequence))
class Client(Node): def __init__(self): super().__init__('action_client_node') #self.node_name='action_client_node' #node = rclpy.create_node(self.node_name) self._action_client = ActionClient(self, MultiDofFollowJointTrajectory, 'action_controller') def generate_trajectory(self): traj_to_execute = MultiDOFJointTrajectory() traj_header = std_msgs.msg.Header() traj_velocities = Twist() traj_accelerations = Twist() traj_to_execute.joint_names = ["virtual_joint"] for i in range(0, 5): traj_header.stamp = self.get_clock().now().to_msg( ) #rclpy.time.Time().msg() traj_to_execute.header = traj_header #traj_quaternion = tf2.transformations.quaternion_from_euler(roll,pitch,yaw) traj_quaternion = [0.0, 0.0, 0.0, 0.0] traj_transforms = Transform() traj_transforms.translation.x = 1.0 * i traj_transforms.translation.y = 2.0 * i traj_transforms.translation.z = 3.0 * i traj_transforms.rotation = Quaternion() traj_transforms.rotation.x = traj_quaternion[0] traj_transforms.rotation.y = traj_quaternion[1] traj_transforms.rotation.z = traj_quaternion[2] traj_transforms.rotation.w = traj_quaternion[3] point_i = MultiDOFJointTrajectoryPoint() point_i.transforms.append(traj_transforms) point_i.velocities.append(traj_velocities) point_i.accelerations.append(traj_accelerations) duration_i = Duration(seconds=1.0).to_msg() point_i.time_from_start = duration_i # self.get_clock().now().to_msg()+Duration(seconds=1.0) traj_to_execute.points.append(point_i) return traj_to_execute def send_goal(self): goal_msg = MultiDofFollowJointTrajectory.Goal() traj_to_execute = self.generate_trajectory() goal_msg.trajectory = traj_to_execute #trajectory_msgs/MultiDOFJointTrajectory self._action_client.wait_for_server() self._action_client.send_goal_async(goal_msg)
def test_send_goal_async_no_server(self): ac = ActionClient(self.node, Fibonacci, 'not_fibonacci') try: future = ac.send_goal_async(Fibonacci.Goal()) self.timed_spin(2.0) self.assertFalse(future.done()) finally: ac.destroy()
class HydrophoneLocaliserAClient(Node): def __init__(self): """Constructor for client node """ super().__init__('Hydrophone_localiser_aclient') self._action_client = ActionClient(self, Hydrophonelocalise, 'hydrophonelocaliser') self.designated_goal = String() self.designated_goal.data = "both" def send_goal(self, request): goal_msg = Hydrophonelocalise.Goal() goal_msg.request = request self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async( goal_msg, feedback_callback=self.feedback_callback) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): """Checks to see whether goal has been accepted or declined Args: future ([type]): [description] """ goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info('Client goal has been rejected.') return self.get_logger().info('Client goal has been accepted.') self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def get_result_callback(self, future): """Checks to see what the result of the functionality is Args: future (goal): Stores result of the functionality of the action server """ result = future.result().result rclpy.shutdown() def feedback_callback(self, feedback_msg): """Provides incremental feedback on the progress of the goal Args: feedback_msg (goal message): the feedback variable """ feedback = feedback_msg.feedback self.get_logger().info('Recieved feedback: {0}'.format( feedback.rawsnapshot)) def set_designated_goal(self, goal): self.designated_goal = goal
class SendGoal(smach.State): #Create state SendGoal def __init__(self): smach.State.__init__(self, outcomes=['success','failed'], input_keys=['move_base_goals_in','from_goal','goal_order'], output_keys=['from_goal']) self.goal_msgs = NavigateToPose.Goal() self.node_ = rclpy.create_node('nav2_client') self.node_.get_logger().info('Created node') self.action_client = ActionClient (self.node_,NavigateToPose,'navigate_to_pose') def execute(self, userdata): if self.action_client.wait_for_server(20)!=1: self.node_.get_logger().error('Action server not available after waiting.') self.destory_node() return 'failed' move_base_goal = userdata.move_base_goals_in[userdata.goal_order[userdata.from_goal]].split(';') userdata.from_goal+=1 self.goal_msgs.pose.header.frame_id='map' self.goal_msgs.pose.header.stamp=self.node_.get_clock().now().to_msg() self.goal_msgs.pose.pose.position.x=float(move_base_goal[0]) self.goal_msgs.pose.pose.position.y=float(move_base_goal[1]) self.goal_msgs.pose.pose.position.z= 0.0 self.goal_msgs.pose.pose.orientation.x = 0.0 self.goal_msgs.pose.pose.orientation.y = 0.0 self.goal_msgs.pose.pose.orientation.z = float(move_base_goal[2]) self.goal_msgs.pose.pose.orientation.w = float(move_base_goal[3]) goal_handle_future=self.action_client.send_goal_async(self.goal_msgs) rclpy.spin_until_future_complete(self.node_,goal_handle_future) goal_handle = goal_handle_future.result() if not goal_handle.accepted: self.node_.get_logger().info('Goal rejected :(') self.destory_node() return 'failed' get_result_future = goal_handle.get_result_async() rclpy.spin_until_future_complete(self.node_,get_result_future) result = get_result_future.result().result status = get_result_future.result().status if status == GoalStatus.STATUS_SUCCEEDED: self.node_.get_logger().info('Succeeded!') elif status == GoalStatus.STATUS_ABORTED: self.node_.get_logger().error('Goal was aborted') self.destory_node() return 'failed' elif status == GoalStatus.STATUS_CANCLED: self.node_.get_logger().error('Goal was canceled') self.destory_node() return 'failed' else: self.node_.get_logger().error('Unknown result code') self.destory_node() return 'failed' if userdata.from_goal == len(userdata.goal_order): self.destory_node() return 'success' def destory_node(self): self.action_client.destroy() self.node_.destroy_node()
class MoveJogRotationClient(Node): def __init__(self): super().__init__('MotionMoveJogRotation') self._action_client = ActionClient(self, MotionJogRotation, 'Motion/MoveJogRotation') self._ret = 0 def send_goal(self, goal_msg): self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async(goal_msg)
class NavigateToPoseActionClient(Node): def __init__(self): super().__init__('navigate_to_pose_action_client') self._action_client = ActionClient(self, NavigateToPose, "navigate_to_pose") self.node = Node def sendGoalToClient(self, posX, posY, yaw = 0): self.goal = NavigateToPose.Goal() self.goal.pose.header.frame_id = "map" # self.goal.pose.header.stamp = self._node.get_clock().now().to_msg() self.goal.pose.pose.position.x = float(posX) self.goal.pose.pose.position.y = float(posY) print("Got Goal!") print(self.goal) # orientation_q = quaternion_from_euler(0, 0, yaw) self.goal.pose.pose.orientation.x = 0.0 self.goal.pose.pose.orientation.y = 0.0 self.goal.pose.pose.orientation.z = 0.0 self.goal.pose.pose.orientation.w = 1.0 # self.goal.target_pose.pose.orientation.x = orientation_q[0] # self.goal.target_pose.pose.orientation.y = orientation_q[1] # self.goal.target_pose.pose.orientation.z = orientation_q[2] # self.goal.target_pose.pose.orientation.w = orientation_q[3] self._action_client.wait_for_server() self._action_client.send_goal_async(self.goal) # self.data = [0, 0] # self.goal = None # def getResultFromClient(self): # if (self.goal): # return self.client.get_result() # else: # return None
class AnimationCapsule: def __init__(self, node: Node): self.node = node self.active = False self.animation_client = ActionClient(self, PlayAnimationAction, 'animation') def play_animation(self, animation): """ plays the animation "ani" and sets the flag "BusyAnimation" :param animation: name of the animation which shall be played """ if self.active: return False if animation is None or animation == "": self.get_logger().warn( "Tried to play an animation with an empty name!") return False first_try = self.animation_client.wait_for_server( Duration( seconds=self.node.get_parameter("hcm/anim_server_wait_time"). get_parameter_value().double_value)) if not first_try: self.get_logger().error( "Animation Action Server not running! Motion can not work without animation action server. " "Will now wait until server is accessible!") self.animation_client.wait_for_server() self.get_logger().warn( "Animation server now running, hcm will go on.") goal = PlayAnimationGoal() goal.animation = animation goal.hcm = False # the animation is from the hcm self.animation_client.send_goal_async(goal, done_cb=self.cb_unset_is_busy) self.active = True def cb_unset_is_busy(self, _p1, _p2): self.active = False def is_animation_busy(self): """Checks if an animation is currently played""" return self.active
class HydrophoneProcessorAClient(Node): def __init__(self): """Constructor for client node """ super().__init__('Hydrophone_processor_aclient') self._action_client = ActionClient(self, Hydrophoneraw, 'hydrophoneprocessor') def send_goal(self, streamseconds): """Send goal function for requesting functionality from action server Args: streamseconds (action): custom action ros file, streamseconds is the goal """ goal_msg = Hydrophoneraw.Goal() goal_msg.streamseconds = streamseconds self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async( goal_msg, feedback_callback=self.feedback_callback) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): """Checks to see whether goal has been accepted or declined Args: future ([type]): [description] """ goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info('Client goal has been rejected.') return self.get_logger().info('Client goal has been accepted.') self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def get_result_callback(self, future): """Checks to see what the result of the functionality is Args: future (goal): Stores result of the functionality of the action server """ result = future.result().result self.get_logger().info('Result: {0}'.format(result.rawdata)) rclpy.shutdown() def feedback_callback(self, feedback_msg): """Provides incremental feedback on the progress of the goal Args: feedback_msg (goal message): the feedback variable """ feedback = feedback_msg.feedback self.get_logger().info('Recieved feedback: {0}'.format( feedback.rawsnapshot))
def single_run(self, goal): def done_cb(state, result): assert state == GoalStatus.SUCCEEDED, "Kick was not successful" self.kick_succeeded = True self.kick_succeeded = False sub = MockSubscriber('kick_motor_goals', JointCommand) self.with_assertion_grace_period( lambda: self.assertNodeRunning("dynamic_kick"), 20) client = ActionClient(self, KickAction, 'dynamic_kick') assert client.wait_for_server(), "Kick action server not running" client.send_goal_async(goal) client.done_cb = done_cb client.wait_for_result() sub.wait_until_connected() sub.assertMessageReceived() assert self.kick_succeeded, "Kick was not successful"
class MinimalActionClient(Node): def __init__(self): super().__init__('minimal_action_client') self._action_client = ActionClient(self, Fibonacci, 'fibonacci') def cancel_done(self, future): cancel_response = future.result() if len(cancel_response.goals_canceling) > 0: self.get_logger().info('Goal successfully canceled') else: self.get_logger().info('Goal failed to cancel') rclpy.shutdown() def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info('Goal rejected :(') return self._goal_handle = goal_handle self.get_logger().info('Goal accepted :)') # Start a 2 second timer self._timer = self.create_timer(2.0, self.timer_callback) def feedback_callback(self, feedback): self.get_logger().info('Received feedback: {0}'.format(feedback.feedback.sequence)) def timer_callback(self): self.get_logger().info('Canceling goal') # Cancel the goal future = self._goal_handle.cancel_goal_async() future.add_done_callback(self.cancel_done) # Cancel the timer self._timer.cancel() def send_goal(self): self.get_logger().info('Waiting for action server...') self._action_client.wait_for_server() goal_msg = Fibonacci.Goal() goal_msg.order = 10 self.get_logger().info('Sending goal request...') self._send_goal_future = self._action_client.send_goal_async( goal_msg, feedback_callback=self.feedback_callback) self._send_goal_future.add_done_callback(self.goal_response_callback)
def test_send_goal_async(self): ac = ActionClient(self.node, Fibonacci, 'fibonacci') try: self.assertTrue(ac.wait_for_server(timeout_sec=2.0)) future = ac.send_goal_async(Fibonacci.Goal()) rclpy.spin_until_future_complete(self.node, future, self.executor) self.assertTrue(future.done()) goal_handle = future.result() self.assertTrue(goal_handle.accepted) finally: ac.destroy()
def one_run(self, direction): def done_cb(state, result): assert state == GoalStatus.SUCCEEDED, "Dynup was not successful" self.kick_succeeded = True self.kick_succeeded = False sub = MockSubscriber('dynup_motor_goals', JointCommand) self.with_assertion_grace_period( lambda: self.assertNodeRunning("dynup"), 20) client = ActionClient(self, DynUpAction, 'dynup') assert client.wait_for_server(), "Dynup action server not running" goal = DynUpGoal() goal.direction = direction client.send_goal_async(goal) client.done_cb = done_cb client.wait_for_result() sub.wait_until_connected() sub.assertMessageReceived() assert self.kick_succeeded, "Dynup was not successful"
class FibonacciActionClient(Node): def __init__(self): super().__init__('fibonacci_action_client') self.action_client = ActionClient(self, Fibonacci, 'fibonacci') self.goal_handle = None self.get_logger().info('=== Fibonacci Action Client Started ====') def send_goal(self, order): goal_msg = Fibonacci.Goal() goal_msg.order = order if self.action_client.wait_for_server(10) is False: self.get_logger().error('Server Not exists') self._send_goal_future = self.action_client.send_goal_async( goal_msg, feedback_callback=self.feedback_callback) self._send_goal_future.add_done_callback(self.goal_response_callback) def feedback_callback(self, feedback_msg): feedback = feedback_msg.feedback self.get_logger().info( f'Received feedback: {feedback.partial_sequence}') def goal_response_callback(self, future): self.goal_handle = future.result() if not self.goal_handle.accepted: self.get_logger().info('Goal rejected') return self.get_logger().info('Goal accepted') # Start a 2 second timer self.timer = self.create_timer(2.0, self.timer_callback) def timer_callback(self): self.get_logger().info('Canceling goal') # Cancel the goal future = self.goal_handle.cancel_goal_async() future.add_done_callback(self.cancel_done) # Cancel the timer self.timer.cancel() def cancel_done(self, future): cancel_response = future.result() if len(cancel_response.goals_canceling) > 0: self.get_logger().info('Goal successfully canceled') else: self.get_logger().info('Goal failed to cancel') rclpy.shutdown()
class MinimalActionClient(Node): def __init__(self): super().__init__('minimal_action_client') self._action_client = ActionClient(self, Fibonacci, 'fibonacci') def cancel_done(self, future): cancel_response = future.result() if len(cancel_response.goals_canceling) > 0: self.get_logger().info('Goal successfully canceled') else: self.get_logger().info('Goal failed to cancel') rclpy.shutdown() def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info('Goal rejected :(') return self._goal_handle = goal_handle self.get_logger().info('Goal accepted :)') # Start a 2 second timer self._timer = self.create_timer(2.0, self.timer_callback) def feedback_callback(self, feedback): self.get_logger().info('Received feedback: {0}'.format( feedback.feedback.sequence)) def timer_callback(self): self.get_logger().info('Canceling goal') # Cancel the goal future = self._goal_handle.cancel_goal_async() future.add_done_callback(self.cancel_done) # Cancel the timer self._timer.cancel() def send_goal(self): self.get_logger().info('Waiting for action server...') self._action_client.wait_for_server() goal_msg = Fibonacci.Goal() goal_msg.order = 10 self.get_logger().info('Sending goal request...') self._send_goal_future = self._action_client.send_goal_async( goal_msg, feedback_callback=self.feedback_callback) self._send_goal_future.add_done_callback(self.goal_response_callback)
def test_send_goal_multiple(self): ac = ActionClient(self.node, Fibonacci, 'fibonacci', callback_group=ReentrantCallbackGroup()) executor = MultiThreadedExecutor(context=self.context) try: self.assertTrue(ac.wait_for_server(timeout_sec=2.0)) future_0 = ac.send_goal_async(Fibonacci.Goal()) future_1 = ac.send_goal_async(Fibonacci.Goal()) future_2 = ac.send_goal_async(Fibonacci.Goal()) rclpy.spin_until_future_complete(self.node, future_0, executor) rclpy.spin_until_future_complete(self.node, future_1, executor) rclpy.spin_until_future_complete(self.node, future_2, executor) self.assertTrue(future_0.done()) self.assertTrue(future_1.done()) self.assertTrue(future_2.done()) self.assertTrue(future_0.result().accepted) self.assertTrue(future_1.result().accepted) self.assertTrue(future_2.result().accepted) finally: ac.destroy()
class FibonacciActionClient(Node): def __init__(self): super().__init__('fibonacci_action_client') self._action_client = ActionClient(self, Fibonacci, 'fibonacci') def send_goal(self, order): goal_msg = Fibonacci.Goal() goal_msg.order = order self._action_client.wait_for_server() return self._action_client.send_goal_async(goal_msg)
class MoveEyeClientVertical(Node): def __init__(self): super().__init__('eye_client_node') # Action client to send goals to jaw controller self.eye_action_client_vertical = ActionClient(self, FollowJointTrajectory, '/eyes_controller/follow_joint_trajectory') # Subscriber to get data from face tracker self.get_data = self.create_subscription( Float32, 'move_eye_vertical', self.send_goal, 10, ) def send_goal(self, position): # Message type in JointTrajectory goal_msg = JointTrajectory() # Joint trajectory points jtp = JointTrajectoryPoint() jtp.velocities = [0.0] jtp.time_from_start.sec = 0 jtp.time_from_start.nanosec = 0 jtp.positions = [position.data] # Build message goal_msg.points = [jtp] goal_msg.joint_names = ["eyes_shift_vertical_joint"] # Assign goal goal = FollowJointTrajectory.Goal() goal.trajectory = goal_msg self.eye_action_client_vertical.wait_for_server() self.get_logger().info('Goal: %f' % jtp.positions[0]) self.eye_action_client_vertical.send_goal_async(goal)
class HelloClient(Node): def __init__(self): super().__init__('hello_action_test_client') self.hello_client = ActionClient(self, Hello, 'hello_action') def goal_responseCB(self, future): # dunno what the future handles... goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info( 'Goal rejected: {0.accepted}'.format(goal_handle)) return self.get_logger().info( 'Goal accepted: {0.accepted}'.format(goal_handle)) self.get_result_future = goal_handle.get_result_async() self.get_result_future.add_done_callback(self.get_resultCB) def feedbackCB(self, feedback): # Gets feedback data by "goal_handle.publish_feedback" function from server self.get_logger().info('Received feedback:\n{0.status}'.format( feedback.feedback)) def get_resultCB(self, future): result = future.result().result status = future.result( ).status # probably "goal_handle.succeed()" from server? if status == GoalStatus.STATUS_SUCCEEDED: self.get_logger().info( 'Goal succeeded!\n\t Result: {0.status}'.format(result)) else: self.get_logger().info( 'Goal failed with status: {}'.format(status)) def send_goal(self): self.get_logger().info('Waiting for action server...') self.hello_client.wait_for_server() goal_msg = Hello.Goal() goal_msg.hello_goal = 'Hello World!' self.get_logger().info('Sending goal request...') self.send_goal_future = self.hello_client.send_goal_async( goal_msg, feedback_callback=self.feedbackCB) self.send_goal_future.add_done_callback(self.goal_responseCB) def destroy(self): self.hello_client.destroy() self.destroy_node()
def send_goals(node, action_type, tests): import rclpy from rclpy.action import ActionClient action_client = ActionClient(node, action_type, 'test/action/' + action_type.__name__) if not action_client.wait_for_server(20): print('Action server not available after waiting', file=sys.stderr) return 1 test_index = 0 while rclpy.ok() and test_index < len(tests): print('Sending goal for test number {}'.format(test_index), file=sys.stderr) test = tests[test_index] invalid_feedback = False # On feedback, check the feedback is valid def feedback_callback(feedback): nonlocal invalid_feedback if not test.is_feedback_valid(feedback): invalid_feedback = True goal_handle_future = action_client.send_goal_async( test.goal, feedback_callback=feedback_callback) rclpy.spin_until_future_complete(node, goal_handle_future) goal_handle = goal_handle_future.result() if not goal_handle.accepted: print('Goal rejected', file=sys.stderr) return 1 get_result_future = goal_handle.get_result_async() rclpy.spin_until_future_complete(node, get_result_future) result = get_result_future.result() if not test.is_result_valid(result): return 1 if invalid_feedback: return 1 test_index += 1 return 0
class MinimalActionClient(Node): def __init__(self): super().__init__('minimal_action_client') self._action_client = ActionClient(self, Fibonacci, 'fibonacci') def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info('Goal rejected :(') return self.get_logger().info('Goal accepted :)') self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def feedback_callback(self, feedback): self.get_logger().info('Received feedback: {0}'.format(feedback.feedback.sequence)) def get_result_callback(self, future): result = future.result().result status = future.result().status if status == GoalStatus.STATUS_SUCCEEDED: self.get_logger().info('Goal succeeded! Result: {0}'.format(result.sequence)) else: self.get_logger().info('Goal failed with status: {0}'.format(status)) # Shutdown after receiving a result rclpy.shutdown() def send_goal(self): self.get_logger().info('Waiting for action server...') self._action_client.wait_for_server() goal_msg = Fibonacci.Goal() goal_msg.order = 10 self.get_logger().info('Sending goal request...') self._send_goal_future = self._action_client.send_goal_async( goal_msg, feedback_callback=self.feedback_callback) self._send_goal_future.add_done_callback(self.goal_response_callback)