mpc_controller.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 MPC_CONTROLLER__MPC_CONTROLLER_HPP_
#define MPC_CONTROLLER__MPC_CONTROLLER_HPP_

#include <mpc_controller/visibility_control.hpp>
#include <autoware_auto_msgs/msg/trajectory.hpp>
#include <autoware_auto_msgs/msg/vehicle_control_command.hpp>
#include <autoware_auto_msgs/msg/vehicle_kinematic_state.hpp>

#include <controller_common/controller_base.hpp>
#include <mpc_controller/config.hpp>

#include <ostream>
#include <string>

namespace motion
{
namespace control
{
namespace mpc_controller
{
class MPC_CONTROLLER_PUBLIC MpcController : public controller_common::ControllerBase
{
public:
  constexpr static std::chrono::nanoseconds solver_time_step{std::chrono::milliseconds{100LL}};
  // Disable other constructors, assignment operators: autogenerated code has annoying static
  // variables
  MpcController(const MpcController &) = delete;
  MpcController(MpcController &&) noexcept = delete;
  MpcController & operator=(const MpcController &) = delete;
  MpcController & operator=(MpcController &&) noexcept = delete;

  using Command = autoware_auto_msgs::msg::VehicleControlCommand;
  using State = autoware_auto_msgs::msg::VehicleKinematicState;
  using Trajectory = autoware_auto_msgs::msg::Trajectory;

  explicit MpcController(const Config & config);
  ~MpcController() override = default;

  const Config & get_config() const;
  void set_config(const Config & config);

  const Trajectory & get_computed_trajectory() const noexcept;
  ControlDerivatives get_computed_control_derivatives() const noexcept;

  void debug_print(std::ostream & out) const;

  std::string name() const override;

  Index get_compute_iterations() const override;

protected:
  bool check_new_trajectory(const Trajectory & trajectory) const override;
  const Trajectory & handle_new_trajectory(const Trajectory & trajectory) override;
  Command compute_command_impl(const State & state) override;

private:
  MPC_CONTROLLER_LOCAL void solve();
  MPC_CONTROLLER_LOCAL bool update_references(Index current_idx);
  MPC_CONTROLLER_LOCAL void initial_conditions(const Point & state);
  MPC_CONTROLLER_LOCAL std::chrono::nanoseconds x0_time_offset(const State & state, Index idx);
  MPC_CONTROLLER_LOCAL Command interpolated_command(std::chrono::nanoseconds x0_time_offset);
  MPC_CONTROLLER_LOCAL bool ensure_reference_consistency(Index horizon);
  MPC_CONTROLLER_LOCAL void apply_config(const Config & cfg);
  MPC_CONTROLLER_LOCAL void apply_parameters(const VehicleConfig & cfg) noexcept;
  MPC_CONTROLLER_LOCAL void apply_bounds(const LimitsConfig & cfg) noexcept;
  MPC_CONTROLLER_LOCAL void apply_weights(const OptimizationConfig & cfg);
  MPC_CONTROLLER_LOCAL void apply_nominal_weights(const StateWeight & cfg, Index start, Index end);
  MPC_CONTROLLER_LOCAL void apply_terminal_weights(const Index idx);
  MPC_CONTROLLER_LOCAL
  void set_reference(const Trajectory & traj, Index y_start, Index traj_start, Index count);
  MPC_CONTROLLER_LOCAL void zero_nominal_weights(Index start, Index end);
  MPC_CONTROLLER_LOCAL void zero_terminal_weights() noexcept;
  MPC_CONTROLLER_LOCAL void set_terminal_weights(const StateWeight & cfg) noexcept;
  MPC_CONTROLLER_LOCAL void set_terminal_reference(const Point & pt) noexcept;
  MPC_CONTROLLER_LOCAL void advance_problem(Index count);
  MPC_CONTROLLER_LOCAL void backfill_reference(Index count);

  Config m_config;
  Trajectory::UniquePtr m_interpolated_trajectory{nullptr};
  mutable Trajectory m_computed_trajectory;
  Index m_last_reference_index;
};  // class MpcController
}  // namespace mpc_controller
}  // namespace control
}  // namespace motion
#endif  // MPC_CONTROLLER__MPC_CONTROLLER_HPP_