【autoware】障碍物停止逻辑

本文详细描述了一个名为obstacle_stop_planner的节点,用于处理自动驾驶中的障碍物停止规划。它在接收到输入消息时,通过等待并同步必要的数据,如感知对象、障碍物点云、当前速度和加速度等,然后进行轨迹处理、障碍物搜索和插入减速点,确保车辆安全行驶。
摘要由CSDN通过智能技术生成

包名:planning-obstacle_stop_planner

核心代码和逻辑

void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_msg)
{
  // 加锁,防止多线程访问
  mutex_.lock();
  // NOTE: these variables must not be referenced for multithreading
  const auto vehicle_info = vehicle_info_;
  const auto stop_param = stop_param_;
  const auto obstacle_ros_pointcloud_ptr = obstacle_ros_pointcloud_ptr_;
  const auto object_ptr = object_ptr_;
  const auto current_odometry_ptr = current_odometry_ptr_;
  const auto current_acceleration_ptr = current_acceleration_ptr_;
  mutex_.unlock();

  // 等待 necessary data
  {
    const auto waiting = [this](const auto & str) {
      RCLCPP_WARN_THROTTLE(
        get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(), "waiting for %s ...",
        str);
    };

    // 等待 perception object
    if (!object_ptr && node_param_.use_predicted_objects) {
      waiting("perception object");
      return;
    }

    // 等待 obstacle pointcloud
    if (!obstacle_ros_pointcloud_ptr && !node_param_.use_predicted_objects) {
      waiting("obstacle pointcloud");
      return;
    }

    // 等待 current velocity
    if (!current_odometry_ptr) {
      waiting("current velocity");
      return;
    }

    // 等待 current acceleration
    if (!current_acceleration_ptr) {
      waiting("current acceleration");
      return;
    }

    // 检查输入消息是否为空
    if (input_msg->points.empty()) {
      return;
    }
  }

  // 获取当前速度和加速度
  const auto current_vel = current_odometry_ptr->twist.twist.linear.x;
  const auto current_acc = current_acceleration_ptr->accel.accel.linear.x;

  // TODO(someone): support backward path
  // 检查是否为forward path
  const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(input_msg->points);
  is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_;
  if (!is_driving_forward_) {
    RCLCPP_WARN_THROTTLE(
      get_logger(), *get_clock(), 3000, "Backward path is NOT supported. publish input as it is.");
    pub_trajectory_->publish(*input_msg);
    return;
  }

  // 初始化PlannerData
  PlannerData planner_data{};

  // 获取当前pose
  planner_data.current_pose = current_odometry_ptr_->pose.pose;

  // 将输入消息转换为Trajectory和TrajectoryPoints
  Trajectory output_trajectory = *input_msg;
  TrajectoryPoints output_trajectory_points =
    motion_utils::convertToTrajectoryPointArray(*input_msg);

  // trim trajectory from self pose
  // 去除自车自身
  TrajectoryPoints base_trajectory = trimTrajectoryWithIndexFromSelfPose(
    motion_utils::convertToTrajectoryPointArray(*input_msg), planner_data.current_pose,
    planner_data.trajectory_trim_index);

  // extend trajectory to consider obstacles after the goal
  // 扩展轨迹,以考虑障碍物
  if (stop_param.enable_stop_behind_goal_for_obstacle) {
    base_trajectory = extendTrajectory(base_trajectory, stop_param.max_longitudinal_margin);
  }
  // decimate trajectory for calculation cost
  // 降采样轨迹,以减少计算成本
  const auto decimate_trajectory = decimateTrajectory(
    base_trajectory, stop_param.step_length, planner_data.decimate_trajectory_index_map);

  // 使用预测对象搜索障碍物
  if (node_param_.use_predicted_objects) {
    searchPredictedObject(
      decimate_trajectory, output_trajectory_points, planner_data, input_msg->header, vehicle_info,
      stop_param);
  } else {
    // search obstacles within slow-down/collision area
    // 搜索障碍物,以考虑慢速/碰撞区域
    searchObstacle(
      decimate_trajectory, output_trajectory_points, planner_data, input_msg->header, vehicle_info,
      stop_param, obstacle_ros_pointcloud_ptr);
  }

  // insert slow-down-section/stop-point
  // 插入慢速/停止点
  insertVelocity(
    output_trajectory_points, planner_data, input_msg->header, vehicle_info, current_acc,
    current_vel, stop_param);

  // 检查是否需要慢速
  const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_;
  if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_) {
    // 重置外部速度限制
    resetExternalVelocityLimit(current_acc, current_vel);
  }

  // 将输出轨迹转换为Trajectory
  auto trajectory = motion_utils::convertToTrajectory(output_trajectory_points);
  // 发布调试数据
  publishDebugData(planner_data, current_acc, current_vel);

  // 添加头部
  trajectory.header = input_msg->header;
  // 发布轨迹
  pub_trajectory_->publish(trajectory);
}

  • 5
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值