Raph Navigation (ROS 2 Jazzy)

This repository contains a ROS 2 navigation stack for Raph Rover with:

• A top-level navigation package: raph_nav
• A customized Nav2 controller plugin package: nav2_ackermann_rpp_controller

The stack is designed for Ackermann steering and supports both mapping mode (SLAM) and localization mode (AMCL + map server).

Repository layout

• raph_nav/
  • launch files for navigation, AMCL, and SLAM Toolbox
  • Nav2 parameter files (navigation.yaml, amcl.yaml, slam_toolbox.yaml)
  • behavior trees
  • maps
• nav2_ackermann_rpp_controller/
  • custom fork of Nav2 Regulated Pure Pursuit with Ackermann-oriented changes

Main capabilities

raph_nav capabilities

• Runs a full Nav2 pipeline in a single composable container:
  • planner server (nav2_smac_planner)
  • controller server (nav2_ackermann_rpp_controller)
  • behavior server, BT navigator, waypoint follower, lifecycle manager
• Supports two localization modes:
  • SLAM mode via slam_toolbox (mapping)
  • localization-only mode via nav2_amcl + nav2_map_server
• Fuses obstacle sensing from:
  • filtered LiDAR scan (/rplidar/scan_filtered)
  • stereo depth point cloud generated by depth_image_proc::PointCloudXyzNode
• Uses Reeds-Shepp-compatible planning (SmacPlannerHybrid) with reverse maneuvers.
• Uses a custom RPP controller variant tailored for Ackermann control and cusp-aware path following.

Custom controller capabilities

• Ackermann command publishing (ackermann_msgs/AckermannDrive) on controller/cmd_ackermann
• Optional Dynamic Window Pure Pursuit in Ackermann space
• Steering feedback gating using /joint_states servo positions
• Cusp-aware path truncation (first viable segment selection)

Build and install

From your workspace root:

rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install
source install/setup.bash

Running

1. Mapping mode (SLAM Toolbox)

ros2 launch raph_nav navigation.launch.xml localization:=false

2. Localization mode (AMCL + map)

ros2 launch raph_nav navigation.launch.xml localization:=true map_file:=/absolute/path/to/map.yaml

Navigation launch file (raph_nav/launch/navigation.launch.xml)

Arguments:

• localization (default: false)
  • false: includes slam_toolbox.launch.py
  • true: includes amcl.launch.py
• slam_params_file (default: raph_nav/config/slam_toolbox.yaml)
• amcl_params_file (default: raph_nav/config/amcl.yaml)
• map_file (default: raph_nav/maps/empty_map.yaml)
• navigation_params_file (default: raph_nav/config/navigation.yaml)

Notable behavior:

• controller_server remaps cmd_vel to dummy/cmd_vel.
  • Rationale: Nav2 controller interface still requires Twist output, but this stack uses Ackermann commands as the actuator command path.
  • This prevents accidental consumption of Twist commands by other nodes while keeping Nav2 internals intact.

Custom changes to nav2_ackermann_rpp_controller

1. Native Ackermann Messaging & Twist Remapping

While the standard Nav2 controller interface relies strictly on Twist messages, Raph Rover utilizes an Ackermann drivetrain. To bridge this gap without breaking Nav2 compatibility, the controller returns a standard geometry_msgs/TwistStamped but simultaneously broadcasts the true actuator commands via ackermann_msgs/AckermannDrive.

This secondary stream provides a direct, explicit control path for the firmware, outputting commanded speed, steering angle, steering angle rate, acceleration, and jerk. Additionally, the default cmd_vel topic is intentionally remapped to dummy/cmd_vel to prevent downstream motion nodes from accidentally consuming unconfigured Twist outputs.

2. Ackermann-Aware Dynamic Windowing

Standard dynamic window approaches operate within regular $(v, \omega)$ space, which fails to capture physical steering limits and rate constraints. This modification adapts the dynamic window step directly to Ackermann kinematics.

The algorithm samples and computes feasible linear velocities based on acceleration/deceleration boundaries, alongside feasible steering targets derived from physical steering-rate and angle limits. Once a valid steering target ($\delta$) is selected based on the desired path curvature, it is converted back into angular velocity ($\omega$) to maintain Nav2 compatibility using:

$$\omega = \frac{v \cdot \tan(\delta)}{\text{wheelbase}}$$

This prevents the local planner from issuing kinematically impossible commands to the steering hardware.

3. Steering Feedback Gate (Hysteresis Latch)

To protect the steering mechanics and prevent tracking errors, the vehicle should avoid driving forward while the wheels are still pivoting to a newly requested angle.

This feature reads the actual steering joint positions (servo_l_joint, servo_r_joint) from /joint_states to calculate the current physical steering angle. If the error between the commanded and measured steering angle exceeds the designated tolerance, a safety gate trips, forcing the robot to wait until servos are in correct positions.

To prevent rapid "chattering" near the threshold, a hysteresis latch is used:

• Gate Turns ON: When steering error exceeds steering_angle_tolerance.
• Gate Turns OFF: Only after the error drops back below steering_angle_release_tolerance.

4. Kinematic Curvature Clamping

To ensure the controller never requests maneuvers that exceed the physical capabilities of the vehicle's chassis, both lookahead curvature and regulated curvature are clamped. The maximum achievable curvature ($k_{\max}$) is calculated directly from the vehicle's geometry:

$$k_{\max} = \frac{\tan(\delta_{\max})}{L}$$

Where, $\delta_{\max}$ is the maximum steering angle and $L$ is the wheelbase.

By hard-clamping requested path curvatures to $[-k_{\max}, +k_{\max}]$, erratic controller behaviors can be eliminated and tracking stability can be improved.

5. Cusp-Aware Path Truncation

Global planners (such as Reeds-Shepp) often generate paths containing immediate direction reversals (cusps) separated by tiny, negligible path segments. Attempting to track these micro-segments causes highly unstable driving behaviors on Ackermann vehicles.

This functionality scans ahead, intercepts the planned path, and splits it at any directional cusp. The controller isolates the first viable segment and discards the rest of the downstream path until the segment is completed. Viability is determined by an arc-length threshold - if a segment is too short, the controller avoids micro-maneuvering and focuses on stable progression.

While cusp-clamping exists natively in RPP controller, it still can make the "carrot" jump around when the robot is on top of the cusp. Completely removing other segments from the path passed to the controller and removing micro segments improves stability.

New custom RPP parameters

Each custom parameter added in this RPP fork is listed once below.

Parameter	Description
wheelbase	Front-to-rear axle distance in meters; enables Ackermann geometry conversion and steering-feasibility limits when set greater than zero.
track_width	Left-to-right steering track width in meters; stored as vehicle geometry metadata for Ackermann configuration consistency.
max_steering_angle	Maximum allowed steering angle in radians; used to clamp steering commands and bound feasible curvature.
max_steering_angle_velocity	Maximum steering rate in radians per second; limits how fast steering can change between control cycles.
ackermann_acceleration	Acceleration value written into published Ackermann commands as feedforward acceleration.
ackermann_jerk	Jerk value written into published Ackermann commands as feedforward jerk.
steering_angle_tolerance	Steering error threshold to engage the steering safety gate and stop motion until alignment improves.
steering_angle_release_tolerance	Lower steering error threshold to release the safety gate (hysteresis release point).
min_segment_length	Minimum arc length a cusp-separated segment must have to be selected as the active segment to track.