Naviformer Path Follow - MPC Path Follower

A Model Predictive Control (MPC) based path follower for mobile robots with ROS1 integration.

Quick Start

Build the Package

cd /Titan/code/robohike_ws
catkin build naviformer_path_follow
source devel/setup.bash

Launch the Path Follower

roslaunch naviformer_path_follow mpc_path_follower.launch \
    max_speed:=1.5 \
    max_yaw_rate:=90.0

Publish a Path

# Example: Publish a simple path
rostopic pub /path nav_msgs/Path "{
  header: {frame_id: 'map'},
  poses: [
    {pose: {position: {x: 0, y: 0}}},
    {pose: {position: {x: 1, y: 0}}},
    {pose: {position: {x: 2, y: 1}}},
    {pose: {position: {x: 3, y: 1}}}
  ]
}" --once

Package Structure

naviformer_path_follow/
├── src/
│   ├── mpc_controller.py          # Core MPC algorithm (ROS-independent)
│   └── mpc_path_follower_ros1.py  # ROS1 wrapper
├── test/
│   └── test_mpc_path_follower.py  # Unit tests for MPC controller
├── launch/
│   └── mpc_path_follower.launch   # ROS1 launch file
├── CMakeLists.txt
├── package.xml
├── REFACTORING_SUMMARY.md         # Detailed refactoring documentation
└── README.md                      # This file

Architecture

The package is split into two main components:

1. Core MPC Controller (mpc_controller.py)

• Pure Python implementation without ROS dependencies
• Can be used standalone or in other frameworks
• Implements MPC optimization using CasADi
• Handles differential drive robot dynamics

2. ROS1 Wrapper (mpc_path_follower_ros1.py)

• ROS1 interface for the MPC controller
• Handles topic subscriptions/publications
• TF frame transformations
• Joystick integration
• Safety features

Features

MPC Controller

• ✅ Model Predictive Control with optimization
• ✅ Trajectory tracking with minimal error
• ✅ Velocity and angular rate constraints
• ✅ Warm-starting for better performance
• ✅ Configurable prediction horizon
• ✅ Reference trajectory interpolation

ROS1 Integration

• ✅ Path subscription from planners
• ✅ Odometry integration
• ✅ Velocity command publishing
• ✅ TF frame transformation support
• ✅ Joystick control override
• ✅ Autonomy mode switching
• ✅ Safety stops (inclination, emergency)
• ✅ Speed command integration

ROS Interface

Subscribed Topics

Topic	Type	Description
odom_topic	nav_msgs/Odometry	Robot state (pose, velocity)
path_topic	nav_msgs/Path	Reference trajectory to follow
/joy	sensor_msgs/Joy	Joystick input for manual control
/speed	std_msgs/Float32	Speed command in autonomy mode
/stop	std_msgs/Int8	Emergency stop signal (0=none, 1=stop linear, 2=stop all)

Published Topics

Topic	Type	Description
command_topic	geometry_msgs/TwistStamped	Velocity commands for robot

Parameters

Parameter	Type	Default	Description
max_speed	double	1.5	Maximum linear velocity (m/s)
max_yaw_rate	double	90.0	Maximum angular velocity (deg/s)
autonomy_mode	bool	true	Enable autonomous control
autonomy_speed	double	1.5	Speed in autonomy mode (m/s)
odom_topic	string	/state_estimation	Odometry topic name
command_topic	string	/cmd_vel	Command topic name
path_topic	string	/path	Path topic name
world_frame_id	string	map	World coordinate frame
robot_frame_id	string	vehicle	Robot coordinate frame
use_inclination_to_stop	bool	false	Enable inclination-based safety stop
inclination_threshold	double	45.0	Inclination threshold (degrees)
stop_time	double	5.0	Duration of safety stop (seconds)
no_rotation_at_stop	bool	false	Disable rotation when stopped
joy_to_speed_delay	double	2.0	Delay before speed command takes effect (s)

Usage Examples

Example 1: Basic Path Following

# Launch with default settings
roslaunch naviformer_path_follow mpc_path_follower.launch

Example 2: Custom Speed Limits

roslaunch naviformer_path_follow mpc_path_follower.launch \
    max_speed:=2.0 \
    max_yaw_rate:=120.0

Example 3: With Safety Features

roslaunch naviformer_path_follow mpc_path_follower.launch \
    use_inclination_to_stop:=true \
    inclination_threshold:=30.0

