doc:add comment

master
邱棚 2024-07-15 16:05:48 +08:00
parent 5b4e465fec
commit 416edce6c4
1 changed files with 34 additions and 26 deletions

View File

@ -89,13 +89,13 @@ void MpcLocalPlannerROS::reconfigureCollisionCB(CollisionReconfigureConfig& conf
void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)
{ {
// check if the plugin is already initialized // 检查插件是否已经初始化
if (!_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); 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/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/yaw_goal_tolerance", _params.yaw_goal_tolerance, _params.yaw_goal_tolerance);
nh.param("controller/global_plan_overwrite_orientation", _params.global_plan_overwrite_orientation, 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); nh.param("controller/global_plan_viapoint_sep", _params.global_plan_viapoint_sep, _params.global_plan_viapoint_sep);
_controller.setInitialPlanEstimateOrientation(_params.global_plan_overwrite_orientation); _controller.setInitialPlanEstimateOrientation(_params.global_plan_overwrite_orientation);
// special parameters // 特殊参数
nh.param("odom_topic", _params.odom_topic, _params.odom_topic); 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); 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, nh.param("collision_avoidance/collision_check_min_resolution_angular", _params.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_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_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, nh.param("costmap_converter_spin_thread", _costmap_conv_params.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); _obstacles.reserve(700);
// init other variables // 初始化其他变量
_tf = tf; _tf = tf;
_costmap_ros = costmap_ros; _costmap_ros = costmap_ros;
_costmap = _costmap_ros->getCostmap(); // locking should be done in MoveBase. _costmap = _costmap_ros->getCostmap(); // 应在MoveBase中进行锁定
_costmap_model = std::make_shared<base_local_planner::CostmapModel>(*_costmap); _costmap_model = std::make_shared<base_local_planner::CostmapModel>(*_costmap);
_global_frame = _costmap_ros->getGlobalFrameID(); _global_frame = _costmap_ros->getGlobalFrameID();
_robot_base_frame = _costmap_ros->getBaseFrameID(); _robot_base_frame = _costmap_ros->getBaseFrameID();
// create robot footprint/contour model for optimization // 创建用于优化的机器人足迹/轮廓模型
_robot_model = getRobotFootprintFromParamServer(nh, _costmap_ros); _robot_model = getRobotFootprintFromParamServer(nh, _costmap_ros);
// create the planner instance // 创建规划器实例
if (!_controller.configure(nh, _obstacles, _robot_model, _via_points)) if (!_controller.configure(nh, _obstacles, _robot_model, _via_points))
{ {
ROS_ERROR("Controller configuration failed."); ROS_ERROR("Controller configuration failed.");
return; return;
} }
// create visualization instance // 创建规划器实例
_publisher.initialize(nh, _controller.getRobotDynamics(), _global_frame); _publisher.initialize(nh, _controller.getRobotDynamics(), _global_frame);
// Initialize a costmap to polygon converter // 初始化代价地图到多边形的转换器
if (!_costmap_conv_params.costmap_converter_plugin.empty()) if (!_costmap_conv_params.costmap_converter_plugin.empty())
{ {
try try
{ {
_costmap_converter = _costmap_converter_loader.createInstance(_costmap_conv_params.costmap_converter_plugin); _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); 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, "::", "/"); boost::replace_all(converter_name, "::", "/");
_costmap_converter->setOdomTopic(_params.odom_topic); _costmap_converter->setOdomTopic(_params.odom_topic);
_costmap_converter->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name)); _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 else
ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles."); 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(); _footprint_spec = _costmap_ros->getRobotFootprint();
costmap_2d::calculateMinAndMaxDistances(_footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius); 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); _odom_helper.setOdomTopic(_params.odom_topic);
// setup dynamic reconfigure // 设置动态重配置
ros::NodeHandle controller_nh(nh, "controller"); ros::NodeHandle controller_nh(nh, "controller");
dynamic_controller_recfg_ = boost::make_shared<dynamic_reconfigure::Server<ControllerReconfigureConfig>>(controller_nh); dynamic_controller_recfg_ = boost::make_shared<dynamic_reconfigure::Server<ControllerReconfigureConfig>>(controller_nh);
dynamic_reconfigure::Server<ControllerReconfigureConfig>::CallbackType controller_cb = dynamic_reconfigure::Server<ControllerReconfigureConfig>::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); boost::bind(&MpcLocalPlannerROS::reconfigureFootprintCB, this, _1, _2);
dynamic_footprint_recfg_->setCallback(footprint_cb); dynamic_footprint_recfg_->setCallback(footprint_cb);
// validate optimization footprint and costmap footprint // 验证优化足迹和代价地图足迹
validateFootprints(_robot_model->getInscribedRadius(), _robot_inscribed_radius, _controller.getInequalityConstraint()->getMinimumDistance()); 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); _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); _via_points_sub = nh.subscribe("via_points", 1, &MpcLocalPlannerROS::customViaPointsCB, this);
// additional move base params // 其他move_base参数
ros::NodeHandle nh_move_base("~"); ros::NodeHandle nh_move_base("~");
nh_move_base.param("controller_frequency", _params.controller_frequency, _params.controller_frequency); nh_move_base.param("controller_frequency", _params.controller_frequency, _params.controller_frequency);
// set initialized flag // 设置已初始化标志
_initialized = true; _initialized = true;
ROS_DEBUG("mpc_local_planner plugin initialized."); 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 truefalse
*/
bool MpcLocalPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) bool MpcLocalPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
{ {
// check if plugin is initialized // 检查插件是否已初始化
if (!_initialized) if (!_initialized)
{ {
ROS_ERROR("mpc_local_planner has not been initialized, please call initialize() before using this planner"); ROS_ERROR("mpc_local_planner has not been initialized, please call initialize() before using this planner");
return false; return false;
} }
// store the global plan // 存储全局路径计划
_global_plan.clear(); _global_plan.clear();
_global_plan = orig_global_plan; _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. // 这里不清除局部规划器因为每当全局规划器更新计划时setPlan 会被频繁调用。
// the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. // 局部规划器在每次速度计算步骤中检查是否需要重新初始化轨迹。
// reset goal_reached_ flag // 重置目标到达标志
_goal_reached = false; _goal_reached = false;
return true; return true;