ros2 --nav2

 一.路径规划 入口：nav2_planner>src>main.cpp--nav2_planner::PlannerServer

 1.1在配置参数PlannerServer::on_configure里面配置相关插件 获取代价地图

        costmap_ = costmap_ros_->getCostmap();

        创建发布： plan_publisher_

        全局路径规划服务：

        action_server_ = std::make_unique<ActionServer>(

            rclcpp_node_,

            "compute_path_to_pose",

            std::bind(&PlannerServer::computePlan, this));

1.2 服务回调函数：PlannerServer::computePlan()：

        获取目标：goal = action_server_->get_current_goal();

        得到起点：costmap_ros_->getRobotPose(start)

        通过起点终点得到全局路径：result->path = getPlan(start, goal->pose, goal->planner_id);

        发布全局路径：publishPlan(result->path);

1.3全局路径计算：planners_[planner_id]->createPlan(start, goal)  转到

NavfnPlanner::createPlan(

  const geometry_msgs::msg::PoseStamped & start,

  const geometry_msgs::msg::PoseStamped & goal)

通过约束得到路径：makePlan(start.pose, goal.pose, tolerance_, path)

getPointPotential(p.position)获取代价值最优的点作为路径点

---

二。导航控制

1.导航控制器程序入口：nav2_controller>src>main.cpp---nav2_controller::ControllerServer

         1.1在配置参数ControllerServer::on_configure里面配置相关插件和速度，控制频率等参数。

        1.2订阅：odom_sub_     vel_publisher_

        1.3.  主要控制服务ControllerServer::computeControl

        action_server_ = std::make_unique<ActionServer>(

            rclcpp_node_, "follow_path",

            std::bind(&ControllerServer::computeControl, this));

2.控制进程：

        2.1.获取目标得到路径

        setPlannerPath(action_server_->get_current_goal()->path); 

2.2 控制循环：

        是否取消导航：action_server_->is_cancel_requested() 是退出循环

        更新全局路径：updateGlobalPath();

        计算速度并运行：computeAndPublishVelocity();

        是否到达： if (isGoalReached())是退出循环

2.3计算速度并发布速度 computeAndPublishVelocity

        （1）得到当前速度：twist = getThresholdedTwist(odom_sub_->getTwist());

        （2）根据当前位置和速度计算下一步速度：

        cmd_vel_2d =    controllers_[current_controller_]->computeVelocityCommands(

            pose,

            nav_2d_utils::twist2Dto3D(twist))

        （3）发布反馈：action_server_->publish_feedback(feedback);

          （4）：发布速度：publishVelocity(cmd_vel_2d);

2.4更新全局路径通过 findControllerId(goal->controller_id, current_controller)

2.5判断到达目标点：goal_checker_->isGoalReached(pose.pose, end_pose_, velocity);

---

三，局部路径规划

.controllers_[current_controller_]->computeVelocityCommands转到局部路径规划：        dwb_core::DWBLocalPlanner

        根据当前位置和速度计算下一步速度

        DWBLocalPlanner::computeVelocityCommands(

          const geometry_msgs::msg::PoseStamped & pose,

          const geometry_msgs::msg::Twist & velocity)

         computeVelocityCommands(

              nav_2d_utils::poseStampedToPose2D(pose),

              nav_2d_utils::twist3Dto2D(velocity), results);

获取全局路径：prepareGlobalPlan(pose, transformed_plan, goal_pose);

计算各个约束的代价值：critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan)

获取得分最高的局部路径：best = coreScoringAlgorithm(pose.pose, velocity, results);

得到速度，；

发布局部路径，代价栅格：

        pub_->publishLocalPlan(pose.header, best.traj);

         pub_->publishCostGrid(costmap_ros_, critics_);

 

        

---