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)