parking_planner_types.hpp

Go to the documentation of this file.

// Copyright 2020 Embotech AG, Zurich, Switzerland. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef PARKING_PLANNER__PARKING_PLANNER_TYPES_HPP_
#define PARKING_PLANNER__PARKING_PLANNER_TYPES_HPP_

#include <stdint.h>
#include <vector>
#include <string>

#include "geometry.hpp"

namespace autoware
{

namespace motion
{

namespace planning
{

namespace parking_planner
{
// -- VehicleState ------------------------------------------------------------
template<typename T>
class VehicleState
{
public:
  //         This is used for example in constructs such as VehicleState{}.
  VehicleState() noexcept
  : VehicleState({}, {}, {}, {}, {}) {}

  VehicleState(T x, T y, T velocity, T heading, T steering) noexcept
  {
    m_position_x = x;
    m_position_y = y;
    m_velocity = velocity;
    m_heading = heading;
    m_steering = steering;
  }

  std::vector<T> serialize(void) const
  {
    return std::vector<T>{m_position_x, m_position_y, m_velocity, m_heading, m_steering};
  }

  // \brief Turn a previously serialized state vector back into a VehicleState object
  static VehicleState deserialize(std::vector<T> serialized_states)
  {
    if (serialized_states.size() != VehicleState::internal_states_number) {
      throw std::length_error {"Need a vector of length " +
              std::string{VehicleState::internal_states_number}
      };
    }

    return VehicleState(
      serialized_states[0], serialized_states[1], serialized_states[2],
      serialized_states[3],
      serialized_states[4]);
  }

  // Getters and setters, nothing fancy now but may get checks later
  T get_x() const noexcept {return m_position_x;}
  T get_y() const noexcept {return m_position_y;}
  T get_velocity() const noexcept {return m_velocity;}
  T get_heading() const noexcept {return m_heading;}
  T get_steering() const noexcept {return m_steering;}
  void set_x(T x) noexcept {m_position_x = x;}
  void set_y(T y) noexcept {m_position_y = y;}
  void set_velocity(T velocity) noexcept {m_velocity = velocity;}
  void set_heading(T heading) noexcept {m_heading = heading;}
  void set_steering(T steering) noexcept {m_steering = steering;}

  static constexpr std::size_t get_serialized_length() noexcept
  {
    return internal_states_number;
  }

  bool operator==(const VehicleState<T> & other) const noexcept
  {
    return (this->m_position_x == other.m_position_x) &&
           (this->m_position_y == other.m_position_y) &&
           (this->m_velocity == other.m_velocity) &&
           (this->m_heading == other.m_heading) &&
           (this->m_steering == other.m_steering);
  }

private:
  static constexpr std::size_t internal_states_number = 5;

  T m_position_x;

  T m_position_y;

  T m_velocity;

  T m_heading;

  T m_steering;
};


// -- VehicleCommand ----------------------------------------------------------
template<typename T>
class VehicleCommand
{
public:
  // No-input constructor sets everything to the zero of the template parameter.
  // This is used when one creates empty std::vectors of those types.
  VehicleCommand() noexcept
  : VehicleCommand({}, {}) {}

  VehicleCommand(T steering_rate, T throttle) noexcept
  {
    m_steering_rate = steering_rate;
    m_throttle = throttle;
  }

  std::vector<T> serialize(void) const
  {return std::vector<T>{m_steering_rate, m_throttle};}

  static VehicleCommand deserialize(std::vector<T> serialized_commands)
  {
    if (serialized_commands.size() != VehicleCommand::internal_commands_number) {
      throw std::length_error {"Need a vector of length " +
              std::string{VehicleCommand::internal_commands_number}
      };
    }

    return VehicleCommand(serialized_commands[0], serialized_commands[1]);
  }

  T get_steering_rate() const noexcept {return m_steering_rate;}
  T get_throttle() const noexcept {return m_throttle;}
  void set_steering_rate(T steering_rate) noexcept {m_steering_rate = steering_rate;}
  void set_throttle(T throttle) noexcept {m_throttle = throttle;}

  static constexpr std::size_t get_serialized_length() noexcept
  {return internal_commands_number;}

private:
  static constexpr std::size_t internal_commands_number = 2;

  T m_steering_rate;

  T m_throttle;
};


// -- PlanningResult ----------------------------------------------------------
template<typename T>
class TrajectoryStep
{
public:
  TrajectoryStep(const VehicleCommand<T> command, const VehicleState<T> state) noexcept
  {
    m_command = command;
    m_state = state;
  }

  VehicleState<T> get_state() const noexcept {return this->m_state;}
  VehicleCommand<T> get_command() const noexcept {return this->m_command;}

private:
  VehicleCommand<T> m_command;
  VehicleState<T> m_state;
};

template<typename T>
using Trajectory = std::vector<TrajectoryStep<T>>;

}  // namespace parking_planner
}  // namespace planning
}  // namespace motion
}  // namespace autoware

#endif  // PARKING_PLANNER__PARKING_PLANNER_TYPES_HPP_