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

完善夹爪状态判断逻辑

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-13 11:52:28 +08:00
parent c503bc42db
commit 2adc702a59
1 changed files with 25 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
gripper_dev/gripper_dev/gripper_dev_node.py
View File

@@ -62,13 +62,14 @@ class GripperDevNode(Node):
origin_torque = self.clawControl.getClampCurrentTorque(self.devid)
origin_state = self.clawControl.getClampCurrentState(self.devid)
self.state=origin_state[0]
self.cur_loc=origin_loc[0]
self.get_logger().info(f"origin state: {self.state}")
self.get_logger().info(f"origin loc: {origin_loc[0]} -> {self.target_loc}")
self.get_logger().info(f"origin loc: {self.cur_loc} -> {self.target_loc}")
self.get_logger().info(f"origin torque: {origin_torque[0]} -> {self.target_torque}")
if (self.target_mode==0 or self.target_mode==1 or self.target_mode==2) : # loc/torque/both mode
if (self.target_torque == 0 or self.target_loc == 0) : # open gripper
if (self.state == "手指已到达指定的位置，没有检测到物体") :
if (self.cur_loc == 0 and self.state == "手指已到达指定的位置，没有检测到物体") :
result=GripperCmd.Result()
result.state=self.state
result.result=1
@@ -76,7 +77,7 @@ class GripperDevNode(Node):
self.get_logger().info(f'result: {result}\n')
return result
else: # close gripper
if (self.state == "手指在闭合检测到物体") :
if (self.cur_loc > 0 and self.state == "手指在闭合检测到物体") :
result=GripperCmd.Result()
result.state=self.state
result.result=1
@@ -111,20 +112,27 @@ class GripperDevNode(Node):
self.get_logger().info(f"loc: {self.cur_loc} {self.target_loc}")
self.get_logger().info(f"state: {self.state}")
if (self.state == "手指已到达指定的位置，没有检测到物体") :
result=GripperCmd.Result()
result.state=self.state
result.result=1
goal.succeed()
self.get_logger().info(f'result: {result}\n')
return result
elif (self.state == "手指在闭合检测到物体") :
result=GripperCmd.Result()
result.state=self.state
result.result=1
goal.succeed()
self.get_logger().info(f'result: {result}\n')
return result
if (self.cur_loc == 0 and (self.state == "手指已到达指定的位置，没有检测到物体" or self.state == "手指在张开检测到物体")) : #手指张开到位
result=GripperCmd.Result()
result.state=self.state
result.result=1
goal.succeed()
self.get_logger().info(f'result: {result}\n')
return result
elif (self.cur_loc > 0 and self.state == "手指在闭合检测到物体") : #手指闭合，并检测到物体
result=GripperCmd.Result()
result.state=self.state
result.result=1
goal.succeed()
self.get_logger().info(f'result: {result}\n')
return result
elif (self.cur_loc > 0 and self.cur_loc == self.target_loc and self.state == "手指已到达指定的位置，没有检测到物体"): #手指闭合，未检测到物体
result=GripperCmd.Result()
result.state=self.state
result.result=0
goal.abort()
self.get_logger().info(f'result: {result}\n')
return result
else:
pass