@@ -42,30 +42,28 @@ def send_goal(self, desired_position: Point):
42
42
while not self .action_client .wait_for_server (timeout_sec = 1.0 ):
43
43
self .get_logger ().info (f'service { self .action_client .action_name } not available, waiting...' )
44
44
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 )
47
46
self .send_goal_future .add_done_callback (self .goal_response_callback )
48
47
49
48
def goal_response_callback (self , future ):
50
49
goal_handle = future .result ()
50
+ self .get_logger ().info (f'The class of the future.result() is { goal_handle .__class__ } ' )
51
+
51
52
if not goal_handle .accepted :
52
- self .get_logger ().info ('Goal rejected :( ' )
53
+ self .get_logger ().info ('Goal was rejected by the server. ' )
53
54
return
54
-
55
- self .get_logger ().info ('Goal accepted :)' )
55
+ self .get_logger ().info ('Goal was accepted by the server.' )
56
56
57
57
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 )
59
59
60
- def get_result_callback (self , future ):
60
+ def action_result_callback (self , future ):
61
61
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 ))
64
63
65
- def feedback_callback (self , feedback_msg ):
64
+ def action_feedback_callback (self , feedback_msg ):
66
65
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 ))
69
67
70
68
71
69
def main (args = None ):
@@ -78,6 +76,12 @@ def main(args=None):
78
76
79
77
node = MoveStraightIn2DActionClientNode ()
80
78
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
+
81
85
rclpy .spin (node )
82
86
except KeyboardInterrupt :
83
87
pass
0 commit comments