diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp
index 4366c1a1c0c6a5637dd59dbdcf9148b4f562588f..533c96a2c789f390cf7017421faeb8d9af6a02e6 100644
--- a/nav2_controller/include/nav2_controller/nav2_controller.hpp
+++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp
@@ -35,21 +35,75 @@ namespace nav2_controller
{
class ProgressChecker;
-
+/**
+ * @class nav2_controller::ControllerServer
+ * @brief This class hosts variety of plugins of different algorithms to
+ * complete control tasks from the exposed FollowPath action server.
+ */
class ControllerServer : public nav2_util::LifecycleNode
{
public:
typedef std::unordered_map<std::string, nav2_core::LocalPlanner::Ptr> ControllerMap;
+
+ /**
+ * @brief Constructor for nav2_controller::ControllerServer
+ */
ControllerServer();
+ /**
+ * @brief Destructor for nav2_controller::ControllerServer
+ */
~ControllerServer();
protected:
- // The lifecycle interface
+ /**
+ * @brief Configures controller parameters and member variables
+ *
+ * Configures controller plugin and costmap; Initialize odom subscriber,
+ * velocity publisher and follow path action server.
+ * @param state LifeCycle Node's state
+ * @return Success or Failure
+ * @throw pluginlib::PluginlibException When failed to initialize controller
+ * plugin
+ */
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+ /**
+ * @brief Activates member variables
+ *
+ * Activates controller, costmap, velocity publisher and follow path action
+ * server
+ * @param state LifeCycle Node's state
+ * @return Success or Failure
+ */
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+ /**
+ * @brief Deactivates member variables
+ *
+ * Deactivates follow path action server, controller, costmap and velocity
+ * publisher. Before calling deactivate state, velocity is being set to zero.
+ * @param state LifeCycle Node's state
+ * @return Success or Failure
+ */
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+ /**
+ * @brief Calls clean up states and resets member variables.
+ *
+ * Controller and costmap clean up state is called, and resets rest of the
+ * variables
+ * @param state LifeCycle Node's state
+ * @return Success or Failure
+ */
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+ /**
+ * @brief Called when in Shutdown state
+ * @param state LifeCycle Node's state
+ * @return Success or Failure
+ */
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+ /**
+ * @brief Called when in Error state
+ * @param state LifeCycle Node's state
+ * @return Success or Failure
+ */
nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::ComputeControl>;
@@ -57,18 +111,53 @@ protected:
// Our action server implements the ComputeControl action
std::unique_ptr<ActionServer> action_server_;
- // The action server callback
- void computeControl();
-
+ /**
+ * @brief ComputeControl action server callback. Handles action server updates and
+ * spins server until goal is reached
+ *
+ * Provides global path to controller received from action client. Twist
+ * velocities for the robot are calculated and published using controller at
+ * the specified rate till the goal is reached.
+ * @throw nav2_core::PlannerException
+ */
+void computeControl();
+
+ /**
+ * @brief Assigns path to controller
+ * @param path Path received from action server
+ */
void setPlannerPath(const nav_msgs::msg::Path & path);
+ /**
+ * @brief Calculates velocity and publishes to "cmd_vel" topic
+ */
void computeAndPublishVelocity();
+ /**
+ * @brief Calls setPlannerPath method with an updated path received from
+ * action server
+ */
void updateGlobalPath();
+ /**
+ * @brief Calls velocity publisher to publish the velocity on "cmd_vel" topic
+ * @param velocity Twist velocity to be published
+ */
void publishVelocity(const geometry_msgs::msg::TwistStamped & velocity);
+ /**
+ * @brief Calls velocity publisher to publish zero velocity
+ */
void publishZeroVelocity();
+ /**
+ * @brief Checks if goal is reached
+ * @return true or false
+ */
bool isGoalReached();
+ /**
+ * @brief Obtain current pose of the robot
+ * @param pose To store current pose of the robot
+ * @return true if able to obtain current pose of the robot, else false
+ */
bool getRobotPose(geometry_msgs::msg::PoseStamped & pose);
- // The local controller needs a costmap node
+ // The controller needs a costmap node
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
diff --git a/nav2_controller/include/nav2_controller/progress_checker.hpp b/nav2_controller/include/nav2_controller/progress_checker.hpp
index ab9290539e47e899abad6a6bf76ea09de8be27df..fc87316e3f5112efb2e796b4bf7ce365e55a0492 100644
--- a/nav2_controller/include/nav2_controller/progress_checker.hpp
+++ b/nav2_controller/include/nav2_controller/progress_checker.hpp
@@ -21,16 +21,42 @@
namespace nav2_controller
{
-
+/**
+ * @class nav2_controller::ProgressChecker
+ * @brief This class is used to check the position of the robot to make sure
+ * that it is actually progressing towards a goal.
+ */
class ProgressChecker
{
public:
+ /**
+ * @brief Constructor of ProgressChecker
+ * @param node Node pointer
+ */
explicit ProgressChecker(const rclcpp::Node::SharedPtr & node);
+ /**
+ * @brief Checks if the robot has moved compare to previous
+ * pose
+ * @param current_pose Current pose of the robot
+ * @throw nav2_core::PlannerException when failed to make progress
+ */
void check(geometry_msgs::msg::PoseStamped & current_pose);
+ /**
+ * @brief Reset class state upon calling
+ */
void reset() {baseline_pose_set_ = false;}
protected:
+ /**
+ * @brief Calculates robots movement from baseline pose
+ * @param pose Current pose of the robot
+ * @return true, if movement is greater than radius_, or false
+ */
bool is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose);
+ /**
+ * @brief Resets baseline pose with the current pose of the robot
+ * @param pose Current pose of the robot
+ */
void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose);
rclcpp::Node::SharedPtr nh_;