Skip to content

Commit 673212a

Browse files
tanghuongcamtrietngh
authored andcommittedMay 7, 2024
STABLE : publisher and topic arm controller done
1 parent 2f7b536 commit 673212a

File tree

3 files changed

+31
-21
lines changed

3 files changed

+31
-21
lines changed
 

‎src/arm_controller/arm_controller/arm_controller_client.py

+13-9
Original file line numberDiff line numberDiff line change
@@ -5,22 +5,22 @@
55
from rclpy.action.client import ClientGoalHandle
66
from arm_interface.action import ExecArmAction
77

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
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
1616

1717
class ArmControllerClient(Node):
1818
def __init__(self):
1919
super().__init__('arm_controller_client')
2020
self.exec_arm_action_client_ = ActionClient(
2121
self,
2222
ExecArmAction,
23-
"arm_action")
23+
"ArmController")
2424
self.get_logger().info('Arm Controller Client Started')
2525

2626
def send_request(self, action_id):
@@ -36,6 +36,8 @@ def goal_response_callback(self, future):
3636
if self.goal_handle_.accepted:
3737
self.get_logger().info('Goal accepted')
3838
self.goal_handle_.get_result_async().add_done_callback(self.get_result_callback)
39+
else:
40+
self.get_logger().info('Goal rejected')
3941

4042
def get_result_callback(self, future):
4143
result = future.result().result
@@ -47,6 +49,8 @@ def main(args=None):
4749
rclpy.init(args=args)
4850
arm_controller_client = ArmControllerClient()
4951
arm_controller_client.send_request(action_request)
52+
arm_controller_client.send_request(action_request+1)
53+
arm_controller_client.send_request(action_request+2)
5054
rclpy.spin(arm_controller_client)
5155
except KeyboardInterrupt:
5256
rclpy.shutdown()

‎src/arm_controller/arm_controller/arm_controller_server.py

+17-12
Original file line numberDiff line numberDiff line change
@@ -58,31 +58,36 @@ def arm_execute_callback(self, goal_handle : ServerGoalHandle):
5858
result.action_result = True
5959
return result
6060

61-
def send_arm_cmd(self, action_id):
61+
def send_arm_cmd(self, id_action):
6262
msg = ServoCmd()
6363
msg.dest = ARM_STM_ID
6464
#Switch case on the type of command
65+
print(f"Action ID: {id_action}")
6566
try:
66-
match action_id :
67+
match id_action :
6768
case ArmAction.MOVE_HOME_POS.value:
68-
msg.command_id = ArmCmdID.CMD_ID_MOVE_HOME_POS
69+
msg.command_id = ArmCmdID.CMD_ID_MOVE_HOME_POS.value
6970
case ArmAction.MOVE_READY_POS.value:
70-
msg.command_id = ArmCmdID.CMD_ID_MOVE_READY_POS
71+
msg.command_id = ArmCmdID.CMD_ID_MOVE_READY_POS.value
7172
case ArmAction.MOVE_PUT_TO_STOCK.value:
72-
msg.command_id = ArmCmdID.CMD_ID_MOVE_PUT_TO_STOCK
73+
msg.command_id = ArmCmdID.CMD_ID_MOVE_PUT_TO_STOCK.value
7374
case ArmAction.MOVE_GET_FROM_STOCK.value:
74-
msg.command_id = ArmCmdID.CMD_ID_MOVE_GET_FROM_STOCK
75+
msg.command_id = ArmCmdID.CMD_ID_MOVE_GET_FROM_STOCK.value
7576
case ArmAction.MOVE_GRAB.value:
76-
msg.command_id = ArmCmdID.CMD_ID_MOVE_GRAB
77+
msg.command_id = ArmCmdID.CMD_ID_MOVE_GRAB.value
7778
case ArmAction.MOVE_RELEASE.value:
78-
msg.command_id = ArmCmdID.CMD_ID_MOVE_RELEASE
79+
msg.command_id = ArmCmdID.CMD_ID_MOVE_RELEASE.value
7980
case _:
8081
pass
81-
except:
82-
self.get_logger().info('Invalid Herkulek action ID')
83-
return
82+
except Exception as err :
83+
print(f"Unexpected {err=}, {type(err)=}")
84+
pass
8485
# Publish the message
85-
self.arm_pub_.publish(msg)
86+
if(msg.command_id != 0):
87+
self.get_logger().info('Sending command: %d' % msg.command_id)
88+
self.arm_pub_.publish(msg)
89+
else:
90+
self.get_logger().info('Invalid action ID: %d' % id_action)
8691

8792

8893
def main(args=None):

‎src/arm_controller/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99

1010
<depend>rclpy</depend>
1111
<depend>arm_interface</depend>
12+
<depend>can_interface</depend>
1213

1314
<test_depend>ament_copyright</test_depend>
1415
<test_depend>ament_flake8</test_depend>

0 commit comments

Comments
 (0)
Please sign in to comment.