nav2 基于插件的控制 + pp_controller

nav2_controller

on_configure

• 1、读取插件参数

  • progress_checker_ids_、goal_checker_ids_、controller_ids_

• 2、控制参数

  • 控制周期，最小速度阈值(x,y,theta)
  • 速度限制，失败容忍，使用实时优先级，发布零速度

• 3、costmap 配置，并 独立线程运行

  • costmap_thread_ = std::make_unique<nav2::NodeThread>(costmap_ros_);

• 4、控制类型、目标检测类型、进度检测类型 插件加载

• 5、订阅里程计，发布 “cmd_vel”

• 6、代价地图 更新超时参数读取

  • costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl)

• 7、创建动作服务器（我们使用 followPath 方法实现的），如果用户没有实时权限，则可能会由于实时优先级而抛出此错误

  •   try {action_server_ = create_action_server<Action>("follow_path",std::bind(&ControllerServer::computeControl, this),nullptr,std::chrono::milliseconds(500),true /*spin thread*/, use_realtime_priority_ /*soft realtime*/);} catch (const std::runtime_error & e) {RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what());on_cleanup(state);return nav2::CallbackReturn::FAILURE;}
    

• 8、订阅 速度限制 topic

  •   speed_limit_sub_ = create_subscription<nav2_msgs::msg::SpeedLimit>(speed_limit_topic,std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1));
    

speedLimitCallback

• 迭代 遍历控制器，设置 速度限制

•   ControllerMap::iterator it;for (it = controllers_.begin(); it != controllers_.end(); ++it) {it->second->setSpeedLimit(msg->speed_limit, msg->percentage);}
  

computeControl

• 1、取当前 goal，根据 goal中 控制Id、目标检测Id、进程检测Id 确定对应的value

  • 提供的 每种Id 都进行校验，无效则抛出异常

• 2、设置路径 setPlannerPath

  • setPlannerPath

    • 控制器 设置路径：controllers_[current_controller_]->setPlan(path);
    • 目标检测器 重置：goal_checkers_[current_goal_checker_]->reset();
    • 保留当前路径：current_path_ = path

• 3、进度检测器 重置： progress_checkers_[current_progress_checker_]->reset();

