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_