From 416edce6c40e207b2439d5843b825e598c338663 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Mon, 15 Jul 2024 16:05:48 +0800 Subject: [PATCH] doc:add comment --- .../src/mpc_local_planner_ros.cpp | 60 +++++++++++-------- 1 file changed, 34 insertions(+), 26 deletions(-) diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index dd0214a..ca0ba3a 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp @@ -89,13 +89,13 @@ void MpcLocalPlannerROS::reconfigureCollisionCB(CollisionReconfigureConfig& conf void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) { - // check if the plugin is already initialized + // 检查插件是否已经初始化 if (!_initialized) { - // create Node Handle with name of plugin (as used in move_base for loading) + // 创建一个Node Handle，使用插件的名称（如在move_base中加载） ros::NodeHandle nh("~/" + name); - // load plugin related main parameters + // 加载插件相关的主要参数 nh.param("controller/xy_goal_tolerance", _params.xy_goal_tolerance, _params.xy_goal_tolerance); nh.param("controller/yaw_goal_tolerance", _params.yaw_goal_tolerance, _params.yaw_goal_tolerance); nh.param("controller/global_plan_overwrite_orientation", _params.global_plan_overwrite_orientation, @@ -105,7 +105,7 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm nh.param("controller/global_plan_viapoint_sep", _params.global_plan_viapoint_sep, _params.global_plan_viapoint_sep); _controller.setInitialPlanEstimateOrientation(_params.global_plan_overwrite_orientation); - // special parameters + // 特殊参数 nh.param("odom_topic", _params.odom_topic, _params.odom_topic); nh.param("footprint_model/is_footprint_dynamic", _params.is_footprint_dynamic, _params.is_footprint_dynamic); @@ -118,46 +118,46 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm nh.param("collision_avoidance/collision_check_min_resolution_angular", _params.collision_check_min_resolution_angular, _params.collision_check_min_resolution_angular); - // costmap converter plugin related parameters + // 代价地图转换器插件相关参数 nh.param("costmap_converter_plugin", _costmap_conv_params.costmap_converter_plugin, _costmap_conv_params.costmap_converter_plugin); nh.param("costmap_converter_rate", _costmap_conv_params.costmap_converter_rate, _costmap_conv_params.costmap_converter_rate); nh.param("costmap_converter_spin_thread", _costmap_conv_params.costmap_converter_spin_thread, _costmap_conv_params.costmap_converter_spin_thread); - // reserve some memory for obstacles + // 预留一些内存用于障碍物 _obstacles.reserve(700); - // init other variables + // 初始化其他变量 _tf = tf; _costmap_ros = costmap_ros; - _costmap = _costmap_ros->getCostmap(); // locking should be done in MoveBase. + _costmap = _costmap_ros->getCostmap(); // 应在MoveBase中进行锁定 _costmap_model = std::make_shared(*_costmap); _global_frame = _costmap_ros->getGlobalFrameID(); _robot_base_frame = _costmap_ros->getBaseFrameID(); - // create robot footprint/contour model for optimization + // 创建用于优化的机器人足迹/轮廓模型 _robot_model = getRobotFootprintFromParamServer(nh, _costmap_ros); - // create the planner instance + // 创建规划器实例 if (!_controller.configure(nh, _obstacles, _robot_model, _via_points)) { ROS_ERROR("Controller configuration failed."); return; } - // create visualization instance + // 创建规划器实例 _publisher.initialize(nh, _controller.getRobotDynamics(), _global_frame); - // Initialize a costmap to polygon converter + // 初始化代价地图到多边形的转换器 if (!_costmap_conv_params.costmap_converter_plugin.empty()) { try { _costmap_converter = _costmap_converter_loader.createInstance(_costmap_conv_params.costmap_converter_plugin); std::string converter_name = _costmap_converter_loader.getName(_costmap_conv_params.costmap_converter_plugin); - // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace + // 将'::'替换为'/'，以将C++命名空间转换为NodeHandle命名空间 boost::replace_all(converter_name, "::", "/"); _costmap_converter->setOdomTopic(_params.odom_topic); _costmap_converter->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name)); @@ -179,14 +179,14 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm else ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles."); - // Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. + // 获取机器人的足迹，以及从机器人中心到其足迹顶点的最小和最大距离 _footprint_spec = _costmap_ros->getRobotFootprint(); costmap_2d::calculateMinAndMaxDistances(_footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius); - // init the odom helper to receive the robot's velocity from odom messages + // 初始化里程计助手以从里程计消息中接收机器人的速度 _odom_helper.setOdomTopic(_params.odom_topic); - // setup dynamic reconfigure + // 设置动态重配置 ros::NodeHandle controller_nh(nh, "controller"); dynamic_controller_recfg_ = boost::make_shared>(controller_nh); dynamic_reconfigure::Server::CallbackType controller_cb = @@ -205,20 +205,20 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm boost::bind(&MpcLocalPlannerROS::reconfigureFootprintCB, this, _1, _2); dynamic_footprint_recfg_->setCallback(footprint_cb); - // validate optimization footprint and costmap footprint + // 验证优化足迹和代价地图足迹 validateFootprints(_robot_model->getInscribedRadius(), _robot_inscribed_radius, _controller.getInequalityConstraint()->getMinimumDistance()); - // setup callback for custom obstacles + // 设置自定义障碍物的回调函数 _custom_obst_sub = nh.subscribe("obstacles", 1, &MpcLocalPlannerROS::customObstacleCB, this); - // setup callback for custom via-points + // 设置自定义路径点的回调函数 _via_points_sub = nh.subscribe("via_points", 1, &MpcLocalPlannerROS::customViaPointsCB, this); - // additional move base params + // 其他move_base参数 ros::NodeHandle nh_move_base("~"); nh_move_base.param("controller_frequency", _params.controller_frequency, _params.controller_frequency); - // set initialized flag + // 设置已初始化标志 _initialized = true; ROS_DEBUG("mpc_local_planner plugin initialized."); @@ -229,23 +229,31 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm } } +/** + * @brief 设置全局路径计划 + * + * 该函数用于将全局路径计划设置到局部规划器中。 + * + * @param orig_global_plan 输入的全局路径计划，包含一系列的目标位姿。 + * @return bool 返回true表示设置成功，返回false表示设置失败（如插件未初始化）。 + */ bool MpcLocalPlannerROS::setPlan(const std::vector& orig_global_plan) { - // check if plugin is initialized + // 检查插件是否已初始化 if (!_initialized) { ROS_ERROR("mpc_local_planner has not been initialized, please call initialize() before using this planner"); return false; } - // store the global plan + // 存储全局路径计划 _global_plan.clear(); _global_plan = orig_global_plan; - // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan. - // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. + // 这里不清除局部规划器，因为每当全局规划器更新计划时，setPlan 会被频繁调用。 + // 局部规划器在每次速度计算步骤中检查是否需要重新初始化轨迹。 - // reset goal_reached_ flag + // 重置目标到达标志 _goal_reached = false; return true;