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

add gripper status

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-20 10:32:56 +08:00
parent 2adc702a59
commit e307fa2ad3
3 changed files with 110 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
gripper_dev/gripper_dev/gripper_dev_node.py
View File

@@ -4,15 +4,18 @@ import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from rclpy.action import ActionServer
from std_msgs.msg import String
from interfaces.msg import GripperStatus
from rclpy.executors import MultiThreadedExecutor
from interfaces.action import GripperCmd
import sys
import os
import threading
class GripperDevNode(Node):
def __init__(self):
super().__init__('gripper_dev_node')
self.lock = threading.Lock()
self.declare_parameter('port','/dev/ttyUSB1')
param = self.get_parameter('port')
self.com_dev = param.value
@@ -23,6 +26,9 @@ class GripperDevNode(Node):
self.declare_parameter('name','/gripper_cmd0')
param = self.get_parameter('name')
self.srv_name = param.value
self.declare_parameter('status_topic','gripper/status')
status_param = self.get_parameter('status_topic')
self.status_topic = status_param.value
self.get_logger().info(f'param: {self.com_dev} {self.devid} {self.srv_name}')
self.get_logger().info(f'ros param:{self.com_dev},{self.devid},{self.srv_name}')
self.action=ActionServer(self,GripperCmd,self.srv_name,self.gripper_cmd_callback_action)
@@ -38,16 +44,50 @@ class GripperDevNode(Node):
self.target_mode=0
self.enable_ok=False
self.state=''
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:
self.get_logger().info(f'{self.com_dev} enable fail!')
self.status_pub = self.create_publisher(GripperStatus, self.status_topic, 10)
self.status_timer = self.create_timer(0.1, self.status_timer_callback)
with self.lock:
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:
self.get_logger().info(f'{self.com_dev} enable fail!')
self.get_logger().info(f'gripper_dev_node init: {self.com_dev}')
#self.feedback_timer = self.create_timer(0.1, self.update_feedback)
def status_timer_callback(self):
if not self.enable_ok:
return
# Query all status in one lock cycle to minimize context switching
with self.lock:
try:
loc = self.clawControl.getClampCurrentLocation(self.devid)
speed = self.clawControl.getClampCurrentSpeed(self.devid)
torque = self.clawControl.getClampCurrentTorque(self.devid)
state = self.clawControl.getClampCurrentState(self.devid)
except Exception as e:
self.get_logger().error(f"Serial read error: {e}")
return
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]
msg = GripperStatus()
msg.header.stamp = self.get_clock().now().to_msg()
msg.loc = float(self.cur_loc)
msg.speed = float(self.cur_speed)
msg.torque = float(self.cur_torque)
msg.state = self.state
self.status_pub.publish(msg)
def gripper_cmd_callback_action(self,goal):
self.cur_goal=goal
req=goal.request
@@ -58,14 +98,10 @@ class GripperDevNode(Node):
self.get_logger().info(f'\n----------\nrecv goal {self.devid} {self.target_loc} {self.target_speed} {self.target_torque} {self.target_mode}')
if self.enable_ok:
origin_loc = self.clawControl.getClampCurrentLocation(self.devid)
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]
# Use local copies of latest status to avoid blocking the serial port again
self.get_logger().info(f"origin state: {self.state}")
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}")
self.get_logger().info(f"origin torque: {self.cur_torque} -> {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
@@ -92,21 +128,32 @@ class GripperDevNode(Node):
self.get_logger().info(f'result: {result}\n')
return result
self.clawControl.runWithParam(self.devid, self.target_loc, self.target_speed, self.target_torque)
# Retry sending command up to 3 times to ensure stability
max_retries = 3
cmd_sent = False
for i in range(max_retries):
with self.lock:
try:
self.clawControl.runWithParam(self.devid, self.target_loc, self.target_speed, self.target_torque)
cmd_sent = True
break
except Exception as e:
self.get_logger().warn(f"Failed to send command (attempt {i+1}): {e}")
time.sleep(0.05)
if not cmd_sent:
result=GripperCmd.Result()
result.state="Serial command failed after retries"
result.result=0
goal.abort()
return result
loop_flag=True
loop_times=0
while loop_flag:
loc = self.clawControl.getClampCurrentLocation(self.devid)
speed = self.clawControl.getClampCurrentSpeed(self.devid)
torque = self.clawControl.getClampCurrentTorque(self.devid)
state = self.clawControl.getClampCurrentState(self.devid)
self.cur_loc=loc[0]
self.cur_speed=speed[0]
self.cur_torque = torque[0]
self.state=state[0]
# Use cached values from the periodic status timer to reduce serial traffic and collisions
# The timer runs at 10Hz, which is frequent enough for the action monitor
self.get_logger().info(f"mode: {self.target_mode}")
self.get_logger().info(f"torque: {self.cur_torque} {self.target_torque}")
self.get_logger().info(f"loc: {self.cur_loc} {self.target_loc}")
@@ -136,17 +183,21 @@ class GripperDevNode(Node):
else:
pass
# print('feedback:', self.cur_loc, speed, torque)
feedback_msg = GripperCmd.Feedback()
feedback_msg.loc=self.cur_loc
feedback_msg.speed=self.cur_speed
feedback_msg.torque=self.cur_torque
feedback_msg.state=self.state
feedback_msg.loc=int(self.cur_loc)
feedback_msg.speed=int(self.cur_speed)
feedback_msg.torque=int(self.cur_torque)
try:
# Check if the interface supports state in feedback
feedback_msg.state=self.state
except AttributeError:
pass
self.get_logger().info(f'update feedback: {feedback_msg}')
self.cur_goal.publish_feedback(feedback_msg)
time.sleep(1)
goal.publish_feedback(feedback_msg)
time.sleep(1) # Monitor every 1 second
loop_times+=1
if loop_times>10:
if loop_times>20: # Increased timeout since we are less invasive now
result=GripperCmd.Result()
result.state="loop timeout!!!"
result.result=0
@@ -174,30 +225,34 @@ class GripperDevNode(Node):
return result
def destroy_node(self):
super.destroy_node()
flag = self.clawControl.serialOperation(self.com_dev, 115200, False)
super().destroy_node()
with self.lock:
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
self.cur_torque=request.torque
self.get_logger().info(f'\n------------\nrecv gripper cmd: {self.cur_loc} {self.cur_speed} {self.cur_torque}')
flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
if flag == 1:
#self.runCmd()
#self.runCmd()
response.state = self.clawControl.getClampCurrentState(self.devid)
self.clawControl.serialOperation(self.com_dev, 115200, False)
self.get_logger().info(f'send gripper sta: {self.devid} {response.state}')
else:
response.state = 'uart open failed'
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()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
# Using more threads to ensure the timer and multiple action/service requests don't block each other
executor = MultiThreadedExecutor(num_threads=10)
executor.add_node(node)
try:
executor.spin()
finally:
node.destroy_node()
rclpy.shutdown()
pass
if __name__ == '__main__':
main()

6
gripper_dev/launch/gripper_dev.launch.py
View File

@@ -3,7 +3,9 @@ from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(package="gripper_dev",executable="gripper_dev_node",name="gripper0_dev_node",
parameters=[{'port':'/dev/robot/gripper0'}, {'name':'/gripper_cmd0'}, {'devid':9}]),
parameters=[{'port':'/dev/robot/gripper0'}, {'name':'/gripper_cmd0'}, {'devid':9},
{'status_topic':'/gripper0/status'}]),
Node(package="gripper_dev",executable="gripper_dev_node",name="gripper1_dev_node",
parameters=[{'port':'/dev/robot/gripper1'}, {'name':'/gripper_cmd1'}, {'devid':6}])
parameters=[{'port':'/dev/robot/gripper1'}, {'name':'/gripper_cmd1'}, {'devid':6},
{'status_topic':'/gripper1/status'}])
])

1
gripper_dev/package.xml
View File

@@ -13,6 +13,7 @@
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>rclcpp</depend>
<depend>rclpy</depend>