Skip to content

Commit 2f7b536

Browse files
committedMay 7, 2024
Add basic action client for arm_controller
1 parent eb14e64 commit 2f7b536

File tree

3 files changed

+57
-1
lines changed

3 files changed

+57
-1
lines changed
 
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
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()

‎src/arm_controller/arm_controller/arm_controller_server.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ class ArmCmdID(Enum):
3232
class ArmController(Node):
3333
def __init__(self):
3434
super().__init__('arm_controller')
35-
self.exec_arm_action_ = ActionServer(
35+
self.exec_arm_action_server_ = ActionServer(
3636
self,
3737
ExecArmAction,
3838
"ArmController",

‎src/arm_controller/setup.py

+1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
entry_points={
2222
'console_scripts': [
2323
'arm_controller_server = arm_controller.arm_controller_server:main',
24+
'arm_controller_client = arm_controller.arm_controller_client:main'
2425
],
2526
},
2627
)

0 commit comments

Comments
 (0)
Please sign in to comment.