TrajectoryVehicle.hpp

Go to the documentation of this file.

// ----------------- BEGIN LICENSE BLOCK ---------------------------------
//
// Copyright (C) 2020-2021 Intel Corporation
//
// SPDX-License-Identifier: LGPL-2.1-only
//
// ----------------- END LICENSE BLOCK -----------------------------------
#pragma once
 
#include <ad/geometry/DebugDrawing.hpp>
#include <ad/geometry/GeometryOperation.hpp>
#include <ad/geometry/Types.hpp>
#include <ad/physics/RatioValue.hpp>
#include <algorithm>
#include <vector>
#include "ad/rss/core/Physics.hpp"
#include "ad/rss/core/RelativeObjectState.hpp"
#include "ad/rss/unstructured/TrajectoryCommon.hpp"
 
namespace ad {
namespace rss {
namespace unstructured {
 
class TrajectoryVehicle
{
public:
  TrajectoryVehicle()
  {
  }
 
  bool calculateTrajectorySets(core::RelativeObjectState const &vehicleState,
                               ::ad::geometry::Polygon &brakePolygon,
                               ::ad::geometry::Polygon &continueForwardPolygon);
 
private:
  ad::physics::AngularVelocity calculateYawRate(ad::physics::AngularVelocity const &yaw_rate,
                                                ad::physics::Duration const &timeInMovementUntilResponseTime,
                                                ad::physics::AngularAcceleration const &maxYawRateChange,
                                                ad::physics::RatioValue const &ratio) const;
 
  bool getResponseTimeTrajectoryPoints(core::RelativeObjectState const &vehicleState,
                                       TrajectorySetStep &frontSide,
                                       TrajectorySetStep &backSide) const;
 
  bool getResponseTimeTrajectoryPoints(core::RelativeObjectState const &vehicleState,
                                       physics::Acceleration const &acceleration,
                                       physics::RatioValue const &ratioDiff,
                                       TrajectoryPoint::SpeedMode const &speedMode,
                                       TrajectorySetStep &step) const;
 
  bool calculateTrajectoryPoint(TrajectoryPoint const &currentPoint,
                                world::RssDynamics const &dynamics,
                                ad::physics::Duration const &duration,
                                ad::physics::Acceleration const &acceleration,
                                ad::physics::RatioValue const &yawRateChangeRatio,
                                TrajectoryPoint &resultTrajectoryPoint) const;
 
  bool calculateTrajectoryPointOnCircle(TrajectoryPoint &currentPoint,
                                        physics::Acceleration const &acceleration,
                                        physics::Duration const &duration,
                                        ::ad::rss::world::RssDynamics const &dynamics) const;
 
  bool getTimeInMovement(ad::physics::Speed const &speed,
                         ad::physics::Acceleration const &acceleration,
                         ad::physics::Duration &timeInMovement) const;
 
  bool calculateBrake(core::RelativeObjectState const &vehicleState,
                      ad::physics::Duration const &timeAfterResponseTimeSpeedMax,
                      TrajectorySetStep const &responseTimeFrontSide,
                      ad::physics::Duration const &timeAfterResponseTimeSpeedMin,
                      TrajectorySetStep const &responseTimeBackSide,
                      ::ad::geometry::Polygon &resultPolygon,
                      TrajectorySetStepVehicleLocation &brakeMinStepVehicleLocation) const;
 
  bool calculateContinueForward(core::RelativeObjectState const &vehicleState,
                                physics::Duration const &timeAfterResponseTime,
                                TrajectorySetStep const &responseTimeFrontSide,
                                ::ad::geometry::Polygon const &brakePolygon,
                                TrajectorySetStepVehicleLocation const &brakeMinStepVehicleLocation,
                                ::ad::geometry::Polygon &resultPolygon) const;
 
  bool calculateTrajectorySetStepOnCircle(core::RelativeObjectState const &vehicleState,
                                          physics::Duration const &timeAfterResponseTime,
                                          physics::Acceleration const &acceleration,
                                          TrajectorySetStep &step) const;
};
 
} // namespace unstructured
} // namespace rss
} // namespace ad