STABLE : publisher and topic arm controller done

Author: tanghuongcam

src/arm_controller/arm_controller/arm_controller_client.py

@@ -5,22 +5,22 @@
 from rclpy.action.client import ClientGoalHandle
 from arm_interface.action import ExecArmAction
 
-### CONSTANT ###
-# ID for action
-MOVE_HOME_POS = 1
-MOVE_READY_POS = 2
-MOVE_PUT_TO_STOCK = 3
-MOVE_GET_FROM_STOCK = 4
-MOVE_GRAB = 5
-MOVE_RELEASE = 6
+# ### CONSTANT ###
+# # ID for action
+# MOVE_HOME_POS = 1
+# MOVE_READY_POS = 2
+# MOVE_PUT_TO_STOCK = 3
+# MOVE_GET_FROM_STOCK = 4
+# MOVE_GRAB = 5
+# MOVE_RELEASE = 6
 
 class ArmControllerClient(Node):
     def __init__(self):
         super().__init__('arm_controller_client')
         self.exec_arm_action_client_ = ActionClient(
             self,
             ExecArmAction,
-            "arm_action")
+            "ArmController")
         self.get_logger().info('Arm Controller Client Started')
 
     def send_request(self, action_id):
@@ -36,6 +36,8 @@ def goal_response_callback(self, future):
         if self.goal_handle_.accepted:
             self.get_logger().info('Goal accepted')
             self.goal_handle_.get_result_async().add_done_callback(self.get_result_callback)
+        else:
+            self.get_logger().info('Goal rejected')
 
     def get_result_callback(self, future):
         result = future.result().result
@@ -47,6 +49,8 @@ def main(args=None):
         rclpy.init(args=args)
         arm_controller_client = ArmControllerClient()
         arm_controller_client.send_request(action_request)
+        arm_controller_client.send_request(action_request+1)
+        arm_controller_client.send_request(action_request+2)
         rclpy.spin(arm_controller_client)
     except KeyboardInterrupt:
         rclpy.shutdown()

src/arm_controller/arm_controller/arm_controller_server.py

@@ -58,31 +58,36 @@ def arm_execute_callback(self, goal_handle : ServerGoalHandle):
         result.action_result = True
         return result
 
-    def send_arm_cmd(self, action_id):
+    def send_arm_cmd(self, id_action):
         msg = ServoCmd()
         msg.dest = ARM_STM_ID
         #Switch case on the type of command 
+        print(f"Action ID: {id_action}")
         try:
-            match action_id :
+            match id_action :
                 case ArmAction.MOVE_HOME_POS.value:
-                    msg.command_id = ArmCmdID.CMD_ID_MOVE_HOME_POS
+                    msg.command_id = ArmCmdID.CMD_ID_MOVE_HOME_POS.value
                 case ArmAction.MOVE_READY_POS.value:
-                    msg.command_id = ArmCmdID.CMD_ID_MOVE_READY_POS
+                    msg.command_id = ArmCmdID.CMD_ID_MOVE_READY_POS.value
                 case ArmAction.MOVE_PUT_TO_STOCK.value:
-                    msg.command_id = ArmCmdID.CMD_ID_MOVE_PUT_TO_STOCK
+                    msg.command_id = ArmCmdID.CMD_ID_MOVE_PUT_TO_STOCK.value
                 case ArmAction.MOVE_GET_FROM_STOCK.value:
-                    msg.command_id = ArmCmdID.CMD_ID_MOVE_GET_FROM_STOCK
+                    msg.command_id = ArmCmdID.CMD_ID_MOVE_GET_FROM_STOCK.value
                 case ArmAction.MOVE_GRAB.value:
-                    msg.command_id = ArmCmdID.CMD_ID_MOVE_GRAB
+                    msg.command_id = ArmCmdID.CMD_ID_MOVE_GRAB.value
                 case ArmAction.MOVE_RELEASE.value:
-                    msg.command_id = ArmCmdID.CMD_ID_MOVE_RELEASE
+                    msg.command_id = ArmCmdID.CMD_ID_MOVE_RELEASE.value
                 case _:
                     pass
-        except:
-            self.get_logger().info('Invalid Herkulek action ID')
-            return
+        except Exception as err :
+            print(f"Unexpected {err=}, {type(err)=}")
+            pass
         # Publish the message
-        self.arm_pub_.publish(msg)
+        if(msg.command_id != 0):
+            self.get_logger().info('Sending command: %d' % msg.command_id)
+            self.arm_pub_.publish(msg)            
+        else:
+            self.get_logger().info('Invalid action ID: %d' % id_action)
 
 
 def main(args=None):

src/arm_controller/package.xml

@@ -9,6 +9,7 @@
 
   <depend>rclpy</depend>
   <depend>arm_interface</depend>
+  <depend>can_interface</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>