def test_can_transform_valid_transform(self):
buffer = Buffer()
clock = rclpy.clock.Clock()
rclpy_time = clock.now()
transform = self.build_transform('foo', 'bar', rclpy_time)
self.assertEqual(buffer.set_transform(transform, 'unittest'), None)
self.assertEqual(buffer.can_transform('foo', 'bar', rclpy_time), True)
output = buffer.lookup_transform('foo', 'bar', rclpy_time)
self.assertEqual(transform.child_frame_id, output.child_frame_id)
self.assertEqual(transform.transform.translation.x, output.transform.translation.x)
self.assertEqual(transform.transform.translation.y, output.transform.translation.y)
self.assertEqual(transform.transform.translation.z, output.transform.translation.z)
def test_buffer_non_default_cache(self):
buffer = Buffer(cache_time=rclpy.duration.Duration(seconds=10.0))
clock = rclpy.clock.Clock()
rclpy_time = clock.now()
transform = self.build_transform('foo', 'bar', rclpy_time)
self.assertEqual(buffer.set_transform(transform, 'unittest'), None)
self.assertEqual(buffer.can_transform('foo', 'bar', rclpy_time), True)
output = buffer.lookup_transform('foo', 'bar', rclpy_time)
self.assertEqual(transform.child_frame_id, output.child_frame_id)
self.assertEqual(transform.transform.translation.x,
output.transform.translation.x)
self.assertEqual(transform.transform.translation.y,
output.transform.translation.y)
self.assertEqual(transform.transform.translation.z,
output.transform.translation.z)
class Executor(Node):
def __init__(self):
super().__init__('executor')
"""Update frequency
Single targets: {baudrate} / ({uart frame length} * {cmd length} * {number of joint})
= 115200 / (9 * 4 * 18) ~= 177Hz <- autodetect-baudrate on maestro
= 200000 / (9 * 4 * 1x8) ~= 300Hz <- fixed-baudrate on maestro
Multiple targets: {baudrate} / ({uart frame length} * ({header length} + {mini-cmd length} * {number of joints}))
= 115200 / (9 * (3 + 2 * 18)) ~= 320Hz
= 200000 / (9 * (3 + 2 * 18)) ~= 560Hz"""
self.subscription = self.create_subscription(
Path,
'sparse_path',
self.new_trajectory,
10
)
self.spi_bridge = self.create_publisher(
UInt8MultiArray, 'stm32_cmd', 10
)
self.steps_trajectory = []
self.init_test_path()
self._tf_buffer = Buffer()
self._tf_listener = TransformListener(self._tf_buffer, self)
self.broadcaster = self.create_publisher(TFMessage, '/tf', 10)
self.publish_transform = get_transform_publisher(self.broadcaster, self.get_clock())
self.pub = self.create_publisher(JointState, 'joint_states', 10)
self.base_link_start_height = tf.translation_matrix((0.0, 0.0, 0.1))
self.path_proxy = PathProxy(self.steps_trajectory, self.get_logger())
self.trajectory_generator = TrajectoryGenerator(self.path_proxy)
self.trajectory_encoder = TrajectoryEncoder()
self.front_point = Pose()
self.rear_point = Pose()
self.init_points()
self.inv_kin_calc = TripodInverseKinematics(self._tf_buffer, self.get_clock())
self.prev_time = self.get_clock().now()
first_step = TrajectoryPoint()
first_step.timestamp = self.prev_time.nanoseconds / 10**9
first_step.left_moving = True
first_step.left_pose = tf.translation_matrix((0, 0, 0))
first_step.right_pose = tf.translation_matrix((0, 0, 0))
self.prev_trajectory = self.trajectory_generator.generate_trajectory(
first_step)
self.first_time = False
self.timer_callback()
self.first_time = False
self.start_timer = self.create_timer(START_DELAY, self.timer_callback)
self.timer = None
self.tf_viz_tim = self.create_timer(0.1, self.tf_viz_callback)
def new_trajectory(self, msg: Path):
new_path = [
pose_to_matrix(pose_stamped.pose)
for pose_stamped in msg.poses
]
self.steps_trajectory.clear()
self.steps_trajectory.extend(new_path)
def timer_callback(self):
if self.first_time:
self.first_time = False
self.start_timer.cancel()
self.timer = self.create_timer(UPDATE_PERIOD, self.timer_callback)
start = self.get_clock().now()
diff = start.nanoseconds - self.prev_time.nanoseconds
point_i = int(SERVO_FREQ * diff / (10 ** 9))
print(diff/10**9, point_i)
if point_i >= len(self.prev_trajectory):
point_i = len(self.prev_trajectory) - 1
print(point_i)
current_point = self.prev_trajectory[point_i]
new_trajectory = self.trajectory_generator.generate_trajectory(current_point)
inversed_trajectory = [
self.inv_kin_calc.calc_point(point)
for point in new_trajectory
]
data = self.trajectory_encoder.encode_trajectory(inversed_trajectory)[1:]
computation_time = self.get_clock().now() - start
cmd_size = 3 + 18 * 2
point_k = int(SERVO_FREQ * computation_time.nanoseconds / (10 ** 9))
points_passed = min(point_i + point_k, len(self.prev_trajectory) - 1) - point_i
print(points_passed)
data = bytearray([42]) + data[points_passed * cmd_size:(points_passed + 250 * 2) * cmd_size]
spi_msg = UInt8MultiArray(data=data)
self.spi_bridge.publish(spi_msg)
self.prev_time = start
self.prev_trajectory = new_trajectory[points_passed:points_passed + 250 * 2]
front_point = self.path_proxy.first_unused()
triangle = self.inv_kin_calc.triangle(front_point, left_side=False)
self.publish_transform('odom', 'front_point', front_point)
self.publish_transform('odom', 'tri_a', triangle[0])
self.publish_transform('odom', 'tri_b', triangle[1])
self.publish_transform('odom', 'tri_c', triangle[2])
elapsed = self.get_clock().now() - start
# self.get_logger().debug(f'Elapsed {elapsed.nanoseconds / 10**9:.3f}s')
print(f'Elapsed {elapsed.nanoseconds / 10**9:.3f}s')
def init_points(self):
if self._tf_buffer.can_transform('odom', 'front_point', Time()):
transform = self._tf_buffer.lookup_transform(
'odom', 'front_point', Time())
self.front_point.position.x = transform.transform.translation.x
self.front_point.position.y = transform.transform.translation.y
self.front_point.position.z = transform.transform.translation.z
else:
self.publish_transform(
'odom', 'front_point', tf.translation_matrix((0, 0, 0)))
if self._tf_buffer.can_transform('odom', 'rear_point', Time()):
transform = self._tf_buffer.lookup_transform(
'odom', 'rear_point', Time())
self.rear_point.position.x = transform.transform.translation.x
self.rear_point.position.y = transform.transform.translation.y
self.rear_point.position.z = transform.transform.translation.z
else:
self.publish_transform(
'odom', 'rear_point', tf.translation_matrix((0, 0, 0)))
while self.pub.get_subscription_count() < 1:
pass
sleep(2)
self.publish_transform('odom', 'base_link', self.base_link_start_height)
def init_test_path(self):
self.steps_trajectory = [
tf.translation_matrix((0, 0, 0)),
]
def tf_viz_callback(self):
now = self.get_clock().now()
point_i = int(SERVO_FREQ * (now.nanoseconds - self.prev_time.nanoseconds) / (10**9)) # TODO
if point_i >= len(self.prev_trajectory):
point_i = len(self.prev_trajectory) - 1
point = self.prev_trajectory[point_i]
angles = self.inv_kin_calc.calc_point(point)
names, positions = [], []
for name, position in angles.items():
names.append(name)
positions.append(position)
msg = JointState()
msg.header.stamp = header_stamp(self.get_clock().now())
msg.name = names
msg.position = positions
self.pub.publish(msg)
# base = tf.concatenate_matrices(point.base_link_pose, self.base_link_height)
base = point.base_link_pose
self.publish_transform('odom', 'base_link', base)
self.publish_transform('odom', 'right_tri', point.right_pose)
self.publish_transform('odom', 'left_tri', point.left_pose)
