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)