@@ -58,31 +58,36 @@ def arm_execute_callback(self, goal_handle : ServerGoalHandle):
58
58
result .action_result = True
59
59
return result
60
60
61
- def send_arm_cmd (self , action_id ):
61
+ def send_arm_cmd (self , id_action ):
62
62
msg = ServoCmd ()
63
63
msg .dest = ARM_STM_ID
64
64
#Switch case on the type of command
65
+ print (f"Action ID: { id_action } " )
65
66
try :
66
- match action_id :
67
+ match id_action :
67
68
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
69
70
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
71
72
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
73
74
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
75
76
case ArmAction .MOVE_GRAB .value :
76
- msg .command_id = ArmCmdID .CMD_ID_MOVE_GRAB
77
+ msg .command_id = ArmCmdID .CMD_ID_MOVE_GRAB . value
77
78
case ArmAction .MOVE_RELEASE .value :
78
- msg .command_id = ArmCmdID .CMD_ID_MOVE_RELEASE
79
+ msg .command_id = ArmCmdID .CMD_ID_MOVE_RELEASE . value
79
80
case _:
80
81
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
84
85
# 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 )
86
91
87
92
88
93
def main (args = None ):
0 commit comments