Path Tracking PID offers a tuneable PID control loop, decouling steering and forward velocity

Overview

path_tracking_pid

Overview

Path Tracking PID offers a tuneable PID control loop decouling steerting and forward velocity. The forward velocity is generated in an open loop fashion by using target velocities and accelerations.

One of the tracking options uses a carrot of length l in front of the robot to determine the steering action based on the lateral error between the current Global Point (GP) and the Control point (CP):

Tracking carrot

If a smooth path is provided, the controller has the option to track the path with the base_link directly instead of lagging behind a carrot. In this case a Projected Global Point (PGP) is computed as well which is tracked by the CP. In this mode, the yaw error can also be used as control input.

Tracking Base Link

The PID contains two closed loops: Lateral and angular loops, and an open loop: Longitudinal. For non-holonomic robots, the lateral and angular loops can be combine to compute steering actions.

Model Predictive Control

As an optional feature, we use a Model Predictive Control (MPC) concept to regulate the forward velocity. The user can specify a maximum desired error bound to track the path. Using the model of the robot plus the controller settings, a predicted path is calculated and if the error bound is violated, the forward velocity of the robot is decreased until the predicted path lies within acceptable bounds.

MPC

Tricycle model

The tricycle model is supported as well.

Tricycle Model

It is assumed the front wheel is steered and driven. The steered wheel link (SL) can be asymmetrically placed with respect to the BL. Limitations of steering and actuation are also taken into account.

Anti collision

When used as a plugin, Path Tracking PID uses the costmap (and the robot footprint from the costmap) for anti-collision. No evasive maneuvers will be preformed (sticking to the path is key). However when obstacles are detected in close proximity to the robot or in the direct line of direction of the robot, the forward velocity will be decreased. This results in safer velocities around obstacles. If an obstacle cannot be avoided or is too close to the robot, a standstill will occur and the navigation is cancelled.

The maximum velocity of the robot will be scale linear with the costmap values around or in path of the robot. One can use the inflation layers (or even social layers) to construct these gradients in the costmap.

Keywords: tracking, pid, local_planner, trajectory, model predictive control

License

TBD

Author: Cesar Lopez, [email protected].

Maintainer: Cesar Lopez, [email protected].

Affiliation: Nobleo Projects

The path_tracking_pid package has been tested under ROS Melodic and Ubuntu 18.04.

Build Status

Installation

Building from Source

Dependencies

Building

To build from source, clone the latest version from this repository into your catkin workspace and compile the package using

cd catkin_workspace/src
git clone https://bitbucket.org/nobleo/path_tracking_pid.git
cd ../
catkin_make

Unit Tests

Run the unit tests with catkin run_tests path_tracking_pid

Usage

The path_tracking_pid is a plugin for move_base_flex.

To run move_base_flex with path_tracking_pid plugin:

roslaunch path_tracking_pid path_tracking_pid_mbf.launch

Static parameters

  • base_link_frame (string, default: base_link) Name of the base link frame.

  • holonomic_robot (bool, default: false) True for an holonomic robot. --> Unmaintained, expect bugs

  • estimate_pose_angle (bool, default: false) Whether to take the pose angles from the path directly or to estimate them form consecutive path poses.

  • use_tricycle_model (bool, default: false) True for using tricycle model instead of differential drive.

  • steered_wheel_frame (string, default: steer) Name of the steered wheel frame.

  • use_mpc (bool, default: false) True for using MPC to regulate x velocity.

Configuring in RQT

Tracking_pid parameters are all available through (rqt_)dynamic_reconfigure. The main parameters are:

  • l (double, default: 0.5) Following distance from robot's rotational point to trajectory.
  • track_base_link (bool, default: false) Whether to track the path using the base_link instead of the control point ahead. A smooth path is needed.

