Merge branch 'master' into mult_control

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_;
 

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_;