(2) Next, separate the upper layer from the chassis. Follow these steps for the different chassis configurations:
* **Mecanum Wheel Configuration**
Disconnect the motor wires and the red and black power wires from the STM32 controller.
* **Tracked Configuration**
Disconnect the motor wires and the red and black power wires from the STM32 controller.
* **Ackerman Configuration**
Disconnect the motor wires, the front steering servo wire, and the red and black power wires from the STM32 controller.
### 2.1.2 Chassis Assembly
After completing the disassembly steps, you can now choose the appropriate chassis type for installation. Follow these instructions:
* **Mecanum Wheel Configuration**
Connect the four motors and the battery wires on the Mecanum wheel chassis (red to red, black to black) as illustrated.
* **Tracked Configuration**
Connect the two motors and the battery wires on the tracked chassis (red to red, black to black) as illustrated.
* **Ackerman Configuration**
Connect the two motors and the battery wires on the Ackerman chassis (red to red, black to black) as illustrated.
## 2.2 Mecanum Wheel
### 2.2.1 Hardware Structure
The Mecanum wheel comprises rollers and an axle. The axle functions as the main support structure for the entire wheel, with rollers attached to it. The axle axis is positioned at a 45-degree angle to the roller axis. Typically, Mecanum wheels operate in groups of four, with two left wheels and two right wheels. Wheels A and B are symmetrical.
There are various combinations of four Mecanum wheels, such as AAAA, BBBB, AABB, ABAB, BABA. However, not all combinations allow the robot car to move in all directions, including forward, backward, and sideways. The Mecanum-wheel chassis combination is ABAB, enabling omnidirectional movement.
### 2.2.2 Physical Characteristics
The vehicle achieves omnidirectional motion by summing up the propelling forces of the ground-engaging rollers. This summation can occur in any direction through adjustments in wheel rotation direction and torque magnitude for the four wheels.
Because of the rollers' specific orientation at a certain angle to the wheel circumference, Mecanum wheels have the ability to slip sideways. The generatrix of these small rollers is unique. As the Mecanum wheel rotates around its fixed axle, each small roller's envelope forms a cylindrical surface, allowing the wheel to continuously roll forward.
### 2.2.3 Motion Principle and Formula
When conducting kinematic analysis, we can consider the kinematic model of Mecanum wheels, which includes the following parameters:
(1) VX: Velocity of the Mecanum wheel in the X-axis (typically front and rear direction).
(2) VY: Velocity of the Mecanum wheel in the Y-axis (typically left and right direction).
(3) ω: Angular velocity of the Mecanum wheel chassis (rotation speed of the chassis around its own center).
(4) Real-time velocities of the four wheels of the Mecanum wheel.
(5) The motion of the right front wheel in the plane can be decomposed into:
(6) VBx: Velocity of the Mecanum wheel in the X-axis (typically front and rear direction).
(7) VBy: Velocity of the Mecanum wheel in the Y-axis (typically left and right direction).
(8) L: Distance between the centers of the left and right wheels.
(9) H: Distance between the centers of the front and rear wheels.
(10) θ: Angle formed by the chassis body center and the center of the right front wheel, typically 45°.
(11) With these parameters, we can perform kinematic analysis of the Mecanum wheel chassis. The following are key mathematical formulas:
* **Kinematics Formula**
To simplify the mathematical model for kinematics, we make the following two idealized assumptions:
(1) Omni-directional wheels do not slip on the ground, and there is sufficient friction with the ground.
(2) The 4 wheels are distributed at the corners of a rectangle or square, with the wheels parallel to each other.
Here, we decompose the rigid body motion of the car into three components linearly. By calculating the velocities of the four wheels when the output Mecanum wheel chassis translates along the X+ and Y+ directions and rotates along the Z+ direction, we can combine these three simple motions using formulas to determine the required speeds of the four wheels.
Here VA, VB, VC and VD represent the rotational speeds of wheels A, B, C, and D, respectively, i.e. the motor speed. VX is the translation speed of the car along the X-axis, VY is the translation speed along the Y-axis, and ω is the rotational speed along the Z-axis. L/2 is half of the car's wheelbase, and H/2 is half of the car's axle distance.
* **Velocity Components for X-axis Translation**
The velocity components of each wheel during the robot's translation along the X-axis can be calculated using the following formulas:
VA = VX , VB = VX , VC = VX , VD = VX
Where:
- VA, VB, VC, VD: Real-time velocities of the four Mecanum wheels
- VX: Velocity of the Mecanum wheel in the X-axis direction
* **Velocity Components for Y-axis Translation**
When the robot translates along the Y-axis, the speed component of each wheel can be calculated using the following formula:
VA = VY , VB = -VY , VC = VY , VD = -VY
Where VY is the velocity of the robot in the Y-axis direction.
* **Velocity Components for Z-axis Rotation**
When the robot rotates along the Z-axis, the speed component of each wheel can be calculated using the following formulas:
VA = ω(L+H)/2 , VB = ω(L+H)/2
VC = ω(L+H)/2 , VD = ω(L+H)/2
Where ω is the angular velocity of the Mecanum wheel chassis (i.e., the speed at which the chassis rotates around its own center).
* **Combined Motion Calculation**
Combining the velocities in the X, Y, and Z directions allows for the computation of the rotation speeds of the four wheels based on the motion state of the car.
VA = VX + VY + ω(L+H)/2 , VB = VX - VY + ω(L+H)/2
VC = VX + VY + ω(L+H)/2 , VD = VX - VY + ω(L+H)/2
### 2.2.4 Program Analysis
[Source Code](../_static/source_code/driver.zip)
The program file is saved in: [ros2_ws/src/driver/controller/controller/mecanum.py](../_static/source_code/driver.zip)
Mecanum wheel kinematics class, used to calculate wheel speeds and implement Mecanum wheel kinematics.
* **Init Function**
{lineno-start=11}
```python
def __init__(self, wheelbase=0.216, track_width=0.195, wheel_diameter=0.097):
self.wheelbase = wheelbase
self.track_width = track_width
self.wheel_diameter = wheel_diameter
```
Initialize the wheel dimensions for convenient subsequent calculations.
* **Speed Convert Function**
{lineno-start=16}
```python
def speed_covert(self, speed):
"""
covert speed m/s to rps/s
:param speed:
:return:
"""
# distance / circumference = rotations per second
return speed / (math.pi * self.wheel_diameter)
```
Convert m/s to rps based on the wheel's parameters.
* **Set Velocity Function**
{lineno-start=25}
```python
def set_velocity(self, linear_x, linear_y, angular_z):
"""
Use polar coordinates to control moving
x
v1 motor1| ↑ |motor3 v3
+ y - | |
v2 motor2| |motor4 v4
:param speed: m/s
:param direction: Moving direction 0~2pi, 1/2pi<--- ↑ ---> 3/2pi
:param angular_rate: The speed at which the chassis rotates rad/sec
:param fake:
:return:
"""
# vx = speed * math.sin(direction)
# vy = speed * math.cos(direction)
# vp = angular_rate * (self.wheelbase + self.track_width) / 2
# v1 = vx - vy - vp
# v2 = vx + vy - vp
# v3 = vx + vy + vp
# v4 = vx - vy + vp
# v_s = [self.speed_covert(v) for v in [v1, v2, -v3, -v4]]
motor1 = (linear_x - linear_y - angular_z * (self.wheelbase + self.track_width) / 2)
motor2 = (linear_x + linear_y - angular_z * (self.wheelbase + self.track_width) / 2)
motor3 = (linear_x + linear_y + angular_z * (self.wheelbase + self.track_width) / 2)
motor4 = (linear_x - linear_y + angular_z * (self.wheelbase + self.track_width) / 2)
v_s = [self.speed_covert(v) for v in [motor1, motor2, -motor3, -motor4]]
data = []
for i in range(len(v_s)):
msg = MotorState()
msg.id = i + 1
msg.rps = float(v_s[i])
data.append(msg)
msg = MotorsState()
msg.data = data
return msg
```
Based on the input speed parameters, decompose them, calculate the speeds using `speed_convert`, and then publish the calculated radian speeds to the motors.
## 2.3 Ackermann Chassis
### 2.3.1 Hardware Component
The transmission mechanism of the Ackermann chassis front wheels includes a servo, linkage, and wheels. The servo is linked to the linkage, and the linkage is connected to the wheels. The servo's rotation governs the extent of the linkage's rotation, thereby affecting the steering of the front wheels.
During a turn, the front two wheels are in a parallel state, with both wheels having the same angles of rotation. The control of the rear wheels is managed by the motor and wheels, where the motor's rotation determines the robot's forward, backward, and speed movements.
### 2.3.2 Physical Characteristics
The design objective of the Ackermann chassis is to provide excellent steering performance and stability. It employs a principle called **"Ackermann geometry"** to achieve this. Ackermann geometry refers to the difference in steering angles between the front and rear wheels. By allowing the inner front wheel to have a greater steering angle, the Ackermann chassis makes it easier to control the vehicle during turns and reduces the risk of sliding during steering.
Additionally, the Ackermann chassis features a well-designed suspension system. The suspension system is a crucial component connecting the wheels and the vehicle body, significantly influencing the physical characteristics of the chassis. Ackermann chassis typically adopts an independent suspension system, enabling the independent control of each wheel's movement. This design enhances suspension performance, improving vehicle stability and ride comfort.
Furthermore, the Ackermann chassis considers the position of the vehicle's center of gravity. The center of gravity's location has a significant impact on the vehicle's stability and handling performance. Generally, the Ackermann chassis places the center of gravity lower to reduce the risk of vehicle tilt and sliding.
Lastly, the physical characteristics of the Ackermann chassis include the braking system and power transmission system. The braking system influences the vehicle's braking performance and stability by controlling the braking force on the wheels. The power transmission system is responsible for transferring the engine's power to the wheels, affecting the vehicle's acceleration and driving performance.
### 2.3.3 Kinematics Principle and Formula
When conducting kinematic analysis of the Ackermann chassis, we can use the following mathematical formulas and parameters to describe its motion characteristics:
To achieve pure rolling motion for the Ackermann car (meaning no side slip during turns), it is necessary to ensure that the normals of the four wheels' motion directions (lines perpendicular to the direction of tire rolling) intersect at a single point, which is the center of rotation.
To simplify the model, let's assume that the front wheels have only one wheel (the theoretical concept remains consistent) located in the middle position of the front axle, as depicted by the dashed line in the diagram:
(1) Front Wheel Steering Angle (θ): The rotation angle of the front wheels, indicating the angle by which the front wheels deviate from the vehicle's forward direction. It is typically measured in radians (rad).
(2) Vehicle Linear Velocity (V): The overall linear speed of the vehicle, representing its translational velocity. It is usually measured in meters per second (m/s). The left rear wheel speed is denoted as (VL), and the right rear wheel speed is denoted as (VR).
(3) Vehicle Track Width (D): The distance between the wheels on the left and right sides of the vehicle, measured in meters (m).
(4) Wheelbase of the Vehicle (H): The distance between the front and rear wheels of the vehicle, measured in meters (m).
(5) Vehicle Turning Radius (R): The radius of the circle described by the vehicle during a turn, measured in meters (m). The turning radius for the left wheel is (RL), and for the right wheel is (RR).
* **Process for Calculating Robot Speed and Angle**
Consistency of angular velocity
ω = V/R = VL/RL = VR/RR
In this context:
ω represents the angular velocity of the vehicle
R denotes the turning radius of the vehicle
V is the linear velocity of the vehicle
VL is the linear velocity of the left rear wheel
VR is the linear velocity of the right rear wheel
RL is the turning radius of the left wheel
RR is the turning radius of the right wheel
* **Relationship between front wheel steering angle and turning radius**
R = H/tan(θ), RL = R - D/2, RL = R + D/2
Where:
H represents the distance between the front and rear wheels of the vehicle
R signifies the turning radius of the vehicle
D denotes the distance between the wheels on the left and right sides of the vehicle
θ indicates the steering angle of the front wheels
* **Left and right wheel speeds calculation**
VL = V(R - D/2)/R
* **Right wheel speed calculation**
VR = V(R + D/2)/R
By knowing the wheelbase, track width, robot speed, and the steering angle of the servo, it is possible to calculate the speeds of the two rear wheels of the robot.
### 2.3.4 Program Analysis
[Source Code](../_static/source_code/driver.zip)
The program files are located in: [ros2_ws/src/driver/controller/controller/ackermann.py](../_static/source_code/driver.zip)
Ackermann wheel kinematics module, used to calculate wheel speeds and implement Ackermann wheel kinematics.
* **Init Function**
{lineno-start=12}
```python
def __init__(self, wheelbase=0.213, track_width=0.222, wheel_diameter=0.101):
self.wheelbase = wheelbase
self.track_width = track_width
self.wheel_diameter = wheel_diameter
```
Initialize the wheel dimensions for easier subsequent calculations.
* **Speed Convert Function**
{lineno-start=17}
```python
def speed_covert(self, speed):
"""
covert speed m/s to rps/s
:param speed:
:return:
"""
return speed / (math.pi * self.wheel_diameter)
```
Convert m/s to rps based on the wheel parameters.
* **Set Velocity Function**
{lineno-start=25}
```python
def set_velocity(self, linear_speed, angular_speed, reset_servo=True):
servo_angle = 500
data = []
if abs(linear_speed) >= 1e-8:
if abs(angular_speed) >= 1e-8:
theta = math.atan(self.wheelbase*angular_speed/linear_speed)
steering_angle = theta
# print(math.degrees(steering_angle))
if abs(steering_angle) > math.radians(37):
steering_angle = math.radians(37)
# for i in range(4):
# msg = MotorState()
# msg.id = i + 1
# msg.rps = 0.0
# data.append(msg)
# msg = MotorsState()
# msg.data = data
# return None, msg
servo_angle = 500 + 1000*math.degrees(steering_angle)/240
vr = linear_speed + angular_speed*self.track_width/2
vl = linear_speed - angular_speed*self.track_width/2
v_s = [self.speed_covert(v) for v in [0, vl, 0, -vr]]
for i in range(len(v_s)):
msg = MotorState()
msg.id = i + 1
msg.rps = float(v_s[i])
data.append(msg)
msg = MotorsState()
msg.data = data
return servo_angle, msg
else:
for i in range(4):
msg = MotorState()
msg.id = i + 1
msg.rps = 0.0
data.append(msg)
msg = MotorsState()
msg.data = data
return None, msg
```
Based on the input speed parameters, decompose them, calculate the speeds using `speed_convert`, and then publish the calculated radian speeds to the motors. Calculate the required steering angle from the linear and angular velocities, convert it, and send it to the servos.
## 2.4 Tank Chassis
### 2.4.1 Hardware Structure
The track is a flexible chain loop composed of a drive wheel, surrounded by idler wheels, load wheels, guide wheels, and carrier wheels. It consists of track plates and track pins.
### 2.4.2 Physical Characteristic
* **Drive Wheel**
The track propulsion device consists of components that transfer power from the transmission system to the track wheels. Its primary function is to convey power to the track and is usually installed at the rear of the vehicle.
* **Load Wheel**
The load wheel is mainly used to bear the weight of the vehicle and align the track. Its primary function is to ensure even distribution of pressure from the track to the ground, thereby improving the smoothness of travel.
* **Guide Wheel**
The position of the guide wheel is determined by the location of the drive wheel and is typically installed at the front of the vehicle. Its main purpose is to guide the track in the correct rotation to prevent deviation and derailment.
* **Carrier Wheel**
The carrier wheel functions to support the track, preventing excessive sagging and lateral slipping. This helps minimize bouncing of the track during motion.
* **Chassis**
The tracked chassis utilizes tracks for its movement. When the tracked chassis is in motion, it moves as if on laid tracks. The power generated by the tracked movement is continuously transmitted by the drive wheel, resulting in a continuous rolling motion of the tracks. During the advancement process, on one hand, the tracks unrolled by the guide wheel are laid on the ground and pressed under the load wheel rolling forward. On the other hand, the tracks rolled by the rear load wheel are wound up by the drive wheel.
### 2.4.3 Kinematic Principles and Formulas
When performing kinematic analysis, we can explore the kinematic model of a tracked chassis, incorporating the following parameters:
(1) B: Track Width, the distance between the two drive wheels on the track, usually measured in meters (m)
(2) R: Turning Radius (R) generated when the robot simultaneously moves forward and rotates, usually measured in meters (m)
(3) Vx: Target forward/backward velocity of the robot at point O, positive for forward movement, typically measured in meters per second (m/s)
(4) Vy: Target left/right velocity of the robot at point O, positive for leftward movement, measured in m/s (ignored for two-wheel configurations)
(5) Vw: Target rotational velocity of the robot around point O, positive for counterclockwise rotation, measured in rad/s
(6) VL: Left wheel velocity of the robot, positive in the forward direction, typically measured in meters per second (m/s)
(7) SR: Right wheel velocity of the robot, positive in the forward direction, typically measured in meters per second (m/s)
(8) SL: Path covered by the left wheel in a certain time t
(9) SM: Path covered by the midpoint O in a certain time t
(10) SR: Path covered by the right wheel in a certain time t
(11) θ: Angle of rotation of the robot in a certain time, measured in radians (rad)
* **Formula Calculation**
(1) Displacement Integration
Next, we will explore their relationships and derive the kinematic forward and inverse formulas for the tracked chassis (two-wheel differential drive type). By integrating velocity with respect to time, we can obtain the displacement:
SL = VL × t, SM = VX × t, SR = VR × t
(2) Arc Length to Angle Relationship
Dividing the arc length by the radius equals the angle in radians:
θ = SO/R
Where SO = (SL + SR)/2
(3) Time Integration
Dividing both sides of the formula by 't,' i.e., integrating with respect to time, yields:
ω = V/R = (VL + VR)/(2R)
* **Inverse Kinematics Solution**
To solve the inverse kinematics equation based on the above formula, the objective is to determine the drive wheel velocities VL and VR, given the robot's target velocity V and ω.
VL = V - ωB/2, VR = V + ωB/2
* **Forward Kinematics Solution**
By using the inverse kinematics equation, we can solve the forward kinematics equation. If the current velocities of the drive wheels VL and VR are known, we can calculate the real-time velocity of the robot.
V = (VL + VR)/2, ω = (VR - VL)/B
### 2.4.4 Program Analysis
[Source Code](../_static/source_code/driver.zip)
The program file is located in: [ros2_ws/src/driver/controller/controller/odom_publisher_node.py](../_static/source_code/driver.zip)
The tracked chassis also involves differential motion, so the kinematics of the Mecanum wheel can be directly applied.
* **Speed Settings**
{lineno-start=191}
```python
def cmd_vel_callback(self, msg):
# msg.linear.x *= self.linear_factor
# msg.linear.y *= self.linear_factor
# msg.angular.z *= self.angular_factor
if self.machine_type == 'JetRover_Mecanum':
self.linear_x = msg.linear.x
self.linear_y = msg.linear.y
else:
self.linear_x = msg.linear.x
self.linear_y = msg.linear.y
if abs(msg.linear.y) > 1e-8:
self.linear_x = 0.0
else:
self.linear_x = msg.linear.x
self.linear_y = 0.0
```
Here, the linear velocity is assessed. If `linear.y` is greater than 1e-8, it indicates that the vehicle needs to translate sideways. Since the tracked chassis cannot perform sideways translation, the linear velocity will be set to zero in this case.
* **Speed Publishing**
{lineno-start=215}
```python
elif self.machine_type == 'JetRover_Acker':
if msg.angular.z != 0:
r = self.linear_x/msg.angular.z
if r == 0:
self.angular_z = 0.0
else:
self.angular_z = msg.angular.z
```
The speed is published to the kinematics model of the Mecanum wheels. Since the tracked chassis only uses two motors and `linear_y` must be zero in the input, the two motors will handle differential motion based on `angular_z` to achieve turning.
## 2.5 IMU, Linear Velocity and Angular Velocity Calibration
:::{Note}
* The robot has been calibrated before leaving the factory and does not require additional calibration. The information is provided for reference only. If you observe significant deviations during robot movement, such as noticeable drifting to one side when moving forward or an inability to travel straight, you can consult the following tutorial for calibration.
* Calibration aims to minimize deviations, but actual hardware variations are inherent. Hence, adjust the calibration to a level that reasonably suits your requirements.
:::
If the robot exhibits deviations during operation, it may require IMU calibration. Once the calibration process is completed, the robot can resume normal operation.
### 2.5.1 IMU Calibration
IMU (Inertial Measurement Unit) is a device that measures the three-axis attitude angles (angular velocity) and acceleration of an object. The gyroscope and accelerometer are the main components of the IMU, providing a total of 6 degrees of freedom to measure the angular velocity and acceleration of the object in three-dimensional space.
The image above shows the positive directions of the IMU's three axes: x, y, and z. Refer to this coordinate system during the calibration process. After the node receives the first IMU message, it will prompt you to hold the IMU in a specific direction and press Enter to record the measurement. Once all six directions are completed, the node will calculate the calibration parameters and save them to a designated YAML file. Follow these steps:
:::{Note}
The input command is case-sensitive, and keywords can be completed using the Tab key.
:::
(1) Start the robot, and access the robot system desktop using NoMachine.
(2) Click on 
to start the command-line terminal.
(3) Execute the command to disable the app auto-start service.
```
sudo systemctl stop start_app_node.service
```
(4) Run the command and hit Enter key to enable chassis control node:
```
ros2 launch ros_robot_controller ros_robot_controller.launch.py
```
(5) Create a new command-line terminal, then input the following command, and hit Enter key to initiate IMU calibration:
```
ros2 run imu_calib do_calib --ros-args -r imu:=/ros_robot_controller/imu_raw --param output_file:=/home/ubuntu/ros2_ws/src/calibration/config/imu_calib.yaml
```
(6) When the prompt appears, orient the robot in the first-person perspective so that it is facing forward, then press Enter. (The direction of this initial orientation will be defined as the front, and all subsequent placements should be based on this reference.)
After you successfully calibrate all direction, the following prompt will appear.
(7) Align the robot to the rear, then press Enter.
(8) Align the robot to the left, then hit Enter.
(9) Align the robot to the right, then press Enter.
(10) Lift the robot, place it facing upwards, and press Enter. When positioning vertically, be careful to avoid instability or collisions. Use your hand for support to prevent damage to the depth camera or screen.
(11) Place the robot as pictured, then hit Enter.
(12) If the below prompt shows up, it means the calibration is complete. To exit, use short-cut **'ctrl+c'**.
(13) After calibration, execute the command to verify the calibrated model.
```
ros2 launch peripherals imu_view.launch.py
```
### 2.5.2 Angular Velocity Calibration
In the Ackerman configuration of the JetRover, angular velocity calibration is not necessary. However, if you need to perform angular velocity calibration, the robot must rotate in place. It's crucial to clearly mark the robot's orientation to help observe any deviations. Follow these specific steps:
:::{Note}
The input command is case-sensitive, and keywords can be completed using the Tab key.
:::
(1) Place the robot on a flat surface and place a piece of tape or other marker in front of the robot's center.
(2) Start the robot, and connect it to the robot system desktop using NoMachine.
(3) Click on to start the command-line terminal.
(4) Run the command and hit Enter to disable the app auto-start service.
```
sudo systemctl stop start_app_node.service
```
(5) Before starting the calibration, navigate to the directory of the calibration configuration file and open the file.
```
cd ~/ros2_ws/src/driver/controller/config && vim calibrate_params.yaml
```
(6) Change the angular velocity parameter to 1.0, then proceed with the subsequent calibration.
(7) After making the changes, press the **'ESC'** key, type **':wq'**, and then exit and save the file.
(8) Next, enter the command and press Enter to start the chassis control node.
```
ros2 launch ros_robot_controller ros_robot_controller.launch.py
```
(9) Open a new command line terminal, enter the command, and press Enter to begin the angular velocity calibration.
```
ros2 launch calibration angular_calib.launch.py
```
Click on **"calibrate_angular"** on the left side. The calibration interface will appear as shown below.
The parameters on the left side of the interface are defined as follows:
The first parameter, **"test_angle"**, represents the test rotation angle, with a default value of 360°.
The second parameter, **"speed"**, represents the linear velocity with a default value of 0.15 meters per second.
The third parameter, **"tolerance"**, represents the error value. A smaller error value results in more significant robot oscillations after reaching the target position.
The fourth parameter, **"odom_angle_scale_correction"**, represents the odometry angle scale correction.
The seventh parameter, **"start_test"**, is the button to start testing the odometry angle scale correction.
Ensure the robot is properly aligned, with the marker placed in front of it. Check the **"start_test"** option and the robot will rotate in place. If it fails to complete a full rotation, you need to adjust the **"odom_angule_scale_correction"** value, which controls the motor's rotation scale. It is recommended to adjust this value in increments of 0.01. Adjust the settings until the robot completes a full rotation, then record this value.
(10) Once the calibration is complete, enter the command to go to the calibration configuration file directory and update it with the new calibrated parameter values.
```
cd ~/ros2_ws/src/driver/controller/config && vim calibrate_params.yaml
```
Press **'I'** key to navigate to the editing mode, and modify the value of **"angular_correctqion_factor"** to the adjusted value of **"odom_angule_scale_correction"**
:::{Note}
The aforementioned operations are conducted on the Mecanum-wheel version and are equally applicable to the tank chassis version.
:::
(11) After modification, press the **"ESC"** key, enter **":wq"** to exit and save the changes.
### 2.5.3 Linear Velocity Calibration
Angular velocity calibration is not required in the Ackerman configuration of the JetRover, and the same applies to the Mecanum wheel and tracked configurations.
:::{Note}
The input command is case-sensitive, and keywords can be completed using the Tab key.
:::
Position the robot on a flat and open surface. Mark the starting point with tape or any other indicator in front of the robot, and position the endpoint tape or another marker 1 meter ahead of the robot.
(1) Start the robot, and access the robot system desktop using NoMachine.
(2) Click on to start the command-line terminal.
(3) Execute the command and hit Enter to disable the app auto-start service.
```
sudo systemctl stop start_app_node.service
```
(4) Before starting the calibration, navigate to the directory of the calibration configuration file and open it.
```
cd ~/ros2_ws/src/driver/controller/config && vim calibrate_params.yaml
```
(5) Set the linear velocity parameter, `linear_correction_factor`, to 1.0, then proceed with the calibration.
(6) After making the changes, press the **'ESC'** key, type **':wq'**, and exit while saving the file.
(7) Enter the following command and press Enter to start the chassis control node:
```
ros2 launch ros_robot_controller ros_robot_controller.launch.py
```
(8) Open a new command line terminal and enter this command to activate the linear velocity calibration feature:
```
ros2 launch calibration linear_calib.launch.py
```
(9) Once the calibration program is running, click **'calibrate_linear'** on the left side. The calibration interface will be displayed as shown below:
(10) Click on **"calibrate_linear"** on the left side, and the calibration interface will appear as follows.
The meanings of parameters on the left side of the interface are as follows:
The first parameter `test_distance` represents the testing distance, with a default value of 1 meter.
The second parameter `speed` represents the linear velocity, with a default value of 0.2 meters per second.
The third parameter `tolerance` represents the error value. A smaller error value results in greater robot shaking after reaching the target position.
The fourth parameter `odom_linear_scale_correction` represents the odometer linear scale correction.
The fifth parameter `start_test` is the button to start testing the odometer linear scale correction.
(11) Ensure the robot is properly aligned and positioned at the starting point marker. Check the box for **"start_test"** and the robot will move forward. Observe if the robot travels in a straight line. If there is deviation, adjust the value of **"odom_linear_scale_correction"**. This value adjusts the motor's scaling factor for forward movement. It is recommended to adjust this value by increments of 0.01 each time.
(12) After the calibration is complete, enter the command to navigate to the calibration configuration file directory and update it with the calibrated parameter values.
```
cd ~/ros2_ws/src/driver/controller/config && vim calibrate_params.yaml
```
(13) Press the `I` key to enter edit mode and modify the value of `linear_correction_factor` to the adjusted value of `odom_linear_scale_correction`.
(14) After making the modifications, press the `ESC` key, enter `:wq` to exit and save the changes.
(15) If you need to terminate this program, use short-cut **'Ctrl+C'**.
## 2.6 Publish IMU and Odometer Data
In robot navigation, accurately calculating real-time position is essential. Normally, we obtain odometer information using motor encoders and the robot's kinematic model. However, in specific situations, like when the robot's wheels rotate in place or when the robot is lifted, it may move a distance without the wheels actually turning.
To address wheel slip or accumulated errors in such cases, combining IMU and odometer data can yield more precise odometer information. This improves mapping and navigation accuracy in scenarios where wheel slip or cumulative errors may occur.
### 2.6.1 Introduction to IMU and Odometer
The IMU (Inertial Measurement Unit) is a device that measures the three-axis attitude angles (angular velocity) and acceleration of an object. It consists of the gyroscope and accelerometer as its main components, providing a total of 6 degrees of freedom to measure the object's angular velocity and acceleration in three-dimensional space.
An odometer is a method used to estimate changes in an object's position over time using data obtained from motion sensors. This method is widely applied in robotic systems to estimate the distance traveled by the robot relative to its initial position.
There are common methods for odometer positioning, including the wheel odometer, visual odometer, and visual-inertial odometer. In robotics, we specifically use the wheel odometer. To illustrate the principle of the wheel odometer, consider a carriage where you want to determine the distance from point A to point B. By knowing the circumference of the carriage wheels and installing a device to count wheel revolutions, you can calculate the distance based on wheel circumference, time taken, and the number of wheel revolutions.
While the wheel odometer provides basic pose estimation for wheeled robots, it has a significant drawback: accumulated error. In addition to inherent hardware errors, environmental factors such as slippery tires due to weather conditions contribute to increasing odometer errors with the robot's movement distance.
Therefore, both IMU and odometer are essential components in a robot. These two components are utilized to measure the three-axis attitude angles (or angular velocity) and acceleration of the object, as well as to estimate the distance, pose, velocity, and direction of the robot relative to its initial position.
To address these errors, we combine IMU data with odometer data to obtain more accurate information. IMU data is published through the `/imu` topic, and odometer data is published through `/odom`. After obtaining data from both sources, the data is fused using the `ekf` package in ROS, and the fused localization information is then republished.
### 2.6.2 IMU Data Publishing
* **Initiate Service**
:::{Note}
When entering commands, it is essential to strictly distinguish between uppercase and lowercase letters, and keywords can be autocompleted using Tab key.
:::
(1) Start the robot, and access the robot system desktop using NoMachine.
(2) Click on to start the command-line terminal.
(3) Execute the command, and hit Enter to disable the app auto-start service.
```
sudo systemctl stop start_app_node.service
```
(4) Next, enter the command and press Enter to start the chassis control node.
```
ros2 launch ros_robot_controller ros_robot_controller.launch.py
```
(5) Open a new command line terminal, enter the command, and press Enter to begin publishing IMU data:
```
ros2 launch peripherals imu_filter.launch.py
```
* **Data Viewing**
(1) Open a new command-line terminal, and execute the command to check the current topic.
```
ros2 topic list
```
(2) Enter the command to view the type, publisher, and subscribers of the `/imu` topic. You can replace `/imu` with the topic you want to view. The type of this topic is `sensor_msgs/msg/Imu`.
```
ros2 topic info /imu
```
(3) Enter the command to display the topic message content. You can replace it with the topic you wish to view:
```
ros2 topic echo /imu
```
The terminal will display the data from the three axes of the IMU.
### 2.6.3 Odometer Data Publishing
* **Initiate Service**
:::{Note}
When entering commands, it is essential to strictly distinguish between uppercase and lowercase letters, and keywords can be autocompleted using Tab key.
:::
(1) Start the robot, and access the robot system desktop using NoMachine.
(2) Double-click to start the command line terminal.
(3) Execute the command, and hit Enter to disable app auto-start service.
```
sudo systemctl stop start_app_node.service
```
(4) Run the command to publish the odometer data.
```
ros2 launch controller odom_publisher.launch.py
```
* **Data Viewing**
(1) Open a new command line terminal, and run the command below to check the current topic.
```
ros2 topic list
```
(2) Enter the command to view the type, publisher, and subscribers of the `/odom_raw` topic. You can replace `/odom_raw` with the topic you want to view. The type of this topic is `nav_msgs/msg/Odometry`.
```
ros2 topic echo /odom_raw
```
This message contains the acquired pose and velocity (twist) data.
## 2.7 Robot Speed Control
Speed control is achieved by adjusting the linear velocity parameter.
### 2.7.1 Program Logic
Based on the robot's movement characteristics, control the active wheels to achieve forward, backward, and turning movements.
In the program, subscribe to the `/controller/cmd_vel` movement control topic to obtain the set linear and angular velocities. Then, analyze and calculate based on these velocities to determine the car's movement speed.
The source code for this program can be found at: [/home/ubuntu/ros2_ws/src/driver/controller/controller/odom_publisher_node.py](../_static/source_code/driver.zip)
### 2.7.2 Disable APP Service and Initiate Speed Control
:::{Note}
When entering commands, it is essential to strictly distinguish between uppercase and lowercase letters, and keywords can be autocompleted using Tab key.
:::
(1) Start the robot, and access the robot system desktop using NoMachine.
(2) Click on 
to start the command-line terminal.
(3) Execute the command and hit Enter to disable the app auto-start service.
```
sudo systemctl stop start_app_node.service
```
(4) Enter the command to enable motion control service.
```
ros2 launch controller controller.launch.py
```
(5) Open a new ROS2 command-line terminal, then enter the following command to enable the speed control.
```
ros2 topic pub /controller/cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"
```
Here, `linear` sets the linear velocity, with the robot's first-person perspective defining the positive X direction as the front of the robot, and there is no movement in the Y and Z directions.
`Angular` sets the angular velocity, where a positive Z value causes the robot to turn left, and a negative Z value causes it to turn right, with no movement in the X and Y directions.
:::{Note}
* In this scenario, the linear velocity (x) is measured in meters per second, and it is advisable to maintain it within the range of **"-0.6 to 0.6"**.
* The angular velocity (z) denotes the turning speed and is determined by the formulas V=ωR (linear velocity equals angular velocity times radius) and tanΦA=D/R (where z=ω, D=0.213, and ΦA represents the turning angle). The angle ΦA should be within the range of 0 to 36 degrees.
:::
Use the left and right arrow keys on the keyboard to adjust the relevant parameters. For instance, to move forward, set the linear velocity X value to `0.1` and press Enter to apply the change.
(6) If you need to stop the robot, open a new terminal and change the linear velocity back to `0.0`. Then press Enter to apply the change.
(7) If you need to terminate this program, use short-cut **'Ctrl+C'**.
:::{Note}
To bring the robot car to a stop, please create a new terminal and adjust the linear velocity. Using the **'Ctrl+C'** shortcut alone may not effectively halt the robot car.
:::
### 2.7.3 Change Forward Speed
By modifying the linear velocity value (X), the robot can achieve forward movement at variable speeds. For instance, to make the robot shift diagonally to the left front, during step 5 in **'2. Disable App Service and Initiate Speed Control'**, set X to **'X: 0.3'**.
Upon pressing Enter key, the robot will move forward at a speed of 0.3 meters per second in a straight direction.
### 2.7.4 Program Outcome
After the game starts, the robot will go forward at the speed of 0.3m/s.
### 2.7.5 Program Analysis
[Source Code](../_static/source_code/driver.zip)
The files include `hiwonder_controller.launch` for launch configuration, `calibrate_params.yaml` for parameter configuration, and `odom_publisher.py` for program execution. During startup, the launch file is executed first. It loads the YAML configuration file and passes the parameters to the ROS nodes. Subsequently, the nodes initialize by reading the configuration parameters from the ROS nodes and communicate with other nodes to collaboratively implement functionalities.
* **Launch File**
The launch file is located in: [/home/ubuntu/ros2_ws/src/driver/controller/launch/controller.launch.py](../_static/source_code/driver.zip)
{lineno-start=1}
```python
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch.conditions import IfCondition
from nav2_common.launch import ReplaceString
from launch import LaunchDescription, LaunchService
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction, TimerAction, OpaqueFunction
def launch_setup(context):
compiled = os.environ['need_compile']
namespace = LaunchConfiguration('namespace', default='')
use_namespace = LaunchConfiguration('use_namespace', default='false').perform(context)
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
enable_odom = LaunchConfiguration('enable_odom', default='true')
map_frame = LaunchConfiguration('map_frame', default='map')
odom_frame = LaunchConfiguration('odom_frame', default='odom')
base_frame = LaunchConfiguration('base_frame', default='base_footprint')
imu_frame = LaunchConfiguration('imu_frame', default='imu_link')
frame_prefix = LaunchConfiguration('frame_prefix', default='')
namespace_arg = DeclareLaunchArgument('namespace', default_value=namespace)
use_namespace_arg = DeclareLaunchArgument('use_namespace', default_value=use_namespace)
use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value=use_sim_time)
enable_odom_arg = DeclareLaunchArgument('enable_odom', default_value=enable_odom)
map_frame_arg = DeclareLaunchArgument('map_frame', default_value=map_frame)
odom_frame_arg = DeclareLaunchArgument('odom_frame', default_value=odom_frame)
base_frame_arg = DeclareLaunchArgument('base_frame', default_value=base_frame)
imu_frame_arg = DeclareLaunchArgument('imu_frame', default_value=imu_frame)
frame_prefix_arg = DeclareLaunchArgument('frame_prefix', default_value=frame_prefix)
```
① Set the storage path
Retrieve the paths for the three packages: peripherals, controller, and servo_controller.
{lineno-start=34}
```python
if compiled == 'True':
peripherals_package_path = get_package_share_directory('peripherals')
controller_package_path = get_package_share_directory('controller')
servo_controller_package_path = get_package_share_directory('servo_controller')
else:
peripherals_package_path = '/home/ubuntu/ros2_ws/src/peripherals'
controller_package_path = '/home/ubuntu/ros2_ws/src/driver/controller'
servo_controller_package_path = '/home/ubuntu/ros2_ws/src/driver/servo_controller'
```
② Initiate other Launch files
{lineno-start=43}
```python
odom_publisher_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(controller_package_path, 'launch/odom_publisher.launch.py')
]),
launch_arguments={
'namespace': namespace,
'use_namespace': use_namespace,
'imu_frame': imu_frame,
'frame_prefix': frame_prefix,
'base_frame': base_frame,
'odom_frame': odom_frame
}.items()
)
```
`odom_publisher_launch` Odometer launch
`imu_filter_launch` IMU launch
③ Initiate Node
Launch the EKF fusion node.
{lineno-start=66}
```python
ekf_filter_node = Node(
package='robot_localization',
executable='ekf_node',
name='ekf_filter_node',
output='screen',
parameters=[ekf_param, {'use_sim_time': use_sim_time}],
remappings=[
('/tf', 'tf'),
('/tf_static', 'tf_static'),
('odometry/filtered', 'odom'),
('cmd_vel', 'controller/cmd_vel')
],
condition=IfCondition(enable_odom),
)
servo_controller_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(servo_controller_package_path, 'launch/servo_controller.launch.py')
]),
launch_arguments={
'base_frame': base_frame,
}.items()
)
```
* **Python Program**
The Python program is saved in: [/home/ubuntu/ros2_ws/src/driver/controller/controller/odom_publisher_node.py]()
① Import Library
{lineno-start=3}
```python
import os
import math
import time
import rclpy
import signal
import threading
from rclpy.node import Node
from std_srvs.srv import Trigger
from nav_msgs.msg import Odometry
from controller import ackermann, mecanum
from ros_robot_controller_msgs.msg import MotorsState, BusServoState, SetBusServoState
from geometry_msgs.msg import Pose2D, Pose, Twist, PoseWithCovarianceStamped, TransformStamped
```
② Main Function
{lineno-start=279}
```python
def main():
node = Controller('odom_publisher')
rclpy.spin(node) # Loop waiting for ROS2 to exit(循环等待ROS2退出)
```
The `controller` class is invoked here, and wait for the node to exit.
③ Global Parameter
{lineno-start=}
```python
ODOM_POSE_COVARIANCE = list(map(float,
[1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3]))
ODOM_POSE_COVARIANCE_STOP = list(map(float,
[1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9]))
ODOM_TWIST_COVARIANCE = list(map(float,
[1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3]))
ODOM_TWIST_COVARIANCE_STOP = list(map(float,
[1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9]))
```
| ODOM_POSE_COVARIANCE | Odometer POSE covariance |
| ODOM_POSE_COVARIANCE_STOP | When speed is 0, Mileage POSE covariance |
| ODOM_TWIST_COVARIANCE | Mileage TWIST covariance |
| ODOM_TWIST_COVARIANCE_STOP | When speed is 0, Mileage TWIST covariance |
| get_node_state | Used to obtain the current node status, as a callback function |
| shutdown | Used to shut down the ROS node, as a callback function |
| load_callbrate_param | Used to read the current motion parameters,as a callback function |
| set_odom | Used to read and set odometry, as a callback function |
| app_cmd_vel_callback | Used to set the app speed, as a callback function |
| cmd_vel_callback | Used to set velocity, as a callback function |
| cal_odom_fun | Used to publish odometry data |