doc:add comment
parent
5b4e465fec
commit
416edce6c4
|
|
@ -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<base_local_planner::CostmapModel>(*_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<dynamic_reconfigure::Server<ControllerReconfigureConfig>>(controller_nh);
|
||||
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);
|
||||
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<geometry_msgs::PoseStamped>& 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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue