12345qiupeng
/
mpc_local_planner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

doc:add comment

master
邱棚 2024-07-15 16:52:25 +08:00
parent 3b8e1e5c73
commit ff7117c075
1 changed files with 145 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

293
mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h
View File

@ -72,8 +72,8 @@ namespace mpc_local_planner {
/** /**
* @class MpcLocalPlannerROS * @class MpcLocalPlannerROS
* @brief Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract * @brief 实现了 nav_core::BaseLocalPlanner 和 mbf_costmap_core::CostmapController 抽象接口，
* interfaces, so the teb_local_planner plugin can be used both in move_base and move_base_flex (MBF). * 使得 teb_local_planner 插件可以在 move_base 和 move_base_flex (MBF) 中使用。
* @todo Escape behavior, more efficient obstacle handling * @todo Escape behavior, more efficient obstacle handling
*/ */
class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController
@ -92,47 +92,47 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
public: public:
/** /**
* @brief Default constructor of the plugin * @brief 插件的默认构造函数
*/ */
MpcLocalPlannerROS(); MpcLocalPlannerROS();
/** /**
* @brief Destructor of the plugin * @brief 插件的析构函数
*/ */
~MpcLocalPlannerROS(); ~MpcLocalPlannerROS();
/** /**
* @brief Initializes the teb plugin * @brief 初始化 teb 插件
* @param name The name of the instance * @param name 实例的名称
* @param tf Pointer to a tf buffer * @param tf 指向 tf 缓冲区的指针
* @param costmap_ros Cost map representing occupied and free space * @param costmap_ros 表示占用和空闲空间的代价地图
*/ */
void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros); void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
/** /**
* @brief Set the plan that the teb local planner is following * @brief 设置 teb 局部规划器正在跟随的计划
* @param orig_global_plan The plan to pass to the local planner * @param orig_global_plan 要传递给局部规划器的计划
* @return True if the plan was updated successfully, false otherwise * @return 如果计划成功更新，返回 true；否则返回 false
*/ */
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan); bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
/** /**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base * @brief 根据机器人的当前位置、方向和速度计算要发送到基础的速度命令
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base * @param cmd_vel 将填充要传递给机器人底盘的速度命令
* @return True if a valid trajectory was found, false otherwise * @return 如果找到有效的轨迹，返回 true；否则返回 false
*/ */
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel); bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
/** /**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base. * @brief 根据机器人的当前位置、方向和速度计算要发送到基础的速度命令。
* @remark Extended version for MBF API * @remark Extended version for MBF API
* @param pose the current pose of the robot. * @param pose 机器人的当前位置.
* @param velocity the current velocity of the robot. * @param velocity 机器人的当前速度.
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base. * @param cmd_vel 将填充要传递给机器人底盘的速度命令.
* @param message Optional more detailed outcome as a string * @param message 可选的更详细的结果字符串
* @return Result code as described on ExePath action result: * @return 根据 ExePath 操作结果描述的结果代码:
* SUCCESS = 0 * SUCCESS = 0
* 1..9 are reserved as plugin specific non-error results * 1..9 保留为插件特定的非错误结果
* FAILURE = 100 Unspecified failure, only used for old, non-mfb_core based plugins * FAILURE = 100 未指定的失败，仅用于旧的非 mfb_core 基于的插件
* CANCELED = 101 * CANCELED = 101
* NO_VALID_CMD = 102 * NO_VALID_CMD = 102
* PAT_EXCEEDED = 103 * PAT_EXCEEDED = 103
@ -147,17 +147,17 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
* NOT_INITIALIZED = 112 * NOT_INITIALIZED = 112
* INVALID_PLUGIN = 113 * INVALID_PLUGIN = 113
* INTERNAL_ERROR = 114 * INTERNAL_ERROR = 114
* 121..149 are reserved as plugin specific errors * 121..149 保留为插件特定的错误
*/ */
uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity,
geometry_msgs::TwistStamped& cmd_vel, std::string& message); geometry_msgs::TwistStamped& cmd_vel, std::string& message);
/** /**
* @brief Check if the goal pose has been achieved * @brief 检查目标位置是否已达到
* *
* The actual check is performed in computeVelocityCommands(). * 实际检查在 computeVelocityCommands() 中执行。
* Only the status flag is checked here. * 此处仅检查状态标志。
* @return True if achieved, false otherwise * @return 如果已达到，返回 true；否则返回 false
*/ */
bool isGoalReached(); bool isGoalReached();
@ -167,41 +167,41 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
bool isGoalReached(double xy_tolerance, double yaw_tolerance) { return isGoalReached(); }; bool isGoalReached(double xy_tolerance, double yaw_tolerance) { return isGoalReached(); };
/** /**
* @brief Requests the planner to cancel, e.g. if it takes too much time * @brief 请求规划器取消，例如如果它花费的时间过长
* @remark New on MBF API * @remark New on MBF API
* @return True if a cancel has been successfully requested, false if not implemented. * @return 如果成功请求取消，返回 true；如果未实现，返回 false。
*/ */
bool cancel() { return false; }; bool cancel() { return false; };
/** @name Public utility functions/methods */ /** @name 公共实用函数/方法 */
//@{ //@{
/** /**
* @brief Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities. * @brief 将 tf::Pose 类型转换为包含平移和角速度的 Eigen::Vector2d。
* *
* Translational velocities (x- and y-coordinates) are combined into a single translational velocity (first component). * 平移速度（x 和 y 坐标）合并为单个平移速度（第一个分量）。
* @param tf_vel tf::Pose message containing a 1D or 2D translational velocity (x,y) and an angular velocity (yaw-angle) * @param tf_vel 包含 1D 或 2D 平移速度（x, y）和角速度（偏航角）的 tf::Pose 消息
* @return Translational and angular velocity combined into an Eigen::Vector2d * @return 平移和角速度合并为 Eigen::Vector2d
*/ */
static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel); static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel);
/** /**
* @brief Get the current robot footprint/contour model * @brief 获取当前的机器人足迹/轮廓模型
* @param nh const reference to the local ros::NodeHandle * @param nh const reference to the local ros::NodeHandle
* @param costmap_ros pointer to an intialized instance of costmap_2d::Costmap2dROS * @param costmap_ros pointer to an intialized instance of costmap_2d::Costmap2dROS
* @return Robot footprint model used for optimization * @return 用于优化的机器人足迹模型
*/ */
static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros = nullptr); static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros = nullptr);
/** /**
* @brief Get the current robot footprint/contour model * @brief 用于优化的机器人足迹模型
* @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS * @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS
* @return Robot footprint model used for optimization * @return 获取当前的机器人足迹/轮廓模型
*/ */
static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS& costmap_ros); static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS& costmap_ros);
/** /**
* @brief Set the footprint from the given XmlRpcValue. * @brief 从给定的 XmlRpcValue 设置足迹。
* @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros
* @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point
* @param footprint_xmlrpc should be an array of arrays, where the top-level array should have 3 or more elements, and the * @param footprint_xmlrpc should be an array of arrays, where the top-level array should have 3 or more elements, and the
@ -213,7 +213,7 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name); static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name);
/** /**
* @brief Get a number from the given XmlRpcValue. * @brief 从给定的 XmlRpcValue 中获取数值。
* @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros
* @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point
* @param value double value type * @param value double value type
@ -224,7 +224,7 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name); static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name);
/** /**
* @brief Return the internal config mutex * @brief 返回内部配置互斥锁
*/ */
boost::mutex& configMutex() { return config_mutex_; } boost::mutex& configMutex() { return config_mutex_; }
@ -232,112 +232,110 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
protected: protected:
/** /**
* @brief Update internal obstacle vector based on occupied costmap cells * @brief 根据被占用的代价地图单元格更新内部障碍物向量
* @remarks All occupied cells will be added as point obstacles. * @remarks 所有被占用的单元格将被添加为点障碍物
* @remarks All previous obstacles are cleared. * @remarks 所有以前的障碍物都将被清除。
* @sa updateObstacleContainerWithCostmapConverter * @sa updateObstacleContainerWithCostmapConverter
* @todo Include temporal coherence among obstacle msgs (id vector) * @todo 在障碍物消息之间包含时间一致性（id 向量）
* @todo Include properties for dynamic obstacles (e.g. using constant velocity model) * @todo 包括动态障碍物的属性（例如使用恒速模型）
*/ */
void updateObstacleContainerWithCostmap(); void updateObstacleContainerWithCostmap();
/** /**
* @brief Update internal obstacle vector based on polygons provided by a costmap_converter plugin * @brief根据代价地图转换器插件提供的多边形更新内部障碍物向量
* @remarks Requires a loaded costmap_converter plugin. * @remarks 需要加载代价地图转换器插件。
* @remarks All previous obstacles are cleared. * @remarks 需要加载代价地图转换器插件。
* @sa updateObstacleContainerWithCostmap * @sa updateObstacleContainerWithCostmap
*/ */
void updateObstacleContainerWithCostmapConverter(); void updateObstacleContainerWithCostmapConverter();
/** /**
* @brief Update internal obstacle vector based on custom messages received via subscriber * @brief 根据通过订阅者接收到的自定义消息更新内部障碍物向量
* @remarks All previous obstacles are NOT cleared. Call this method after other update methods. * @remarks 所有以前的障碍物不会被清除。请在其他更新方法之后调用此方法。
* @sa updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter * @sa updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter
*/ */
void updateObstacleContainerWithCustomObstacles(); void updateObstacleContainerWithCustomObstacles();
/** /**
* @brief Update internal via-point container based on the current reference plan * @brief 根据当前参考计划更新内部路径点容器
* @remarks All previous via-points will be cleared. * @remarks 所有以前的路径点将被清除。
* @param transformed_plan (local) portion of the global plan (which is already transformed to the planning frame) * @param transformed_plan 已转换到规划框架的全局计划的（局部）部分
* @param min_separation minimum separation between two consecutive via-points * @param min_separation 连续路径点之间的最小间隔
*/ */
void updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation); void updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation);
/** /**
* @brief Callback for the dynamic_reconfigure controller node. * @brief 动态重配置控制器节点的回调函数。
* *
* This callback allows to modify parameters dynamically at runtime without restarting the node * 此回调函数允许在运行时动态修改参数而无需重新启动节点
* @param config Reference to the dynamic reconfigure config * @param config 动态重配置配置的引用
* @param level Dynamic reconfigure level * @param level 动态重配置级别
*/ */
void reconfigureControllerCB(ControllerReconfigureConfig& config, uint32_t level); void reconfigureControllerCB(ControllerReconfigureConfig& config, uint32_t level);
/** /**
* @brief Callback for the dynamic_reconfigure collision node. * @brief 动态重配置碰撞节点的回调函数。
* *
* This callback allows to modify parameters dynamically at runtime without restarting the node * 此回调函数允许在运行时动态修改参数而无需重新启动节点
* @param config Reference to the dynamic reconfigure config * @param config 动态重配置配置的引用
* @param level Dynamic reconfigure level * @param level 动态重配置级别
*/ */
void reconfigureCollisionCB(CollisionReconfigureConfig& config, uint32_t level); void reconfigureCollisionCB(CollisionReconfigureConfig& config, uint32_t level);
/** /**
* @brief Callback for the dynamic_reconfigure footprint collision node. * @brief 动态重配置足迹碰撞节点的回调函数。
* *
* This callback allows to modify parameters dynamically at runtime without restarting the node * 此回调函数允许在运行时动态修改参数而无需重新启动节点
* @param config Reference to the dynamic reconfigure config * @param config 动态重配置配置的引用
* @param level Dynamic reconfigure level * @param level 动态重配置级别
*/ */
void reconfigureFootprintCB(FootprintReconfigureConfig& config, uint32_t level); void reconfigureFootprintCB(FootprintReconfigureConfig& config, uint32_t level);
/** /**
* @brief Callback for custom obstacles that are not obtained from the costmap * @brief 自定义障碍物的回调函数，不是从代价地图获取的
* @param obst_msg pointer to the message containing a list of polygon shaped obstacles * @param obst_msg 包含多边形形状障碍物列表的消息指针
*/ */
void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg); void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);
/** /**
* @brief Callback for custom via-points * @brief 自定义路径点的回调函数
* @param via_points_msg pointer to the message containing a list of via-points * @param via_points_msg 包含路径点列表的消息指针
*/ */
void customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg); void customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg);
/** /**
* @brief Prune global plan such that already passed poses are cut off * @brief 修剪全局计划，使已经通过的位姿被截断
* *
* The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform. * 通过获取最近的 tf 变换，将机器人的位姿转换到全局计划的框架中。
* If no valid transformation can be found, the method returns \c false. * 如果找不到有效的变换，方法返回 \c false.
* The global plan is pruned until the distance to the robot is at least \c dist_behind_robot. * 全局计划被修剪，直到与机器人之间的距离至少为 \c dist_behind_robot.
* If no pose within the specified treshold \c dist_behind_robot can be found, * 如果在指定的阈值 \c dist_behind_robot 内找不到位姿，
* nothing will be pruned and the method returns \c false. * 则不会进行修剪，方法返回 \c false.
* @remarks Do not choose \c dist_behind_robot too small (not smaller the cellsize of the map), otherwise nothing will be pruned. * @remarks 不要将 \c dist_behind_robot 选择得太小（不小于地图的单元格大小），否则不会进行修剪。
* @param tf A reference to a tf buffer * @param tf tf缓冲区的引用
* @param global_pose The global pose of the robot * @param global_pose 机器人的全局位姿
* @param[in,out] global_plan The plan to be transformed * @param[in,out] global_plan 要转换的计划
* @param dist_behind_robot Distance behind the robot that should be kept [meters] * @param dist_behind_robot 应保留在机器人后方的距离（米）
* @return \c true if the plan is pruned, \c false in case of a transform exception or if no pose cannot be found inside the threshold * @return 如果计划被修剪，返回 \c true；在变换异常或未找到位姿时返回 \c false
*/ */
bool pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose, bool pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose,
std::vector<geometry_msgs::PoseStamped>& global_plan, double dist_behind_robot = 1); std::vector<geometry_msgs::PoseStamped>& global_plan, double dist_behind_robot = 1);
/** /**
* @brief Transforms the global plan of the robot from the planner frame to the local frame (modified). * @brief 将机器人的全局计划从规划框架转换为局部框架（修改版）.
* *
* The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h * 该方法替代 base_local_planner/goal_functions.h 中定义的 transformGlobalPlan，
* such that the index of the current goal pose is returned as well as * 返回当前目标位姿的索引以及全局计划和规划框架之间的变换。
* the transformation between the global plan and the planning frame. * @param tf tf 缓冲区的引用
* @param tf A reference to a tf buffer * @param global_plan 要转换的计划
* @param global_plan The plan to be transformed * @param global_pose 机器人的全局位姿
* @param global_pose The global pose of the robot * @param costmap 用于计算变换窗口大小的代价地图引用
* @param costmap A reference to the costmap being used so the window size for transforming can be computed * @param global_frame 要将计划转换到的框架
* @param global_frame The frame to transform the plan to * @param max_plan_length 指定转换计划的最大长度（累计欧几里得距离）[如果 <=0：禁用；长度也受局部代价地图大小的限制！]
* @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also * @param[out] transformed_plan 填充转换后的计划
* bounded by the local costmap size!] * @param[out] current_goal_idx 全局计划中当前（局部）目标位姿的索引
* @param[out] transformed_plan Populated with the transformed plan * @param[out] tf_plan_to_global 全局计划和全局规划框架之间的变换
* @param[out] current_goal_idx Index of the current (local) goal pose in the global plan * @return 如果全局计划被转换，返回 \c true；否则返回 \c false
* @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame
* @return \c true if the global plan is transformed, \c false otherwise
*/ */
bool transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan, bool transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame,
@ -345,94 +343,93 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
geometry_msgs::TransformStamped* tf_plan_to_global = NULL) const; geometry_msgs::TransformStamped* tf_plan_to_global = NULL) const;
/** /**
* @brief Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner. * @brief 估计作为局部规划器局部目标的位姿方向。
* *
* If the current (local) goal point is not the final one (global) * 如果当前（局部）目标点不是最终目标（全局），
* substitute the goal orientation by the angle of the direction vector between * 则通过局部目标和全局计划的后续位姿之间的方向向量的角度替换目标方向。
* the local goal and the subsequent pose of the global plan. * 如果全局规划器不考虑方向，这通常是有用的。 \n
* This is often helpful, if the global planner does not consider orientations. \n * 使用移动平均滤波器来平滑方向。
* A moving average filter is utilized to smooth the orientation. * @param global_plan 全局计划
* @param global_plan The global plan * @param local_goal 当前局部目标
* @param local_goal Current local goal * @param current_goal_idx 全局计划中当前（局部）目标位姿的索引
* @param current_goal_idx Index of the current (local) goal pose in the global plan * @param[out] tf_plan_to_global 全局计划和全局规划框架之间的变换
* @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame * @param moving_average_length 要考虑的全局计划的未来位姿数
* @param moving_average_length number of future poses of the global plan to be taken into account * @return 方向（偏航角）估计
* @return orientation (yaw-angle) estimate
*/ */
double estimateLocalGoalOrientation(const std::vector<geometry_msgs::PoseStamped>& global_plan, const geometry_msgs::PoseStamped& local_goal, double estimateLocalGoalOrientation(const std::vector<geometry_msgs::PoseStamped>& global_plan, const geometry_msgs::PoseStamped& local_goal,
int current_goal_idx, const geometry_msgs::TransformStamped& tf_plan_to_global, int current_goal_idx, const geometry_msgs::TransformStamped& tf_plan_to_global,
int moving_average_length = 3) const; int moving_average_length = 3) const;
/** /**
* @brief Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint * @brief V 验证优化的足迹、障碍物距离和代价地图足迹的当前参数值
* *
* This method prints warnings if validation fails. * 如果验证失败，此方法将打印警告。
* @remarks Currently, we only validate the inscribed radius of the footprints * @remarks 当前，我们仅验证足迹的内切半径
* @param opt_inscribed_radius Inscribed radius of the RobotFootprintModel for optimization * @param opt_inscribed_radius 优化的 RobotFootprintModel 的内切半径
* @param costmap_inscribed_radius Inscribed radius of the footprint model used for the costmap * @param costmap_inscribed_radius 优化的 RobotFootprintModel 的内切半径
* @param min_obst_dist desired distance to obstacles * @param min_obst_dist 期望的障碍物距离
*/ */
void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist); void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist);
private: private:
// Definition of member variables // Definition of member variables
// external objects (store weak pointers) // 外部对象（存储弱指针）
costmap_2d::Costmap2DROS* _costmap_ros; //!< Pointer to the costmap ros wrapper, received from the navigation stack costmap_2d::Costmap2DROS* _costmap_ros; //!< 指向从导航栈接收的代价地图 ros 包装器的指针
costmap_2d::Costmap2D* _costmap; //!< Pointer to the 2d costmap (obtained from the costmap ros wrapper) costmap_2d::Costmap2D* _costmap; //!< 指向 2D 代价地图的指针（从代价地图 ros 包装器获取）
tf2_ros::Buffer* _tf; //!< pointer to tf buffer tf2_ros::Buffer* _tf; //!< tf 缓冲区的指针
// internal objects // 内部对象
Controller _controller; Controller _controller;
ObstContainer _obstacles; //!< Obstacle vector that should be considered during local trajectory optimization ObstContainer _obstacles; //!< 局部轨迹优化期间应考虑的障碍物向量
Publisher _publisher; Publisher _publisher;
std::shared_ptr<base_local_planner::CostmapModel> _costmap_model; std::shared_ptr<base_local_planner::CostmapModel> _costmap_model;
corbo::TimeSeries::Ptr _x_seq = std::make_shared<corbo::TimeSeries>(); corbo::TimeSeries::Ptr _x_seq = std::make_shared<corbo::TimeSeries>();
corbo::TimeSeries::Ptr _u_seq = std::make_shared<corbo::TimeSeries>(); corbo::TimeSeries::Ptr _u_seq = std::make_shared<corbo::TimeSeries>();
std::vector<geometry_msgs::PoseStamped> _global_plan; //!< Store the current global plan std::vector<geometry_msgs::PoseStamped> _global_plan; //!< 存储当前的全局计划
base_local_planner::OdometryHelperRos _odom_helper; //!< Provides an interface to receive the current velocity from the robot base_local_planner::OdometryHelperRos _odom_helper; //!< 提供从机器人接收当前速度的接口
pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> _costmap_converter_loader; //!< Load costmap converter plugins at runtime pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> _costmap_converter_loader; //!< 在运行时加载代价地图转换器插件
boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> _costmap_converter; //!< Store the current costmap_converter boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> _costmap_converter; //!< 存储当前的代价地图转换器
boost::shared_ptr<dynamic_reconfigure::Server<ControllerReconfigureConfig>> boost::shared_ptr<dynamic_reconfigure::Server<ControllerReconfigureConfig>>
dynamic_controller_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime dynamic_controller_recfg_; //!< 动态重配置服务器，允许在运行时修改配置
boost::shared_ptr<dynamic_reconfigure::Server<CollisionReconfigureConfig>> boost::shared_ptr<dynamic_reconfigure::Server<CollisionReconfigureConfig>>
dynamic_collision_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime dynamic_collision_recfg_; //!< 动态重配置服务器，允许在运行时修改配置
boost::shared_ptr<dynamic_reconfigure::Server<FootprintReconfigureConfig>> boost::shared_ptr<dynamic_reconfigure::Server<FootprintReconfigureConfig>>
dynamic_footprint_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime dynamic_footprint_recfg_; //!< 动态重配置服务器，允许在运行时修改配置
ros::Subscriber _custom_obst_sub; //!< Subscriber for custom obstacles received via a ObstacleMsg. ros::Subscriber _custom_obst_sub; //!< 订阅通过 ObstacleMsg 接收的自定义障碍物。
std::mutex _custom_obst_mutex; //!< Mutex that locks the obstacle array (multi-threaded) std::mutex _custom_obst_mutex; //!< 锁定障碍物数组的互斥锁（多线程）
costmap_converter::ObstacleArrayMsg _custom_obstacle_msg; //!< Copy of the most recent obstacle message costmap_converter::ObstacleArrayMsg _custom_obstacle_msg; //!< 最近一次障碍物消息的副本
ViaPointContainer _via_points; ViaPointContainer _via_points;
ros::Subscriber _via_points_sub; //!< Subscriber for custom via-points received via a Path msg. ros::Subscriber _via_points_sub; //!< 订阅通过 Path 消息接收的自定义路径点。
bool _custom_via_points_active = false; //!< Keep track whether valid via-points have been received from via_points_sub_ bool _custom_via_points_active = false; //!< 跟踪是否从 via_points_sub_ 收到有效的路径点
std::mutex _via_point_mutex; //!< Mutex that locks the via_points container (multi-threaded) std::mutex _via_point_mutex; //!< 锁定路径点容器的互斥锁（多线程）
PoseSE2 _robot_pose; //!< Store current robot pose PoseSE2 _robot_pose; //!< 存储当前的机器人位姿
PoseSE2 _robot_goal; //!< Store current robot goal PoseSE2 _robot_goal; //!< 存储当前的机器人目标
geometry_msgs::Twist _robot_vel; //!< Store current robot translational and angular velocity (vx, vy, omega) geometry_msgs::Twist _robot_vel; //!< 存储当前的机器人平移和角速度（vx, vy, omega）
bool _goal_reached = false; //!< store whether the goal is reached or not bool _goal_reached = false; //!< 存储目标是否达到
ros::Time _time_last_infeasible_plan; //!< Store at which time stamp the last infeasible plan was detected ros::Time _time_last_infeasible_plan; //!< 存储上次检测到不可行计划的时间戳
int _no_infeasible_plans = 0; //!< Store how many times in a row the planner failed to find a feasible plan. int _no_infeasible_plans = 0; //!< 存储连续失败的计划次数。
geometry_msgs::Twist _last_cmd; //!< Store the last control command generated in computeVelocityCommands() geometry_msgs::Twist _last_cmd; //!< 存储 computeVelocityCommands() 中生成的最后一个控制命令
ros::Time _time_last_cmd; ros::Time _time_last_cmd;
RobotFootprintModelPtr _robot_model; RobotFootprintModelPtr _robot_model;
std::vector<geometry_msgs::Point> _footprint_spec; //!< Store the footprint of the robot std::vector<geometry_msgs::Point> _footprint_spec; //!< 存储机器人的足迹
double _robot_inscribed_radius; //!< The radius of the inscribed circle of the robot (collision possible) double _robot_inscribed_radius; //!< 机器人的内切圆半径（可能发生碰撞）
double _robot_circumscribed_radius; //!< The radius of the circumscribed circle of the robot double _robot_circumscribed_radius; //!< 机器人的外接圆半径
std::string _global_frame; //!< The frame in which the controller will run std::string _global_frame; //!< The frame in which the controller will run
std::string _robot_base_frame; //!< Used as the base frame id of the robot std::string _robot_base_frame; //!< Used as the base frame id of the robot
// flags // flags
bool _initialized; //!< Keeps track about the correct initialization of this class bool _initialized; //!< 跟踪此类的正确初始化
struct Parameters struct Parameters
{ {
@ -452,7 +449,7 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
} _params; } _params;
boost::mutex config_mutex_; //!< Mutex for config accesses and changes boost::mutex config_mutex_; //!< 用于配置访问和更改的互斥锁
struct CostmapConverterPlugin struct CostmapConverterPlugin
{ {