• 4、根据控制周期 + 独立线程进行控制 while (rclcpp::ok()) {

  • 记录 开始时间 start_time = this->now()

  • 若 action_server 为空 或者不活跃时，直接 return

  • 若 action_server 正在关闭时，控制器 进行关闭

    • 若控制器已经关闭，则 onGoalExit(true)

      • void onGoalExit(bool force_stop)

        • 若需要发布零速 且 强制停止
          • publishZeroVelocity
        • 重置 控制器

    • 否则，打印 等待控制器关闭 日志

  • 在代价地图有效之前（清除代价地图之后）无需计算轨迹

    • 检测当前 代价地图时最新的，是，则跳过
    • 不是最新，等待更新到最新，超出 最大时间抛出异常

  • 更新全局路径 updateGlobalPath()

    • 若有 action_server_->is_preempt_requested()

      • is_preempt_requested()

        • 允许 且 有 新的goal 时，返回 true

      • 取 新goal，根据 goal中 控制Id、目标检测Id、进程检测Id 确定对应的value

      • 设置路径 setPlannerPath

  • 计算和发布速度 computeAndPublishVelocity()

  • 判断是否到点 isGoalReached()

    • 到点后，则 break

  • 根据控制周期进行延时

    • 计算控制耗时，auto cycle_duration = this->now() - start_time;
    • 基于控制周期计算出延时值，并 sleep

computeAndPublishVelocity

• 1、取机器人位姿，失败，则 抛出异常 getRobotPose

  • if (!costmap_ros_->getRobotPose(current_pose)) {

    ​ return false;}

    pose = current_pose;

    return true;

  • costmap_ros_->getRobotPose(current_pose)

    • tf_buffer_ 读取
    • global_frame_, robot_base_frame_, transform_tolerance_

• 2、进程检测器 检测位姿

  • progress_checkers_[current_progress_checker_]->check(pose)

    • 1、前一位姿 与 当前位姿 距离差 小于某一阈值
    • 2、前一位姿 与 当前位姿 角度差 小于某一阈值
    • 3、前一位姿 与 当前位姿 时间差 小于某一阈值

  • 若 检测不符合条件，则抛出异常

• 3、里程计 得到 线速度

• 4、控制器 调用 计算速度指令 computeVelocityCommands

  • 采用 try {} catch(const std::runtime_error& e)
  • 若异常，则 控制指令置0
    • 检测 异常持续时间，若时间过长，则 抛出异常

• 5、发布速度 publishVelocity(cmd_vel_2d);

  • 校验速度指令：有 Nans or Infs，忽略

    若 速度发布器活跃 且有接受

    • 则 发布该速度

• 6、在全局路径上找到与当前姿势最接近的姿势，并发送

  • 控制 中期回调相应：
    • 当前发布的速度 cmd_vel_2d
    • 当前距目标的距离：distance_to_goal
      • 先找到 当前Pose 距离 current_path_ 最近的 index
      • 基于 该index 计算 剩余路径
    • action_server_->publish_feedback(feedback);

isGoalReached

• 1、取 当前机器人位姿 pose
• 2、得到当前 速度 velocity
• 3、基于 位姿+目标位姿+速度 进行判断 isGoalReached

SimpleGoalChecker::isGoalReached

• 若 检测 check_xy_
  • 当前位姿 与 目标位姿 距离超出阈值，则 return false
• 角度 检测
  • 当前位姿 与 目标位姿 角度差 小于阈值，则 return true

PositionGoalChecker::isGoalReached

• 位姿校验

End

PP_Controller

RegulatedPurePursuitController

computeVelocityCommands

  /*** @brief 根据当前姿态和速度计算最佳命令，并附带可能的调试信息** 与上述 computeVelocityCommands 相同，但包含调试结果。* 如果结果指针不为空，则有关扭曲的附加信息* 将在调用后的结果中显示。** @param pose 当前机器人姿态* @param speed 当前机器人速度* @param goal_checker 指向此任务的目标检查器的指针，以便在计算命令时有用* @return 最佳命令*/geometry_msgs::msg::TwistStamped computeVelocityCommands(const geometry_msgs::msg::PoseStamped & pose,const geometry_msgs::msg::Twist & velocity,nav2_core::GoalChecker * /*goal_checker*/) override;

步骤：

• 1、检测目标 容忍：GoalChecker::getTolerances(pose_tolerance, vel_tolerance)

• 2、路径转换 路径到机器人坐标系 transformed_plan

• 3、基于速度得到 自适应 前探距离 lookahead_dist

• 4、是否允许后退，修正前探距离

• 5、基于 lookahead_dist 计算前探位姿 carrot_pose

• 6、基于 carrot_pose 计算前探曲率 lookahead_curvature

  • K = 1 / R
  • $K=\frac{2y}{x^2+y^2}$

• 7、若使用固定 前探距离时，重新计算 curvature_lookahead_pose + regulation_curvature

• 8、基于前探距离 确定 速度的方向

• 9、确定满足于基本约束， shouldRotateToGoalHeading or shouldRotateToPath or 正常运行

  • shouldRotateToGoalHeading(carrot_pose)

    • 1、参数中配置 需旋转，否则返回

      2、到目标点距离小于 容忍距离goal_dist_tol_

    • 0、配置标记 is_rotating_to_heading_ = true;

    • 1、计算目标角度 angle_yaw = tf2::getYaw(transformed_path.back().pose.orientation)

    • 2、rotateToHeading

  • shouldRotateToPath(rotate_to_path_carrot_pose, angle_to_heading, x_vel_sign)

    • 1、计算 萝卜点 的角度angle_to_path

      • angle_to_path = atan2(carrot_pose.y,.x)
      • x_vel_sign < 0 ? angle_to_path + PI

      2、参数配置需旋转 且 angle_to_path > min_angle

    • 0、配置标记 is_rotating_to_heading_ = true;

    • 1、rotateToHeading

  • 否则，就正常运行：

    • 0、配置标记 is_rotating_to_heading_ = false;

    • 1、基于曲率，代价，路径、线速度、线速度 flag 确定速度 applyConstraints

      • 1、应用曲率约束规则来控制线速度

        • 平滑且成比例的减速机制 curvatureConstraint

        2、通过接近障碍物来限制线速度

        • costConstraint

        3、最小速度限制 regulated_linear_scaling_min_speed

        4、接近目标点限速 approachVelocityConstraint

        • 与 限速距离成正比，且有最小速度limit

        5、基于外层限速距离限制

        • std::clamp(fabs(linear_vel), 0.0, params_->desired_linear_vel

        6、基于 sign 确定 最终速度

    • 若来关闭信号时：

      • 计算 关闭线速度 linear_vel = speed.linear.x - x_vel_sign * dt * params_->cancel_deceleration;
      • x_vel_sign > 0 ？ linear_vel <= 0) 时，完成关闭，结束关闭

    • 角速度 基于 曲率得出

      • angular_vel = linear_vel * regulation_curvature;

• 10、碰撞检测 进行线速度

  • isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)