Proportional, Integral and Derivative actions for the two closed loops: Lateral and angular loops.

  • Kp_lat (double, default: 1.0) Proportional action gain for lateral loop.
  • Ki_lat (double, default: 0.0) Integral action gain for lateral loop.
  • Kd_lat (double, default: 0.3) Derivative action gain for lateral loop.
  • Kp_ang (double, default: 1.0) Proportional action gain for angular loop.
  • Ki_ang (double, default: 0.0) Integral action gain for angular loop.
  • Kd_ang (double, default: 0.3) Derivative action gain for angular loop.

Each loop can be enabled/disabled separetly.

  • feedback_lat (boolean, default: true) Enable feedback lateral loop.
  • feedback_ang (boolean, default: false) Enable feedback angular loop.

Moreover, feedforward using trajectory velocity can be enabled/disabled.

  • feedforward_lat (boolean, default: true) Enable velocity feedforward for lateral loop.
  • feedforward_ang (boolean, default: false) Enable velocity feedforward for angular loop.

Target velocities and accelerations for generating the open loop forward velocity:

  • target_x_vel (double, default: 2.0) Nominal target forward velocity.
  • target_end_x_vel (double, default: 0.0) Target forward velocity at the end of the path.
  • target_x_acc (double, default: 2.0) Desired acceleration at the beginning of the path.
  • target_x_decc (double, default: 2.0) Desired decceleration at the end of the path.

Constraints on the generated velocities:

  • abs_minimum_x_vel (double, default: 0.025) Minimum velocity of the vehicle, used to reach the very end of the path.
  • max_error_x_vel (double, default: 1.0) Maximum allowed x velocity error.
  • max_yaw_vel (double, default: 2.0) Maximum allowed yaw velocity
  • max_yaw_acc (double, default: 2.0) Maximum allowed yaw acceleration
  • min_turning_radius (double, default: 0.0) Minimum turning radius of the vehicle.

Constraints on the steered wheel for the tricycle model:

  • max_steering_angle (double, default: 3.1416) Maximum steering angle for tricycle model.

  • max_steering_x_vel (double, default: 3.0) Maximum steering x velocity for tricycle model.

  • max_steering_x_acc (double, default: 2.0) Maximum steering x acceleration for tricycle model.

  • max_steering_yaw_vel (double, default: 0.5) Maximum steering yaw velocity for tricycle model.

  • max_steering_yaw_acc (double, default: 0.5) Maximum steering yaw acceleration for tricycle model.

Anti-collision parameters:

  • collision_look_ahead_length_offset (double, default: 1.0) Offset in length to project rectangle collision along path.
  • collision_look_ahead_resolution (double, default: 1.0) Spatial resolution to project rectangle collision along path.

Debug topic enable:

  • controller_debug_enabled (boolean, default: false) Enable debug topic.

Parameters to configure MPC behavior:

  • mpc_max_error_lat (double, default: 0.5) MPC maximum allowed lateral error.
  • mpc_min_x_vel (double, default: 0.5) MPC minimum absolute forward velocity.
  • mpc_simulation_sample_time (double, default: 0.05) MPC simulation sample time MPC maximum allowed iterations forward in time.
  • mpc_max_fwd_iterations (int, default: 200) Prediction iterations. Total simulation time will be then mpc_max_fwd_iterations*mpc_simulation_sample_time.
  • mpc_max_vel_optimization_iterations (int, default: 5) MPC maximum allowed velocity optimization iterations.

RQT reconfigure Tracking PID

Launch files

Plugin

path_tracking_pid/TrackingPidLocalPlanner

For use in move_base_flex. See launch/path_tracking_pid_mbf.launch to see an example.

Subscribed Topics

  • path ([nav_msgs/Path]) The path to follow.

  • odom ([nav_msgs/Odometry]) Robot odometry.

  • /tf ([tf2_msgs/TFMessage]) The position of the robot wrt. the frame in which the map is received.

  • vel_max ([std_msgs/Float64]) Dynamic maximum velocity. To be used for example when maximum power demands are reached. Higher level software could also use the reconfigure interface for this and set new speed values, but when required in a feedback-loop, this streaming topic is preferred.

