Class SimpleController

Inheritance Relationships

Base Type

  • public ControllerMethodBase

Class Documentation

class SimpleController : public ControllerMethodBase

A simple path-following controller using PID and look-ahead strategy.

Public Functions

SimpleController()
~SimpleController() override

Destructor.

void on_initialize() override

Initializes parameters and PID controllers.

Throws:

std::runtime_error – on initialization error.

void update_rt(NavState &nav_state) override

Updates the controller using the given NavState.

Parameters:

nav_state – Current navigation state, including odometry and planned path.

Protected Functions

geometry_msgs::msg::Pose get_ref_pose(const nav_msgs::msg::Path &path, double look_ahead)

Gets the reference pose at look-ahead distance on the path.

Parameters:

  • path – The planned path.

  • look_ahead – Distance to look ahead in meters.

Returns:

PoseStamped representing the goal reference.

double get_distance(const geometry_msgs::msg::Pose &a, const geometry_msgs::msg::Pose &b)

Computes the Euclidean distance between two poses.

Parameters:

  • a – First pose.

  • b – Second pose.

Returns:

Distance in meters.

double get_angle(const geometry_msgs::msg::Point &from, const geometry_msgs::msg::Point &to)

Computes the angle between two points.

Parameters:

  • from – Start point.

  • to – End point.

Returns:

Angle in radians.

double get_diff_angle(const geometry_msgs::msg::Quaternion &a, const geometry_msgs::msg::Quaternion &b)

Computes the angular difference between two quaternions (yaw).

Parameters:

  • a – First quaternion.

  • b – Second quaternion.

Returns:

Angle difference in radians within [-π, π].

Protected Attributes

std::shared_ptr<PIDController> linear_pid_

PID controller for linear velocity.

std::shared_ptr<PIDController> angular_pid_

PID controller for angular velocity.

double max_linear_speed_ = {1.0}

Maximum linear speed in m/s.

double max_angular_speed_ = {1.0}

Maximum angular speed in rad/s.

double max_linear_acc_ = {0.3}

Maximum linear acceleration in m/s².

double max_angular_acc_ = {0.3}

Maximum angular acceleration in rad/s².

double look_ahead_dist_ = {1.0}

Distance ahead of the robot to track in meters.

double tolerance_dist_ = {0.05}

Distance threshold to switch to orientation tracking.

double k_rot_ = {0.5}

Gain to reduce linear speed based on angular velocity.

double final_goal_angle_tolerance_ = {0.1}

Angular tolerance at the final goal in radians.

double linear_kp_ = {0.95}

Proportional gain for linear PID.

double linear_ki_ = {0.03}

Integral gain for linear PID.

double linear_kd_ = {0.08}

Derivative gain for linear PID.

double angular_kp_ = {1.5}

Proportional gain for angular PID.

double angular_ki_ = {0.03}

Integral gain for angular PID.

double angular_kd_ = {0.08}

Derivative gain for angular PID.

double last_vlin_ = {0.0}

Previous linear velocity for acceleration limiting.

double last_vrot_ = {0.0}

Previous angular velocity for acceleration limiting.

rclcpp::Time last_update_ts_

Timestamp of the last control update.

geometry_msgs::msg::TwistStamped twist_stamped_

Current velocity command.