std_msgs/Header header
# Vector 0 head and tail points
uint32 m0_x0 # Tail of vector @ x
uint32 m0_y0 # Tail of vector @ y
uint32 m0_x1 # Head of vector @ x
uint32 m0_y1 # Head of vector @ y
# Vector 1 head and tail points
uint32 m1_x0 # Tail of vector @ x
uint32 m1_y0 # Tail of vector @ y
uint32 m1_x1 # Head of vector @ x
uint32 m1_y1 # Head of vector @ yros2 launch electrode electrode.launch.py #sim:=true (If you are using simulation)// Follow line function in velocity.c
static void follow_line(context* ctx)
{
double frame_width = 78;
double frame_height = 51;
double window_center = frame_width / 2;
double linear_velocity=0.7;
double angular_velocity=-0.6;
double single_line_steer_scale=0.6;
double x = 0.0;
double y = 0.0;
double steer = 0.0;
double speed = 0.0;
int num_vectors = 0;
if(!(ctx->pixy_vector.m0_x0 == 0 && ctx->pixy_vector.m0_x1 == 0 && ctx->pixy_vector.m0_y0 == 0 && ctx->pixy_vector.m0_y1 == 0)) {
num_vectors++;
}
if(!(ctx->pixy_vector.m1_x0 == 0 && ctx->pixy_vector.m1_x1 == 0 && ctx->pixy_vector.m1_y0 == 0 && ctx->pixy_vector.m1_y1 == 0)) {
num_vectors++;
}
switch(num_vectors) {
case 0:
speed = 0.0;
steer = 0.0;
break;
case 1:
if(ctx->pixy_vector.m0_x1 > ctx->pixy_vector.m0_x0) {
x = (ctx->pixy_vector.m0_x1 - ctx->pixy_vector.m0_x0) / frame_width;
y = (ctx->pixy_vector.m0_y1 - ctx->pixy_vector.m0_y0) / frame_height;
} else {
x = (ctx->pixy_vector.m0_x0 - ctx->pixy_vector.m0_x1) / frame_width;
y = (ctx->pixy_vector.m0_y0 - ctx->pixy_vector.m0_y1) / frame_height;
}
if((ctx->pixy_vector.m0_x0 != ctx->pixy_vector.m0_x1) && ((y > 0.0) || (y <0.0))) {
steer = -angular_velocity * (x / y) * single_line_steer_scale;
} else {
steer = 0.0;
}
speed = linear_velocity;
break;
case 2:
if((ctx->pixy_vector.m1_x0 >= ctx->pixy_vector.m1_x1) && (ctx->pixy_vector.m0_x0 <= ctx->pixy_vector.m0_x1)){
steer = angular_velocity * (((ctx->pixy_vector.m0_x1 + ctx->pixy_vector.m1_x1) / 2.0) - window_center) / frame_width;
}else if ((ctx->pixy_vector.m1_x0 < ctx->pixy_vector.m1_x1) && (ctx->pixy_vector.m0_x0 <= ctx->pixy_vector.m0_x1)){
steer = angular_velocity * (((ctx->pixy_vector.m0_x1 + ctx->pixy_vector.m1_x0) / 2.0) - window_center) / frame_width;
}else if ((ctx->pixy_vector.m1_x0 > ctx->pixy_vector.m1_x1) && (ctx->pixy_vector.m0_x0 > ctx->pixy_vector.m0_x1)){
steer = angular_velocity * (((ctx->pixy_vector.m0_x0 + ctx->pixy_vector.m1_x1) / 2.0) - window_center) / frame_width;
}else {
steer = angular_velocity * (((ctx->pixy_vector.m0_x0 + ctx->pixy_vector.m1_x0) / 2.0) - window_center) / frame_width;
}
speed = linear_velocity;
break;
}
double vel_linear_x = speed * (1 - fabs(2 * steer));
double turn_angle = 0;
double omega_fwd = 0;
double V = vel_linear_x;
double omega = steer;
double delta = 0;
CASADI_FUNC_ARGS(ackermann_steering);
args[0] = &ctx->wheel_base;
args[1] = ω
args[2] = &V;
res[0] = δ
CASADI_FUNC_CALL(ackermann_steering);
omega_fwd = V / ctx->wheel_radius;
if (fabs(V) > 0.01) {
turn_angle = delta;
}
b3rb_set_actuators(&ctx->actuators, turn_angle, omega_fwd);
}cd ~/cognipilot/ws/cerebri
west update
west build -b native_sim app/b3rb/ -p -t installcd ~/cognipilot/ws/cerebri
west update
west build -b mr_canhubk3 app/b3rb -p
west flashdef statusCallback(self,data):
#Putting robot in AUTO mode
if(data.mode!=2):
joystick_msg=sensor_msgs.msg.Joy()
joystick_msg.header.stamp=ROSClock().now().to_msg()
joystick_msg.axes=[0.0,0.0,0.0,0.0]
joystick_msg.buttons = [0, 1, 0, 0, 0, 0, 0, 0]
#self.JoystickPub.publish(joystick_msg)
#If robot is in AUTO mode -> arming the robot
elif(data.mode==2 and data.arming!=2):
joystick_msg=sensor_msgs.msg.Joy()
joystick_msg.header.stamp=ROSClock().now().to_msg()
joystick_msg.axes=[0.0,0.0,0.0,0.0]
joystick_msg.buttons = [0, 0, 0, 0, 0, 0, 0, 1]
#self.JoystickPub.publish(joystick_msg)sleep(15.0)ros2 launch b3rb_gz_bringup sil.launch.pyros2 launch electrode electrode.launch.py sim:=true#!/bin/bash
declare -A repos=(
["cranium/src/synapse_protobuf"]="https://github.com/NXPHoverGames/synapse_protobuf"
["cranium/src/synapse_tinyframe"]="https://github.com/NXPHoverGames/synapse_tinyframe"
["cranium/src/synapse_msgs"]="https://github.com/NXPHoverGames/synapse_msgs"
["cranium/src/synapse_ros"]="https://github.com/NXPHoverGames/synapse_ros"
["cranium/src/dream_world"]="https://github.com/NXPHoverGames/dream_world"
["cranium/src/b3rb_simulator"]="https://github.com/NXPHoverGames/b3rb_simulator"
["electrode/src/electrode"]="https://github.com/NXPHoverGames/electrode"
["ws/cerebri"]="https://github.com/NXPHoverGames/cerebri"
["cranium/src/nxp_cup_vision"]="https://github.com/NXPHoverGames/nxp_cup_vision"
)
# Update existing repositories or clone if they don't exist
for repo in "${!repos[@]}"; do
echo "Processing $repo..."
repo_path="${repo}" # Derive the full path
# Check if the repository directory exists. If it does not, clone it.
if [ ! -d "${repo_path}" ]; then
echo "Repository ${repo_path} does not exist. Cloning..."
git clone --branch nxp-cup "${repos[$repo]}" "${repo_path}"
echo "Cloned ${repo} into ${repo_path}."
fi
# Navigate to the repository directory
cd "${repo_path}" || { echo "Failed to change directory to ${repo_path}. Does it exist?"; continue; }
# Set the new remote URL
git remote set-url origin "${repos[$repo]}"
echo "Remote changed to ${repos[$repo]}"
# Fetch changes from the new remote
git fetch origin
echo "Fetched changes from origin."
# Checkout the specific branch
git checkout nxp-cup
if [ $? -eq 0 ]; then
echo "Checked out nxp-cup branch."
# Pull the latest changes from the branch
git pull origin nxp-cup
echo "Pulled latest changes from nxp-cup branch."
else
echo "Failed to checkout nxp-cup branch. Does it exist?"
fi
# Return to the original directory
cd - > /dev/null
donecd ~/cognipilot
chmod +x update_repos_native.sh
./update_repos_native.shcd ~/cognipilot/cranium/
colcon build --symlink-install
cd ~/cognipilot/cranium/
source install/setup.bashcd ~/cognipilot/electrode/
colcon build --symlink-install
cd ~/cognipilot/electrode/
source install/setup.bashcd ~/cognipilot/ws/cerebri
west update
west build -b native_sim app/b3rb/ -p -t install
source ~/.bashrcros2 launch b3rb_gz_bringup sil.launch.py world:=nxp_raceway_octagonros2 launch electrode electrode.launch.py sim:=true 
Cognipilot MR-B3RB Robot installation specific instructions for nxp-cup
cd ~/cognipilot/ws/cerebri
git pullcd ~/cognipilot/ws/cerebri
git statusOn branch nxp-cup
Your branch is up to date with 'origin/nxp-cup'.
nothing to commit, working tree cleangit checkout nxp-cupcd ~/cognipilot/ws/cerebri
west updatecd ~/cognipilot/ws/cerebri
west build -b mr_canhubk3 app/b3rb -pcd ~/cognipilot/ws/cerebri
west flashJLinkExe
connect
S32K344
S
4000
loadbin /home/$user/cognipilot/ws/cerebri/build/zephyr/zephyr.elf 0x400000
exitssh <username>@<hostname>.local#!/bin/bash
declare -A repos=(
["cranium/src/synapse_protobuf"]="https://github.com/NXPHoverGames/synapse_protobuf"
["cranium/src/synapse_tinyframe"]="https://github.com/NXPHoverGames/synapse_tinyframe"
["cranium/src/synapse_msgs"]="https://github.com/NXPHoverGames/synapse_msgs"
["cranium/src/synapse_ros"]="https://github.com/NXPHoverGames/synapse_ros"
["cranium/src/b3rb_robot"]="https://github.com/NXPHoverGames/b3rb_robot"
["cranium/src/nxp_cup_vision"]="https://github.com/NXPHoverGames/nxp_cup_vision"
)
# Update existing repositories or clone if they don't exist
for repo in "${!repos[@]}"; do
echo "Processing $repo..."
repo_path="${repo}" # Derive the full path
# Check if the repository directory exists. If it does not, clone it.
if [ ! -d "${repo_path}" ]; then
echo "Repository ${repo_path} does not exist. Cloning..."
git clone --branch nxp-cup "${repos[$repo]}" "${repo_path}"
echo "Cloned ${repo} into ${repo_path}."
fi
# Navigate to the repository directory
cd "${repo_path}" || { echo "Failed to change directory to ${repo_path}. Does it exist?"; continue; }
# Set the new remote URL
git remote set-url origin "${repos[$repo]}"
echo "Remote changed to ${repos[$repo]}"
# Fetch changes from the new remote
git fetch origin
echo "Fetched changes from origin."
# Checkout the specific branch
git checkout nxp-cup
if [ $? -eq 0 ]; then
echo "Checked out nxp-cup branch."
# Pull the latest changes from the branch
git pull origin nxp-cup
echo "Pulled latest changes from nxp-cup branch."
else
echo "Failed to checkout nxp-cup branch. Does it exist?"
fi
# Return to the original directory
cd - > /dev/null
donecd ~/cognipilot
chmod +x update_repos_navqplus.sh
./update_repos_navqplus.shcd ~/cognipilot/cranium/
colcon build --symlink-install
cd ~/cognipilot/cranium/
source install/setup.bashros2 launch b3rb_bringup robot.launch.pyros2 launch electrode electrode.launch.py