nav6d is a ROS 2 package targeting full 6-DoF local navigation, from collision-aware path planning in 3D to PD-based trajectory tracking.

Note: This is an older demo recorded before trajectory smoothing was implemented. The current version includes improved motion smoothing for smoother trajectory execution.

• Overview
• Features
• Getting Started
• Dependencies
• Launching nav6d
• Runtime Requirements
• Configuration Highlights
• Example Usage
• Node Overview
• Next Steps
• License

At a high level, nav6d assumes:

• A 3D occupancy map from OctoMap
• A pose estimate for the robot body frame
• Goals expressed as PoseStamped in the map frame on /nav6d/goal

The package is split into a planner, two controller variants, and a path evaluator:

• n6d_planner: consumes the OctoMap and robot pose, then computes a collision-free 3D path (nav_msgs/Path) with consistent orientations along the way.
• n6d_velocity_controller: consumes the planned path, current pose, and IMU; it projects the robot onto the path, selects a lookahead “carrot” pose, runs a 6‑DoF PD law, and publishes body-frame velocity commands (geometry_msgs/Twist).
• n6d_force_controller: shares the same control core but publishes wrench commands (geometry_msgs/Wrench) for actuators that expect force/torque inputs.
• n6d_path_evaluator: scores any nav_msgs/Path against the OctoMap and publishes nav6d/msg/PathQuality on /nav6d/path_quality. It computes geometric metrics (clearance/narrowness/turn/efficiency) and a Monte Carlo path robustness estimate.

Planner and the selected controller are typically launched together via n6d.launch.py, but you can also run only the planner or only a controller.

• A* path planning over OctoMap voxel grids (position only)
• SLERP-based orientation planning along the path
• 6-DoF PD controller for trajectory tracking (position + attitude)
• Planner-agnostic path quality scoring against OctoMap geometry
• Monte Carlo path robustness estimate for simple "good vs risky" visualization

Clone this repository (or add it as a submodule) inside the src/ directory of your ROS 2 workspace, then build:

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select nav6d

nav6d requires the OctoMap message definitions:

sudo apt install ros-${ROS_DISTRO}-octomap-msgs

An active OctoMap is also required, but it can be published by any package that provides an octomap_msgs/msg/Octomap topic.

The path evaluator publishes nav6d/msg/PathQuality, which is defined in the nav6d package. The message layout is unchanged; for backward compatibility, PathQuality.heuristic is now used to carry the Monte Carlo path goodness score (1 - risk).

For testing, the standard OctoMap packages work out of the box and are recommended:

sudo apt install ros-${ROS_DISTRO}-octomap ros-${ROS_DISTRO}-octomap-ros ros-${ROS_DISTRO}-octomap-server

The octomap_server node bundled with octomap_server has been successfully tested with nav6d and is suggested as the default /octomap_full provider.

You can launch planner and controller together, or each component independently.

Planner + controller (recommended):

ros2 launch nav6d n6d.launch.py

This starts n6d_planner plus the velocity controller (controller_type:=velocity by default). Switch to the force-based controller with:

ros2 launch nav6d n6d.launch.py controller_type:=force

Planner + controller + optional path evaluator:

ros2 launch nav6d n6d.launch.py enable_path_evaluator:=true

Planner only:

ros2 launch nav6d n6d_planner.launch.py

Controller only:

ros2 launch nav6d n6d_controller.launch.py

Pass controller_type:=force to launch only the wrench-based controller.

Path evaluator only:

ros2 launch nav6d n6d_path_evaluator.launch.py

Note: Topic names are configurable; see the YAML files in config/.

Each newly received goal triggers a replanning pass. Markers are only published if debug_markers is enabled.

Parameters are managed via the YAML configuration file.

Additional controller-specific parameters from config/n6d_force_controller.yaml (the velocity controller uses the same keys but publishes twists instead of wrenches):

Path quality parameters from config/n6d_path_evaluator.yaml:

Tune these parameters to match your robot geometry, map resolution, and search performance requirements. yaw_tolerance_rad is still accepted for legacy configs, but orientation_tolerance_rad is preferred. The bundled n6d_planner.yaml uses the /**: wildcard so the same values apply whether you launch the node directly (ros2 run) or via ros2 launch with a namespace.

Performance tip: OctoMap resolution has a noticeable impact on planning speed—finer grids explode the number of voxels the A* search and collision checks must touch. In our tests a 0.2 m resolution offered a good trade-off between fidelity and runtime; use coarser maps if you need faster replans.

• clearance_score, narrow_score, turn_score, and efficiency_score are still published as geometric diagnostics.
• heuristic now represents Monte Carlo path goodness in [0, 1] (higher is better).
• A simple interpretation for UI coloring is:
  • green: high goodness
  • yellow: medium goodness
  • red: low goodness
• The Monte Carlo score estimates geometric robustness under pose uncertainty; it is not a full controller/physics success simulation.

1. Start an OctoMap server (or another compatible map publisher):

   ros2 run octomap_server octomap_server_node map:=/octomap_full

2. Launch planner and controller together (recommended):

   ros2 launch nav6d n6d.launch.py

   Append controller_type:=force if you need wrench (force/torque) outputs instead of velocity commands.

3. Publish a goal to trigger path planning and closed-loop tracking.
   The example below targets a pose at (2, 0, 1) with a 90° yaw about +Z:

   ros2 topic pub --once /nav6d/goal geometry_msgs/msg/PoseStamped "{
     header: {frame_id: 'map'},
     pose: {
       position: {x: 2.0, y: 0.0, z: 1.0},
       orientation: {x: 0.3799282, y: 0.5963678, z: 0.3799282, w: 0.5963678}
     }
   }"

4. Visualize the result in RViz by adding:

   • A Path display on /nav6d/planner/path
   • A MarkerArray display on /nav6d/planner/path_markers
   • Fixed frame: map