HiveCoreRD/hivecore_robot_drivers
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

gripper add 15s timeout & gripper enable once

Browse Source
This commit is contained in:
hehe
2026-01-05 18:01:25 +08:00
parent 7144e76127
commit 9c723941a9
1 changed files with 36 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
gripper_dev/gripper_dev/gripper_dev_node.py
View File

@@ -36,6 +36,14 @@ class GripperDevNode(Node):
self.cur_goal=None
self.timer_on=False
self.target_mode=0
self.enable_ok=False
flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
if flag==1:
ret = self.clawControl.enableClamp(self.devid, True)
if ret==1:
self.enable_ok=True
else:
print(self.comd_dev,'enble fail!')
print('gripper_dev_node init:',self.com_dev)
#self.feedback_timer = self.create_timer(0.1, self.update_feedback)
def gripper_cmd_callback_action(self,goal):
@@ -46,20 +54,22 @@ class GripperDevNode(Node):
self.target_torque=req.torque
self.target_mode=req.mode
print('recv goal',self.devid,self.target_loc,self.target_speed,self.target_torque,self.target_mode)
flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
if flag==1:
ret = self.clawControl.enableClamp(self.devid, True)
if ret!=1:
print('enble fail!')
result=GripperCmd.Result()
result.state="gripper enable fail!"
result.result=0
goal.abort()
return result
print('start run...')
#flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
#if flag==1:
# ret = self.clawControl.enableClamp(self.devid, True)
# if ret!=1:
# print('enble fail!')
# result=GripperCmd.Result()
# result.state="gripper enable fail!"
# result.result=0
# goal.abort()
# return result
# print('start run...')
if self.enable_ok:
self.clawControl.runWithParam(self.devid,self.target_loc,self.target_speed,self.target_torque)
##feedback
loop_flag=True
loop_times=0
while loop_flag:
loc = self.clawControl.getClampCurrentLocation(self.devid)
speed = self.clawControl.getClampCurrentSpeed(self.devid)
@@ -80,9 +90,18 @@ class GripperDevNode(Node):
print('update feedback:',feedback_msg)
self.cur_goal.publish_feedback(feedback_msg)
time.sleep(1)
loop_times+=1
if loop_times>15:
result=GripperCmd.Result()
result.state="loop timeout!!!"
result.result=0
goal.abort()
loop_times=0
return result
else:
result=GripperCmd.Result()
result.state="uart open fail!"
result.state="uart open fail & enable fail!"
result.result=0
goal.abort()
return result
@@ -96,7 +115,10 @@ class GripperDevNode(Node):
goal.succeed()
print('action finish',state)
return result
def destroy_node(self):
super.destroy_node()
flag = self.clawControl.serialOperation(self.com_dev, 115200, False)
def gripper_cmd_callback(self, request, response):
self.cur_loc=request.loc
self.cur_speed=request.speed
@@ -116,6 +138,7 @@ def main():
rclpy.init()
node=GripperDevNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
pass
if __name__ == '__main__':