旋转到目标

rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed)

• 1 确定期望速度 角速度 = dir * param.angle_vel, 线速度 = 0
  • dir 由于 base系，故 angle_to_path > 0.0 ? 1.0 : -1.0;
• 2 确定 当前可行速度：期望速度 & 最大可行 & 最小可行
  • 可行速度边界 ± 最大加速度 * 控制周期
  • 当前可行速度 = clamp(期望速度,最小可行,最大可行)
• 3、检查是否需要减速以避免超调
  • $v_f=v_i+at$
  • $s=\frac{v_i+v_f}{2}t$
  • 消去时间，得到：$v_i^2=v_f^2-2as$
  • 由目标角为0，有：$v_i=\sqrt{-2as}$

cancel()

bool RegulatedPurePursuitController::cancel()
{// if false then publish zero velocityif (!params_->use_cancel_deceleration) {return true;}cancelling_ = true;return finished_cancelling_;
}

曲率

这个曲率计算公式 $k=\frac{2\times y}{x^2+y^2}$ 是纯跟踪 (Pure Pursuit) 算法中的核心公式之一。

它基于一个简化的几何模型，用于计算机器人从当前位置（通常是后轴中心或车辆几何中心）到前方预瞄点所需遵循的圆弧的曲率。

推导过程：

我们来一步步推导这个公式：

1. 假设：

   • 机器人位于原点 (0,0)，并且其前进方向与 x 轴正方向对齐。这是机器人自身坐标系下的表示。
   • 预瞄点（或目标点，通常称为“胡萝卜点”）位于机器人前方路径上的某处，其在机器人坐标系下的坐标为 (x,y)。
   • 机器人将沿着一个圆弧行驶，该圆弧经过机器人当前位置 (0,0) 和预瞄点 (x,y)。
   • 圆弧的圆心位于 y 轴上。这是纯跟踪算法的一个关键简化，因为它假设机器人通过调整转向角，使得其运动轨迹形成一个以 y 轴上某点为圆心的圆弧。

2. 圆的方程：

   • 由于圆心在 y 轴上，我们可以设圆心坐标为 (0,R)，其中 R 是圆的半径。
   • 圆的方程为 $x^2+(y-R{)}^2=R^2$。

3. 利用机器人位置：

   • 圆弧经过机器人当前位置 (0,0)。将 (0,0) 代入圆的方程：
     • $0^2+(0-R{)}^2=R^2$
     • $R^2=R^2$
   • 这说明机器人当前位置总是在这个圆弧上，这个条件是满足的。

4. 利用预瞄点位置：

   • 圆弧也经过预瞄点 (x,y)。将 (x,y) 代入圆的方程：

     • $x^2+(y-R{)}^2=R^2$
     • $x^2+y^2-2Ry+R^2=R^2$

     两边消去 $R^2$：

     $x^2+y^2-2Ry=0$

5. 解出半径 R：

   • 从上式中，我们可以解出半径 R：
   • $R=\frac{x^2+y^2}{2y}$

6. 计算曲率 k：

   • 曲率 k 定义为半径 R 的倒数：$k=R^{-1}$。
   • 将 R 的表达式代入：
   • $R=\frac{2y}{x^2+y^2}$

为什么选择这个公式？

• 简化模型，易于计算： 这个公式的推导基于一个简单的几何模型，使得曲率的计算非常高效，尤其适合实时机器人控制。
• 直观的物理意义：
  • y 的符号决定了转弯的方向。如果 y 为正（预瞄点在机器人左侧），则曲率为正，机器人应向左转。如果 y 为负（预瞄点在机器人右侧），则曲率为负，机器人应向右转。
  • $x^2+y^2$ 是预瞄点到机器人的距离的平方（即弦长的平方）。距离越大，通常意味着需要更小的曲率（更大的转弯半径），这符合直觉。
