Robot spec and sensor mounting

The robot's own geometry comes in two parts, and each is given by a different component.

Use the standard component for the sensor mounting. RobotSpecManager does not give sensor mounts; it gives the outline, the dimensions and mass, and the motion limits of the robot, which the standard component does not have.

The two files cannot be merged. The Sensor Calibration File Parser cannot read a file that has an element it does not know (for example <footprint>), so keep the robot spec and the sensor calibration in separate files.

Coordinates

• Robot coordinates follow ROS REP-103: right-handed, X forward, Y left, Z up, in meters. The origin is the point that the robot's pose (pose2D) refers to.
• A mounting orientation is given as roll, pitch and yaw, in the order R = Rz(yaw)·Ry(pitch)·Rx(roll), the same as Rotation3D and lidar_mount_* of the ICP components.

RobotSpecManager: the outline of the robot

RobotSpecManager reads a robot spec file and outputs, once at the start, the outline and its radii as the custom structure SMSL_RobotSpec (output robot_spec). Components that use the outline (inflation, planning, safety stop, display in NavigationToolkit) take it by wire, so they do not have their own properties for the robot's size.

The only property is spec_file, the path of the robot spec file.

<robot_spec>
  <!-- Choose one of rectangle / circle / polygon -->
  <footprint shape="rectangle" front_in_m="0.3" rear_in_m="0.3" left_in_m="0.2" right_in_m="0.2"/>
  <!-- <footprint shape="circle" radius_in_m="0.25"/> -->
  <!-- <footprint shape="polygon">0.3 0.2  -0.3 0.2  -0.3 -0.2  0.3 -0.2</footprint> -->
</robot_spec>

• The output outline is always counterclockwise. A polygon given clockwise is reversed.
• The inscribed radius is the shortest distance from the origin to an edge; the circumscribed radius is the longest distance from the origin to a vertex. For a circle, both are radius_in_m (the outline is a regular 16-gon that contains the circle).
• The component stops with an error when the file cannot be read, when the polygon crosses itself or has no area, or when the origin is not inside the outline. RobotSpecManager shows the values in the log (Robot spec: vertices = ..., inscribed_radius_in_m = ..., circumscribed_radius_in_m = ...) because the structure cannot be seen in DataViewer.
• A margin around the robot (like footprint_padding of ROS) is not part of the robot spec. The component that uses the outline has its own margin.

Type, body and motion limits

Besides the outline, the robot spec file can give the physical values and the performance limits of the robot, so that the components after it need fewer parameters. Write the type of the robot in type of <robot_spec> (required when <body> or <motion_limits> is written): differential (two-wheel differential drive, crawler, AGV), omni (mecanum or omni wheels), ackermann (front-wheel steering) or drone (multirotor).

• <body> (optional): the height and the lowest height of the body (height_in_m, ground_clearance_in_m), the mass and the center of gravity (mass_in_kg, cog_xyz_in_m), and the wheels (wheel_radius_in_m, track_width_in_m, wheelbase_in_m, drive_axle_x_in_m, max_steering_angle_in_rad, max_drive_force_in_N). They are output in the same structure SMSL_RobotSpec (output robot_spec) as the outline. For a ground robot, z = 0 of the robot coordinates is the ground.
• <motion_limits> (optional): the limits of the machine, such as the maximum velocities (forward, backward, lateral, up, down), the maximum yaw rate, the maximum accelerations and decelerations (including the emergency deceleration, which decides the stopping distance of a safety stop) for a drone, the maximum tilt angle and, for an Ackermann robot, the maximum rate of steering (max_steering_rate_in_rad_per_s; the yaw rate and its change are not written because they follow from the speed, the steering angle and the wheelbase: ω = v·tan(δ)/L, dω/dt = (a·tan δ + v·δ̇/cos²δ)/L). They are output in the structure SMSL_RobotMotionLimits (output robot_motion_limits), which exists only when <motion_limits> is written. Write only the limits of the machine. The speed you use today ("slowly") stays in the properties of the component that uses it, below the limit.
• Which attributes are required, optional or not allowed depends on the type. A value that does not fit the type (for example wheelbase_in_m for a differential robot) stops the component with an error. A direction in which the robot cannot move is 0 in the output.
• The friction coefficient is not here: it belongs to the floor, not to the robot.

<robot_spec type="differential">
  <footprint shape="rectangle" front_in_m="0.3" rear_in_m="0.3" left_in_m="0.2" right_in_m="0.2"/>
  <body height_in_m="0.8" mass_in_kg="40" cog_xyz_in_m="0.05 0 0.25"
        wheel_radius_in_m="0.1" track_width_in_m="0.4" drive_axle_x_in_m="0"/>
  <motion_limits max_velocity_forward_in_m_per_s="1.0" max_velocity_backward_in_m_per_s="0.3"
          max_yaw_rate_in_rad_per_s="1.5"
          max_acceleration_in_m_per_s2="0.5" max_deceleration_in_m_per_s2="1.0"
          max_yaw_acceleration_in_rad_per_s2="3.0"/>
