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

                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(

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

     

            


  • 相关阅读:
    controller调用service层报错Invalid bound statement (not found)
    java int类型数据溢出问题 时钟 原码 反码 补码 二进制
    逍遥自在学C语言 | 位运算符&的高级用法
    派生属性-架构案例2020(三十七)
    nested字段聚合
    066:mapboxGL的marker的drag,dragstart,dragend三种触发事件示例
    【UI自动化(uiautomation)】专栏目录介绍导航
    Python读取postgresql数据库
    昨天阅读量900
    中高级前端开发需要掌握的vue知识点
  • 原文地址:https://blog.csdn.net/chilian12321/article/details/126642763