• 纯跟踪算法的基础： 纯跟踪算法的核心思想就是通过计算一个合适的曲率来引导机器人驶向预瞄点，而这个公式正是其计算曲率的基石。

注意事项：

• y 不能为零： 当预瞄点恰好位于机器人正前方（即 y = 0）时，根据公式，R 将变为无穷大，k 将变为 0。这表示机器人应该走直线。
• 坐标系： 这个公式是基于预瞄点在机器人自身坐标系下的表示。如果预瞄点是全局坐标，需要先将其转换到机器人局部坐标系下。
• 适用性： 这个公式适用于差分驱动机器人或阿克曼转向车辆的转向控制，通过调整轮速差或前轮转向角来匹配所需的曲率。

总而言之，这个公式是纯跟踪算法中一个巧妙的几何简化，它提供了一种快速且有效的方法来计算机器人向目标点转向所需的曲率。

PathHandle

transformGlobalPlan

/**
@brief 将全局规划变换至与姿态相同的帧，并裁剪不符合前瞻点条件的姿态。如果点符合以下任一条件，则不符合前瞻点的条件：位于本地代价地图之外（无法确保避免碰撞）
* @param pose 待变换的姿态
* @param max_robot_pose_search_dist 搜索匹配最近路径点的距离
* @param rejection_unit_path 如果为 true，则路径只有一个姿态，则失败
* @return 新帧中的路径  
*/
nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose,double max_robot_pose_search_dist, bool reject_unit_path = false);

步骤：

• 1、check
  • 全局位姿没有，则抛出异常 throw InvalidPath(“Received plan with zero length”);
  • 路径必须大于1个时？ 则抛出异常 throwInvalidPath(“Received plan with length of one”);
• 2、将机器人位姿 转到 路径坐标系下
  • global_path.frame_id,pose.frame_id, tf.transform
• 3、找出机器人 查询位姿的上边界 closest_pose_upper_bound
  • first_after_integrated_distance(global_plan, max_robot_pose_search_dist)
• 4、首先找到路径上与机器人最接近的姿势 transformation_begin
  • 查询范围： global_plan_.poses.begin(), closest_pose_upper_bound
  • 当路径转弯时（如果转弯的话），这样我们就不会从路径的后面部分获取姿势
  • 确保路径 上至少有两个点
    • transformation_begin == closest_pose_upper_bound ？
    • transformation_begin = std::prev(std::prev(closest_pose_upper_bound))
• 5、找到 path 中位于代价地图之外的第一个点 transformation_end
• 6、将 [transformation_begin,transformation_end] 转换到 local系，(机器人坐标系)
  • 得到 transformed_plan
• 7、路径中 删除 global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin
• 8、Check路径。

CollisionChecker

isCollisionImminent

  /**
* @brief 是否即将发生碰撞
* @param robot_pose 机器人姿态
* @param cart_pose 胡萝卜姿态
* @param linear_vel 向前投射的线速度
* @param angular_vel 向前投射的角速度
* @param cart_dist 到胡萝卜的距离（用于PP）
* @return 是否即将发生碰撞*/bool isCollisionImminent(const geometry_msgs::msg::PoseStamped &,const double &, const double &,const double &);

步骤：

