Vylos Docs

Technical documentation for each of our products.

Navigator

Technical documentation for Navigator autonomous navigation products.

Navigator v0.01

Welcome to the Autonomous Navigation Documentation repository. This project serves as a comprehensive knowledge base, reference guide, and practical resource for mobile robot navigation, SLAM (Simultaneous Localization and Mapping), motion planning, and ROS/ROS2 implementation.

📌 Overview

This repository aggregates architectural concepts, algorithmic explanations, setup guides, and configuration references required to deploy autonomous navigation stacks on mobile robots (e.g., differential drive, ackermann steering, holonomic platforms).

Key Focus Areas

• Localization & Mapping: 2D/3D SLAM algorithms (Cartographer, GMapping, RTAB-Map, AMCL).
• Path Planning & Navigation: Global planners (A*, Dijkstra) and local controllers (TEB, DWA, Regulated Pure Pursuit).
• Robot Operating System: Integration guides for ROS 1 and ROS 2 (Nav2).
• Sensor Fusion: Integrating LiDAR, IMU, Wheel Odometry, and Depth Cameras (robot_localization, Extended Kalman Filters).

🏗 System Architecture

The typical navigation pipeline documented in this repository follows the standard ROS 2 / Nav2 conceptual framework:

+-----------------------------------------------------------------------+
|                                 MAP                                   |
+-----------------------------------------------------------------------+
|                                                 |
v                                                 v
+--------------+       +-------------------+     +---------------+
| Global Planner| ----> |  Behavior Tree    | --> | Local Planner |
|  (A*, Dijkstra|       |  (BT Navigator)   |     | (TEB / DWA)   |
+--------------+       +-------------------+     +---------------+
|
v
+--------------+
| Motor Control|
|  (/cmd_vel)  |
+--------------+

📁 Repository Structure

Autnomous-nav-DOCS/
├── docs/
├── slam/                  # SLAM algorithms setup & tuning guidelines
│   ├── 2d_slam.md
│   └── 3d_slam.md
├── navigation/            # Path planning, obstacle avoidance, & costmaps
│   ├── costmap_config.md
│   └── controller_tuning.md
├── sensors/               # Sensor calibration, driver setups, & fusion
│   ├── imu_calibration.md
│   └── lidar_fusion.md
├── ros2_nav2/             # Nav2 stack setup, launch files, and param templates
└── assets/                # Diagrams, architecture flowcharts, & media

🚀 Quick Start & Prerequisites

Prerequisites

• OS: Ubuntu 22.04 LTS (Jammy Jellyfish) or later
• Framework: ROS 2 Humble / Iron / Jazzy
• Dependencies:

  sudo apt update
  sudo apt install ros-<ros2-distro>-navigation2 \
                   ros-<ros2-distro>-nav2-bringup \
                   ros-<ros2-distro>-turtlebot3*
  
  

### Getting Started

1. **Clone the Repository:**
```bash
git clone [https://github.com/hermanumrao/Autnomous-nav-DOCS.git](https://github.com/hermanumrao/Autnomous-nav-DOCS.git)
cd Autnomous-nav-DOCS

2. Explore Documentation: Browse through the docs/ or specific topic directories to view detailed guides on system configuration and sensor tuning.

⚙️ Hardware & Simulation Setup

Simulated Testbench

To test navigation workflows without physical hardware:

export TURTLEBOT3_MODEL=waffle
ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True

Physical Robot Target Specs

• Microcontroller/SBC: Raspberry Pi 4 / NVIDIA Jetson Orin Nano
• Sensors: 2D RPLiDAR A1/A2, 6-axis IMU (MPU6050/BNO055), Wheel Encoders
• Motor Drivers: L298N / Cytron MDD10A driven via Arduino/ESP32 ROS2 nodes

🤝 Contributing

Contributions, fixes, and updates to documentation are welcome!

1. Fork the Project.
2. Create your Feature Branch (git checkout -b feature/NewDocSection).
3. Commit your Changes (git commit -m 'Add documentation for 3D LiDAR SLAM').
4. Push to the Branch (git checkout origin feature/NewDocSection).
5. Open a Pull Request.

📄 License

Distributed under the MIT License. See LICENSE for more details.

Autonomous Navigation

2D LiDAR

	Range Frequency 5000Hz		Scan Frequency 6-12Hz
	Range Distance 0.12-10m		Scan Angle 360°
	Angle Resolution 0.43-0.85°		Size 110.671.152.3mm

SDK

https://github.com/YDLIDAR/YDLidar-SDK/tree/master You can install these packages using apt:

sudo apt install cmake pkg-config

In the YDLidar SDK directory, run the following commands to compile the project:

git clone https://github.com/YDLIDAR/YDLidar-SDK.git
cd YDLidar-SDK
mkdir build
cd build
cmake ..
make
sudo make install

ROS2 Driver

https://github.com/YDLIDAR/ydlidar_ros2_driver

1. Clone ydlidar_ros2_driver master branch from github for old version:

   git clone https://github.com/YDLIDAR/ydlidar_ros2_driver.git ydlidar_ros2_ws/src/ydlidar_ros2_driver

   Clone ydlidar_ros2_driver humble branch from github for humble,jazzy,etc:

   git clone -b humble https://github.com/YDLIDAR/ydlidar_ros2_driver.git ydlidar_ros2_ws/src/ydlidar_ros2_driver

2. Build ydlidar_ros2_driver package :

   cd ydlidar_ros2_ws
   colcon build --symlink-install
   

   Note: install colcon see

   

   Note: If the following error occurs, Please install YDLIDAR/YDLidar-SDK first.

   

3. Package environment setup :

   source ./install/setup.bash

   Note: Add permanent workspace environment variables. It’s convenientif the ROS2 environment variables are automatically added to your bash session every time a new shell is launched:

   echo "source ~/ydlidar_ros2_ws/install/setup.bash" >> ~/.bashrc
   source ~/.bashrc
   

4. Confirmation To confirm that your package path has been set, printenv the grep -i ROS variable.

   printenv | grep -i ROS
   

   You should see something similar to: OLDPWD=/home/tony/ydlidar_ros2_ws/install

5. Create serial port Alias [optional]

   chmod 0777 src/ydlidar_ros2_driver/startup/*
   sudo sh src/ydlidar_ros2_driver/startup/initenv.sh
   

   Note: After completing the previous operation, replug the LiDAR again.

Configure LiDAR Default parameter file

ydlidar_ros2_driver_node:
  ros__parameters:
    port: /dev/ttyUSB0
    frame_id: laser_frame
    ignore_array: ""
    baudrate: 230400
    lidar_type: 1
    device_type: 0
    isSingleChannel: false
    intensity: false
    intensity_bit: 0
    sample_rate: 9
    abnormal_check_count: 4
    fixed_resolution: true
    reversion: false
    inverted: false
    auto_reconnect: true
    support_motor_dtr: false
    angle_max: 180.0
    angle_min: -180.0
    range_max: 64.0
    range_min: 0.01
    frequency: 10.0
    invalid_range_is_inf: false
    debug: false

Note: It needs to be modified according to LiDAR actual situation.

Run ydlidar_ros2_driver

Run ydlidar_ros2_driver using launch file

The command format is :

ros2 launch ydlidar_ros2_driver [launch file].py

1. Connect LiDAR uint(s).

   ros2 launch ydlidar_ros2_driver ydlidar_launch.py 
   

   or

   launch $(ros2 pkg prefix ydlidar_ros2_driver)/share/ydlidar_ros2_driver/launch/ydlidar.py 
   

2. RVIZ

   ros2 launch ydlidar_ros2_driver ydlidar_launch_view.py 
   

3D LiDAR

Livox Mid 360

Spec

Livox SDK

refer https://github.com/Livox-SDK/Livox-SDK2

1. Install the CMake using apt:

$ sudo apt install cmake

2. Compile and install the Livox-SDK2:

$ git clone https://github.com/Livox-SDK/Livox-SDK2.git
$ cd ./Livox-SDK2/
$ mkdir build
$ cd build
$ cmake .. && make -j
$ sudo make install

Configuring the SDK

Start by checking out this: samples/livox_lidar_quick_start/mid360_config.json

change the ip of the host_ip and multicast_ip

{
  "MID360": {
    "lidar_net_info" : {
      "cmd_data_port"  : 56100,
      "push_msg_port"  : 56200,
      "point_data_port": 56300,
      "imu_data_port"  : 56400,
      "log_data_port"  : 56500
    },
    "host_net_info" : [
      {
        "host_ip"        : "192.168.1.82",
        "multicast_ip"   : "224.1.1.82",
        "cmd_data_port"  : 56101,
        "push_msg_port"  : 56201,
        "point_data_port": 56301,
        "imu_data_port"  : 56401,
        "log_data_port"  : 56501
      }
    ]
  }
}

ROS for Livox

Livox has ros driver packages for interfacing the SDK with ros so that we can publish the data to ROS1/ROS2. Fun part this package is good but might play glitchy.

https://github.com/Livox-SDK/livox_ros2_driver

config setting:

{
  "lidar_summary_info" : {
    "lidar_type": 8
  },
  "MID360": {
    "lidar_net_info" : {
      "cmd_data_port": 56100,
      "push_msg_port": 56200,
      "point_data_port": 56300,
      "imu_data_port": 56400,
      "log_data_port": 56500
    },
    "host_net_info" : {
      "cmd_data_ip" : "192.168.1.82",
      "cmd_data_port": 56101,
      "push_msg_ip": "192.168.1.82",
      "push_msg_port": 56201,
      "point_data_ip": "192.168.1.82",
      "point_data_port": 56301,
      "imu_data_ip" : "192.168.1.82",
      "imu_data_port": 56401,
      "log_data_ip" : "",
      "log_data_port": 56501
    }
  },
  "lidar_configs" : [
    {
      "ip" : "192.168.1.182",
      "pcl_data_type" : 1,
      "pattern_mode" : 0,
      "extrinsic_parameter" : {
        "roll": 0.0,
        "pitch": 0.0,
        "yaw": 0.0,
        "x": 0,
        "y": 0,
        "z": 0
      }
    }
  ]
}

Msgs

There are two formats in which the ROS driver comminicates:

• the standard PCL2 format
• the custom msgs, this is used in almost all the default packes from livox and even a few other packages.

4.1 Launch file configuration instructions

https://github.com/Livox-SDK/livox_ros2_driver#41-launch-file-configuration-instructions

All launch files of livox_ros2_driver are in the “ws_livox/src/livox_ros2_driver/launch” directory. Different launch files have different configuration parameter values and are used in different scenarios :

4.2 Livox_ros2_driver internal main parameter configuration instructions

https://github.com/Livox-SDK/livox_ros2_driver#42-livox_ros2_driver-internal-main-parameter-configuration-instructions

All internal parameters of Livox_ros2_driver are in the launch file. Below are detailed descriptions of the three commonly used parameters :

    livox_ros2_driver pointcloud data detailed description :

1. Livox pointcloud2 (PointXYZRTL) point cloud format, as follows :

float32 x               # X axis, unit:m
float32 y               # Y axis, unit:m
float32 z               # Z axis, unit:m
float32 intensity         # the value is reflectivity, 0.0~255.0
uint8 tag               # livox tag
uint8 line              # laser number in lidar

2. Livox customized data package format, as follows :

Header header             # ROS standard message header
uint64 timebase           # The time of first point
uint32 point_num          # Total number of pointclouds
uint8  lidar_id           # Lidar device id number
uint8[3]  rsvd            # Reserved use
CustomPoint[] points      # Pointcloud data

    Customized Point Cloud (CustomPoint) format in the above customized data package :

uint32 offset_time      # offset time relative to the base time
float32 x               # X axis, unit:m
float32 y               # Y axis, unit:m
float32 z               # Z axis, unit:m
uint8 reflectivity      # reflectivity, 0~255
uint8 tag               # livox tag
uint8 line              # laser number in lidar

3. The standard pointcloud2 (pcl :: PointXYZI) format in the PCL library (Not currently supported) :

    Please refer to the pcl :: PointXYZI data structure in the point_types.hpp file of the PCL library.

RealSense D435i

SDK

https://github.com/realsenseai/librealsense

follow the instructions and setup the SDK then test with realsense-viewer only if the viewer works, the setup is right or else something is missing.

installation

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

sudo apt install curl # if you haven't already installed curl
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -

ROS compatibility

https://github.com/realsenseai/realsense-ros

• Configure your Ubuntu repositories
• Install all realsense ROS packages by sudo apt install ros-<ROS_DISTRO>-librealsense2*
  • For example, for Humble distro: sudo apt install ros-humble-librealsense2*

Start the camera node

with ros2 run:

ros2 run realsense2_camera realsense2_camera_node
# or, with parameters, for example - temporal and spatial filters are enabled:
ros2 run realsense2_camera realsense2_camera_node --ros-args -p enable_color:=false -p spatial_filter.enable:=true -p temporal_filter.enable:=true

with ros2 launch:

ros2 launch realsense2_camera rs_launch.py
ros2 launch realsense2_camera rs_launch.py depth_module.depth_profile:=1280x720x30 pointcloud.enable:=true

Chassis

Steering

Arduino

This code uses Ibus, Thus it will only work with Arduino MEGA series

#include <IBusBM.h>
#include <Servo.h>

// --- RC setup ---
IBusBM ibus;

// --- Motor Pins (BTS7960) ---
const int RPWM = 6;
const int LPWM = 5;
const int R_EN = 4;
const int L_EN = 3;

// --- Servo Pins ---
const int SERVO1_PIN = 8;
const int SERVO2_PIN = 9;
Servo servo1;
Servo servo2;

const int SERVO_DEADBAND = 50;
int lastServo1 = 1500;
int lastServo2 = 1500;

// --- Heartbeat Watchdog ---
unsigned long lastCommandTime = 0;
const unsigned long timeoutMs = 1000;   // stop motors if no command for 1s

// --- Mode control ---
bool rosMode = false; // false = RC mode, true = ROS mode

void setup() {
  Serial.begin(115200);    // ROS serial
  Serial1.begin(115200);   // IBUS serial (use Serial1 on Arduino Mega / hardware UART)
  ibus.begin(Serial1);

  pinMode(RPWM, OUTPUT);
  pinMode(LPWM, OUTPUT);
  pinMode(R_EN, OUTPUT);
  pinMode(L_EN, OUTPUT);

  digitalWrite(R_EN, HIGH);
  digitalWrite(L_EN, HIGH);

  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);

  stopMotor();
  lastCommandTime = millis();
  Serial.println("BTS7960 + Servo + RC/ROS Switch Ready");
}

void loop() {
  ibus.loop();

  // --- Check CH5 for mode switch ---
  int ch5 = ibus.readChannel(4); // CH5 index=4
  rosMode = (ch5 > 1500);        // >1500 µs = ROS mode, else RC mode

  if (rosMode) {
    handleROS();
  } else {
    handleRC();
  }

  // --- Heartbeat Timeout Check (only for ROS mode) ---
  if (rosMode && (millis() - lastCommandTime > timeoutMs)) {
    stopMotor();
    Serial.println("Motor Stopped (watchdog timeout)");
    lastCommandTime = millis();  // reset to avoid spamming
  }
}

// ================= ROS MODE =================
void handleROS() {
  if (Serial.available() > 0) {
    String command = Serial.readStringUntil('\n');
    command.trim();

    if (command.startsWith("F")) {
      int speed = command.substring(1).toInt();
      forwardMotor(speed);
      Serial.print("Forward, Speed: ");
      Serial.println(speed);
    }
    else if (command.startsWith("B")) {
      int speed = command.substring(1).toInt();
      backwardMotor(speed);
      Serial.print("Backward, Speed: ");
      Serial.println(speed);
    }
    else if (command == "STOP") {
      stopMotor();
      Serial.println("Motor Stopped (STOP cmd)");
    }
    else if (command == "HEARTBEAT") {
      Serial.println("Heartbeat OK");
    }
    else if (command.startsWith("SERVO1")) {
      int angle = command.substring(6).toInt();
      angle = constrain(angle, 0, 180);
      servo1.write(angle);
      Serial.print("Servo1 angle: ");
      Serial.println(angle);
    }
    else if (command.startsWith("SERVO2")) {
      int angle = command.substring(6).toInt();
      angle = constrain(angle, 0, 180);
      servo2.write(angle);
      Serial.print("Servo2 angle: ");
      Serial.println(angle);
    }

    lastCommandTime = millis();  // reset watchdog
  }
}

// ================= RC MODE =================
void handleRC() {
  // CH3 -> Power
  int powerRaw = ibus.readChannel(2);
  int power = map(powerRaw, 1000, 2000, 0, 255);

  // CH2 -> Direction
  int dirRaw = ibus.readChannel(1);
  int direction = map(dirRaw, 1000, 2000, -3, 3);

  int pwm = (power > 0) ? (power * direction / 3) : 0;

  if (pwm != 0) {
    driveMotor(pwm);
  } else {
    stopMotor();
  }

  updateServo1();
  updateServo2();
}

// ================= MOTOR CONTROL =================
void forwardMotor(int speed) {
  speed = constrain(speed, 0, 255);
  analogWrite(RPWM, 0);
  analogWrite(LPWM, speed);
}

void backwardMotor(int speed) {
  speed = constrain(speed, 0, 255);
  analogWrite(LPWM, 0);
  analogWrite(RPWM, speed);
}

void driveMotor(int pwm) {
  if (pwm > 0) {
    analogWrite(RPWM, pwm);
    analogWrite(LPWM, 0);
  } else if (pwm < 0) {
    analogWrite(RPWM, 0);
    analogWrite(LPWM, -pwm);
  } else {
    stopMotor();
  }
}

void stopMotor() {
  analogWrite(RPWM, 0);
  analogWrite(LPWM, 0);
}

// ================= SERVO CONTROL =================
void updateServo1() {
  int ch1Raw = ibus.readChannel(0); // CH1
  if (abs(ch1Raw - lastServo2) > SERVO_DEADBAND) {
    servo2.writeMicroseconds(ch1Raw);
    lastServo2 = ch1Raw;
  }
}

void updateServo2() {
  int ch4Raw = ibus.readChannel(3); // CH4
  if (abs(ch4Raw - lastServo1) > SERVO_DEADBAND) {
    servo1.writeMicroseconds(ch4Raw);
    lastServo1 = ch4Raw;
  }
}

Burn this code youshould know the methods for understanding refer circuitry

Motors

250 RPM High torque 12v brushed motors without encoder was used

Circuitry

🎮 Channel Mapping (FS-i6)

Drive-by-wire

Power Distribution

Arduino Motor Control

1. Overview

This project implements a hybrid control system for a DC motor and servos using:

• ROS 2 (Python node) as a communication interface
• Arduino Mega 2560 as the hardware controller
• BTS7960 motor driver for high-current motor control
• IBUS RC receiver for manual override

The system supports:

• RC Mode → Manual control via transmitter
• ROS Mode → Autonomous control via ROS 2 over serial

🔌 Hardware Requirements

• Arduino Mega 2560
• BTS7960 motor driver
• IBUS receiver
• 2x Servo motors
• External motor power supply

2. System Architecture

ROS 2 Node (/motor_command topic)
        ↓
   std_msgs/String
        ↓
Serial (USB, 115200 baud)
        ↓
     Arduino Mega
        ↓
  BTS7960 Motor Driver
        ↓
      DC Motor

        +
   Servo Outputs
        +
   IBUS RC Receiver (mode switching)

3. Installation & Setup

3.1 Install ROS 2 Package

Step 1: Create Workspace (if not already)

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

Step 2: Add Package

Clone your package:

git clone    

Step 3: Install Dependencies

cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y

Step 4: Build the Package

colcon build

Step 5: Source Workspace

source install/setup.bash

Step 6: Run Node

ros2 run arduino_motor motor_node

3.2 Install Python Serial Library

Your ROS node depends on pyserial. Install using:

pip install pyserial

Or (recommended for ROS environments):

pip3 install pyserial

Verify Installation

python3 -c "import serial; print(serial.__version__)"

3.3 Arduino Mega 2560 Setup

Install Arduino IDE

Install Required Libraries

In Arduino IDE:

1. Go to Sketch → Include Library → Manage Libraries
2. Install:
   • IBusBM
   • Servo (usually pre-installed)

Upload (Burn) Code to Arduino Mega

Step 1: Connect Arduino

• Plug Arduino via USB
• Identify port:

ls /dev/ttyUSB*

Step 2: Select Board

In Arduino IDE:

• Tools → Board → Arduino Mega or Mega 2560
• Tools → Processor → ATmega2560
• Tools → Port → /dev/ttyUSB0 (or your port)

Step 3: Upload Code

• Paste your Arduino code into IDE
• Click Upload

Step 4: Verify Serial Output

Open Serial Monitor:

• Baud rate: 115200

You should see:

BTS7960 + Servo + RC/ROS Switch Ready

3.4 Serial Permissions (Linux)

If you get permission errors:

sudo usermod -a -G dialout $USER

Then logout/login.

4. ROS Interface

Topic: /motor_command

• Type: std_msgs/msg/String
• Direction: ROS → Arduino

Behavior

The ROS node:

• Receives string
• Converts to uppercase
• Sends directly via serial

5. Serial Command Protocol

5.1 Motor Commands

Speed range: 0–255

5.2 Servo Commands

Angle range: 0–180

5.3 System Commands

6. Example ROS Commands

ros2 topic pub /motor_command std_msgs/msg/String "{data: 'F150'}"
ros2 topic pub /motor_command std_msgs/msg/String "{data: 'B200'}"
ros2 topic pub /motor_command std_msgs/msg/String "{data: 'STOP'}"
ros2 topic pub /motor_command std_msgs/msg/String "{data: 'SERVO190'}"
ros2 topic pub /motor_command std_msgs/msg/String "{data: 'SERVO245'}"

7. Arduino Behavior

7.1 Mode Switching (RC vs ROS)

• Controlled via IBUS Channel 5

7.2 ROS Mode

• Reads serial commands
• Executes motor/servo control
• Uses watchdog safety

7.3 RC Mode

• Direct IBUS control
• Motor + servo mapping

8. Watchdog Safety

• Timeout: 1000 ms
• Stops motor if no ROS command received

9. Hardware Mapping

Motor Driver (BTS7960)

Servos

Serial

10. Design Notes

Advantages

• Simple architecture
• Fast communication
• Easy debugging
• Reliable hybrid control

Limitations

• No structured ROS messages
• No feedback to ROS
• Requires strict command formatting

11. Summary

This system combines:

• ROS 2 (high-level interface)
• Arduino (low-level control)
• RC fallback (manual safety)

The ROS node acts as a transparent communication bridge, while the Arduino performs all control logic.

12. Usage Workflow

1. Upload Arduino code
2. Connect hardware
3. Run ROS node
4. Switch to ROS mode via RC
5. Publish commands

13. Notes

• Ensure correct serial port (/dev/ttyUSB0)
• Always send valid commands
• System is fully tested and operational

Decision Package

1. Overview

The decision_package implements a ROS 2 decision-making node that processes obstacle data and determines robot behavior.

It acts as an intermediate intelligence layer between perception and actuation.

Purpose

• Interpret obstacle detection results
• Classify risk levels
• Generate appropriate motion commands
• Forward commands to the motor control system

2. System Architecture

/obstacle_clusters
        ↓
   decision_node
        ↓
/motor_command
        ↓
   motor_node
        ↓
    Arduino
        ↓
   Motor + Servos

3. Package Description

Package Name

decision_package

Node Name

decision_node

Language

Python (rclpy)

4. ROS Interfaces

4.1 Subscribed Topic

4.2 Published Topic

5. Input Data Format

The node expects obstacle classification as a string containing keywords.

Supported Keywords

Example Inputs

"SAFE"
"MODERATE obstacle detected"
"CRITICAL"

6. Decision Logic

The node evaluates incoming data and maps it to motor commands.

Logic

if "CRITICAL" in data:
    command = "STOP"
elif "MODERATE" in data:
    command = "F80"
else:
    command = "F150"

Behavior Mapping

7. Output Command Protocol

Commands are compatible with the Arduino motor protocol.

Motor Commands

F<speed>   → Forward
B<speed>   → Backward
STOP       → Stop

Examples

F150
F80
STOP

8. Execution Flow

1. Receive obstacle data
2. Convert to uppercase
3. Match keyword
4. Generate motor command
5. Publish to /motor_command
6. Motor node forwards to Arduino

9. Integration with Motor Package

This package does NOT directly communicate with hardware.

Instead:

• It publishes commands
• arduino_motor package handles serial communication

Correct Integration

decision_node → /motor_command → motor_node → Arduino

10. Installation & Setup

10.1 Add to Workspace

cd ~/ros2_ws/src
cp -r decision_package .

10.2 Install Dependencies

cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y

10.3 Build

colcon build

10.4 Source Workspace

source install/setup.bash

10.5 Run Node

ros2 run decision_package decision_node

11. Testing

Publish Test Data

ros2 topic pub /obstacle_clusters std_msgs/msg/String "{data: 'SAFE'}"
ros2 topic pub /obstacle_clusters std_msgs/msg/String "{data: 'MODERATE'}"
ros2 topic pub /obstacle_clusters std_msgs/msg/String "{data: 'CRITICAL'}"

Monitor Output

ros2 topic echo /motor_command

12. Design Considerations

Advantages

• Clear separation of perception and control
• Modular ROS architecture
• Easy to extend decision logic
• Compatible with existing Arduino protocol

Limitations

• Uses string-based messages (not structured)
• Keyword matching is fragile
• No probabilistic decision-making
• No feedback loop

13. Future Improvements

• Replace strings with custom ROS messages
• Add velocity-based control (/cmd_vel)
• Introduce state machine or behavior tree
• Integrate sensor fusion
• Add feedback from motor system

14. Summary

The decision_package provides a lightweight decision-making layer that:

• Interprets obstacle data
• Converts it into motor commands
• Interfaces cleanly with the motor control package

It forms a critical part of a ROS-based autonomous control pipeline, enabling reactive behavior based on environmental perception.

FAST-LIO

1. Background and Problem Statement

Robotic systems (autonomous vehicles, UAVs, handheld mapping devices, mobile robots) require accurate real-time localization and mapping in environments where GPS may not be available.

2. Goal of FAST-LIO2

The authors propose FAST-LIO2, a LiDAR-IMU fusion framework that aims to:

• Achieve high-speed real-time odometry
• Work with different LiDAR types
• Avoid feature extraction
• Provide accurate mapping
• Run on resource-constrained hardware

The system can achieve >100 Hz odometry update rates and works even with high angular velocity motions (≈1000°/s).

3. Key Contributions of the Paper

FAST-LIO2 introduces two major innovations.

3.1 Direct LiDAR Scan-to-Map Registration

Most LIO systems: LiDAR scan → feature extraction → match features → pose update

FAST-LIO2 instead performs:

LiDAR raw points → scan-to-map matching → pose update

This removes the need for manually engineered features.

Advantages:

• Works with any LiDAR scanning pattern
• Uses all geometric information
• Reduces preprocessing computation

This approach improves accuracy and robustness because subtle geometric features can still influence the optimization.

3.2 Incremental KD-Tree Map (ikd-Tree)

FAST-LIO2 introduces a new dynamic spatial data structure called:

ikd-Tree (Incremental KD-Tree).

Capabilities:

• Efficient nearest-neighbor search
• Incremental point insertion
• Dynamic deletion
• Automatic rebalancing
• Built-in voxel downsampling

Compared with structures like:

• Octrees
• R-trees
• static KD-trees

ikd-Tree provides better real-time performance for large maps.

4. System Architecture

FAST-LIO2 consists of three main components:

IMU Preintegration
↓
State Estimator (Iterated EKF)
↓
LiDAR Scan-to-Map Matching
↓
Map Update (ikd-tree)

Pipeline:

1. IMU propagation
2. Motion compensation
3. Point-to-map registration
4. EKF update
5. Map update

5. Mathematical Formulation

FAST-LIO2 uses an Iterated Extended Kalman Filter (IEKF).

State vector:

x = [R, p, v, bg, ba]

Where:

• R → rotation
• p → position
• v → velocity
• bg → gyroscope bias
• ba → accelerometer bias

5.1 IMU Propagation

The IMU predicts motion using:

Rotation: R_k = R_{k-1} * Exp((ω - bg)Δt)

Velocity: v_k = v_{k-1} + (R(a - ba) + g)Δt

Position: p_k = p_{k-1} + vΔt + ½(R(a - ba) + g)Δt²

This provides a high-frequency motion estimate.

5.2 Motion Compensation (Deskewing)

Because LiDAR scans take time (~100 ms), points correspond to different poses.

FAST-LIO2 uses IMU interpolation to transform each LiDAR point to a common reference frame.

This process is called:

scan deskewing

5.3 Scan-to-Map Registration

Instead of feature matching, FAST-LIO2 performs:

direct point-to-plane optimization

For each point:

1. Find nearest neighbors in map
2. Fit local plane
3. Minimize point-to-plane distance

Error function:

e = nᵀ (R p + t − p_map)

Where:

• n = plane normal
• p = LiDAR point
• p_map = map point

This becomes the measurement update in the Kalman filter.

5.4 Iterated Kalman Update

FAST-LIO2 performs iterative EKF updates to improve convergence.

Process:

prediction → measurement update → iterate → final state

Benefits:

• Higher accuracy
• Robust against nonlinearities

6. Map Management with ikd-Tree

Traditional SLAM systems store maps using:

• voxel grids
• octrees

FAST-LIO2 instead uses ikd-Tree.

Advantages:

Incremental update

Points inserted directly into tree.

Efficient nearest neighbor search

Used for scan-to-map matching.

Dynamic map maintenance

Points can be removed when outside local map region.

Built-in downsampling

Reduces map size automatically.

This dramatically reduces computation load.

7. Performance Evaluation

The paper evaluates FAST-LIO2 on:

• 19 benchmark sequences
• Multiple LiDAR types
• Indoor and outdoor environments

Datasets include:

• Livox datasets
• UAV datasets
• handheld mapping

Results show:

• Lower trajectory error than LIO-SAM and LINS
• Real-time mapping at 100 Hz
• Stable operation on ARM processors

Example Performance

Handheld mapping:

• Speed: 7 m/s
• Drift: < 6 cm

UAV experiment:

• aggressive motion
• accurate dense mapping

8. Advantages of FAST-LIO2

1. Feature-free

Works with any LiDAR.

2. High speed

100 Hz update rate.

3. High robustness

Handles fast rotations (~1000°/s).

4. Sensor flexibility

Supports:

• Velodyne
• Ouster
• Livox
• solid-state LiDAR

5. Embedded support

Runs on ARM boards like:

• Raspberry Pi
• Jetson

9. Limitations

Despite its strengths, FAST-LIO2 has some limitations.

No loop closure

It performs odometry and local mapping, not full SLAM. Long-term drift can occur.

Sensitive to IMU calibration

Accurate LiDAR-IMU extrinsics are required.

Degenerate environments

Feature-poor environments (e.g., tunnels, corridors) can reduce accuracy.

10. Applications

FAST-LIO2 is widely used for:

Autonomous robots

UGVs and mobile robots.

UAV navigation

High-speed drone mapping.

Handheld 3D scanning

Underground exploration

Autonomous vehicles

11. Why FAST-LIO2 Became Popular

FAST-LIO2 is widely adopted because it:

1. Eliminates fragile feature extraction
2. Uses efficient Kalman filtering
3. Maintains a fast map structure
4. Works with modern LiDAR types (especially Livox)

This combination made it one of the most practical LIO systems in robotics.

12. Comparison with Other SLAM Systems

13. Setup

Clone the repository and colcon build:

cd <ros2_ws>/src # cd into a ros2 workspace folder
git clone https://github.com/Ericsii/FAST_LIO.git --recursive
cd ..
rosdep install --from-paths src --ignore-src -y
colcon build --symlink-install

• Remember to source the livox_ros_driver before build (follow 1.3 livox_ros_driver)
• If you want to use a custom build of PCL, add the following line to ~/.bashrc export PCL_ROOT={CUSTOM_PCL_PATH}

14. Run

Launch livox ros driver. Use MID360 as an example.

source ~/ws_livox/install/local_setup.bash 
source ~/fastlio/install/local_setup.bash

ros2 launch fast_lio mapping.launch.py config_file:=mid360.yaml & ros2 launch livox_ros_driver2 msg_MID360_launch.py

to save maps:

source ~/fastlio/install/local_setup.bash
ros2 service call /map_save std_srvs/srv/Trigger {}

Here’s a clean, technical README summary you can directly use (or slightly tweak) for your ROS 2 package:

🧭 Obstacle Detector (ROS 2)

Overview

obstacle_detector is a lightweight ROS 2 package for real-time obstacle detection and classification using 2D LiDAR data (sensor_msgs/LaserScan). It processes incoming scan data, filters noise, groups nearby points into clusters, and categorizes obstacles into safety zones based on distance.

The node publishes both visualization markers for RViz and a high-level obstacle state for downstream decision-making (e.g., navigation, control).

✨ Features

• 📡 Subscribes to LiDAR scan data (/scan)
• 🧹 Applies moving-average filtering to reduce noise
• 📍 Converts polar scan data → Cartesian coordinates
• 🧠 Classifies obstacles into:
  • CRITICAL
  • MODERATE
  • FRIENDLY
  • CLEAR
• 🔗 Performs simple spatial clustering of obstacle points
• 🎨 Publishes clustered obstacles as RViz markers
• 📢 Publishes system state for behavior/control modules

🏗️ Node Architecture

Node: obstacle_detector

Subscriptions

Publications

⚙️ Parameters

Processing Pipeline

1. Noise Filtering

A sliding window (moving average) is applied to smooth LiDAR range data:

• Window size: ±2 samples
• Ignores NaN and inf values

2. Coordinate Transformation

Each valid scan point is converted:

• Polar → Cartesian

x = r * cos(θ)
y = r * sin(θ)

3. Distance-Based Classification

Points are categorized into zones:

• Critical: r ≤ critical_radius
• Moderate: critical_radius < r ≤ moderate_radius
• Friendly: moderate_radius < r ≤ friendly_radius

4. Clustering

A simple proximity-based clustering algorithm:

• Threshold: 0.4 m
• Groups nearby points into obstacle clusters
• Computes:
  • Cluster centroid
  • Distance and angle
  • Cluster size

5. Visualization

• Uses Marker::SPHERE_LIST
• Each cluster gets a unique color
• Published to /obstacle_clusters for RViz

6. Mode Estimation

The system publishes a global state:

Priority-based logic:

CRITICAL > MODERATE > FRIENDLY > CLEAR

Example:

• Any critical point → CRITICAL
• Else if moderate → MODERATE
• Else if friendly → FRIENDLY
• Else → CLEAR

Launch

ros2 launch obstacle_detector detector_launch.py

Dependencies

• rclcpp
• sensor_msgs
• visualization_msgs
• std_msgs

Example Use Cases

• Autonomous rover safety monitoring
• Reactive obstacle avoidance
• Navigation stack augmentation
• Visualization/debugging of LiDAR perception

Limitations

• Uses a naive clustering algorithm (not DBSCAN or Euclidean clustering)
• No temporal tracking of obstacles
• No dynamic obstacle prediction
• Assumes relatively clean LiDAR input

Future Improvements

• Replace clustering with DBSCAN / PCL
• Add obstacle tracking over time
• Integrate velocity estimation
• Fuse with camera/depth sensors
• Adaptive thresholds based on speed

Obstacle Detector Norm

Overview

obstacle_detector_norm is an advanced ROS 2 perception package for normalized, noise-robust obstacle detection from 2D LiDAR data. It extends conventional scan-based detection pipelines by introducing signal normalization, adaptive filtering, and structured spatial reasoning, enabling improved robustness under noisy or inconsistent sensor conditions.

The package is designed for real-time robotic systems, providing both low-level geometric representations and high-level semantic safety states.

Core Design Philosophy

Unlike naive obstacle detectors, this package emphasizes:

• Signal normalization before interpretation
• Distance-invariant feature extraction
• Robustness to sensor noise and dropouts
• Deterministic, low-latency processing (no heavy ML dependencies)

Key Features

• Real-time LaserScan processing
• Multi-stage filtering (temporal + spatial)
• Range normalization for consistent clustering behavior
• Adaptive clustering based on normalized distance metrics
• Geometric feature extraction (centroids, spread, density)
• Multi-zone obstacle classification
• RViz visualization with structured cluster encoding
• State abstraction for navigation/control layers

System Architecture

Node: obstacle_detector_norm

A single high-performance node implementing a full perception pipeline:

LaserScan → Filtering → Normalization → Projection → Clustering → Classification → Publishing

Processing Pipeline (Deep Dive)

1. Signal Conditioning

1.1 Temporal Filtering

Applies a sliding window over scan frames:

• Reduces jitter between consecutive scans
• Handles transient noise spikes

1.2 Spatial Smoothing

Local neighborhood averaging:

• Windowed smoothing across adjacent beams
• Rejects:
  • NaN
  • inf
  • Out-of-range values

2. Range Normalization

A key differentiator of this package.

Motivation:

Raw LiDAR data exhibits range-dependent sparsity:

• Close objects → dense points
• Far objects → sparse points

Approach:

Each range value r is normalized:

r_norm = r / r_max

or optionally:

r_norm = (r - r_min) / (r_max - r_min)

Effects:

• Equalizes clustering sensitivity across distances
• Prevents bias toward near-field obstacles
• Stabilizes threshold-based logic

3. Coordinate Projection

Standard polar → Cartesian transformation:

x = r * cos(θ)
y = r * sin(θ)

Optionally includes:

• Sensor frame offsets
• Robot base frame alignment

4. Adaptive Clustering

Key Improvement:

Clustering threshold is scaled using normalized distance:

d_threshold = α * (1 + r_norm)

Where:

• α = base clustering constant
• r_norm = normalized range

Benefits:

• Tight clustering for near objects
• Relaxed clustering for far objects
• Reduces over-segmentation in sparse regions

Algorithm:

• Sequential scan grouping (O(n))
• Euclidean proximity check
• Cluster merging (optional optimization)

5. Feature Extraction

For each cluster:

• Centroid
• Point count (density proxy)
• Bounding radius
• Angular span
• Distance from robot

Derived metrics:

• Compactness
• Spatial variance

6. Obstacle Classification

Multi-zone classification using normalized + absolute metrics:

Hybrid logic:

• Distance thresholds (absolute)
• Density thresholds (relative)
• Cluster geometry

7. Mode Aggregation

Global system state is determined via priority reduction:

CRITICAL > MODERATE > FRIENDLY > CLEAR

This ensures fail-safe behavior for navigation stacks.

Interfaces

Subscriptions

Publications

Parameters

Geometry

Filtering

Normalization

Clustering

Classification

Launch

ros2 launch obstacle_detector_norm detector_norm.launch.py

Performance Characteristics

Design Trade-offs

Advantages

• Deterministic (no ML inference latency)
• Robust to noise and sparsity
• Adaptive to varying environments
• Lightweight and embedded-friendly

Limitations

• No temporal tracking (stateless clustering)
• No semantic classification (object type)
• Assumes planar LiDAR
• Limited performance in highly dynamic scenes

Future Work

• Temporal tracking (Kalman / JPDAF)
• Velocity estimation from scan differencing
• 3D LiDAR support
• Sensor fusion (camera + depth)
• Learned clustering (hybrid ML + geometry)
• Integration with Nav2 costmaps

Integration Notes

This package is ideal for:

• Pre-processing layer before Nav2
• Safety watchdog nodes
• Behavior trees (BT Navigator)
• Edge robotics platforms (Jetson, Raspberry Pi)

Dependencies

• rclcpp
• sensor_msgs
• visualization_msgs
• std_msgs

Occupancy Grid Mapper

System Overview

This package implements a custom 2D occupancy grid mapping pipeline in ROS2 using:

• Sensor fusion inputs:

  • sensor_msgs/LaserScan → spatial observations
  • nav_msgs/Odometry → robot pose
  • sensor_msgs/Imu → motion filtering

• Core output:

  • nav_msgs/OccupancyGrid published on /map

• Auxiliary capability:

  • Periodic map persistence via:
    • Internal PGM writer (C++)
    • External Nav2 SaveMap service (Python node)

Core Architecture

Node: OccupancyGridMapper (C++)

Subscriptions

• /scan → main mapping input
• /odom → pose tracking
• /livox/imu → motion quality filtering

Publisher

• /map → real-time occupancy grid

Internal State

std::vector<int8_t> grid_;     // occupancy values: {-1, 0, 100}
std::vector<int> last_seen_;   // temporal persistence tracking
geometry_msgs::msg::Pose robot_pose_;

Grid Configuration

• Resolution: 0.1 m/cell
• Initial size: 200 × 200
• Origin: centered around robot using offset

Mapping Pipeline

1. Coordinate Transformation

Laser scan points are transformed:

$[ (x_{world}, y_{world}) = T_{robot} \cdot (r\cos\theta, r\sin\theta) ]$

Where:

• T_robot derived from odometry pose + yaw (via tf2::getYaw)
• No TF tree used → direct pose-based transform

2. Ray Casting (Free Space Inference)

Algorithm: Bresenham Line Tracing

Function:

std::vector<int> traceRay(int x0, int y0, int x1, int y1)

• Computes all grid cells between robot and endpoint
• Marks them as free (0)

Key behavior:

if (grid_[idx] != 100) grid_[idx] = 0;

Occupied cells are not overwritten by free space immediately

3. Occupancy Update

For each valid laser return:

• Ray path → free cells
• Endpoint → occupied

grid_[idx] = 100;
last_seen_[idx] = frame_count_;

4. Temporal Decay Model

Purpose:

Handle dynamic environments / stale obstacles

Mechanism:

if ((frame_count_ - last_seen_[idx]) > decay_threshold_) {
    grid_[idx] = 0;
}

• decay_threshold_ = 30 frames
• Implements time-based occupancy decay
• Prevents ghost obstacles

5. Frame Skipping via IMU (Motion Filtering)

Problem Addressed:

• Motion distortion in LiDAR scans
• Mapping errors during rapid rotation

IMU Filtering Strategy

Step 1: Low-pass filtering

$[ \omega_{filtered} = \alpha \cdot \omega_{raw} + (1-\alpha)\cdot \omega_{prev} ]$

• α = 0.01 → strong smoothing

Step 2: Angular displacement estimation

$[ \Delta \theta = |\omega| \cdot dt ]$

• dt = 1/200 (Livox IMU rate)

Step 3: Adaptive spike detection

Conditions:

delta_roll > threshold || delta_pitch > threshold || delta_yaw > threshold

Step 4: Dynamic hysteresis model

Key innovation:

dynamic_threshold ∈ [0.1s, 2.0s]

• Increases during motion spikes
• Decreases during calm periods

Behavior:

This avoids over-filtering during continuous motion

Dynamic Map Resizing

Function: ensureMapContains()

Trigger:

• When robot or scan endpoints approach boundary

Strategy:

• Expand grid by +40 cells
• Re-center around robot

Data Migration:

new_grid[new_idx] = old_grid[old_idx]

Maintains spatial consistency

Optimization Constraint

if (dx > 20 || dy > 20) return;

• Only expands when near robot
• Avoids unbounded growth

Map Representation

Encoding

Publishing

• Frame: "map"
• Origin:
  $(-\frac{width \cdot resolution}{2}, -\frac{height \cdot resolution}{2})$

Robot stays approximately centered

Map Persistence

Method 1: Internal (C++)

• Writes PGM image
• Trigger: every 10 seconds
• Limitation:
  • No YAML metadata (resolution/origin missing)

Method 2: External (Python Node)

Node: AutoMapSaver

• Calls Nav2 service:

    /map_saver/save_map

• Generates timestamped map:

    ~/map_YYYYMMDD_HHMMSS

Produces:

• .pgm + .yaml (Nav2 compatible)

Design Strengths

Robustness

• IMU-based motion-aware filtering
• Temporal decay for dynamic environments

Efficiency

• Lightweight grid (no probabilistic log-odds)
• Bresenham ray tracing (fast, integer-based)

Adaptability

• Dynamic grid resizing
• Hysteresis-based filtering

Simplicity

• No TF dependency
• Minimal external packages

Limitations / Trade-offs

No probabilistic model

• Uses binary occupancy

• Lacks Bayesian update:

  $p(m|z) \neq \text{modeled}$

No sensor noise modeling

• No inverse sensor model
• All hits treated equally

Pose accuracy dependency

• Relies entirely on /odom
• No SLAM loop closure

No multi-layer costmaps

• Not Nav2 costmap-compatible directly

Memory growth

• Grid expands but never shrinks

Notable Innovations

1. Dynamic IMU Hysteresis Filter

• Rare in basic mappers
• Prevents both:
  • motion blur
  • over-filtering

2. Time-decay occupancy

• Lightweight alternative to probabilistic aging

3. Selective map expansion

• Balances:
  • coverage
  • computational cost

Comparison to Standard Approaches

ORB-SLAM

ORB (Oriented FAST and Rotated BRIEF)

As an OpenCV enthusiast, the most important thing about the ORB is that it came from “OpenCV Labs”. This algorithm was brought up by Ethan Rublee, Vincent Rabaud, Kurt Konolige and Gary R. Bradski in their paper ORB: An efficient alternative to SIFT or SURF in 2011. As the title says, it is a good alternative to SIFT and SURF in computation cost, matching performance and mainly the patents. Yes, SIFT and SURF are patented and you are supposed to pay them for its use. But ORB is not !!!

ORB is basically a fusion of FAST keypoint detector and BRIEF descriptor with many modifications to enhance the performance. First it use FAST to find keypoints, then apply Harris corner measure to find top N points among them. It also use pyramid to produce multiscale-features. But one problem is that, FAST doesn’t compute the orientation. So what about rotation invariance? Authors came up with following modification.

It computes the intensity weighted centroid of the patch with located corner at center. The direction of the vector from this corner point to centroid gives the orientation. To improve the rotation invariance, moments are computed with x and y which should be in a circular region of radius , where  is the size of the patch.

Now for descriptors, ORB use BRIEF descriptors. But we have already seen that BRIEF performs poorly with rotation. So what ORB does is to “steer” BRIEF according to the orientation of keypoints. For any feature set of n binary tests at location (x,y), define a (2Xn) matrix, S which contains the coordinates of these pixels. Then using the orientation of patch, 𝜃, its rotation matrix is found and rotates the S to get steered(rotated) version S𝜃.

ORB discretize the angle to increments of 2π/30(12 degrees), and construct a lookup table of precomputed BRIEF patterns. As long as the keypoint orientation S𝜃 is consistent across views, the correct set of points  will be used to compute its descriptor.

BRIEF has an important property that each bit feature has a large variance and a mean near 0.5. But once it is oriented along keypoint direction, it loses this property and become more distributed. High variance makes a feature more discriminative, since it responds differentially to inputs. Another desirable property is to have the tests uncorrelated, since then each test will contribute to the result. To resolve all these, ORB runs a greedy search among all possible binary tests to find the ones that have both high variance and means close to 0.5, as well as being uncorrelated. The result is called rBRIEF.

For descriptor matching, multi-probe LSH which improves on the traditional LSH, is used. The paper says ORB is much faster than SURF and SIFT and ORB descriptor works better than SURF. ORB is a good choice in low-power devices for panorama stitching etc.

ORB in OpenCV

As usual, we have to create an ORB object with the function, cv.ORB() or using feature2d common interface. It has a number of optional parameters. Most useful ones are nFeatures which denotes maximum number of features to be retained (by default 500), scoreType which denotes whether Harris score or FAST score to rank the features (by default, Harris score) etc. Another parameter, WTA_K decides number of points that produce each element of the oriented BRIEF descriptor. By default it is two, ie selects two points at a time. In that case, for matching, NORM_HAMMING distance is used. If WTA_K is 3 or 4, which takes 3 or 4 points to produce BRIEF descriptor, then matching distance is defined by NORM_HAMMING2.

Below is a simple code which shows the use of ORB.

import numpy as np
import cv2 as cv
from matplotlib import pyplot as plt

img = cv.imread('simple.jpg', cv.IMREAD_GRAYSCALE)

# Initiate ORB detector
orb = cv.ORB_create()

# find the keypoints with ORB
kp = orb.detect(img,None)

# compute the descriptors with ORB
kp, des = orb.compute(img, kp)

# draw only keypoints location,not size and orientation
img2 = cv.drawKeypoints(img, kp, None, color=(0,255,0), flags=0)
plt.imshow(img2), plt.show()

Real Time pose estimation of a textured object

Nowadays, augmented reality is one of the top research topic in computer vision and robotics fields. The most elemental problem in augmented reality is the estimation of the camera pose respect of an object in the case of computer vision area to do later some 3D rendering or in the case of robotics obtain an object pose in order to grasp it and do some manipulation. However, this is not a trivial problem to solve due to the fact that the most common issue in image processing is the computational cost of applying a lot of algorithms or mathematical operations for solving a problem which is basic and immediately for humans.

https://docs.opencv.org/3.4/dc/d2c/tutorial_real_time_pose.html –> Pose estimation example

https://docs.opencv.org/3.4/d6/d55/tutorial_table_of_content_calib3d.html –> Other related parts

ORB-Slam

Original work: https://github.com/UZ-SLAMLab/ORB_SLAM3 ORB-SLAM3 is the first real-time SLAM library able to perform Visual, Visual-Inertial and Multi-Map SLAM with monocular, stereo and RGB-D cameras, using pin-hole and fisheye lens models. In all sensor configurations, ORB-SLAM3 is as robust as the best systems available in the literature, and significantly more accurate.

Key Innovations

ORB-SLAM3 introduces several major advancements over ORB-SLAM2:

1 Visual-Inertial Integration

• Uses a tightly-coupled visual–inertial formulation based on Maximum-a-Posteriori (MAP) estimation
• Enables:
  • Metric scale recovery (even for monocular setups)
  • Improved robustness under motion blur or low texture

Multi-Map (Atlas) System

• Introduces an Atlas framework: a collection of multiple maps
• Allows:
  • Creation of new maps when tracking fails
  • Map merging when revisiting known areas
• Enhances long-term operation and relocalization

Improved Place Recognition

• Uses bag-of-words (DBoW) place recognition
• Supports:
  • Loop closure detection
  • Cross-map relocalization
  • Map reuse across sessions

System Architecture**

ORB-SLAM3 follows a multi-threaded architecture with three main parallel modules:

1 Tracking Thread

• Extracts ORB (Oriented FAST + Rotated BRIEF) features
• Estimates current camera pose
• Handles:
  • Frame-to-frame tracking
  • Keyframe insertion
  • Relocalization if tracking is lost

2 Local Mapping Thread

• Maintains and optimizes the local map

• Tasks include:

  • Keyframe insertion and culling
  • Map point creation
  • Local bundle adjustment (BA)

• In inertial mode:

  • Estimates IMU biases, velocity, and gravity

Loop Closing & Map Merging Thread

• Detects loops using place recognition
• Performs:
  • Loop closure optimization
  • Global bundle adjustment (GBA)
• Enables merging of multiple maps in the Atlas

Optimization Framework

ORB-SLAM3 formulates SLAM as a nonlinear optimization problem:

• Minimizes a joint cost function:
  • Visual reprojection error
  • Inertial measurement residuals
• Uses:
  • Bundle Adjustment (BA) for pose and map refinement
  • Sliding-window optimization for scalability

Feature-Based Approach

• Relies on ORB features for:
  • Keypoint detection
  • Descriptor matching
• Advantages:
  • Efficient and rotation-invariant
  • Facilitates loop closure and relocalization
• Limitations:
  • Sensitive to:
    • Motion blur
    • Low-texture environments

IMU Initialization

A key contribution is robust IMU initialization, which includes:

1. Visual-only initialization (structure estimation)
2. Inertial-only estimation (scale, gravity, biases)
3. Joint optimization for refinement

This enables accurate scale estimation and drift reduction

Note: This is highly dependent on the Library versions and might not work until and unless you are on the same version of the dependencies as the author. This majorly because the libraries have structured their files differently in each of their release versions. So it better not to manually use the the APT repo to install these dependencies.

ROS2 Integrations:

https://medium.com/@antonioconsiglio/integrating-orb-slam3-with-ros2-humble-on-raspberry-pi-5-a-step-by-step-guide-78e7b911c361 This is the only tutorial I have found which shows some actual promise but I havn’t paid premium and checked yet. Si yep not sure.

The only other decent one I have found is: https://github.com/Mechazo11/ros2_orb_slam3 But the problem with this second one is that ir work and takes input topic od /camera/image and gives output of /

Key Design Philosophy**

This repo is intentionally simple and stripped down:

• ❌ No RViz integration
• ❌ No TF tree publishing
• ❌ No launch files
• ✅ Focus on core SLAM execution only

💡 Why this matters:
Most ROS packages are heavy; this one is meant for learning + customization.

Package Structure

Typical layout:

ros2_orb_slam3/  
├── src/                 # ROS2 nodes  
├── include/             # headers  
├── orb_slam3/           # core SLAM library  
├── scripts/             # helper scripts  
├── TEST_DATASET/        # sample EuRoC data  
├── package.xml  
├── CMakeLists.txt

👉 Key takeaway:
It bundles both SLAM core + ROS2 interface in one repo.

Features Supported

From what’s implemented:

• Monocular SLAM (primary focus)
• Dataset-based testing (EuRoC sample included)
• Real-time pose estimation

⚠️ Missing / minimal:

• IMU integration (not plug-and-play)
• Visualization tools
• Full ROS2 ecosystem integration

Primary Inputs

• /camera/image_raw → sensor_msgs/Image
• /camera/camera_info → sensor_msgs/CameraInfo

Camera Pose / Odometry

• /orb_slam3/camera_pose OR /odom
  → geometry_msgs/PoseStamped or nav_msgs/Odometry

PCD Publisher

PCD to LaserScan

https://github.com/ros-perception/pointcloud_to_laserscan/tree/humble

this package helps in converting the live PCl data coming from the 3D Li-DaR to a LaserScan format. Basically it converts the the yopic format.

It compresses the data from 3D to 2d using 2 different methods:

1. crops the data.
2. projects the leftover data to XY plane

Key Idea

Many robots use 2D LiDAR-based algorithms (like SLAM and localization).
However, modern sensors often produce 3D point clouds.

This package allows robots to:

• Use 3D LiDAR / depth cameras
• But still run 2D laser-based navigation algorithms

1. Main Functional Components

The package contains two ROS2 nodes:

1. PointCloudToLaserScanNode

2. LaserScanToPointCloudNode

Each node converts data between two ROS message types.

2. PointCloudToLaserScanNode

Purpose

This node converts:

sensor_msgs/PointCloud2 → sensor_msgs/LaserScan It projects a 3D point cloud onto a 2D plane to simulate a laser scan.

Example Use Case

A robot has a 3D LiDAR, but the navigation stack requires a 2D laser scan.

Workflow:

3D LiDAR
↓
PointCloud2
↓
pointcloud_to_laserscan node
↓
LaserScan
↓
SLAM / Navigation

This allows compatibility with:

• AMCL localization
• 2D SLAM
• Navigation2 stack

Subscribed Topics

The node subscribes to: cloud_in (sensor_msgs/msg/PointCloud2)

This topic contains the 3D point cloud input.

Important behavior:

• The node processes data only when someone subscribes to the output scan topic.

This avoids unnecessary computation.

Published Topics

The node publishes: scan (sensor_msgs/msg/LaserScan)

3. Conversion Algorithm (Conceptual)

The conversion follows these steps:

Step 1 — Receive Point Cloud

The node receives a 3D point cloud containing many points:

(x, y, z)

Step 2 — Height Filtering

Points outside a specific vertical range are removed.

Parameters:

min_height
max_height

This isolates a horizontal slice of the environment.

Step 3 — Angle Calculation

Each point is converted into polar coordinates:

angle = atan2(y, x)
distance = sqrt(x² + y²)

Step 4 — Angular Binning

The scan space is divided into angular bins.

Example:

angle_min = -π
angle_max = π
angle_increment = 1°

Each bin represents one laser beam.

Step 5 — Closest Point Selection

For each angle bin:

• The closest point is chosen.
• That distance becomes the laser range.

This mimics how a real LiDAR works.

Step 6 — Publish LaserScan

The node generates a LaserScan message containing:

• angle range
• range measurements
• scan timing
• frame information

4. Key Parameters of PointCloudToLaserScanNode

These parameters control the conversion behavior.

1. Height Filtering

min_height

Minimum height of points to consider.

Removes points below ground.

max_height

Maximum height of accepted points.

Removes ceiling points.

2. Angular Limits

angle_min

Minimum scan angle.

angle_max

Maximum scan angle.

angle_increment

Resolution of the scan.

3. Range Limits

range_min

Minimum measurable distance.

range_max

Maximum measurable distance.

4. Frame Transformation

target_frame

Transforms the point cloud into another coordinate frame before conversion.

transform_tolerance

Allowed delay when looking up transforms.

5. Output Behavior

use_inf

Controls how empty ranges are represented.

If enabled:

range = +∞

Otherwise:

range = range_max + 1

6. Queue Size

queue_size

Controls how many messages are buffered.

Default:

number of CPU cores

7. Scan Time

scan_time

Defines how long a full scan takes. Used only to populate the LaserScan message.

5. Repository Structure

The repository contains typical ROS2 package files.

pointcloud_to_laserscan/
│
├── include/
│ └── pointcloud_to_laserscan
│
├── src/
│ ├── pointcloud_to_laserscan_node.cpp
│ ├── laserscan_to_pointcloud_node.cpp
│
├── launch/
│ └── launch files
│
├── CMakeLists.txt
├── package.xml
├── README.md
└── LICENSE

6. Using 3D LiDAR with 2D Navigation

Many navigation stacks expect LaserScan.

Example:

3D LiDAR
↓
PointCloud2
↓
pointcloud_to_laserscan
↓
LaserScan
↓
Nav2

7. Limitations

Information Loss

Converting 3D → 2D removes vertical information.

Example:

• Overhang obstacles
• Multi-level objects

Assumes Flat Environment

Works best for:

• indoor robots
• ground robots

Not ideal for aerial robots.

Rover Description

Overview of the Package

This is a ROS 2 package designed to model, visualize, and launch a 4-wheel-drive (4WD) rover robot. It focuses on:

• Robot description using URDF/Xacro
• Visualization using RViz
• Launching the robot model for inspection

It does not include control, navigation, or simulation (e.g., Gazebo) — it is primarily a robot description + visualization package.

Package Structure

Core files:

• package.xml → package metadata and dependencies
• CMakeLists.txt → build configuration
• urdf/four_wd_rover.urdf.xacro → robot model
• launch/display.launch.py → basic visualization
• launch/display_all.launch.py → extended launch
• rviz/robot_model.rviz → RViz configuration

1. Package Configuration

package.xml

Defines:

• Package name: four_wd_rover
• ROS 2 dependencies (likely including):
  • robot_state_publisher
  • xacro
  • rviz2

Purpose:

Ensures ROS knows:

• How to build the package
• What dependencies are required

CMakeLists.txt

Handles:

• Installation of:
  • Launch files
  • URDF files
  • RViz configs

Key Role:

Even though this is mostly a description package, this file ensures:

• Proper ROS 2 build integration
• Files are discoverable via ros2 launch

2. Robot Description (URDF/Xacro)

four_wd_rover.urdf.xacro

This is the core of the project.

Key Features:

• Built using Xacro (XML Macros) for flexibility
• Defines:
  • Robot links (body, wheels)
  • Joints connecting wheels to chassis
  • Likely uses repeated macros for wheels

Robot Structure

Main Components:

1. Base Link

   • Central chassis of the rover
   • Reference frame for the robot

2. Four Wheels

   • Front-left
   • Front-right
   • Rear-left
   • Rear-right

3. Joints

   • Each wheel connected via joints (likely continuous joints for rotation)

3. Launch System

display.launch.py

Purpose:

• Launches the robot for visualization

What it does:

• Loads the URDF via robot_state_publisher
• Starts RViz
• Displays the robot model

display_all.launch.py

Likely adds:

• Additional nodes or configurations
• Possibly:
  • Joint state publisher GUI
  • More visualization options

RTAB-Map

git: https://github.com/introlab/rtabmap_ros

Overview of RTAB-Map

RTAB-Map (Real-Time Appearance-Based Mapping) is an open-source SLAM framework designed for real-time localization and mapping using visual and depth data.

RTAB-Map (Real-Time Appearance-Based Mapping) is a RGB-D, Stereo and Lidar Graph-Based SLAM approach based on an incremental appearance-based loop closure detector. The loop closure detector uses a bag-of-words approach to determinate how likely a new image comes from a previous location or a new location. When a loop closure hypothesis is accepted, a new constraint is added to the map’s graph, then a graph optimizer minimizes the errors in the map. A memory management approach is used to limit the number of locations used for loop closure detection and graph optimization, so that real-time constraints on large-scale environnements are always respected. RTAB-Map can be used alone with a handheld Kinect, a stereo camera or a 3D lidar for 6DoF mapping, or on a robot equipped with a laser rangefinder for 3DoF mapping.

Key characteristics include:

• Loop closure detection using appearance-based recognition
• Graph optimization for reducing accumulated drift
• 3D and 2D map generation
• Real-time operation on moderate hardware
• Integration with ROS / ROS2

Because it supports RGB-D sensors, stereo cameras, and LiDAR, RTAB-Map has become widely used in mobile robotics, autonomous navigation, and indoor mapping.

Core Architecture

RTAB-Map operates using three main components:

1. Visual Odometry

Visual odometry estimates the robot’s motion between frames using camera images.

Methods typically used:

• Feature detection(ORB, SURF, SIFT)
• Feature matching between frames
• Pose estimation using PnP or ICP The result is an incremental estimate of robot motion.

2. Loop Closure Detection

One of RTAB-Map’s strongest features is its appearance-based loop closure system.

When the robot revisits a previously seen area:

1. The system compares the current image with stored visual memories.
2. If a match is found, it detects a loop closure
3. The pose graph is optimized to reduce accumulated drift.

This dramatically improves long-term mapping accuracy.

3. Graph Optimization

The robot’s trajectory is stored as a pose graph.

Nodes:

• Camera frames
• Sensor observations

Edges:

• Odometry constraints
• Loop closure constraint Graph optimization algorithms (typically g2o or GTSAM) refine the robot’s trajectory to maintain map consistency.

Illumination-Invariant Visual Re-Localization

Lidar and Visual SLAM

Simultaneous Planning, Localization and Mapping (SPLAM)

Multi-session SLAM

Loop closure detection

Realsense and its integration with RTAB-map

ROS2

I hope you already have either ROS2 humble or Jazzy installed. If you dont please refer the official documentation or one of my quick install scripts.

RTAB-map

Install and setup RTAB map, you can find instructions on their official git repo.

sudo apt install ros-${ROS_DISTRO}-rtabmap-ros  
sudo apt install ros-${ROS_DISTRO}-rtabmap-viz

great now source ROS once more to make sure you have it all setup

source /opt/ros/humble/setup.sh 

source /opt/ros/jazzy/setup.sh 

whichever applies to you.

Realsense SDK

Installing the packages:

• Register the server’s public key:

# Ensure the directory exists
sudo mkdir -p /etc/apt/keyrings

# Download and dearmor
curl -sSf https://librealsense.realsenseai.com/Debian/librealsenseai.asc | \
gpg --dearmor | sudo tee /etc/apt/keyrings/librealsenseai.gpg > /dev/null

Note: The keyring contains both the new RS public key and the Intel public key for old repos, ensuring compatibility with both new and existing packages.

• Make sure apt HTTPS support is installed: sudo apt-get install apt-transport-https

• Add the server to the list of repositories:

echo "deb [signed-by=/etc/apt/keyrings/librealsenseai.gpg] https://librealsense.realsenseai.com/Debian/apt-repo `lsb_release -cs` main" | \
sudo tee /etc/apt/sources.list.d/librealsense.list
sudo apt-get update

• Install the libraries (see section below if upgrading packages):
  sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils
  The above two lines will deploy librealsense2 udev rules, build and activate kernel modules, runtime library and executable demos and tools.

• Optionally install the developer and debug packages:
  sudo apt-get install librealsense2-dev
sudo apt-get install librealsense2-dbg
  With dev package installed, you can compile an application with librealsense using g++ -std=c++11 filename.cpp -lrealsense2 or an IDE of your choice.

Reconnect the RealSense depth camera and run: realsense-viewer to verify the installation.

Verify that the kernel is updated :
modinfo uvcvideo | grep "version:" should include realsense string

relasense ROS

You can either build from source

Option 1: Install Debian package from ROS servers (Foxy EOL distro is not supported by this option):

• Configure your Ubuntu repositories

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt install curl # if you haven't already installed curl
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -

• Install all realsense ROS packages by sudo apt install ros-<ROS_DISTRO>-realsense2-*
• For example, for Humble distro: sudo apt install ros-humble-realsense2-*

Option 2: Install from source

• Create a ROS2 workspace

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src/

• Clone the latest ROS Wrapper for RealSense™ cameras from here into ‘~/ros2_ws/src/’

git clone https://github.com/realsenseai/realsense-ros.git -b ros2-master
cd ~/ros2_ws

• Install dependencies

sudo apt-get install python3-rosdep -y
sudo rosdep init # "sudo rosdep init --include-eol-distros" for Foxy and earlier
rosdep update # "sudo rosdep update --include-eol-distros" for Foxy and earlier
rosdep install -i --from-path src --rosdistro $ROS_DISTRO --skip-keys=librealsense2 -y

• Build

colcon build

• Source environment

ROS_DISTRO=<YOUR_SYSTEM_ROS_DISTRO>  # set your ROS_DISTRO: kilted, jazzy, iron, humble, foxy
source /opt/ros/$ROS_DISTRO/setup.bash
cd ~/ros2_ws
. install/local_setup.bash

Finally run it

ros2 launch rtabmap_examples realsense_d435i_color.launch.py

==Note : == before you start make sure in a surrounding with as less disturbance as possible. Make sure you are pointing your camera in the direction where you can get the most feature points, (Usually a place with less reflections an glares and a lot of obstacles). Move the camera slowly especially while turning.

If you feel the frame rate is low and compute is not being utilized fully try changing the DDS version being used for communication.

export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

test 001

this was for testing ( in future this will be product 2)

Drone testbecn

this ccan be used for PID tuning of drones.