ackermann_nlmpc

This package provides a ROS2 implementation of a Nonlinear Model Predictive Controler (NLMPC) for 2D trajectory tracking for wheeled vehicles. The default setup is tuned for vehicles with Ackermann steering, but can be adapted to other steering configurations as well. A detailed technical documentation of the NLMPC algorithm is available here.

Core components is a library-free C implementation of the NLMPC, generated by a code generator available here. This package provides a ROS2 wrapper around the C implementation and some utility functionality, including a simple ackermann vehicle simulation for testing purposes.

Note that this NLMPC implementation is not a path planner, but a trajectory tracker that requires a reference trajectory as input. The ackermann_nlmpc_msgs package provides a custom Trajectory message format that contains velocity, acceleration, and constraints in addition pose data for each point on the trajectory. It is recommended to use this custom message format for optimal performance.

Installation

Clone this repository into your ROS2 workspace and build it using colcon:

cd path/to/workspace/src
git clone https://git.ime.uni-luebeck.de/public-projects/asl/ackermann_nlmpc.git
cd ..
colcon build
source install/setup.bash

Setup

Check the ackermann_nlmpc/launch/ackermann_nlmpc.launch.py file for available parameters. State penalties and input constraints can be adjusted via ROS2 parameters there. Changes to some MPC parameters require a rebuild of the C code, see the section below.

Code Generation

This repository provides the generated NLMPC C library for a default parameter set (ackermann_nlmpc/codegen/autompc_ros.c). If you wish to change parameters, you can use the code generator available here. Some parameter changes (dt, max_nodes, horizon) need to be reflected in the launch file, adjust it accordingly.

The path to the C library can be set in the launch file via the lib_path parameter. The ackermann_nlmpc node will attempt to compile the C code on first load. You may also compile it manually beforehand by running:

cd ackermann_nlmpc/codegen
gcc -fPIC -shared -o autompc_ros.so autompc_ros.c

Node and Topic Overview

This package provides the following nodes:

• ackermann_nlmpc_node: The main node that runs the NLMPC algorithm. Essentially just a ROS2 wrapper around the C implementation.
  • Subscriptions:
    • /ackermann_nlmpc/odom (nav_msgs/Odometry): Vehicle odometry
    • /ackermann_nlmpc/steering_angle (std_msgs/Float32): Vehicle steering angle
    • /ackermann_nlmpc/trajectory (ackermann_nlmpc_msgs/Trajectory): Desired trajectory in a custom message format defined in ackermann_nlmpc_msgs. Publishing a message on this topic will start the NLMPC algorithm. Publishing new messages afterwards will re-initialize the algorithm with the new trajectory.
  • Publishers:
    • /ackermann_nlmpc/control (ackermann_msgs/AckermannDriveStamped): Control commands for the vehicle, specifically acceleration, velocity, and steering angle velocity.
  • Additional publishers for visualization or debugging:
    • /ackermann_nlmpc/predicted_trajectory (nav_msgs/Path): Predicted trajectory based on the current state and the NLMPC algorithm.
    • /ackermann_nlmpc/reference_trajectory (nav_msgs/Path): The current reference trajectory for the NLMPC algorithm from the latest message on the /ackermann_nlmpc/trajectory topic.
    • /ackermann_nlmpc/trajectory_constraints (geometry_msgs/PolygonStamped): The constraints on the trajectory from the latest message on the /ackermann_nlmpc/trajectory topic, visualized as a polygon.
• ackermann_sim_node: A simple simulation node for testing the NLMPC algorithm. It simulates a vehicle with Ackermann steering using a kinematic bicycle model.
  • Subscriptions:
    • /ackermann_nlmpc/control (ackermann_msgs/AckermannDriveStamped): Control commands for the vehicle.
  • Publishers:
    • /ackermann_nlmpc/odom (nav_msgs/Odometry): Simulated vehicle odometry.
    • /ackermann_nlmpc/steering_angle (std_msgs/Float32): Simulated vehicle steering angle.
• trajectory_converter_node: A utility node that converts nav_msgs/Path and geometry_msgs/PoseStamped to the custom ackermann_nlmpc_msgs/Trajectory format. Note that the ackermann_nlmpc_msgs/Trajectory format contains additional information such as velocity, acceleration, and constraints that are not present in nav_msgs/Path format. For optimal performance, it is recommended to use the custom message format directly.
  • Subscriptions:
    • /ackermann_nlmpc/path (nav_msgs/Path): Reference trajectory in the standard ROS2 path format.
    • /goal_pose (geometry_msgs/PoseStamped): Goal pose for the vehicle.
  • Publishers:
    • /ackermann_nlmpc/trajectory (ackermann_nlmpc_msgs/Trajectory): Reference trajectory in the custom format used by the NLMPC algorithm.

Usage

Launch the nodes using the provided launch file:

ros2 launch ackermann_nlmpc ackermann_nlmpc.launch.py

Use rviz2 to visualize the predicted trajectory, reference trajectory, and trajectory constraints.

The ackermann_nlmpc/examples directory contains several sample scripts that produce reference trajectories and publish them to the /ackermann_nlmpc/trajectory topic.

The ackermann_nlmpc_node requires a functional tf tree, in particular the transform between odom and the frames used in the ackermann_nlmpc_msgs/Trajectory message must be available.