• 1、检测当前位姿是否在障碍中：inCollision(
  • 在障碍中，则返回 true
• 2、数据显示在 cost_map 中，则坐标系 costmap->getGlobalFrameID
• 3、计算时间步长，分 旋转 or 正常运行：
  • 若为 旋转至目标或路径方向 fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01
    • 计算机器人半径最大部分移动到另一代价地图单元所需的角距离：
    • theta_min = 2.0 * sin ((res/2) / r_max)
    • 通过等腰三角形 r_max, r_max, resolution，除以 angular_velocity 即可得出时间步长。
    • projection_time = 2.0 * sin ((res/2) / r_max) / fabs(angular_vel);
  • 否则 为正常运行：
    • projection_time = res / fabs(linear_vel);
• 4、仅在要求的时间内进行模拟

限速

基于曲率限速 curvatureConstraint

/*** @brief apply curvature constraint regulation on the linear velocity* @param raw_linear_velocity Raw linear velocity desired* @param curvature Curvature of the current command to follow the path* @param min_radius Minimum path radius to apply the heuristic* @return Velocity after applying the curvature constraint*/
inline double curvatureConstraint(const double raw_linear_vel, const double curvature, const double min_radius)
{const double radius = fabs(1.0 / curvature);if (radius < min_radius) {return raw_linear_vel * (1.0 - (fabs(radius - min_radius) / min_radius));} else {return raw_linear_vel;}
}

解析：

• 1、计算当前路径的转弯半径。曲率 (k) 是半径 ® 的倒数，即 k=1/R。

• 2、if (radius >= min_radius)：

  • 说明弯道足够缓，车辆可以按照原始线速度行驶，所以直接 return raw_linear_vel;。

• 3、否则：前的弯道比车辆能够安全转过的最小弯道还要急。在这种情况下，就需要减小线速度。

  • fabs(radius - min_radius): 这部分计算的是当前半径与最小允许半径之间的差值。
  • fabs(radius - min_radius) / min_radius：这部分计算的是差值相对于 min_radius 的比例。它表示当前弯道比最小允许弯道急了多少比例。这个比例越大，说明弯道越急。
  • 1.0 - (fabs(radius - min_radius) / min_radius): 这一项是减速系数。
    • 当 radius 远小于 min_radius 时（非常急的弯），fabs(radius - min_radius) / min_radius 的值会接近 1.0 (甚至更大，如果 radius 接近 0)，那么减速系数就会接近 0 (甚至负值，如果 radius 极其小，需要额外处理，比如钳位到0)。
    • 当 radius 接近 min_radius 时，fabs(radius - min_radius) / min_radius 的值会接近 0，那么减速系数就会接近 1.0。
  • raw_linear_vel * (减速系数): 最终得到调整后的线速度。

为什么选择这个公式？

• 平滑性： 随着当前半径 radius 逐渐减小并远离 min_radius，减速系数会逐渐减小，导致线速度平滑地降低。这比突然将速度降到一个固定值要好，可以避免突兀的减速感。
• 比例性： 减速的幅度与当前弯道急剧程度（即 radius 偏离 min_radius 的程度）成正比。弯道越急，减速幅度越大；弯道越接近最小允许半径，减速幅度越小。
• 直观性： 当 radius 等于 min_radius 时，fabs(radius - min_radius) 为 0，减速系数为 1.0，速度不变。这符合逻辑。
• 避免过冲/打滑： 通过这种方式限制速度，可以确保车辆在进入急弯时拥有足够小的线速度，从而避免因离心力过大而导致路径过冲、侧滑甚至翻车。

潜在的极端情况和考虑：

• curvature 为零 (直线)： 如果 curvature 为 0，那么 1.0 / curvature 会导致除零错误。代码中通常会在此之前对曲率为零的情况进行特殊处理，或者在计算曲率时就已经处理了。
• radius 远小于 min_radius： 如果 radius 非常小（接近 0），比如 0.01，而 min_radius 是 1.0，那么 (fabs(radius - min_radius) / min_radius) 将接近 1.0，甚至大于 1.0。
  • 例如，如果 radius = 0.1, min_radius = 1.0: 减速系数 = 1.0 - (0.9/1.0) = 0.1。
  • 如果 radius = 0.01, min_radius = 1.0: 减速系数 = 1.0 - (0.99/1.0) = 0.01。
  • 这意味着速度会被急剧降低，甚至趋近于零。这在物理上是合理的，因为在极度急弯的情况下，可能需要几乎停止才能通过。但在某些应用中，可能需要对这个减速系数设置一个最小值（例如，不允许速度低于某个非常小的正数，或不允许减速系数为负数），以确保车辆不会完全停止或反向加速。

基于代价地图限速

/*** @brief apply cost constraint regulation on the linear velocity* @param raw_linear_velocity Raw linear velocity desired* @param pose_cost Cost at the robot pose* @param costmap_ros Costmap object to query* @param params Parameters* @return Velocity after applying the curvature constraint*/
inline double costConstraint(const double raw_linear_vel,const double pose_cost,std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,Parameters * params)
{using namespace nav2_costmap_2d;  // NOLINTif (pose_cost != static_cast<double>(NO_INFORMATION) &&pose_cost != static_cast<double>(FREE_SPACE)){const double & inscribed_radius = costmap_ros->getLayeredCostmap()->getInscribedRadius();const double min_distance_to_obstacle =(params->inflation_cost_scaling_factor * inscribed_radius - log(pose_cost) + log(253.0f)) /params->inflation_cost_scaling_factor;if (min_distance_to_obstacle < params->cost_scaling_dist) {return raw_linear_vel *(params->cost_scaling_gain * min_distance_to_obstacle / params->cost_scaling_dist);}}return raw_linear_vel;
}

Costmap

• InflationLayer 的主要作用是在障碍物周围膨胀（inflate）代价，使得机器人倾向于远离障碍物行驶。它使用一个指数衰减函数来计算距离障碍物不同位置的代价。

• Costmap 的代价范围：

• FREE_SPACE (0): 自由空间，没有障碍物。

  INSCRIBED_INFLATED_OBSTACLE (253): 机器人内切圆内的区域，被认为是与障碍物碰撞。

  LETHAL_OBSTACLE (254): 致命障碍，直接碰撞。

  NO_INFORMATION (255): 未知区域。

膨胀层的代价值衰减模型

在膨胀层中，代价值随距离障碍物的增加呈指数衰减，公式为：

• $\text{cost}=\text{INSCRIBED\_INFLATED\_OBSTACLE}\cdot e^{-1\cdot (d-r_{\text{inscribed}})}$
  • INSCRIBED_INFLATED_OBSTACLE是内切半径处的代价值（通常为 253）；
  • λ是代价衰减因子（inflation_cost_scaling_factor）；
  • d是当前点到障碍物的距离；
  • $r_{\text{inscribed}}$是机器人的内切圆半径（inscribed_radius）。

该模型的意义是：

• 当 $d=r_{\text{inscribed}}$ 时，代价值为 253（即内切半径处的最大代价值）；
• 随着 d增大，代价值指数衰减，直至为 0（自由空间）

公式的反向推导

用户代码需要根据代价值 pose_cost反推距离 d。对上述衰减公式取对数并解方程：

• $d=r_{\text{inscribed}}+\frac{\log 253-\log (pose\_cost)}{\lambda }$

接近目标点限速

1、计算 距离目标点及 限速距离 approach_velocity_scaling_dist 的比例 velocity_scaling

2、计算接近速度 approach_vel = constrained_linear_vel * velocity_scaling;

3、最小接近速度 进行限制

/*** @brief Velocity on approach to goal heuristic regulation term* @param constrained_linear_vel Linear velocity already constrained by heuristics* @param path The path plan in the robot base frame coordinates* @param min_approach_velocity Minimum velocity to use on approach to goal* @param approach_velocity_scaling_dist Distance away from goal to start applying this heuristic* @return Velocity after regulation via approach to goal slow-down*/
inline double approachVelocityConstraint(const double constrained_linear_vel,const nav_msgs::msg::Path & path,const double min_approach_velocity,const double approach_velocity_scaling_dist)
{double velocity_scaling = approachVelocityScalingFactor(path, approach_velocity_scaling_dist);double approach_vel = constrained_linear_vel * velocity_scaling;if (approach_vel < min_approach_velocity) {approach_vel = min_approach_velocity;}return std::min(constrained_linear_vel, approach_vel);
}

/**
* @brief 计算接近目标时用于线性速度调节的比例因子
* @param transformed_path 用于计算与目标距离的路径
* @param approach_velocity_scaling_dist 应用启发式算法的最小距离
* @return 以最小距离为单位，将与目标距离缩放到 0.0-1.0 之间的比例*/
inline double approachVelocityScalingFactor(const nav_msgs::msg::Path & transformed_path,const double approach_velocity_scaling_dist)
{using namespace nav2_util::geometry_utils;  // NOLINT// Waiting to apply the threshold based on integrated distance ensures we don't// erroneously apply approach scaling on curvy paths that are contained in a large local costmap.const double remaining_distance = calculate_path_length(transformed_path);if (remaining_distance < approach_velocity_scaling_dist) {auto & last = transformed_path.poses.back();// Here we will use a regular euclidean distance from the robot frame (origin)// to get smooth scaling, regardless of path density.return std::hypot(last.pose.position.x, last.pose.position.y) / approach_velocity_scaling_dist;} else {return 1.0;}
}

End