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

更新jz_dev

Browse Source
This commit is contained in:
david
2025-11-21 10:09:03 +08:00
parent 8fd31fb584
commit 0b83c96472
3 changed files with 95 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
jz_dev/jz_dev/jz_dev_node.py
View File

@@ -3,30 +3,90 @@ from jodellSdk.jodellSdkDemo import RgClawControl
import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from std_msgs.msg import String
from interfaces.srv import JzCmd
#from interfaces.srv import JzCmd
from interfaces.action import JzCmd
import sys
import os
class JzDevNode(Node):
def __init__(self,com_dev):
def __init__(self):
super().__init__('jz_dev_node')
self.srv = self.create_service(JzCmd, 'jz_cmd', self.jz_cmd_callback)
self.com_dev = com_dev
self.action=ActionServer(self,JzCmd,'jz_cmd',self.jz_cmd_callback_action)
self.declare_parameter('port','/dev/ttyUSB0')
param = self.get_parameter('port')
#print('input param:',param.value)
self.com_dev = param.value
self.clawControl = RgClawControl()
self.cur_loc=0
self.cur_torque=0
self.cur_speed=0
self.cur_state=0
print('jz_dev_node init')
def runCmd(self,id):
ret=self.clawControl.runWithParam(id,self.cur_loc,self.cur_speed,self.cur_torque)
return ret
def getState(self,id):
loc = self.clawControl.getClampCurrentLocation(id)
speed = self.clawControl.getClampCurrentSpeed(id)
state = self.clawControl.getClampCurrentState(id)
self.target_loc=0
self.target_torque=0
self.target_speed=0
self.cur_loc=-1
self.cur_torque=-1
self.cur_id=0
self.cur_goal=None
self.timer_on=False
self.target_mode=0
print('jz_dev_node init:',self.com_dev)
#self.feedback_timer = self.create_timer(0.1, self.update_feedback)
def jz_cmd_callback_action(self,goal):
self.cur_goal=goal
req=goal.request
self.cur_id=req.devid
self.target_loc=req.loc
self.target_speed=req.speed
self.target_torque=req.torque
self.target_mode=req.mode
print('recv goal',self.cur_id,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.cur_id, True)
if ret!=1:
print('enble fail!')
print('start run...')
self.clawControl.runWithParam(self.cur_id,self.target_loc,self.target_speed,self.target_torque)
##feedback
loop_flag=True
while loop_flag:
loc = self.clawControl.getClampCurrentLocation(self.cur_id)
speed = self.clawControl.getClampCurrentSpeed(self.cur_id)
torque = self.clawControl.getClampCurrentTorque(self.cur_id)
self.cur_loc=loc[0]
self.cur_speed=speed[0]
if self.target_mode==0:
loop_flag=abs(self.cur_loc-self.target_loc)>2
elif self.target_mode==1:
loop_flag=abs(self.cur_torque-self.target_torque)>2
else:
loop_flag=abs(self.cur_speed-self.target_speed)>2 and abs(self.cur_loc-self.target_loc)>2
#print('feedback:',self.cur_loc,speed,torque)
feedback_msg = JzCmd.Feedback()
feedback_msg.devid = self.cur_id
feedback_msg.loc=loc[0]
feedback_msg.speed=speed[0]
feedback_msg.torque=torque[0]
print('update feedback:',feedback_msg)
self.cur_goal.publish_feedback(feedback_msg)
time.sleep(1)
else:
result=JzCmd.Result()
result.devid=self.cur_id
result.state="uart open fail!"
goal.abort()
return result
#print('start timer')
self.timer_on=True
print('location diff:',self.cur_loc,self.target_loc)
state = self.clawControl.getClampCurrentState(self.cur_id)
result=JzCmd.Result()
result.devid=self.cur_id
result.state=state[0]
goal.succeed()
print('action finish',state)
return result
def jz_cmd_callback(self, request, response):
devid=request.devid
self.cur_loc=request.loc
@@ -35,39 +95,16 @@ class JzDevNode(Node):
print('recv jz cmd:',devid,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(devid)
self.clawControl.serialOperation(self.com_dev, 115200, False)
print('send jz sta:',devid,response.state)
else:
response.state = 'uart open failed'
return response
'''devid=6
clawControl = RgClawControl()
comList = clawControl.searchCom()
#ver = clawControl.readSoftwareVersion(devid)
print(comList)
####ret = clawControl.enableClamp(devid, False)
ret=clawControl.forceOpenBrake(devid,True)
print(ret)
if flag==1:
loc = clawControl.getClampCurrentLocation(devid)
speed = clawControl.getClampCurrentSpeed(devid)
state = clawControl.getClampCurrentState(devid)
print('loc:',loc,'speed:',speed,'state:',state)
while True:
ret=clawControl.runWithoutParam(devid,1)
if ret==1:
print('start run')
time.sleep(3)
ret = clawControl.enableClamp(devid, False)
time.sleep(3)'''
def main():
rclpy.init()
node=JzDevNode('/dev/ttyUSB0')
node=JzDevNode()
rclpy.spin(node)
rclpy.shutdown()
pass

15
jz_dev/launch/jz_dev.launch.py Normal file
View File

@@ -0,0 +1,15 @@
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='jz_dev',
executable='jz_dev_node',
name='imu_node',
parameters=[{
'port': '/dev/ttyUSB0',
}]
)
])

2
jz_dev/setup.py
View File

@@ -1,5 +1,6 @@
from setuptools import setup
import os
from glob import glob
package_name='jz_dev'
setup(
name='jz_dev',
@@ -12,6 +13,7 @@ setup(
data_files=[
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
],
install_requires=[
'setuptools',