Published Topics

  • cmd_vel ([geometry_msgs/Twist]) The Twist the robot should realize to track the desired path.

  • feedback ([path_tracking_pid/PidFeedback]) Estimated duration remaining and progress towards final goal.

  • visualization_marker ([visualization_msgs/Marker]) A Marker indicating the current control goal of the towards.

  • collision_footprint_marker ([visualization_msgs/Marker]) A Marker indicating the footprint(s) along the path that are used for ahead collision detection.

  • debug ([path_tracking_pid/PidDebug]) Intermediate values of the PID controller. Topic is silent by default, can be enabled via dynamic_reconfigure.

Bugs & Feature Requests

Please report bugs and request features using the Issue Tracker.

Issues
  • Ensure all members are initialized.

    Ensure all members are initialized.

    Ensure all members are initialized.

    Since the members were previously uninitialized and the software seemed to work fine, the actual value of the initialized variables apparently is not important. So i just went with 0.

    clang-tidy can check this. I will add a clang-tidy file once its warning have been fixed.

    opened by lewie-donckers 8
  • Controller is too conservative in its collision checking

    Controller is too conservative in its collision checking

    The controller checks if there is any cell within the collision polygon that is of value costmap2d::INSCRIBED_INFLATED_OBASTACLE. If there is, it concludes that a collision is imminent. This is not true. the robot footprint is perfectly allowed to enter space of costmap2d::INSCRIBED_INFLATED_OBASTACLE value. Here's an example where there is overlap, but no collision:

    no_collision

    The cheap way to check for imminent collisions, assuming a (approximately) circular base is to check if the cell in which (a predicted) base_link is located is >= costmap2d::INSCRIBED_INFLATED_OBASTACLE. That would mean a guaranteed collision.

    However, with a non-circular base, this is not enough. Because a different orientation may also lead to collision before base_link reaches this space. Instead, you can check if any of the cells in the collision polygon exceeds this value. That means it reaches a value of >= costmap2d::LETHAL_OBSTACLE.

    bug 
    opened by rokusottervanger 8
  • Preliminary boost units implementation.

    Preliminary boost units implementation.

    Preliminary boost units implementation. No proper review required yet. This is just to show what it could look like.

    It contains the same changes to the interfaces and calculations as #70 but this time using the Boost units library.

    This is currently quite verbose but by adding a few typedefs we could reduce that.

    opened by lewie-donckers 7
  • Controller reconfigures itself on cancel

    Controller reconfigures itself on cancel

    When cancelling the exe_path action, the controller changes the configuration set by the user: it sets target_x_vel to zero, and it never changes it back. As a result, the next goal sent on the exe_path action interface is not executed well, because the target_x_vel remains zero.

    duplicate 
    opened by rokusottervanger 7
  • Explicitly disabled copy/move.

    Explicitly disabled copy/move.

    Explicitly disabled copy/move for Controller and TrackingPidLocalPlanner.

    This to avoid accidental copies which could be incorrect/expensive/both.

    And to avoid accidental moves which could cause bound callbacks to break.

    opened by lewie-donckers 7
  • Added filtered error tracker abstraction.

    Added filtered error tracker abstraction.

    I noticed there was a lot of code duplication to keep track of error history in ControllerState. I have 3 PRs lined up to address this. This one is the second.

    Added a filtered error tracker abstraction. The errors in ControllerState are always tracked in pairs. The "raw" errors and the filtered errors. The filtered errors are always calculated in the same way and only need the raw errors and previous filtered errors as input. This abstracts that principle to reduce code duplication and noise in the Controller.

    opened by lewie-donckers 6
  • Documentation of `Controller::distToSegmentSquared()` incorrect

    Documentation of `Controller::distToSegmentSquared()` incorrect

    While working on unit tests for Controller::distToSegmentSquared() I noticed the following.

    The documentation suggests that the pose it returns is the point on the segment PV (start, end) that is closest to W (point). It does not do this.

    I had the following test case:

    TEST(PathTrackingPidCalculations, ClosestPointOnSegment_CloseToStart)
    {
      const auto start = create_transform(2, 2, 0);
      const auto end = create_transform(4, 4, 0);
      const auto point = create_transform(0, 1, 0);
      const auto ref = start;
    
      EXPECT_EQ(ref, closestPointOnSegment(start, end, point, false));
    }
    

    point is closest to start of any point on start-end but the following is returned: [1.759, 2.319, 0].

    bug 
    opened by lewie-donckers 5
  • Added derivative filtered error tracker abstraction.

    Added derivative filtered error tracker abstraction.

    I noticed there was a lot of code duplication to keep track of error history in ControllerState. I have 3 PRs lined up to address this. This one is the third.

    Added a derivative filtered error tracker abstraction. The remaining errors in ControllerState are also always tracked in pairs. The raw errors/filtered errors and their derivatives. The derivatives are always calculated in the same way. This abstracts that principle to reduce code duplication and noise in the Controller.

    opened by lewie-donckers 5
  • Cleaned up includes.

    Cleaned up includes.

    Cleaned up includes according to the following guidelines:

    • use <> for files found through include dirs and "" for files found relative to the current file see https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#sf12-prefer-the-quoted-form-of-include-for-files-relative-to-the-including-file-and-the-angle-bracket-form-everywhere-else
    • regarding the order:
      • first include the corresponding header file (this one is for source files only)
      • then local files (using "")
      • then files from the same lib
      • then files from 3rd party libs (boost/ros/etc)
      • then standard includes
      • then OS includes
    opened by lewie-donckers 5
  • Step response of the lowpass filter has an overshoot

    Step response of the lowpass filter has an overshoot

    I put a step response on the second order lowpass and the output I've printed below. As you can see the filter has an overshoot, which is very strange. I think there is something wrong with it.

    0.292893
    0.87868
    1.12132
    1.02082
    0.979185
    0.996429
    1.00357
    1.00061
    0.999387
    0.999895
    1.00011
    1.00002
    0.999982
    0.999997
    1
    1
    0.999999
    1
    1
    1
    
    opened by Rayman 4
  • Quaternion of carrotTF was not initialized, set to proper value

    Quaternion of carrotTF was not initialized, set to proper value

    As initiated here: https://github.com/nobleo/path_tracking_pid/commit/f603fecff527321a9ee0165845981bd4fc578ad6#commitcomment-66762408 Building with proper flags threw a new error, this fixes it.

    A tf2::Transform without arguments is not initialized, when calling setOrigin() the rotation part is still not initialized.

    opened by MCFurry 4
  • Straight line into turn: starting turn too early

    Straight line into turn: starting turn too early

    After a discussion with @cesar-lopez-mar:

    In the figure below,

    • a straight path is followed by a corner
    • the robot starts executing the straight path, as indicated by the red arrows
    • the debugging markers of the path_tracking_pid are indicated as well
    • halfway, the robot starts rotating. Based on the debug markers, it seems that starts executing the corners.

    Screenshot from 2022-04-15 11-10-34

    opened by whoutman 0
  • Global plan is statically transformed to the global frame

    Global plan is statically transformed to the global frame

    In TrackingPidLocalPlanner::setPlan() the passed global plan is transformed just once into the configured global_frame. If this latter is configured to be odom, the global plan is sensitive to initial localisation faults.

    Other planners (such as dwa_local_planner) convert the inserted global plan every tick into global_frame, using LocalPlannerUtil::getLocalPlan().

    bug 
    opened by PaulVerhoeckx 0
  • Is the calculated stopping distance too conservative?

    Is the calculated stopping distance too conservative?

    The stopping distance in the end phase d_end_phase (here) contains a component which accounts for the distance traveled during the possible delay due to having a sampling time.

    However I wonder if it isn't too conservative, currently it is:

    target_x_vel * 2 * dt
    

    Whereas I think the average delay is 0.5*dt, which would result in the following component:

    target_x_vel * 0.5 * dt
    
    question 
    opened by PaulVerhoeckx 5
  • UB in `TrackingPidLocalPlanner::projectedCollisionCost()` - partially uninitialized object used

    UB in `TrackingPidLocalPlanner::projectedCollisionCost()` - partially uninitialized object used

    In TrackingPidLocalPlanner::projectedCollisionCost() a local variable x_step_tf is created but only it's origin component is initialized; it's basis component is left uninitialized.

    Later this variable is used in computations which is undefined behavior.

    This could be (one of) the reasons our tests sometimes fail.

    bug 
    opened by lewie-donckers 0
  • Does `Controller::findPositionOnPlan()` do what is intended?

    Does `Controller::findPositionOnPlan()` do what is intended?

    While refactoring Controller::findPositionOnPlan(), I noticed the following. I'm not sure if it is intended or not so I would like some feedback. Depending on the feedback we can determine what to do (nothing, document, fix, etc).

    In that function we look ahead in the plan based on a given index. As long as each next point is a better match, we keep looking and update the given index. Once we're done with looking ahead, we start looking backward. Now one of two things will happen:

    • either we found no better match when looking forward, and we start looking backward as expected
    • we did find a better match when looking forward and - because we updated the given index (= starting point) - looking backward from this index will immediately fail.

    So in practice we look either forward or backward but never both. Is this intended?

    I've included the code below and stripped the bits that were less relevant:

      // First look in current position and in front
      for (auto idx_path = global_plan_index; idx_path < global_plan_tf_.size(); idx_path++) {
        ...
        if (...) {
          ...
          global_plan_index = idx_path;
        } else {
          break;
        }
      }
    
      // Then look backwards
      for (auto idx_path = global_plan_index; idx_path > 0; --idx_path) {
        ...
      }
    
    question 
    opened by lewie-donckers 1
  • Angle calculation of closestPointOnSegment() should be changed

    Angle calculation of closestPointOnSegment() should be changed

    Currently, closestPointOnSegment uses the angle of the start pose (or the one defined from start to end) to the returned Pose

    A better idea is to interpolate the angle from start to end (when those angles are given). This will avoid "step" discontinuities of the returned Pose angle between segments, which I saw recently in one of our projects.

    opened by cesar-lopez-mar 5
