Autoware.Auto
mpc_controller.hpp
Go to the documentation of this file.
1 // Copyright 2019 Christopher Ho
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 #ifndef MPC_CONTROLLER__MPC_CONTROLLER_HPP_
15 #define MPC_CONTROLLER__MPC_CONTROLLER_HPP_
16 
18 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
19 #include <autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp>
20 #include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
21 
24 
25 #include <ostream>
26 #include <string>
27 
28 namespace motion
29 {
30 namespace control
31 {
32 namespace mpc_controller
33 {
35 class MPC_CONTROLLER_PUBLIC MpcController : public controller_common::ControllerBase
36 {
37 public:
38  constexpr static std::chrono::nanoseconds solver_time_step{std::chrono::milliseconds{100LL}};
39  // Disable other constructors, assignment operators: autogenerated code has annoying static
40  // variables
41  MpcController(const MpcController &) = delete;
42  MpcController(MpcController &&) noexcept = delete;
43  MpcController & operator=(const MpcController &) = delete;
44  MpcController & operator=(MpcController &&) noexcept = delete;
45 
46  using Command = autoware_auto_vehicle_msgs::msg::VehicleControlCommand;
47  using State = autoware_auto_vehicle_msgs::msg::VehicleKinematicState;
48  using Trajectory = autoware_auto_planning_msgs::msg::Trajectory;
49 
51  explicit MpcController(const Config & config);
52  ~MpcController() override = default;
53 
55  const Config & get_config() const;
57  void set_config(const Config & config);
58 
62  const Trajectory & get_computed_trajectory() const noexcept;
64  ControlDerivatives get_computed_control_derivatives() const noexcept;
65 
67  void debug_print(std::ostream & out) const;
68 
70  std::string name() const override;
71 
73  Index get_compute_iterations() const override;
74 
75 protected:
77  bool check_new_trajectory(const Trajectory & trajectory) const override;
79  const Trajectory & handle_new_trajectory(const Trajectory & trajectory) override;
81  Command compute_command_impl(const State & state) override;
82 
83 private:
85  MPC_CONTROLLER_LOCAL void solve();
87  MPC_CONTROLLER_LOCAL bool update_references(Index current_idx);
89  MPC_CONTROLLER_LOCAL void initial_conditions(const Point & state);
91  MPC_CONTROLLER_LOCAL std::chrono::nanoseconds x0_time_offset(const State & state, Index idx);
93  MPC_CONTROLLER_LOCAL Command interpolated_command(std::chrono::nanoseconds x0_time_offset);
96  MPC_CONTROLLER_LOCAL bool ensure_reference_consistency(Index horizon);
98  MPC_CONTROLLER_LOCAL void apply_config(const Config & cfg);
100  MPC_CONTROLLER_LOCAL void apply_parameters(const VehicleConfig & cfg) noexcept;
102  MPC_CONTROLLER_LOCAL void apply_bounds(const LimitsConfig & cfg) noexcept;
104  MPC_CONTROLLER_LOCAL void apply_weights(const OptimizationConfig & cfg);
106  MPC_CONTROLLER_LOCAL void apply_nominal_weights(const StateWeight & cfg, Index start, Index end);
108  MPC_CONTROLLER_LOCAL void apply_terminal_weights(const Index idx);
110  MPC_CONTROLLER_LOCAL
111  void set_reference(const Trajectory & traj, Index y_start, Index traj_start, Index count);
113  MPC_CONTROLLER_LOCAL void zero_nominal_weights(Index start, Index end);
115  MPC_CONTROLLER_LOCAL void zero_terminal_weights() noexcept;
117  MPC_CONTROLLER_LOCAL void set_terminal_weights(const StateWeight & cfg) noexcept;
119  MPC_CONTROLLER_LOCAL void set_terminal_reference(const Point & pt) noexcept;
121  MPC_CONTROLLER_LOCAL void advance_problem(Index count);
123  MPC_CONTROLLER_LOCAL void backfill_reference(Index count);
124 
125  Config m_config;
126  Trajectory::UniquePtr m_interpolated_trajectory{nullptr};
127  mutable Trajectory m_computed_trajectory;
128  Index m_last_reference_index;
129 }; // class MpcController
130 } // namespace mpc_controller
131 } // namespace control
132 } // namespace motion
133 #endif // MPC_CONTROLLER__MPC_CONTROLLER_HPP_
autoware::motion::control::pure_pursuit::VehicleControlCommand
autoware_auto_vehicle_msgs::msg::VehicleControlCommand VehicleControlCommand
Definition: pure_pursuit.hpp:41
motion::control::mpc_controller::ControlDerivatives
Definition: control/mpc_controller/include/mpc_controller/config.hpp:86
controller_base.hpp
motion::control::mpc_controller::Config
A configuration class for the MpcController.
Definition: control/mpc_controller/include/mpc_controller/config.hpp:55
setup.name
name
Definition: setup.py:6
visibility_control.hpp
motion::control::controller_common::ControllerBase
Definition: controller_base.hpp:76
config.hpp
motion::control::mpc_controller::MpcController::State
autoware_auto_vehicle_msgs::msg::VehicleKinematicState State
Definition: mpc_controller.hpp:47
motion
Definition: controller_base.hpp:31
motion::control::mpc_controller::MpcController
A wrapper around an autogenerated mpc solver for vehicle dynamics control.
Definition: mpc_controller.hpp:35
motion::motion_common::Trajectory
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: motion_common.hpp:41
Point
geometry_msgs::msg::Point32 Point
Definition: cluster2d.cpp:28
motion::control::mpc_controller::MpcController::Command
autoware_auto_vehicle_msgs::msg::VehicleControlCommand Command
Definition: mpc_controller.hpp:46
autoware::trajectory_spoofer::VehicleKinematicState
autoware_auto_vehicle_msgs::msg::VehicleKinematicState VehicleKinematicState
Definition: trajectory_spoofer.hpp:42
motion::control::mpc_controller::MpcController::Trajectory
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: mpc_controller.hpp:48
get_open_port.end
end
Definition: scripts/get_open_port.py:23
autoware::common::geometry::spatial_hash::Index
std::size_t Index
Index type for identifying bins of the hash/lattice.
Definition: spatial_hash_config.hpp:49