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

优化gripper代码

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-13 11:12:30 +08:00
parent ccca72679f
commit c503bc42db
1 changed files with 92 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

161
gripper_dev/gripper_dev/gripper_dev_node.py
View File

@@ -23,7 +23,7 @@ class GripperDevNode(Node):
self.declare_parameter('name','/gripper_cmd0')
param = self.get_parameter('name')
self.srv_name = param.value
print('param:',self.com_dev,self.devid,self.srv_name)
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)
@@ -36,7 +36,17 @@ class GripperDevNode(Node):
self.cur_goal=None
self.timer_on=False
self.target_mode=0
print('gripper_dev_node init:',self.com_dev)
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.get_logger().info(f'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):
self.cur_goal=goal
@@ -45,120 +55,132 @@ class GripperDevNode(Node):
self.target_speed=req.speed
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...')
self.clawControl.runWithParam(self.devid,self.target_loc,self.target_speed,self.target_torque)
##feedback
loop_flag=True
loop_times=0
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)
loc_direction = 0
tor_direction = 0
if (origin_loc[0] < self.target_loc) :loc_direction = 1
if (origin_torque[0] < self.target_torque) :tor_direction = 1
print("origin loc: ", origin_loc[0], self.target_loc, loc_direction)
print("origin torque: ", origin_torque[0], self.target_torque, tor_direction)
origin_state = self.clawControl.getClampCurrentState(self.devid)
self.state=origin_state[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 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 == "手指已到达指定的位置，没有检测到物体") :
result=GripperCmd.Result()
result.state=self.state
result.result=1
goal.succeed()
self.get_logger().info(f'result: {result}\n')
return result
else: # close gripper
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
else:
result=GripperCmd.Result()
result.state="mode error!"
result.result=0
goal.abort()
self.get_logger().info(f'result: {result}\n')
return result
self.clawControl.runWithParam(self.devid, self.target_loc, self.target_speed, self.target_torque)
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]
if self.target_mode==0: # loc mode
# loop_flag=abs(self.cur_loc-self.target_loc)>2
if (loc_direction) :
loop_flag = False if (self.cur_loc >= self.target_loc) or (self.target_loc - self.cur_loc) < 2 else True
else :
loop_flag = False if (self.cur_loc <= self.target_loc) or (self.cur_loc - self.target_loc) < 2 else True
elif self.target_mode==1: # torque mode
# loop_flag=abs(self.cur_torque-self.target_torque)>2
if (tor_direction) :
loop_flag = False if (self.cur_torque >= self.target_torque) or (self.target_torque - self.cur_torque) < 2 else True
else :
loop_flag = False if (self.cur_torque <= self.target_torque) or (self.cur_torque - self.target_torque) < 2 else True
else: #loc or torque
# loop_flag=abs(self.cur_loc-self.target_loc)>2 and abs(self.cur_torque-self.target_torque)>2
if (loc_direction) :
loc_loop_flag = False if (self.cur_loc >= self.target_loc) or (self.target_loc - self.cur_loc) < 2 else True
else :
loc_loop_flag = False if (self.cur_loc <= self.target_loc) or (self.cur_loc - self.target_loc) < 2 else True
if (tor_direction) :
tor_loop_flag = False if (self.cur_torque >= self.target_torque) or (self.target_torque - self.cur_torque) < 2 else True
else :
tor_loop_flag = False if (self.cur_torque <= self.target_torque) or (self.cur_torque - self.target_torque) < 2 else True
print("loc_loop_flag: ", loc_loop_flag)
print("tor_loop_flag: ", tor_loop_flag)
self.state=state[0]
loop_flag = loc_loop_flag and tor_loop_flag
print("mode: ", self.target_mode)
print("torque: ", self.cur_torque, self.target_torque)
print("loc: ", self.cur_loc, self.target_loc)
print("loop_flag: ", loop_flag)
print("loc_direction: ", loc_direction)
print("tor_direction: ", tor_direction)
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}")
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
else:
pass
# print('feedback:', self.cur_loc, speed, torque)
feedback_msg = GripperCmd.Feedback()
feedback_msg.loc=loc[0]
feedback_msg.speed=speed[0]
feedback_msg.torque=torque[0]
print('update feedback:', feedback_msg)
feedback_msg.loc=self.cur_loc
feedback_msg.speed=self.cur_speed
feedback_msg.torque=self.cur_torque
feedback_msg.state=self.state
self.get_logger().info(f'update feedback: {feedback_msg}')
self.cur_goal.publish_feedback(feedback_msg)
time.sleep(1)
loop_times+=1
if loop_times>15:
if loop_times>10:
result=GripperCmd.Result()
result.state="loop timeout!!!"
result.result=0
goal.abort()
loop_times=0
print('loop timeout',state)
self.get_logger().info(f'result: {result}\n')
return result
else:
result=GripperCmd.Result()
result.state="uart open fail!"
result.state="uart open fail & enable fail!"
result.result=0
goal.abort()
self.get_logger().info(f'result: {result}\n')
return result
#print('start timer')
self.timer_on=True
print('location diff:',self.cur_loc,self.target_loc)
self.get_logger().info(f'location diff: {self.cur_loc} {self.target_loc}')
state = self.clawControl.getClampCurrentState(self.devid)
result=GripperCmd.Result()
result.state=state[0]
result.result=1
goal.succeed()
print('action finish',state)
self.get_logger().info(f'action finish {state}\n**********\n')
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
self.cur_torque=request.torque
print('recv gripper cmd:',self.cur_loc,self.cur_speed,self.cur_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)
print('send gripper sta:',self.devid,response.state)
self.get_logger().info(f'send gripper sta: {self.devid} {response.state}')
else:
response.state = 'uart open failed'
return response
@@ -166,6 +188,7 @@ def main():
rclpy.init()
node=GripperDevNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
pass
if __name__ == '__main__':