TrajectoryCommon.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/GeometryOperation.hpp>
#include <ad/geometry/Types.hpp>
#include <ad/physics/Angle.hpp>
#include <ad/physics/Dimension2D.hpp>
#include <ad/physics/Distance.hpp>
#include "ad/rss/core/RelativeObjectState.hpp"
 
namespace ad {
namespace rss {
namespace unstructured {
 
enum class VehicleCorner
{
  frontLeft,
  frontRight,
  backLeft,
  backRight
};
 
struct TrajectoryPoint
{
  TrajectoryPoint()
  {
  }
 
  enum class SpeedMode
  {
    Min,
    Max
  };
 
  explicit TrajectoryPoint(core::RelativeObjectState const &vehicleState, SpeedMode const speedMode)
  {
    position = ::ad::geometry::toPoint(vehicleState.unstructured_object_state.center_point);
    angle = vehicleState.unstructured_object_state.yaw;
    if (speedMode == SpeedMode::Min)
    {
      speed = vehicleState.unstructured_object_state.speed_range.minimum;
    }
    else
    {
      speed = vehicleState.unstructured_object_state.speed_range.maximum;
    }
    yaw_rate = vehicleState.unstructured_object_state.yaw_rate;
  }
 
  TrajectoryPoint(::ad::geometry::Point const &inPoint,
                  physics::Angle const &inAngle,
                  physics::Speed const &inSpeed,
                  physics::AngularVelocity const &inYawRate)
    : position(inPoint)
    , speed(inSpeed)
    , angle(inAngle)
    , yaw_rate(inYawRate)
  {
  }
 
  bool operator==(const TrajectoryPoint &other) const
  {
    return (position == other.position) && (speed == other.speed) && (angle == other.angle)
      && (yaw_rate == other.yaw_rate);
  }
 
  bool operator!=(const TrajectoryPoint &other) const
  {
    return !operator==(other);
  }
 
  ::ad::geometry::Point position;
 
  physics::Speed speed;
 
  physics::Angle angle;
 
  physics::AngularVelocity yaw_rate;
};
 
struct TrajectorySetStep
{
  TrajectorySetStep()
  {
  }
 
  TrajectorySetStep(TrajectoryPoint const &inLeft, TrajectoryPoint const &inRight, TrajectoryPoint const &inCenter)
    : center(inCenter)
  {
    left.push_back(inLeft);
    right.push_back(inRight);
  }
 
  bool operator==(const TrajectorySetStep &other) const
  {
    return (left == other.left) && (right == other.right) && (center == other.center);
  }
 
  bool operator!=(const TrajectorySetStep &other) const
  {
    return !operator==(other);
  }
 
  std::vector<TrajectoryPoint> left;  // with positive yaw rate ratio
  std::vector<TrajectoryPoint> right; // with negative yaw rate ratio
  TrajectoryPoint center;
};
 
::ad::geometry::Point getVehicleCorner(TrajectoryPoint const &point,
                                       core::RelativeObjectState const &vehicleState,
                                       VehicleCorner const corner);
 
struct TrafficParticipantLocation
{
  TrafficParticipantLocation()
  {
  }
 
  TrafficParticipantLocation(TrajectoryPoint const &pt, core::RelativeObjectState const &vehicleState)
  {
    frontLeft = getVehicleCorner(pt, vehicleState, VehicleCorner::frontLeft);
    frontRight = getVehicleCorner(pt, vehicleState, VehicleCorner::frontRight);
    backLeft = getVehicleCorner(pt, vehicleState, VehicleCorner::backLeft);
    backRight = getVehicleCorner(pt, vehicleState, VehicleCorner::backRight);
  }
 
  ::ad::geometry::Polygon toPolygon() const
  {
    ::ad::geometry::Polygon vehiclePolygon;
    boost::geometry::append(vehiclePolygon, frontRight);
    boost::geometry::append(vehiclePolygon, frontLeft);
    boost::geometry::append(vehiclePolygon, backLeft);
    boost::geometry::append(vehiclePolygon, backRight);
    boost::geometry::append(vehiclePolygon, frontRight);
    return vehiclePolygon;
  }
  ::ad::geometry::MultiPoint toMultiPoint() const
  {
    ::ad::geometry::MultiPoint geometry;
    boost::geometry::append(geometry, frontRight);
    boost::geometry::append(geometry, frontLeft);
    boost::geometry::append(geometry, backLeft);
    boost::geometry::append(geometry, backRight);
    boost::geometry::append(geometry, frontRight);
    return geometry;
  }
 
  bool operator==(TrafficParticipantLocation const &other) const
  {
    return (frontLeft == other.frontLeft) && (frontRight == other.frontRight) && (backLeft == other.backLeft)
      && (backRight == other.backRight);
  }
  bool operator!=(TrafficParticipantLocation const &other) const
  {
    return !(*this == other);
  }
  ::ad::geometry::Point frontLeft;
  ::ad::geometry::Point frontRight;
  ::ad::geometry::Point backLeft;
  ::ad::geometry::Point backRight;
};
 
using Trajectory = std::vector<TrajectoryPoint>;
 
struct TrajectorySetStepVehicleLocation
{
  TrafficParticipantLocation left;
  TrafficParticipantLocation right;
  TrafficParticipantLocation center;
 
  bool operator==(TrajectorySetStepVehicleLocation const &other) const
  {
    return (left == other.left) && (right == other.right) && (center == other.center);
  }
 
  bool operator!=(TrajectorySetStepVehicleLocation const &other) const
  {
    return !(*this == other);
  }
};
 
bool calculateEstimationBetweenSteps(::ad::geometry::Polygon &polygon,
                                     TrajectorySetStepVehicleLocation const &previousVehicleLocation,
                                     TrajectorySetStepVehicleLocation const &currentVehicleLocation,
                                     std::string const &debugNamespace);
bool calculateStepPolygon(core::RelativeObjectState const &vehicleState,
                          TrajectorySetStep const &step,
                          std::string const &debugNamespace,
                          ::ad::geometry::Polygon &polygon,
                          TrajectorySetStepVehicleLocation &stepVehicleLocation);
 
bool calculateFrontAndSidePolygon(core::RelativeObjectState const &vehicleState,
                                  TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
                                  std::vector<TrajectorySetStep> const &sideSteps,
                                  TrajectorySetStep const &front,
                                  std::string const &debugNamespace,
                                  ::ad::geometry::Polygon &resultPolygon,
                                  TrajectorySetStepVehicleLocation &frontSideStepVehicleLocation);
 
bool calculateResponseTimePolygon(core::RelativeObjectState const &vehicleState,
                                  TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
                                  std::string const &debugNamespace,
                                  ::ad::geometry::Polygon &resultPolygon);
} // namespace unstructured
} // namespace rss
} // namespace ad