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

force update status

Browse Source 
after control
This commit is contained in:
NuoDaJia02
2026-01-20 15:17:14 +08:00
parent e307fa2ad3
commit 12cebcb98b
1 changed files with 13 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
gripper_dev/gripper_dev/gripper_dev_node.py
View File

@@ -135,6 +135,19 @@ class GripperDevNode(Node):
with self.lock:
try:
self.clawControl.runWithParam(self.devid, self.target_loc, self.target_speed, self.target_torque)
time.sleep(0.05)
# Force update status immediately after command
loc = self.clawControl.getClampCurrentLocation(self.devid)
speed = self.clawControl.getClampCurrentSpeed(self.devid)
torque = self.clawControl.getClampCurrentTorque(self.devid)
state = self.clawControl.getClampCurrentState(self.devid)
if loc and speed and torque and state:
self.cur_loc = loc[0]
self.cur_speed = speed[0]
self.cur_torque = torque[0]
self.state = state[0]
cmd_sent = True
break
except Exception as e:
@@ -228,20 +241,6 @@ class GripperDevNode(Node):
super().destroy_node()
with self.lock:
flag = self.clawControl.serialOperation(self.com_dev, 115200, False)
def gripper_cmd_callback(self, request, response):
self.get_logger().info(f'\n------------\nrecv gripper service cmd: {request.loc} {request.speed} {request.torque}')
# Service callback should not close the port because the status timer needs it
with self.lock:
try:
# Send the command and just return the current state
self.clawControl.runWithParam(self.devid, request.loc, request.speed, request.torque)
response.state = self.clawControl.getClampCurrentState(self.devid)[0]
self.get_logger().info(f'service response: {response.state}')
except Exception as e:
self.get_logger().error(f"Service call failed: {e}")
response.state = 'error'
return response
def main():
rclpy.init()
node=GripperDevNode()