Linear velocity calibration

This lesson uses the Raspberry Pi as an example. For Raspberry Pi and Jetson-Nano boards, you need to open a terminal on the host computer and enter the command to enter the Docker container. Once inside the Docker container, enter the commands mentioned in this lesson in the terminal. For instructions on entering the Docker container from the host computer, refer to [01. Robot Configuration and Operation Guide] -- [4.Enter Docker (For JETSON Nano and RPi 5)]. For RDKX5 and Orin boards, simply open a terminal and enter the commands mentioned in this lesson.

1. Program Description

Run the program and adjust the parameters using the dynamic parameter adjuster to calibrate the linear velocity of the car. To visually demonstrate the calibration linear velocity, give the command to move the car forward 1 meter and observe how far it actually travels and whether it falls within the error range.

2. Program Startup

2.1 Startup Command

For Raspberry Pi and Jetson-Nano boards, you need to enter the Docker container first. For RDKX5 and Orin controllers, this is not necessary.

Enter the Docker container (see [Docker course] --- [4. Docker Startup Script] for steps).

All the following commands must be executed within the same Docker container (see [Docker course] --- [3. Docker Submission and Multi-Terminal Access] for steps).

Start chassis data, input in terminal,

ros2 launch yahboomcar_bringup yahboomcar_bringup_M1_launch.py

xxxxxxxxxx
ros2 run yahboomcar_bringup calibrate_linear_M1

The image below was successfully displayed.

If you encounter an error message indicating no TF transformation during runtime, simply press Ctrl+C to exit the program and then run it again.

Open the dynamic parameter tuner and run the following in the terminal:

xxxxxxxxxx
ros2 run rqt_reconfigure rqt_reconfigure

Click the calibrate_linear node in the left-hand node options:

Note: The above nodes may not be visible initially. Clicking "Refresh" will display all nodes. The displayed calibrate_linear node is the node for calibrating linear velocity.

The RQT interface parameters are explained below:

• test_distance: The distance for calibration testing; here, the test moves forward 1 meter.
• speed: The magnitude of the linear velocity.
• tolerance: The tolerance for error.
• odom_linear_scale_correction: The linear velocity scale factor. This value is modified if the test results are unsatisfactory.
• start_test: Test switch.
• direction: Can be ignored. This value is used for the McLaren wheel structure vehicle. Modifying it allows calibration of the linear velocity for left and right movement.
• base_frame: The name of the base coordinate system.
• odom_frame: The name of the odometry coordinate system.

2.2 Starting Calibration

In the rqt_reconfigure interface, select the calibration_linear node (if it's not displayed, click Refresh).

Select a reference of known length on the ground (measuring tape, tile, etc.): Change test_distance to the actual test distance. Here, we take a 1-meter test distance as an example. Check the start_test box to start calibration.

After clicking start_test, the car will listen to the TF transform of base_footprint and odom, calculate the theoretical distance the car should travel, and wait until the error is less than tolerance. Issue a stop command and print "done" on the terminal. If the actual distance traveled by the car is less than 1 meter, appropriately increase the odom_linear_scale_correction parameter. After modification, click a blank area, click start_test again to reset start_test, and then click start_test again to complete the calibration. Modifying other parameters is the same; you need to click a blank area to write the modified parameters. Record the final odom_linear_scale_correction parameter.

After testing, remember the value of odom_linear_scale_correction and modify the linear_scale_x parameter in yahboomcar_bringup_M1_launch.py.

Path:

xxxxxxxxxx
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_bringup/launch/yahboomcar_bringup_M1_launch.py

3. View the Node Relationship Graph

Open a terminal and enter the command:

xxxxxxxxxx
ros2 run rqt_graph rqt_graph

In the node relationship graph above:

• The imu_filter node is responsible for filtering the raw IMU data from the chassis /imu/data and publishing the filtered data /imu/data.
• The /ekf_filter_node node subscribes to the raw IMU data from the chassis /odom_raw and the filtered IMU data /imu/data, performs data fusion, and publishes the /odom topic.
• The calibrate_linear node listens for the TF transformation of odom->base_footprint and publishes the /cmd_vel topic to control the movement of the robot chassis.

4. Core Source Code Analysis

This program primarily utilizes TensorFlow to monitor coordinate transformations. By listening to the coordinate changes between base_footprint and odom, the robot learns "how far I've traveled/how many degrees I've turned."

Code path,

xxxxxxxxxx
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_bringup/yahboomcar_bringup/calibrate_angular_M1.py

The implementation for monitoring TensorFlow coordinate transformations is the get_position method in the CalibrateLinear class:

​x
def get_position(self):
     try:
        now = rclpy.time.Time()
        transform = self.tf_buffer.lookup_transform(
            self.base_frame,
            self.odom_frame,
            now,
            timeout=rclpy.duration.Duration(seconds=1.0))            
        return transform
​
     except (LookupException, ConnectivityException, ExtrapolationException):
        self.get_logger().info('transform not ready')
        raise

The implementation of determining the robot chassis displacement and controlling its movement is the on_timer method (timer callback function) in the CalibrateLinear class:

xxxxxxxxxx
def on_timer(self):
    move_cmd = Twist()
    #self.get_param()
    self.start_test = self.get_parameter('start_test').get_parameter_value().bool_value
    self.odom_linear_scale_correction = self.get_parameter('odom_linear_scale_correction').get_parameter_value().double_value
    self.rate = self.get_parameter('rate').get_parameter_value().double_value
    self.test_distance = self.get_parameter('test_distance').get_parameter_value().double_value
    self.direction = self.get_parameter('direction').get_parameter_value().double_value
    self.tolerance = self.get_parameter('tolerance').get_parameter_value().double_value
    self.speed = self.get_parameter('speed').get_parameter_value().double_value
    if self.start_test:
        self.position.x = self.get_position().transform.translation.x
        self.position.y = self.get_position().transform.translation.y
        print("self.position.x: ",self.position.x)
        print("self.position.y: ",self.position.y)
        distance = sqrt(pow((self.position.x - self.x_start), 2) +
                            pow((self.position.y - self.y_start), 2))
        distance *= self.odom_linear_scale_correction
        print("distance: ",distance)
        error = distance - self.test_distance
        print("error: ",error)
        #start = time()
        if not self.start_test or abs(error) < self.tolerance:
            self.start_test  = rclpy.parameter.Parameter('start_test',rclpy.Parameter.Type.BOOL,False)
            all_new_parameters = [self.start_test]
            self.set_parameters(all_new_parameters)
​
            print("done")
        else:
            move_cmd.linear.x = copysign(self.speed, -1 * error)
            '''if self.direction:
                print("x")
                move_cmd.linear.x = copysign(self.speed, -1 * error)
            else:
                move_cmd.linear.y = copysign(self.speed, -1 * error)
                print("y")'''
        self.cmd_vel.publish(move_cmd)
​
    else:
        self.x_start = self.get_position().transform.translation.x
        self.y_start = self.get_position().transform.translation.y
​
        self.cmd_vel.publish(Twist())