【autoware】障碍物停止逻辑

最新推荐文章于 2024-07-26 10:10:33 发布

夏染柒玖

最新推荐文章于 2024-07-26 10:10:33 发布

阅读量437

点赞数 5

文章标签： c++ 自动驾驶

本文链接：https://blog.csdn.net/qq_37183161/article/details/135454225

版权

本文详细描述了一个名为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);
}