Angular Velocity Calibration

1. Course Content

1. Learn how to calibrate the robot's angular velocity. After running the angular velocity calibration program and clicking "start" in the visualization interface, the robot chassis will begin to rotate and will stop when the error is less than the tolerance value.

[!IMPORTANT]

• The vehicle has been calibrated before leaving the factory. Under normal circumstances, calibration is not necessary. If there is a significant error due to special circumstances, you can perform angular velocity calibration.

2. Preparations

2.1 Content Description

This lesson uses the Jetson Orin NX as an example. For Raspberry Pi and Jetson Nano boards, you need to open a terminal on the host machine, enter the command to get into the Docker container, and then input the commands mentioned in this lesson in the terminal within the Docker container. A tutorial on how to enter the Docker container from the host can be found in this product's guide [00.Configuration and Operation Guide]-[3.Enter the Docker (For RPI5)]. For ORIN and RDK boards, you can directly open a terminal and enter the commands mentioned in this lesson.

2.2 Start the Agent

Note: If it is already running, you do not need to start it again.

Enter the command in the vehicle's terminal:

start_agent

The terminal will print the following information, indicating a successful connection:

3. Run the Example

3.1 Start the Program

Open a terminal on the vehicle and run the angular velocity calibration node:

For Raspberry Pi 5 users, if the ROS container is not started, you need to start it first,

xxxxxxxxxx
bringup_m3

Open a terminal inside the container,

xxxxxxxxxx
exec_m3

Enter the command,

xxxxxxxxxx
ros2 launch m3_bringup calibration.launch.py calibration_mode:=angular

[!TIP]

calibration_mode is a launch parameter with two options:

• linear: The default parameter, which starts linear velocity calibration.
• angular: Starts angular velocity calibration.

Then, open another terminal and start the dynamic parameter reconfigure tool:

xxxxxxxxxx
ros2 run rqt_reconfigure rqt_reconfigure

Click the calibrate_angular node in the node options on the left:

[!WARNING]

• When you first open it, the above node may not be present. Click Refresh to see all the nodes. The displayed calibrate_angular node is the one for calibrating angular velocity.

RQT interface parameter description:

• test_angle: The angle for the calibration test. Here, it's set to rotate 360 degrees.
• speed: The angular velocity.
• tolerance: The allowable error tolerance.
• odom_angular_scale_correction: The angular velocity scale factor. If the test results are not ideal, modify this value.
• start_test: The test switch.
• base_frame: The name of the base coordinate frame.
• odom_frame: The name of the odometry coordinate frame.

3.2 Start Calibration

1. In the rqt_reconfigure interface, select the calibrate_angular node. For precise calibration, you can use a protractor for measurement. For a rough calibration, you can use a line on the floor as a reference.

   

2. In rqt_reconfigure, check the box next to start_test to begin the calibration.

3. The car will listen to the TF transform between base_footprint and odom, calculate the theoretical angle of rotation, and when the error is less than tolerance, it will publish a stop command.

4. If the actual angle the car rotates is not 360 degrees, modify the odom_angular_scale_correction parameter in RQT. After modifying it, click on a blank area, then uncheck and recheck start_test to reset it, and then click start_test again to calibrate.
5. Repeat the above process to find a suitable odom_angular_scale_correction value.

3.3 Write Calibration Parameters to the Chassis

1. To write parameters to the chassis, you must first disconnect the chassis agent by pressing Ctrl+C or closing the chassis connection agent terminal.

2. Open the config_robot.py file in the home directory on the vehicle.

3. Uncomment line 552, fill in the previous calibration coefficient in the parentheses of robot.set_ros_scale_angluar(xx), and then click save.

4. Open a terminal on the vehicle and enter the command:

xxxxxxxxxx
python3 config_robot.py

Wait for the parameters to be written. The ros_scale_angluar:1.000 in the terminal's printed information is the written parameter. The chassis angular velocity calibration is complete.

4. Source Code Analysis

Source code path:

xxxxxxxxxx
~/yahboomM3_ws/code_ws/src/m3_common_demo/m3_common_demo/calibration/calibrate_angular.py

4.1 View Node Relationship Graph

Open a terminal on the virtual machine and enter the command:

xxxxxxxxxx
ros2 run rqt_graph rqt_graph

In the node relationship diagram above:

• The /ekf_filter_node subscribes to the chassis's raw odometry /odom_raw and IMU data /imu/data_raw, and after data fusion, it publishes the /odom topic.
• The calibrate_angular node listens to the TF transform from odom to base_footprint and publishes the /cmd_vel topic to control the robot chassis movement.

4.2 Source Code Analysis

Listening to the TF coordinate transformation is implemented in the get_odom_angle method of the CalibrateAngular class:

​x
def get_odom_angle(self):
     try:
        now = rclpy.time.Time()
        rot = self.tf_buffer.lookup_transform(
            self.base_frame,
            self.odom_frame,
            now,
            timeout=rclpy.duration.Duration(seconds=1.0))  
        cacl_rot = PyKDL.Rotation.Quaternion(rot.transform.rotation.x, rot.transform.rotation.y, rot.transform.rotation.z,          rot.transform.rotation.w)
        #print("cacl_rot: ",cacl_rot)
        angle_rot = cacl_rot.GetRPY()[2]
        #print("angle_rot: ",angle_rot)
​
     except (LookupException, ConnectivityException, ExtrapolationException):
        # self.get_logger().info('transform not ready')
        return

Judging the robot chassis's rotation angle and controlling its movement is implemented in the on_timer (timer callback function) method of the CalibrateAngular class:

xxxxxxxxxx
def on_timer(self):
    self.start_test = self.get_parameter('start_test').get_parameter_value().bool_value
    self.odom_angular_scale_correction = self.get_parameter('odom_angular_scale_correction').get_parameter_value().double_value
    self.test_angle = self.get_parameter('test_angle').get_parameter_value().double_value
    self.test_angle = radians(self.test_angle) # Convert degrees to radians
    self.speed = self.get_parameter('speed').get_parameter_value().double_value
    move_cmd = Twist()
    self.test_angle *= self.reverse
    #self.test_angle *= self.reverse
    #self.error = self.test_angle - self.turn_angle
    if self.start_test:
        self.error = self.turn_angle - self.test_angle 
        if abs(self.error) > self.tolerance  :
            #move_cmd.linear.x = 0.2
            move_cmd.angular.z = copysign(self.speed, self.error)
            #print("angular: ",move_cmd.angular.z)
            self.cmd_vel.publish(move_cmd)
            self.odom_angle = self.get_odom_angle() 
            self.delta_angle = self.odom_angular_scale_correction * self.normalize_angle(self.odom_angle - self.first_angle)
            #print("delta_angle: ",self.delta_angle)
            self.turn_angle += self.delta_angle
            print("turn_angle: ",self.turn_angle,flush=True)
            #self.error = self.test_angle - self.turn_angle
            print("error: ",self.error,flush=True)
            self.first_angle = self.odom_angle
​
            #print("first_angle: ",self.first_angle)
        else:
            self.error = 0.0
            self.turn_angle = 0.0
            print("done",flush=True)
            self.first_angle = 0
            self.reverse = -self.reverse
            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)                
    else:
        self.error = 0.0
        self.cmd_vel.publish(Twist())
        self.turn_angle = 0.0
        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)