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