NXP-CUP 2024: line follower algorithm
This page explains the software required for the NXP CUP 2024 with the MR-B3RB robot.
The CogniPilot software is integrated into both the NavQPlus, within ROS2, and the MR-CANHUBK344, within the Zephyr RTOS together forming an Ackermann steering robotic development platform.
Please follow the installation instructions to obtain the modified branch of the CogniPilot software, specifically tailored for the NXP CUP 2024; these instructions are explained in the previous sectionNXP-CUP 2024: Simulation Install instructions
ROS2 brief explanation
The ROS2 part of the software is designed for high-level tasks and should be treated as "black box" for the NXP-CUP 2024,meaning that its code should not be modified.If you encounter any errors in this code, please report them to the NXP Cup organizers or through the technical support channel on the official NXP CUP Discord: https://discord.gg/JcWPbw649S.
The main CogniPilot modification in the ROS2 side for the NXP Cup 2024 is addition of the nxp_track_vision
node, which receives camera data, transforms the perspective, and extracts the Pixy Vector message. This message consists of two vectors, each with a tail and a head, defining the borders of the race track. This information is used by the robot controller to ensure that the robot stays within the track boundaries.
Here's the definition of the Pixy Vector ROS2 message:
The std_msgs/Header
in the Pixy Vector ROS2 message includes the timestamp indicating when the message was sent. Additionally, the message is composed by two vectors, with each vector represented by a pair of points: one indicating where the vector begins (tail) and the other where it ends (head). Each of these points is defined by an x and a y value, specifying their position in a two-dimensional space.
The nxp_track_vision_node
also publishes the Camera Debug Image. This image shows the perspective transformation applied to the camera's view, along with the detected lines of the track. Additionally, it displays the vectors being sent by the track vision node. This visual feedback should result helpful for debugging and optimizing the robot;s control software.
This is an example of the debug camera image:
You can visualize this image through Foxglove. For doing that first run:
Then add an Image panel to the B3RB layout provided and configure the ROS2 topic to /nxp_cup/debug_image.
Control algorithm through Cerebri
Now a explanation of the line following control algorithm will be done. This algorithm is located on the Cerebri running on the Zypher RTOS in the MR-CANHUBK344. The Cerebri acts as the brain of our operation, facilitating complex decision-making processes that guide the robot seamlessly along the track.
This code is what you must modify and improve for the NXP CUP 2024.
The main function of the line follower algorithm is located in the velocity state inside the finite state machine logic that manages the robot's behaviour. This file is located in the ~/cognipilot/ws/cerebri/app/src/velocity.c
This is the GitHub location: See code.
The application folder is where the vehicle finite state machine, control, and estimation code reside. For more information visit: https://airy.cognipilot.org/cerebri/platforms/rovers/
In that folder you will find a file called velocity.c. This code is written in C language and has two main operation modes: the cmd_vel modes which listens the /cmd_vel topic from the Nav2 node from ROS2 and the auto mode, this one will listen to the Pixy vector message sendt by the nxp_track_vision_node and will estimate the velocity needed to maintain the robot in the race track:
There are several important variables and functions that are used in the code. A key description of them is given below:
The switch instruction evaluates the number of vectors found in the camera image and what the robot car will do depending on this number:
Case: 0 vectors found
If no vectors are found (case 0), then the vehicle will stop.
Case: 1 vector found
If one vector is found, the algorithm finds the gradient of the vector and stores that in steer, and the speed
Case: 2 vectors found
If two vectors are found, then the example algorithm will find the offset in the x direction of the average of the two head points of each vector. This will give us a steering value that will steer the cup car in the correct direction, which will be stored in steer. The speed value will be calculated and is stored in speed.
After calculating the speed and the steer values, a call to the CASADI function is done to estimate the control variable for the actuators. Then the robot can move the necessary meters that you need.
Modifying the code
Before making changes to the velocity.c file or any files in the ~/cognipilot/ws/cerebri/app/b3rb directory, remember to rebuild the application to see the changes take effect.
For simulation:
For real robot:
Auto mode and Arming the robot automatically
In the nxp_track_vision.py file within the ROS2 package nxp_cup_vision, you'll find the following code snippet:
This code snippet automatically sets the B3RB robot in AUTO mode and arms the robot. By default, the self.JoystickPub.publish(joystick_msg) lines are commented out to allow control through Foxglove Studio during development. Uncomment these lines if you want the robot to automatically enter AUTO mode and arm itself.
If you need the robot to delay its start, add a sleep command:
This makes the robot wait for 15 seconds before starting, which can be particularly useful for the NXP-CUP competition.
Last updated