</robot_spec>

Samples for the four types are robot_spec_differential.xml, robot_spec_omni.xml, robot_spec_ackermann.xml and robot_spec_drone.xml in the example folder. The comments in them tell which attributes each type needs.

Sensor Calibration File Parser: where the sensors are

The Sensor Calibration File Parser reads an XML file in which the transformation between sensor coordinate systems is written, and outputs the 4 × 4 transformation matrices. It also finds the transformation between two sensors that are not written directly, as long as the coordinate systems are connected through other ones (for example, from a LiDAR to a camera through the robot), and the transformation in the reverse direction.

The file

Each <extrinsic from="A" to="B"> gives the transformation of a point from the coordinate system A to B. The transformations inside are applied in the order they are written.

<?xml version="1.0" encoding="UTF-8" ?>
<system>
  <calibration>
    <extrinsic from="front_lidar" to="robot">
      <!-- roll 20 deg about X, then pitch -30 deg about Y, then yaw 40 deg about Z -->
      <rotation><axis_angle><vector size="3">1 0 0</vector><angle unit="deg">20</angle></axis_angle></rotation>
      <rotation><axis_angle><vector size="3">0 1 0</vector><angle unit="deg">-30</angle></axis_angle></rotation>
      <rotation><axis_angle><vector size="3">0 0 1</vector><angle unit="deg">40</angle></axis_angle></rotation>
      <!-- the position of the sensor in robot coordinates (m) -->
      <translation><vector size="3">0.25 0 0.3</vector></translation>
    </extrinsic>
  </calibration>
</system>

• The parser has no roll / pitch / yaw. Write the three rotations about X, Y and Z in this order, then the translation. The result is p_robot = Rz(yaw)·Ry(pitch)·Rx(roll)·p_sensor + t, which is the same as the Rotation3D formula (checked in RTMaps with Rotation3D in Matrix Input mode).
• Other forms of rotation are also available: a 3 × 3 matrix, a Rodrigues vector, a quaternion. A 4 × 4 matrix [R | t] can be written in <transform>.
• The sensors' names (from / to) are free text. Use the same name in the file and in the properties of the component.

The component

Applying it to a point cloud

Connect the output matrix_<i> to the input transformation_matrix_4x4 of Rotation3D and set transfo_source = Matrix Input (4x4). The settings of Rotation3D for point clouds are in the Topic "Point cloud coordinates: from the sensor to the robot" of PointCloudProcessingToolkit.

Example: the lever arm between a GNSS antenna and an IMU

When the GNSS receiver has no built-in IMU, the antenna and the IMU are in different places. The offset between them is the lever arm l. Write the position of each one against the robot, and the parser finds the transformation between them through the robot.

<?xml version="1.0" encoding="UTF-8" ?>
<system>
  <calibration>
    <extrinsic from="gnss_antenna" to="robot">
      <translation><vector size="3">0.5 0 1.2</vector></translation>
    </extrinsic>
    <extrinsic from="imu" to="robot">
      <translation><vector size="3">0.1 0 0.3</vector></translation>
    </extrinsic>
  </calibration>
</system>

Set matrix_0_from = gnss_antenna and matrix_0_to = imu. The translation t of the output matrix [R | t] is the position of the antenna in IMU coordinates, which is l: here l = (0.4, 0, 0.9) m.

• Swapping from and to gives the reverse transformation. Here its translation is (-0.4, 0, -0.9) m.
• l is expressed in the axes of the IMU. If the IMU is mounted turned, add its <rotation> to the file. With the IMU turned 90 deg about Z, l = (0, -0.4, 0.9) m.

These values were checked in RTMaps with the transformation applied by Rotation3D in Matrix Input mode.

The parser gives only this fixed l. Correcting the GNSS position also needs the attitude at each moment: p_antenna = p_imu + R(t)·l, where R(t) is the attitude of the IMU, and the velocity gets the term ω × l. Rotation3D with this matrix does not do that, because the matrix does not change with the attitude. The correction belongs inside the component that estimates the position.

imu_to_pose6D: the orientation of an IMU

imu_to_pose6D converts the orientation quaternion of an IMU to pose6D [0, 0, 0, roll, pitch, yaw]. Two properties take the differences between sensors.

An NED input read as ENU gives the pitch with the wrong sign, and the yaw is not 90 deg - heading. Check the order and the frame of your sensor in the manufacturer's documentation.

The output is the orientation of the IMU itself. When the IMU is mounted rotated on the robot, write the IMU to robot rotation in the calibration file of Sensor Calibration File Parser and apply it where the orientation is used.

An IMU cannot measure a position, so the translation is always 0. The reference of the yaw depends on the sensor: north for a sensor that uses a magnetometer, and a 6-axis sensor drifts.

Which one to use