controller_base.hpp

Go to the documentation of this file.

// Copyright 2019 Christopher Ho
//
// 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 CONTROLLER_COMMON__CONTROLLER_BASE_HPP_
#define CONTROLLER_COMMON__CONTROLLER_BASE_HPP_

#include <controller_common/visibility_control.hpp>
#include <motion_common/motion_common.hpp>

#include <chrono>
#include <string>

#define CONTROLLER_COMMON_COPY_MOVE_ASSIGNABLE(Class) \
  Class(const Class &) = default; \
  Class(Class &&) = default; \
  Class & operator=(const Class &) = default; \
  Class & operator=(Class &&) = default; \
  ~Class() = default;

namespace motion
{
namespace control
{
namespace controller_common
{

using motion_common::Index;
using motion_common::Real;
using motion_common::Command;
using motion_common::Diagnostic;
using motion_common::Heading;
using motion_common::Point;
using motion_common::State;
using motion_common::Trajectory;

enum class ControlReference
{
  TEMPORAL,
  SPATIAL
};  // enum class ControlReference

class CONTROLLER_COMMON_PUBLIC BehaviorConfig
{
public:
  BehaviorConfig(
    Real safe_deceleration_rate_mps2,
    std::chrono::nanoseconds time_step,
    ControlReference type);
  CONTROLLER_COMMON_COPY_MOVE_ASSIGNABLE(BehaviorConfig)

  Real safe_deceleration_rate() const noexcept;
  std::chrono::nanoseconds time_step() const noexcept;
  bool is_spatial_reference() const noexcept;
  bool is_temporal_reference() const noexcept;

private:
  Real m_safe_deceleration_rate_mps2;
  std::chrono::nanoseconds m_time_step;
  bool m_is_spatial_reference;
};  // class BehaviorConfig

class CONTROLLER_COMMON_PUBLIC ControllerBase
{
public:
  explicit ControllerBase(const BehaviorConfig & config);
  ControllerBase(const ControllerBase &) = default;
  ControllerBase(ControllerBase &&) = default;
  ControllerBase & operator=(const ControllerBase &) = default;
  ControllerBase & operator=(ControllerBase &&) = default;
  virtual ~ControllerBase() noexcept = default;

  void set_trajectory(const Trajectory & trajectory);
  const Trajectory & get_reference_trajectory() const noexcept;

  Command compute_command(const State & state);

  Command compute_stop_command(const State & state) const noexcept;

  Index get_current_state_spatial_index() const;
  Index get_current_state_temporal_index() const;

  virtual std::string name() const;

  virtual Index get_compute_iterations() const;

  const BehaviorConfig & get_base_config() const noexcept;

protected:
  virtual bool check_new_trajectory(const Trajectory & trajectory) const;
  virtual const Trajectory & handle_new_trajectory(const Trajectory & trajectory);
  virtual Command compute_command_impl(const State & state) = 0;
  Point predict(const Point & point, std::chrono::nanoseconds dt) noexcept;
  void set_base_config(const BehaviorConfig & config) noexcept;

private:
  CONTROLLER_COMMON_LOCAL bool is_state_ok(const State & state) const noexcept;
  CONTROLLER_COMMON_LOCAL void update_reference_indices(const State & new_state) noexcept;
  CONTROLLER_COMMON_LOCAL bool is_past_trajectory(const State & state) const noexcept;
  CONTROLLER_COMMON_LOCAL void set_trajectory_impl();

  BehaviorConfig m_config;
  Trajectory m_reference_trajectory;
  std::chrono::system_clock::time_point m_latest_reference;
  Index m_reference_spatial_index;
  Index m_reference_temporal_index;
};  // class ControllerBase

CONTROLLER_COMMON_PUBLIC void compute_diagnostic(
  const ControllerBase & ctrl,
  const State & state,
  bool use_temporal_reference,  // TODO(c.ho) kind of a bad interface
  Diagnostic & out);

}  // namespace controller_common
}  // namespace control
}  // namespace motion
#endif  // CONTROLLER_COMMON__CONTROLLER_BASE_HPP_