From 215bfa2dc284f6a0547cfa12bedd5c04a2479ace Mon Sep 17 00:00:00 2001 From: ValsCSGO Date: Fri, 8 Oct 2021 19:14:44 -0400 Subject: [PATCH] Fixed joint_state_broadcaster and changed some encodery shit --- src/ar3_config/config/controller_manager.yaml | 4 +- src/ar3_config/launch/dev.launch.py | 2 +- src/cobot_platform/cobot_platform/move.py | 58 ++++--------------- 3 files changed, 13 insertions(+), 51 deletions(-) diff --git a/src/ar3_config/config/controller_manager.yaml b/src/ar3_config/config/controller_manager.yaml index 17f5f38..00889b3 100644 --- a/src/ar3_config/config/controller_manager.yaml +++ b/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: diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py index ad650d4..db50ff8 100644 --- a/src/ar3_config/launch/dev.launch.py +++ b/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'), diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py index cb7900e..964ff5f 100644 --- a/src/cobot_platform/cobot_platform/move.py +++ b/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))