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

geometry_msgs::msg::PoseStamped getMotionTarget(const double &motion_target_dist, const nav_msgs::msg::Path &path)

Get motion target point.

Parameters:

  • motion_target_dist – Optimal motion target distance

  • path – Current global path

Returns:

Motion target point

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

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

Parameters:

  • robot_pose – Robot pose

  • motion_target – Motion target point

  • costmap_transform – Transform between global and local costmap

  • trajectory – Simulated trajectory

  • 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(const 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

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_
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::PointStamped>> 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_