This repository contains a compact implementation of several core components used in autonomous robotics systems. The project focuses on the algorithmic elements required for mobile robot navigation, including environment mapping, motion planning, manipulator kinematics, and high-level decision making.
The modules reflect the structure of a typical autonomy stack. Sensor measurements are integrated into a spatial representation of the environment, planning algorithms generate feasible trajectories, kinematic models translate motion into actuator commands, and a behavior tree coordinates robot actions during a mission.
The code was originally developed within a ROS2 robotics system. Infrastructure such as simulation environments, launch files, and robot models are intentionally omitted here so the repository focuses on the implemented algorithms and system design.
High-level robot behavior is implemented using a Behavior Tree (BT). Behavior trees provide a structured way to describe autonomous decision making by composing simple behaviors into larger control flows.
The mission begins by activating the robot and initializing localization. The robot performs an initial rotation that allows the localization system to converge by collecting observations from multiple viewpoints. Once localization uncertainty falls below a threshold, the system enters the mission loop.
During normal operation the robot repeatedly requests navigation goals and attempts to reach them using global navigation through Nav2. If the global planner fails or stops near the goal without satisfying the final tolerance, a local controller performs the final approach using the robot pose estimate and laser scan data.
Localization quality is continuously monitored. If the robot becomes poorly localized, the system assumes a kidnapping event and switches to a recovery behavior that spins the robot until localization converges again. Additional safeguards such as navigation timeouts, obstacle checks during the final approach, and recovery maneuvers help ensure robust operation.
The behavior tree used by the mission planner is shown below.
