23
23
"""
24
24
import rclpy
25
25
from rclpy .action import ActionClient
26
+ from rclpy .action .client import ClientGoalHandle
26
27
from rclpy .node import Node
28
+ from rclpy .task import Future
27
29
28
30
from geometry_msgs .msg import Point
29
31
from package_with_interfaces .action import MoveStraightIn2D
@@ -33,37 +35,42 @@ class MoveStraightIn2DActionClientNode(Node):
33
35
34
36
def __init__ (self ):
35
37
super ().__init__ ('move_straight_in_2d_action_client' )
38
+
36
39
self .action_client = ActionClient (self , MoveStraightIn2D , '/move_straight_in_2d' )
37
40
38
- def send_goal (self , desired_position : Point ):
41
+ self .send_goal_future = None # This will be used in `send_goal`
42
+ self .get_result_future = None # This will be used in 'goal_response_callback'
43
+
44
+ def send_goal_async (self , desired_position : Point ) -> None :
39
45
goal_msg = MoveStraightIn2D .Goal ()
40
46
goal_msg .desired_position = desired_position
41
47
42
48
while not self .action_client .wait_for_server (timeout_sec = 1.0 ):
43
49
self .get_logger ().info (f'service { self .action_client } not available, waiting...' )
44
50
51
+ self .get_logger ().info (f'Sending goal: { desired_position } .' )
52
+
45
53
self .send_goal_future = self .action_client .send_goal_async (goal_msg , feedback_callback = self .action_feedback_callback )
46
54
self .send_goal_future .add_done_callback (self .goal_response_callback )
47
55
48
- def goal_response_callback (self , future ):
49
- goal_handle = future .result ()
50
- self .get_logger ().info (f'The class of the future.result() is { goal_handle .__class__ } ' )
56
+ def goal_response_callback (self , future : Future ) -> None :
57
+ goal : ClientGoalHandle = future .result ()
51
58
52
- if not goal_handle .accepted :
59
+ if not goal .accepted :
53
60
self .get_logger ().info ('Goal was rejected by the server.' )
54
61
return
55
62
self .get_logger ().info ('Goal was accepted by the server.' )
56
63
57
- self .get_result_future = goal_handle .get_result_async ()
64
+ self .get_result_future = goal .get_result_async ()
58
65
self .get_result_future .add_done_callback (self .action_result_callback )
59
66
60
- def action_result_callback (self , future ) :
61
- result = future .result (). result
62
- self .get_logger ().info (f'Result : { result .final_position } ' )
67
+ def action_result_callback (self , future : Future ) -> None :
68
+ response : MoveStraightIn2D . Response = future .result ()
69
+ self .get_logger ().info (f'Final position was : { response . result .final_position } . ' )
63
70
64
- def action_feedback_callback (self , feedback_msg ) :
71
+ def action_feedback_callback (self , feedback_msg : MoveStraightIn2D . Feedback ) -> None :
65
72
feedback = feedback_msg .feedback
66
- self .get_logger ().info (f'Received feedback: { feedback .distance } ' )
73
+ self .get_logger ().info (f'Received feedback distance : { feedback .distance } . ' )
67
74
68
75
69
76
def main (args = None ):
@@ -80,7 +87,7 @@ def main(args=None):
80
87
desired_position = Point ()
81
88
desired_position .x = 1.0
82
89
desired_position .y = - 1.0
83
- node .send_goal (desired_position )
90
+ node .send_goal_async (desired_position )
84
91
85
92
rclpy .spin (node )
86
93
except KeyboardInterrupt :
0 commit comments