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 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
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 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 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
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 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 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))
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 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)
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)
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)
async def execute_callback(self, goal_handle):
"""タスク処理
"""
global executor
task_id = goal_handle.request.task_id
data_str = goal_handle.request.arg_json
if data_str != '':
self.arg_data = json.loads(data_str)
else:
self.arg_data = {}
# syntax analyze
task_tree = await self.convert_task_to_subtasks(task_id, self.arg_data)
if task_tree == []: # ERROR
goal_handle.abort()
result = TsReq.Result()
result.message = "Syntax Error"
return result
# generate task_node
task_node = TaskNode(task_tree)
self.task_node_list.append(task_node)
executor.add_node(self.task_node_list[len(self.task_node_list) - 1]) # added last added task
## execute
client = ActionClient(self, TsDoTask, task_node.name, callback_group=ReentrantCallbackGroup())
client.wait_for_server()
goal = TsDoTask.Goal()
goal_handle_client = await client.send_goal_async(goal)
self.goal_handles.append(goal_handle_client)
if not goal_handle_client.accepted:
self.get_logger().info("goal reject")
goal_handle.abort()
result = TsReq.Result()
result.message = "Goal Reject"
return result
self.get_logger().info("goal accept")
self.future_result = await goal_handle_client.get_result_async()
#if not self.future_result.done():
if True:
goal_handle.abort()
result = TsReq.Result()
result.message = "Abort"
return result
# return result
goal_handle.succeed()
result = TsReq.Result()
result.message = "Success"
task_node.destroy_node()
return result
class Turtle_Action_Client(Node):
def __init__(self):
super().__init__('turtle_action_client')
self.turtle_action = ActionClient(self, Go2Goal, 'Coor64')
self.goalx = input("goal x coordinate:")
self.goaly = input("goal y coordinate:")
self.send_goal()
def send_goal(self):
self.get_logger().info('Waiting for action server...')
self.turtle_action.wait_for_server()
goal_action = Go2Goal.Goal()
goal_action.x = int(self.goalx)
goal_action.y = int(self.goaly)
self.goal_future = self.turtle_action.send_goal_async(
goal_action, feedback_callback=self.feedback_callback)
self.goal_future.add_done_callback(self.goal_callback)
def goal_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
print('Goal was rejected')
return
self.get_logger().info('Goal was accepted, will procced...')
self.result_future = goal_handle.get_result_async()
self.result_future.add_done_callback(self.result_callback)
def feedback_callback(self, feedback_action):
feedback = feedback_action.feedback
self.get_logger().info('Remaining distance for Turtle 1: {0}'.format(
feedback.remaining_distance1))
print("Turtle 1 Pose: (", feedback.turtle1_x, ",", feedback.turtle1_y,
")")
self.get_logger().info('Remaining distance for Turtle 2: {0}'.format(
feedback.remaining_distance2))
print("Turtle 2 Pose: (", feedback.turtle2_x, ",", feedback.turtle2_y,
")")
def result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
print('Goal succeeded! Duration:', result.total_time)
print('The Winner is', result.winner)
else:
print('Goal could not be reached')
rclpy.shutdown()
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 main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_action_client')
action_client = ActionClient(node, Fibonacci, 'fibonacci')
node.get_logger().info('Waiting for action server...')
action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
node.get_logger().info('Sending goal request...')
send_goal_future = action_client.send_goal_async(
goal_msg,
feedback_callback=lambda feedback: feedback_cb(node.get_logger(),
feedback))
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
if not goal_handle.accepted:
node.get_logger().info('Goal rejected :(')
action_client.destroy()
node.destroy_node()
rclpy.shutdown()
return
node.get_logger().info('Goal accepted :)')
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
result = get_result_future.result().result
status = get_result_future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
node.get_logger().info('Goal succeeded! Result: {0}'.format(
result.sequence))
else:
node.get_logger().info(
'Goal failed with status code: {0}'.format(status))
action_client.destroy()
node.destroy_node()
rclpy.shutdown()
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)
async def subtask(self, goal_handle, task_tree):
# print("subtask")
command = task_tree[1]
subtask_client = ActionClient(self,
TsDoSubtask,
"subtask_node_" + str(command[0]),
callback_group=ReentrantCallbackGroup())
if not subtask_client.wait_for_server(timeout_sec=5.0):
self.get_logger().error('No action server available')
goal_handle.abort()
result = TsReq.Result()
result.message = "Abort"
return result
goal_msg = TsDoSubtask.Goal()
if len(command) >= 2:
goal_msg.arg_json = command[1]
else:
goal_msg.arg_json = "{}"
print(f'subtask: subtask_node_{command[0]} args:{goal_msg.arg_json}')
subtask_goalhandle = await subtask_client.send_goal_async(goal_msg)
self.subtask_goalhandles[
subtask_goalhandle.goal_id.uuid.tostring()] = subtask_goalhandle
if not subtask_goalhandle.accepted:
self.get_logger().info("goal rejected")
del self.subtask_goalhandles[
subtask_goalhandle.goal_id.uuid.tostring()]
return "Abort"
future_result = await subtask_goalhandle.get_result_async()
del self.subtask_goalhandles[
subtask_goalhandle.goal_id.uuid.tostring()]
self.get_logger().info(f"returned {future_result.result.message}")
return future_result.result.message
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()
async def create_tasknode(self, task_tree):
global executor
# generate task_node
child_tasknode = TaskNode(task_tree)
# self.child_tasknode.append(self.childtask_node)
executor.add_node(child_tasknode) # added last added task
## execute
client = ActionClient(self,
TsDoTask,
child_tasknode.name,
callback_group=ReentrantCallbackGroup())
if not client.wait_for_server(timeout_sec=5.0):
self.get_logger().error('No action server available')
goal_handle.abort()
result = TsDoTask.Result()
result.message = "Abort"
return result
goal = TsDoTask.Goal()
goal_handle_client = await client.send_goal_async(goal)
if not goal_handle_client.accepted:
self.get_logger().info("goal reject")
goal_handle.abort()
result = TsDoTask.Result()
result.message = "Goal Reject"
return result
self.get_logger().info("goal accept")
return goal_handle_client, child_tasknode
class MinClient(Node):
def __init__(self):
super().__init__('min_client')
self._action_client = ActionClient(self, RunVirtualRail,
'run_virtual_rail')
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)
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)
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 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 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)
class TTAC3ActionClient(Node):
def __init__(self):
super().__init__('ttac3_action_client')
self._action_client = ActionClient(
self,
TTAC3,
'ttac3')
def send_goal(self, xyz_goal):
goal_msg = TTAC3.Goal()
goal_msg.xyz_goal = xyz_goal
self._action_client.wait_for_server()
self._send_goal_future = self._action_client.send_goal_async(
goal_msg,
feedback_callback=self.feedback_callback
)
self.get_logger().info('Send Goal Async')
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('Received feedback: {0}'.format(feedback.current_state))
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.is_success))
rclpy.shutdown()
class MinimalActionClient(Node):
def __init__(self):
super().__init__('minimal_action_client')
# fibonacciアクションクライアントを作成
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: {0}'.format(
feedback.sequence))
def get_result_callback(self, future):
# アクションの実行状態の取得
status = future.result().action_status
if status == GoalStatus.STATUS_SUCCEEDED:
# 実行成功ならフィボナッチ数列を標準出力にログ
self.get_logger().info('result: {0}'.format(
future.result().sequence))
# プログラムの終了
rclpy.shutdown()
def send_goal(self):
self.get_logger().info('waiting...')
self._action_client.wait_for_server()
# 10要素のフィボナッチ数列を標値に設定
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 SortActionClient(Node):
def __init__(self):
super().__init__('sort_action_client')
self._action_client = ActionClient(self, Sort, 'sorter')
def send_goal(self, order):
goal_msg = Sort.Goal()
goal_msg.unsorted = random.sample(range(1, 100000), 2048)
self._action_client.wait_for_server()
#self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
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('Received feedback: {0} percent done'.format(
feedback.percent))
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
rclpy.shutdown()
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
if result.sorted.tolist() == sorted(result.sorted):
self.get_logger().info('Data is sorted!')
else:
self.get_logger().info('Data is NOT sorted!')
rclpy.shutdown()
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 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
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
