Class GracefulController

Inheritance Relationships

Base Type

  • public nav2_core::Controller

Class Documentation

class GracefulController : public nav2_core::Controller

Graceful controller plugin.

Public Functions

GracefulController() = default

Constructor for nav2_graceful_controller::GracefulController.

~GracefulController() override = default

Destructor for nav2_graceful_controller::GracefulController.

virtual void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name, std::shared_ptr<tf2_ros::Buffer> tf, std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override

Configure controller state machine.

Parameters:
  • parent – WeakPtr to node

  • name – Name of plugin

  • tf – TF buffer

  • costmap_ros – Costmap2DROS object of environment

virtual void cleanup() override

Cleanup controller state machine.

virtual void activate() override

Activate controller state machine.

virtual void deactivate() override

Deactivate controller state machine.

virtual geometry_msgs::msg::TwistStamped computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::Twist &velocity, nav2_core::GoalChecker *goal_checker) override

Compute the best command given the current pose and velocity.

Parameters:
  • pose – Current robot pose

  • velocity – Current robot velocity

  • goal_checker – Ptr to the goal checker for this task in case useful in computing commands

Returns:

Best command

virtual void setPlan(const nav_msgs::msg::Path &path) override

nav2_core setPlan - Sets the global plan.

Parameters:

path – The global plan

virtual void setSpeedLimit(const double &speed_limit, const bool &percentage) override

Limits the maximum linear speed of the robot.

Parameters:
  • speed_limit – expressed in absolute value (in m/s) or in percentage from maximum robot speed

  • percentage – setting speed limit in percentage if true or in absolute values in false case

Protected Functions

bool simulateTrajectory(const geometry_msgs::msg::PoseStamped &motion_target, const geometry_msgs::msg::TransformStamped &costmap_transform, nav_msgs::msg::Path &trajectory, geometry_msgs::msg::TwistStamped &cmd_vel, bool backward)

Simulate trajectory calculating in every step the new velocity command based on a new curvature value and checking for collisions.

Parameters:
  • motion_target – Motion target point (in costmap local frame?)

  • costmap_transform – Transform between global and local costmap

  • trajectory – Simulated trajectory

  • cmd_vel – Initial command velocity during simulation

  • backward – Flag to indicate if the robot is moving backward

Returns:

true if the trajectory is collision free, false otherwise

geometry_msgs::msg::Twist rotateToTarget(double angle_to_target)

Rotate the robot to face the motion target with maximum angular velocity.

Parameters:

angle_to_target – Angle to the motion target

Returns:

geometry_msgs::msg::Twist Velocity command

bool inCollision(const double &x, const double &y, const double &theta)

Checks if the robot is in collision.

Parameters:
  • x – The x coordinate of the robot in global frame

  • y – The y coordinate of the robot in global frame

  • theta – The orientation of the robot in global frame

Returns:

Whether in collision

void computeDistanceAlongPath(const std::vector<geometry_msgs::msg::PoseStamped> &poses, std::vector<double> &distances)

Compute the distance to each pose in a path.

Parameters:
  • poses – Poses to compute distances with

  • distances – Computed distances

void validateOrientations(std::vector<geometry_msgs::msg::PoseStamped> &path)

Control law requires proper orientations, not all planners provide them.

Parameters:

path – Path to add orientations into, if required

Protected Attributes

std::shared_ptr<tf2_ros::Buffer> tf_buffer_
std::string plugin_name_
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_
std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D*>> collision_checker_
rclcpp::Logger logger_ = {rclcpp::get_logger("GracefulController")}
Parameters *params_
double goal_dist_tolerance_
bool goal_reached_
bool do_initial_rotation_
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> transformed_plan_pub_
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> local_plan_pub_
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseStamped>> motion_target_pub_
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>> slowdown_pub_
std::unique_ptr<nav2_graceful_controller::PathHandler> path_handler_
std::unique_ptr<nav2_graceful_controller::ParameterHandler> param_handler_
std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_