Owner
Nobleo Technology
autonomous | intelligent | systems
Nobleo Technology
MQB steering wheel adapter to retrofit into PQ46 platform (VW Passat b6/b7, CC)

MQB steering wheel adapter (retrofit MQB steering wheel into PQ46 platform - VW Passat b6/b7, CC) The adapter supports on-wheel buttons (media and cru

null 11 Jun 28, 2022
C++11 header-only library that offers small vector, small flat map/set/multimap/multiset.

sfl library This is header-only C++11 library that offers several new containers: small_vector small_flat_set small_flat_map small_flat_multiset small

null 10 Jun 30, 2022
Lee Thomason 290 Jun 9, 2022
HESS HAMILTONIAN PATH COMPLETE (2021) black-box for Hamiltonian Path Problem

The original HESS (Hyper Exponential Space Sorting) is a polynomial black-box optimization algorithm, that work very well with any NP-Complete, or NP-Hard problem

SAT-X 1 Jan 19, 2022
Text - A spicy text library for C++ that has the explicit goal of enabling the entire ecosystem to share in proper forward progress towards a bright Unicode future.

ztd.text Because if text works well in two of the most popular systems programming languages, the entire world over can start to benefit properly. Thi

Shepherd's Oasis 199 Jun 26, 2022
Harsh Badwaik 1 Dec 19, 2021
Control Heidelberg Wallbox Energy Control over WiFi using ESP8266 and configure your own local load management

< scroll down for English version and additional information > wbec WLAN-Anbindung der Heidelberg WallBox Energy Control ├╝ber ESP8266 Die Heidelberg W

null 69 Jun 21, 2022
Bobby Cooke 266 Jun 20, 2022
Budgie Control Center is a fork of GNOME Control Center for the Budgie 10 Series.

Budgie Control Center Budgie Control Center is a fork of GNOME Settings / GNOME Control Center with the intent of providing a simplified list of setti

Buddies of Budgie 10 Jun 19, 2022
Tactile-Arcade-Games - Wrote a C program comprised of four separate games that run in a loop using the PSoC 5LP board and Cypress IDE.

Tactile-Arcade-Games - Wrote a C program comprised of four separate games that run in a loop using the PSoC 5LP board and Cypress IDE. Used two potentiometers, two ADCs to convert their voltages to digital values, a PWM to drive two servos, an 8x8 RGB LED matrix, 40 digital output pins and 8 power MOSFETS to control the matrix, and a character LCD display.

null 2 Dec 24, 2021
Read-Compile-Run-Loop: tiny and powerful interactive C++ compiler (REPL)

Read-Compile-Run-Loop: tiny and powerful interactive C++ compiler (REPL) RCRL is a tiny engine for interactive C++ compilation and execution (implemen

Viktor Kirilov 370 Jun 20, 2022
Loop is an object oriented programming language

Loop Loop is an object oriented programming language. How do I build and run loop? Make sure, you installed the requirements for clang and make: Debia

Loop 24 Aug 9, 2021
The QPEP-Enhanced Direct Sparse Odometry (DSO) with Loop Closure

QPEP-DSO: Quadratic Pose Estimation Problems (QPEP) Enhanced Direct Sparse Odometry with Loop Closure Code Arch The codes are motivated by DSO (https:

Jin Wu 8 Jun 23, 2022
Just loop forever, with sleep for specified seconds

loopever Just loop forever, with sleep for specified time Build & Install $ mkdkir build $ cd build $ cmake .. $ make $ make install Run $ loopever 0

Tomohito Nakayama 1 Oct 24, 2021
This is for interfacing rasberry-pi's (2 cards) with an arduino for sending raw data to form the close loop system to avoid motor heating by acting on a given temperature.

This is for interfacing rasberry-pi's (2 cards) with an arduino for sending raw data to form the close loop system to avoid motor heating by acting on a given temperature. Interface is explained through a master slave approach and client server approach. another camera is used with OPEN-CV platform to interface and collect data aswell.

Younes HAMZA 2 Oct 25, 2021
Thread-safe cross-platform event loop library in C++

Dasynq Version 1.2.2 Dasynq is an event loop library similar to libevent, libev and libuv. Like other such libraries, it is crossplatform / portable.

Davin McCall 145 May 29, 2022
KDevelop plugin for automatic time tracking and metrics generated from your programming activity.

Wakatime KDevelop Plugin Installation instructions Make sure the project is configured to install to the directory of your choice: In KDevelop, select

snotr 6 Oct 13, 2021
Tiny blocker of Windows tracking and telemetry written in plain C++/Win32 API.

Tiny blocker of Windows tracking and telemetry written in plain C++/Win32 API. Just run once as admin and forget. No questions asked. No harmful actions performed like other Windows spying blockers try.

null 5 Mar 30, 2022
Tsdf-plusplus - TSDF++: A Multi-Object Formulation for Dynamic Object Tracking and Reconstruction

TSDF++: A Multi-Object Formulation for Dynamic Object Tracking and Reconstruction TSDF++ is a novel multi-object TSDF formulation that can encode mult

ETHZ ASL 111 Jun 21, 2022