Example 4: Custom Topics

roslaunch naviformer_path_follow mpc_path_follower.launch \
    odom_topic:=/odometry/filtered \
    command_topic:=/mobile_base/cmd_vel \
    path_topic:=/global_planner/path

Joystick Control

The path follower supports joystick override:

• Button 6: Enable full autonomy mode (speed = 1.0)
• Left Stick (axes[3], axes[4]): Manual speed control
  • Forward/back (axes[4]): Linear velocity
  • Left/right (axes[3]): Angular velocity override
• Trigger (axes[2]):
  • Pressed (> -0.1): Manual mode
  • Released (< -0.1): Autonomy mode

Testing

Run MPC Controller Tests

cd /Titan/code/robohike_ws/src/naviformer_path_follow/test
python3 test_mpc_path_follower.py

This will:

• Test MPC with S-curve trajectory
• Test MPC with circular arc
• Test MPC with complex multi-segment path
• Generate visualization plots
• Display performance metrics

Integration Test

# Terminal 1: Launch the follower
roslaunch naviformer_path_follow mpc_path_follower.launch

# Terminal 2: Play a rosbag with odometry
rosbag play your_test_data.bag

# Terminal 3: Publish a test path
rostopic pub /path nav_msgs/Path "..." --once

# Terminal 4: Monitor commands
rostopic echo /cmd_vel

Algorithm Details

MPC Formulation

The controller solves the following optimization problem at each timestep:

Decision Variables:

• States: x[k], y[k], θ[k] for k = 0...N
• Controls: v[k], ω[k] for k = 0...N-1

Dynamics:

x[k+1] = x[k] + v[k] * cos(θ[k]) * dt
y[k+1] = y[k] + v[k] * sin(θ[k]) * dt
θ[k+1] = θ[k] + ω[k] * dt

Cost Function:

minimize Σ(||state[k] - ref[k]||²_Q + ||control[k]||²_R)

Where:

• Q = diag([10.0, 10.0, 0.0]) - State tracking weights
• R = diag([0.02, 0.15]) - Control effort weights

Constraints:

• 0 ≤ v ≤ v_max
• -ω_max ≤ ω ≤ ω_max

MPC Parameters

• Prediction Horizon (N): 15 steps
• Time Step (T): 0.1 seconds
• Reference Gap: 3 steps between reference points
• Control Rate: 100 Hz

Dependencies

Python Packages

pip install numpy scipy casadi matplotlib

ROS Packages

• rospy
• tf
• std_msgs
• nav_msgs
• geometry_msgs
• sensor_msgs

Performance

Typical performance metrics:

• MPC Solve Time: 10-50 ms per iteration
• Control Frequency: 100 Hz
• Tracking Accuracy: < 0.1 m RMS error
• Computational: Single CPU core sufficient

Troubleshooting

Issue: MPC solve fails

Solution: Check that:

• Path has at least 2 waypoints
• Robot state is being published
• CasADi is properly installed

Issue: Robot doesn't move

Solution: Check:

• Autonomy mode is enabled
• No safety stops are active (check /stop topic)
• Path is in correct frame
• Velocity limits are reasonable

Issue: Poor tracking performance

Solution: Tune parameters:

• Increase max_speed if robot is too slow
• Decrease max_yaw_rate if oscillating
• Check MPC weights in mpc_controller.py

Issue: TF transform errors

Solution: Verify:

• TF tree is complete (rosrun tf view_frames)
• Frame IDs match your robot setup
• Transforms are being published

Contributing

When modifying the code:

1. Core Algorithm Changes: Edit mpc_controller.py

   • Run unit tests after changes
   • No ROS dependencies allowed

2. ROS Interface Changes: Edit mpc_path_follower_ros1.py

   • Test with actual ROS topics
   • Maintain parameter compatibility

3. New Tests: Add to test/test_mpc_path_follower.py

References

• Original MPC implementation: NavDP
• CasADi optimization: casadi.org

License

BSD License

Authors

• Original MPC Implementation: NavDP team
• ROS1 Integration: Jincheng Wang, Jianhao Jiao
• Code Refactoring: AI Assistant

See Also

• REFACTORING_SUMMARY.md - Detailed documentation on code refactoring
• test/test_mpc_path_follower.py - Comprehensive test suite
• Naviformer - Vision-based navigation planner