1
+ #!/usr/bin/env python3
2
+ import rclpy
3
+ from rclpy .node import Node
4
+ from rclpy .action import ActionClient
5
+ from rclpy .action .client import ClientGoalHandle
6
+ from arm_interface .action import ExecArmAction
7
+
8
+ ### CONSTANT ###
9
+ # ID for action
10
+ MOVE_HOME_POS = 1
11
+ MOVE_READY_POS = 2
12
+ MOVE_PUT_TO_STOCK = 3
13
+ MOVE_GET_FROM_STOCK = 4
14
+ MOVE_GRAB = 5
15
+ MOVE_RELEASE = 6
16
+
17
+ class ArmControllerClient (Node ):
18
+ def __init__ (self ):
19
+ super ().__init__ ('arm_controller_client' )
20
+ self .exec_arm_action_client_ = ActionClient (
21
+ self ,
22
+ ExecArmAction ,
23
+ "arm_action" )
24
+ self .get_logger ().info ('Arm Controller Client Started' )
25
+
26
+ def send_request (self , action_id ):
27
+ self .exec_arm_action_client_ .wait_for_server ()
28
+
29
+ goal_msg = ExecArmAction .Goal ()
30
+ goal_msg .action_id = action_id
31
+ self .get_logger ().info ('Sending request for action: %d' % action_id )
32
+ self .exec_arm_action_client_ .send_goal_async (goal_msg ).add_done_callback (self .goal_response_callback )
33
+
34
+ def goal_response_callback (self , future ):
35
+ self .goal_handle_ : ClientGoalHandle = future .result ()
36
+ if self .goal_handle_ .accepted :
37
+ self .get_logger ().info ('Goal accepted' )
38
+ self .goal_handle_ .get_result_async ().add_done_callback (self .get_result_callback )
39
+
40
+ def get_result_callback (self , future ):
41
+ result = future .result ().result
42
+ self .get_logger ().info ('Result received: %d' % result .action_result )
43
+
44
+ def main (args = None ):
45
+ try :
46
+ action_request = 1
47
+ rclpy .init (args = args )
48
+ arm_controller_client = ArmControllerClient ()
49
+ arm_controller_client .send_request (action_request )
50
+ rclpy .spin (arm_controller_client )
51
+ except KeyboardInterrupt :
52
+ rclpy .shutdown ()
53
+
54
+ if __name__ == '__main__' :
55
+ main ()
0 commit comments