Fixed joint_state_broadcaster and changed some encodery shit

src/ar3_config/config/controller_manager.yaml

@@ -1,12 +1,12 @@
 controller_manager:
   ros__parameters:
-    update_rate: 100
+    update_rate: 20
 
     joint_trajectory_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
     joint_state_broadcaster:
-      type: joint_state_controller/JointStateBroadcaster
+      type: joint_state_broadcaster/JointStateBroadcaster
 
 joint_trajectory_controller:
   ros__parameters:

src/ar3_config/launch/dev.launch.py

@@ -91,7 +91,7 @@ def generate_launch_description():
             shell=True,
             output='screen'),
         ExecuteProcess(
-            cmd=['ros2 run controller_manager spawner.py joint_state_controller'],
+            cmd=['ros2 run controller_manager spawner.py joint_state_broadcaster'],
             shell=True,
             output='screen'),
 

src/cobot_platform/cobot_platform/move.py

@@ -18,10 +18,8 @@ class CobotMove(Node):
         self._serial = Serial('/dev/ttyACM0', 115200)
 
         self._cur = [0.0] * 6
-        # self._buf = [90, 90, 90] # 3 servos
-        # self._prev_buf = [90, 90, 90]
 
-        self.write_lock = False
+        self._timer = self.create_timer(1 / 40, self._dosomething)
 
         base_topic = 'move/'
 
@@ -32,50 +30,38 @@ class CobotMove(Node):
         self.create_subscription(Float32, base_topic+'j5', self._move_j5_cb, rclpy.qos.qos_profile_sensor_data)
         self.create_subscription(Float32, base_topic+'j6', self._move_j6_cb, rclpy.qos.qos_profile_sensor_data)
 
-        publish_base_topic = 'update/'
-        self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', 10)
-        self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', 10)
-        self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', 10)
-        self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', 10)
-        self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', 10)
-        self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', 10)
-
         qos = QoSProfile(depth=10)
-        self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
+
-        self._joint_state = JointState()
+        publish_base_topic = 'update/'
+        self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', qos)
+        self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', qos)
+        self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', qos)
+        self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', qos)
+        self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', qos)
+        self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', qos)
 
         self._serial.write(b'STA0.0.1\n')
         print(self._serial.readline())
 
-        self._timer = self.create_timer(.001, self._get_encoders)
-
     def _move_j1_cb(self, msg):
         self._cur[0] = int(msg.data / pi / 2. * 8000 * 4)
-        self._move_servo()
 
     def _move_j2_cb(self, msg):
         self._cur[1] = int(msg.data / pi / 2. * 10000 * 4)
-        self._move_servo()
 
     def _move_j3_cb(self, msg):
         self._cur[2] = int(msg.data / pi / 2. * 10000)
-        self._move_servo()
 
     def _move_j4_cb(self, msg):
         self._cur[3] = int(msg.data / pi / 2. * 2000)
-        self._move_servo()
 
     def _move_j5_cb(self, msg):
         self._cur[4] = int(msg.data / pi / 2. * 2000)
-        self._move_servo()
 
     def _move_j6_cb(self, msg):
         self._cur[5] = int(msg.data / pi / 2. * 2000)
-        self._move_servo()
-
-    def _move_servo(self):
-        self.write_lock = True
 
+    def _dosomething(self):
         self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8'))
 
         line = self._serial.readline().decode('UTF-8')
@@ -88,30 +74,6 @@ class CobotMove(Node):
         e = float(blocks[4][1:]) / 2000. * pi * 2.
         f = float(blocks[5][1:]) / 2000. * pi * 2.
 
-        self._joint_state.header.stamp = self.get_clock().now().to_msg()
-        self._joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
-        self._joint_state.position = [a, b, c, d, e, f]
-
-        self._joint_pub.publish(self._joint_state)
-
-        self.write_lock = False
-
-    def _get_encoders(self):
-        if self.write_lock:
-            return
-
-        self._serial.write(f'JP\n'.encode('UTF-8'))
-
-        line = self._serial.readline().decode('UTF-8')
-
-        blocks = re.findall(r'[ABCDEF]-*\d+', line)
-        a = float(blocks[0][1:]) / 8000. / 4. * pi * 2.
-        b = float(blocks[1][1:]) / 10000 / 4. * pi * 2.
-        c = float(blocks[2][1:]) / 10000. * pi * 2.
-        d = float(blocks[3][1:]) / 2000. * pi * 2.
-        e = float(blocks[4][1:]) / 2000. * pi * 2.
-        f = float(blocks[5][1:]) / 2000. * pi * 2.
-
         self.s_j1.publish(Float32(data=a))
         self.s_j2.publish(Float32(data=b))
         self.s_j3.publish(Float32(data=c))