add jz_dev

jz_dev/jz_dev/jz_dev_node.py

@@ -0,0 +1,81 @@
+
+from jodellSdk.jodellSdkDemo import RgClawControl
+import time
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from interfaces.srv import JzCmd
+import sys
+import os
+
+class JzDevNode(Node):
+
+    def __init__(self,com_dev):
+        super().__init__('jz_dev_node')
+        self.srv = self.create_service(JzCmd, 'jz_cmd', self.jz_cmd_callback)
+        self.com_dev = com_dev
+        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)
+    def jz_cmd_callback(self, request, response):
+        devid=request.devid
+        self.cur_loc=request.loc
+        self.cur_speed=request.speed
+        self.cur_torque=request.torque
+        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()
+            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')
+    rclpy.spin(node)
+    rclpy.shutdown()
+    pass
+if __name__ == '__main__':
+    main()
+
+
+
+
+
+

jz_dev/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>jz_dev</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="you@example.com">h</maintainer>
+  <license>TODO: License declaration</license>
+
+ <buildtool_depend>ament_python</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+<exec_depend>rosidl_default_runtime</exec_depend>
+<member_of_group>rosidl_interface_packages</member_of_group>
+  <depend>std_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jz_dev/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/jz_dev
+[install]
+install_scripts=$base/lib/jz_dev

jz_dev/setup.py

@@ -0,0 +1,22 @@
+from setuptools import setup
+import os
+package_name='jz_dev'
+setup(
+    name='jz_dev',
+    version='0.1',
+    description='A simple package for jz_dev',
+    author='JZ Dev',
+    author_email='jz_dev@example.com',
+    url='https://github.com/jz_dev',
+    packages=['jz_dev'],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=[
+        'setuptools',
+    ],
+    entry_points={
+        'console_scripts':['jz_dev_node = jz_dev.jz_dev_node:main']
+    }
+)