Skip to content

Commit 85d5ada

Browse files
author
Murilo Marinho
committed
[move_straight_in_2d_action_client_node.py] Adjusting sample node to match the intended operation.
1 parent 5c009e3 commit 85d5ada

File tree

1 file changed

+16
-12
lines changed

1 file changed

+16
-12
lines changed

ros2_tutorial_workspace/src/python_package_that_uses_the_actions/python_package_that_uses_the_actions/move_straight_in_2d_action_client_node.py

Lines changed: 16 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -42,30 +42,28 @@ def send_goal(self, desired_position: Point):
4242
while not self.action_client.wait_for_server(timeout_sec=1.0):
4343
self.get_logger().info(f'service {self.action_client.action_name} not available, waiting...')
4444

45-
self.send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
46-
45+
self.send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.action_feedback_callback)
4746
self.send_goal_future.add_done_callback(self.goal_response_callback)
4847

4948
def goal_response_callback(self, future):
5049
goal_handle = future.result()
50+
self.get_logger().info(f'The class of the future.result() is {goal_handle.__class__}')
51+
5152
if not goal_handle.accepted:
52-
self.get_logger().info('Goal rejected :(')
53+
self.get_logger().info('Goal was rejected by the server.')
5354
return
54-
55-
self.get_logger().info('Goal accepted :)')
55+
self.get_logger().info('Goal was accepted by the server.')
5656

5757
self.get_result_future = goal_handle.get_result_async()
58-
self.get_result_future.add_done_callback(self.get_result_callback)
58+
self.get_result_future.add_done_callback(self.action_result_callback)
5959

60-
def get_result_callback(self, future):
60+
def action_result_callback(self, future):
6161
result = future.result().result
62-
self.get_logger().info('Result: {0}'.format(result.sequence))
63-
rclpy.shutdown()
62+
self.get_logger().info('Result: {0}'.format(result.final_position))
6463

65-
def feedback_callback(self, feedback_msg):
64+
def action_feedback_callback(self, feedback_msg):
6665
feedback = feedback_msg.feedback
67-
self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence))
68-
66+
self.get_logger().info('Received feedback: {0}'.format(feedback.distance))
6967

7068

7169
def main(args=None):
@@ -78,6 +76,12 @@ def main(args=None):
7876

7977
node = MoveStraightIn2DActionClientNode()
8078

79+
# Send the goal once and then do nothing until the user shuts this node down.
80+
desired_position = Point()
81+
desired_position.x = 1.0
82+
desired_position.y = -1.0
83+
node.send_goal(desired_position)
84+
8185
rclpy.spin(node)
8286
except KeyboardInterrupt:
8387
pass

0 commit comments

Comments
 (0)