datetime:2024/03/11 11:35
author:nzb

导航

https://github.com/ros-planning/navigation2.git

分支：humble

节点：3ed4c2d

规划平滑器服务

  • nav2_smoother | Nav2规划平滑器

  • 主要类

    • SimpleSmoother：平滑实现类，继承于nav2_core::Smoother

      • SimpleSmoother：
      • configure：
        • declare_parameter_if_not_declared初始化声明参数
          • name + .tolerance -> 1e-10
          • name + .max_its -> 1000
          • name + .w_data -> 0.2
          • name + .w_smooth -> 0.3
          • name + .do_refinement -> true
      • cleanup：costmap_sub_.reset();
      • activate
        • deactivate
        • smooth：重写函数
          • 调用findDirectionalPathSegments(path)
          • 路径段的索引差值大于9就处理，否则跳过
            • 调用smoothImpl函数对当前路径段进行平滑操作
            • 将处理更改后的路径组装更新到主路径中
      • smoothImpl：平滑实现
        • 循环执行路径平滑的迭代过程
          • 每次迭代，都会检查迭代次数是否超过了设定的最大迭代次数 max_its_。如果超过了最大迭代次数，会发出警告并将路径恢复到上一次迭代的状态last_path ，并调用 updateApproximatePathOrientations 函数更新路径的方向。
          • 每次迭代都会检查当前时间与平滑操作开始时间的差值是否超过了设定的最大平滑时间。如果超过了最大时间，会发出警告并将路径恢复到上一次迭代的状态last_path ，并调用 updateApproximatePathOrientations 函数更新路径的方向。
          • 除了首尾点之外的路径中的每个点。在每个点上，通过使用该点和其相邻点的坐标值进行平滑计算，并更新当前点的坐标。同时计算平滑程度的变化量 change
          • 在平滑后的点进行地图代价检查，确保平滑后的路径不会与障碍物发生碰撞。如果平滑后的路径发生碰撞，则回滚到上一次迭代的状态last_path ，并调用 updateApproximatePathOrientations 函数更新路径的方向。
          • 在每次迭代平滑成功结束时，将 new_path 更新到 last_path，以保存当前的路径状态
        • 如果启用了路径细化选项，并且细化次数未达到上限（4次），则进行额外的路径细化操作，传入new_path递归调用smoothImpl继续平滑。
        • 最后，更新路径的方向，并将平滑后的路径更新到原始路径中，然后返回操作成功的标志。
      • getFieldByDim：获取维度信息xyz的值
      • setFieldByDim：设置维度信息xyz的值

    • SmootherServer：用于托管不同算法的平滑器类，继承于nav2_util::LifecycleNode

      • SmootherServer：构造函数
        • 节点名称：smoother_server
        • 初始化成员变量
          • default_ids_ -> {simple_smoother}
          • default_types_ -> {nav2_smoother::SimpleSmoother}
        • 初始化节点参数
          • costmap_topic -> global_costmap/costmap_raw
          • footprint_topic -> global_costmap/published_footprint
          • robot_base_frame -> base_link
          • transform_tolerance -> 0.1
          • smoother_plugins -> default_ids_
      • on_configure：配置平滑插件，代价地图，初始化odom订阅，速度发布以及平滑路径动作服务
        • 获取参数
          • smoother_plugins，如果==default_ids_声明设置参数：default_ids_[i] + ".plugin" -> default_types_[i]
          • costmap_topic,footprint_topic,transform_tolerance,robot_base_frame
        • 创建代价地图订阅：costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>
        • 创建机器人足迹订阅：footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>
        • 创建碰撞检测：collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>
        • 调用loadSmootherPlugins
        • 创建平滑规划结果发布话题：plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan_smoothed", 1)
        • 创建平滑动作服务：action_server_ = std::make_unique<ActionServer>(shared_from_this(), "smooth_path",...) ，绑定smoothPlan
      • loadSmootherPlugins：从参数文件加载平滑插件
        • 遍历smoother_ids_查找插件：smoother_types_[i] = nav2_util::get_plugin_type_param
        • 创建插件实例：smoother = lp_loader_.createUniqueInstance(smoother_types_[i])
        • 配置插件：smoother->configure
        • 添加到键值map里面
      • on_activate：激活平滑，代价地图，速度发布和平滑路径动作服务
        • plan_publisher_->on_activate()
        • 遍历插件map，激活activate
        • action_server_->activate()
        • 创建连接createBond()
      • on_deactivate：激活平滑，代价地图，速度发布和平滑路径动作服务，在调用该方法前，速度会被设置成0
        • action_server_->deactivate()
        • 遍历插件map，激活deactivate
        • plan_publisher_->on_deactivate()
        • 销毁连接destroyBond()
      • on_cleanup：平滑和代价地图清理，其余变量reset，插件reset
      • on_shutdown：节点关闭
      • smoothPlan：平滑路径动作服务回调函数，处理动作更新并轮询服务直到到达目标
        • 调用findSmootherId，不存在直接结束返回
        • goal = action_server_->get_current_goal(); result->path = goal->path
        • 调用插件result->was_completed=smoothers_[current_smoother_]->smooth(result->path, ...)
        • 调用plan_publisher_->publish(result->path)发布平滑后路径
        • 如果开启了碰撞检测，遍历路径点，调用collision_checker_->isCollisionFree(pose2d, fetch_data)，存在碰撞直接结束返回
        • 完成返回
      • findSmootherId：查找给定请求有效的平滑ID名称
        • 插件map未找到
          • 如果插件map大小为1，传入的名称为空，则使用插件map的第一个
          • 否则返回失败
        • 插件map找到，返回查询的名称

  • 工具smoother_utils.hpp

    • findDirectionalPathSegments：遍历路径数组组装，同方向的段数组
      • 同方向的段：当前点的前一个点和后一个点的向量点积，小于0，则存在拐点，生成一个段
      • 原点旋转的段：如果当前点和后一个点的xy差值都很小，接近于零，但是角度差超过了某个阈值（1e-4），则说明存在原地旋转，生成一个段
    • updateApproximatePathOrientations：更新路径中每个点的方向（orientation），使其指向路径上相邻点之间的方向
      • 选择了第二个和第三个点进行方向比较，是因为这样更加健壮。例如，如果使用第一个点和第二个点，可能会受到初始点方向的影响， 而这个方向有可能并不代表整个路径段的走向。因此，选择第二个和第三个点进行比较可以减少这种影响
  • VelocitySmoother：速度平滑器，继承于nav2_util::LifecycleNode
    • VelocitySmoother：构造函数
      • 节点名称：velocity_smoother
    • findEtaConstraint：求比例因子，它将轴缩放到加速度范围内
      • 计算目标速度与当前速度之间的速度差dv = v_cmd - v_curr。这个速度差代表了需要调整的速度。
      • 根据速度差的正负以及目标速度和当前速度的大小关系，确定应该使用加速度还是减速度来平滑速度过渡。
      • 如果目标速度的绝对值大于等于当前速度的绝对值，并且目标速度与当前速度的符号相同（即速度不会经过0.0），则选择加速度来平滑过渡。否则选择减速度来平滑过渡。
      • 根据选定的加速度或减速度，计算出速度的最大变化量 v_component_max 和最小变化量 v_component_min。这两个变量分别表示了速度变化的上限和下限。
      • 如果速度差超过了最大变化量 v_component_max，则返回最大变化量与速度差之比作为约束系数 eta，以确保速度变化不超过最大变化量。
      • 如果速度差低于最小变化量 v_component_min，则返回最小变化量与速度差之比作为约束系数 eta，以确保速度变化不低于最小变化量。
      • 如果速度变化在最大变化量和最小变化量之间，则返回 -1.0，表示不需要进行速度调整。
    • applyConstraints：应用加速度和比例因子约束，return v_curr + std::clamp(eta * dv, v_component_min, v_component_max)
    • on_configure：重写函数
      • 初始化和声明获取节点参数
        • smoothing_frequency -> 20.0
        • feedback -> OPEN_LOOP：开环闭环
        • scale_velocities -> false
        • max_velocity -> {0.50, 0.0, 2.5}
        • min_velocity -> {-0.50, 0.0, -2.5}
        • max_accel -> {2.5, 0.0, 3.2}
        • max_decel -> {-2.5, 0.0, -3.2}
        • odom_topic -> odom
        • odom_duration -> 0.1
        • deadband_velocity -> {0.0, 0.0, 0.0}
        • velocity_timeout -> 1.0
      • 如果feedback == CLOSED_LOOP：初始化odom 平滑实例odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration_, odom_topic_)
      • 初始化平滑后速度发布实例：smoothed_cmd_pub_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel_smoothed", 1)
      • 初始化速度订阅：cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>("cmd_vel",...)，绑定inputCommandCallback函数
    • on_activate：重写函数
      • smoothed_cmd_pub_->on_activate()
      • 创建定时器：timer_ = create_wall_timer()绑定smootherTimer函数
      • 绑定参数变更回调dynamicParametersCallback
      • 调用createBond()
    • on_deactivate：重写函数
      • 定时器重置
      • 调用smoothed_cmd_pub_->on_deactivate()
      • 调用destroyBond()
    • on_cleanup：重写函数，属性实例重置
    • on_shutdown：重写函数
    • inputCommandCallback：速度指令回调
      • 校验数据：nav2_util::validateTwist(*msg)
      • 属性赋值
    • smootherTimer：平滑定时器
      • 等待第一条速度达到
      • 检测订阅接收的速度是否超时，如果超时，发布0减速
      • 根据反馈控制类型获取当前速度
        • 开环：等于最后的速度last_cmd_
        • 闭环：odom_smoother_->getTwist()
      • 调用std::clamp限制x,y,z速度在指定范围内
      • 如果scale_velocities_==true分布调用findEtaConstraint计算x,y,z的因子
      • 调用applyConstraints计算x,y,z的速度
      • 应用死区速度限制并发布smoothed_cmd_pub_->publish(std::move(cmd_vel))
    • dynamicParametersCallback：参数变更回调

results matching ""

    No results matching ""