thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Fixed joint_state_broadcaster and changed some encodery shit

Browse source
This commit is contained in:
John Farrell 2021-10-08 19:14:44 -04:00
parent fb576de834
commit 215bfa2dc2
Signed by: john
GPG key ID: 10543A0DA2EC1E12
3 changed files with 13 additions and 51 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
src/ar3_config/config/controller_manager.yaml
View file

@ -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:

2
src/ar3_config/launch/dev.launch.py
View file

@ -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'),

58
src/cobot_platform/cobot_platform/move.py
View file

@ -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))