├── CMakeLists.txt ├── LICENSE ├── README.md ├── asset └── demo_video.gif ├── include └── nav2_mpc_controller │ ├── mpc_controller.hpp │ └── mpc_core.hpp ├── nav2_mpc_controller.xml ├── package.xml └── src ├── mpc_controller.cpp └── mpc_core.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_mpc_controller) 3 | 4 | # Nav2_Mpc_Controller 5 | find_package(ament_cmake REQUIRED) 6 | find_package(nav2_common REQUIRED) 7 | find_package(nav2_core REQUIRED) 8 | find_package(nav2_costmap_2d REQUIRED) 9 | find_package(nav2_util REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(geometry_msgs REQUIRED) 12 | find_package(nav_msgs REQUIRED) 13 | find_package(pluginlib REQUIRED) 14 | find_package(tf2 REQUIRED) 15 | 16 | # MPC_CORE 17 | find_package(Eigen3 REQUIRED) 18 | 19 | nav2_package() 20 | set(CMAKE_CXX_STANDARD 17) 21 | 22 | include_directories( 23 | include 24 | ) 25 | 26 | set(dependencies 27 | rclcpp 28 | geometry_msgs 29 | nav2_costmap_2d 30 | pluginlib 31 | nav_msgs 32 | nav2_util 33 | nav2_core 34 | tf2 35 | Eigen3 36 | ) 37 | 38 | # MPC_CORE 39 | add_library(mpc_core src/mpc_core.cpp) 40 | target_link_libraries(mpc_core ipopt) 41 | include_directories(${Eigen3_INCLUDE_DIRS}) 42 | 43 | # Nav2_Mpc_controller 44 | set(library_name nav2_mpc_controller) 45 | add_library(${library_name} SHARED 46 | src/mpc_controller.cpp) 47 | target_link_libraries(${library_name} mpc_core) 48 | 49 | # prevent pluginlib from using boost 50 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 51 | 52 | ament_target_dependencies(${library_name} 53 | ${dependencies} 54 | ) 55 | 56 | install(TARGETS ${library_name} 57 | ARCHIVE DESTINATION lib 58 | LIBRARY DESTINATION lib 59 | RUNTIME DESTINATION bin 60 | ) 61 | 62 | install(DIRECTORY include/ 63 | DESTINATION include/ 64 | ) 65 | 66 | # if(BUILD_TESTING) 67 | # find_package(ament_lint_auto REQUIRED) 68 | # # the following line skips the linter which checks for copyrights 69 | # set(ament_cmake_copyright_FOUND TRUE) 70 | # ament_lint_auto_find_test_dependencies() 71 | # add_subdirectory(test) 72 | # endif() 73 | 74 | ament_export_include_directories(include) 75 | ament_export_libraries(${library_name}) 76 | ament_export_dependencies(${dependencies}) 77 | 78 | pluginlib_export_plugin_description_file(nav2_core nav2_mpc_controller.xml) 79 | 80 | ament_package() 81 | 82 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | > This is not official package for [**navigation2**](https://github.com/ros-planning/navigation2) 2 | 3 | # Nav2_MPC_Controller Plugin 4 | 5 | This is ros2 package and plugin for controller interface of Navigation2.You can use this plugin with controller server of navigation2 without any special implementation. 6 | 7 | ![demo_video](asset/demo_video.gif) 8 | 9 | ### Installation 10 | 11 | Follow the official [documentation](https://navigation.ros.org/build_instructions/index.html#install) below to install. 12 | Requires Navigation2 and ROS2 (Default Foxy) installation. 13 | 14 | ### How to use 15 | 16 | This is an implementation of the plugin defined by Nav2. 17 | `bringup` package of Navigation2 package is a basic execution example. 18 | There are two main settings to run this plug-in. 19 | 20 | * Dependency Settings on `package.xml` 21 | Register this repository(pakcage). 22 | ``` 23 | "in package.xml" 24 | ... 25 | nav2_mpc_controller 26 | ... 27 | ``` 28 | 29 | * Loading Params on `.py` from launch files 30 | Load params .yaml file inclued this plugin configuration. 31 | 32 | ``` 33 | "in tb3_simulation_with_mpc_launch.py" 34 | ... 35 | declare_params_file_cmd = DeclareLaunchArgument( 36 | 'params_file', 37 | default_value=os.path.join(bringup_dir, 'params', 'nav2_mpc_params.yaml'), 38 | description='Full path to the ROS2 parameters file to use for all launched nodes') 39 | ... 40 | ``` 41 | 42 | Need to create new param file like `nav2_mpc_params.yaml`. 43 | It should include the configuration for this plugin like below. 44 | 45 | ``` 46 | ... 47 | FollowPath: 48 | plugin: "nav2_mpc_controller::MPCController" 49 | ... 50 | ``` 51 | -------------------------------------------------------------------------------- /asset/demo_video.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harderthan/nav2_mpc_controller/825990a3e5c008bdde4329cbe465c94ccb19f556/asset/demo_video.gif -------------------------------------------------------------------------------- /include/nav2_mpc_controller/mpc_controller.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Harderthan 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 | 15 | #ifndef NAV2_MPC_CONTROLLER__MPC_CONTROLLER_HPP_ 16 | #define NAV2_MPC_CONTROLLER__MPC_CONTROLLER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "geometry_msgs/msg/pose2_d.hpp" 24 | #include "nav2_core/controller.hpp" 25 | #include "nav2_util/odometry_utils.hpp" 26 | #include "pluginlib/class_list_macros.hpp" 27 | #include "pluginlib/class_loader.hpp" 28 | #include "rclcpp/rclcpp.hpp" 29 | 30 | #include "mpc_core.hpp" 31 | #include 32 | #include 33 | 34 | namespace nav2_mpc_controller { 35 | /** 36 | * @class nav2_mpc_controller::MPCController 37 | * @brief mpc controller plugin 38 | */ 39 | class MPCController : public nav2_core::Controller { 40 | public: 41 | /** 42 | * @brief Constructor for nav2_mpc_controller::MPCController 43 | */ 44 | MPCController() = default; 45 | 46 | /** 47 | * @brief Destrructor for nav2_mpc_controller::MPCController 48 | */ 49 | ~MPCController() override = default; 50 | 51 | /** 52 | * @brief Configure controller state machine 53 | * @param parent WeakPtr to node 54 | * @param name Name of plugin 55 | * @param tf TF buffer 56 | * @param costmap_ros Costmap2DROS object of environment 57 | */ 58 | void configure(const rclcpp_lifecycle::LifecycleNode::SharedPtr &parent, 59 | std::string name, const std::shared_ptr &tf, 60 | const std::shared_ptr 61 | &costmap_ros) override; 62 | 63 | /** 64 | * @brief Cleanup controller state machine 65 | */ 66 | void cleanup() override; 67 | 68 | /** 69 | * @brief Activate controller state machine 70 | */ 71 | void activate() override; 72 | 73 | /** 74 | * @brief Deactivate controller state machine 75 | */ 76 | void deactivate() override; 77 | 78 | /** 79 | * @brief Compute the best command given the current pose and velocity, with 80 | * possible debug information 81 | * 82 | * Same as above computeVelocityCommands, but with debug results. 83 | * If the results pointer is not null, additional information about the twists 84 | * evaluated will be in results after the call. 85 | * 86 | * @param pose Current robot pose 87 | * @param velocity Current robot velocity 88 | * @param results Output param, if not NULL, will be filled in with full 89 | * evaluation results 90 | * @return Best command 91 | */ 92 | geometry_msgs::msg::TwistStamped 93 | computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose, 94 | const geometry_msgs::msg::Twist &velocity) override; 95 | 96 | /** 97 | * @brief nav2_core setPlan - Sets the global plan 98 | * @param path The global plan 99 | */ 100 | void setPlan(const nav_msgs::msg::Path &path) override; 101 | 102 | protected: 103 | Eigen::VectorXd& impCoefficients(const geometry_msgs::msg::PoseStamped &pose, const nav_msgs::msg::Path& global_plan); 104 | double impThetaError(double theta, const Eigen::VectorXd& coeffs, 105 | int sample_size, int sample_ratio); 106 | void convertCoordinateSystem(Eigen::VectorXd& x_veh, Eigen::VectorXd y_veh); 107 | double polyeval(Eigen::VectorXd coeffs, double x); 108 | Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, 109 | int order); 110 | 111 | std::shared_ptr tf_; 112 | std::string plugin_name_; 113 | std::shared_ptr costmap_ros_; 114 | nav2_costmap_2d::Costmap2D *costmap_; 115 | rclcpp::Logger logger_{rclcpp::get_logger("MPCController")}; 116 | 117 | MPCCore _mpc; 118 | map _mpc_params; 119 | double mpc_steps_, ref_cte_, ref_etheta_, ref_vel_, w_cte_, w_etheta_, w_vel_, 120 | w_angvel_, w_accel_, w_angvel_d_, w_accel_d_, max_angvel_, max_throttle_, 121 | bound_value_; 122 | 123 | double dt_, w_, throttle_, speed_, max_speed_; 124 | double pathLength_, goalRadius_, waypointsDist_; 125 | int controller_freq_, downSampling_, thread_numbers_; 126 | bool goal_received_, goal_reached_, path_computed_, pub_twist_flag_, 127 | debug_info_, delay_mode_; 128 | 129 | tf2::Duration transform_tolerance_; 130 | double control_duration_; 131 | bool use_cost_regulated_linear_velocity_scaling_; 132 | double inflation_cost_scaling_factor_; 133 | 134 | nav_msgs::msg::Path global_plan_; 135 | std::shared_ptr> 136 | global_path_pub_; 137 | std::shared_ptr< 138 | rclcpp_lifecycle::LifecyclePublisher> 139 | carrot_pub_; 140 | std::shared_ptr> 141 | carrot_arc_pub_; 142 | }; 143 | } // namespace nav2_mpc_controller 144 | 145 | #endif // NAV2_MPC_CONTROLLER__MPC_CONTROLLER_HPP_ 146 | -------------------------------------------------------------------------------- /include/nav2_mpc_controller/mpc_core.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | # Copyright 2021 Harderthan 3 | # Original Code is from https://github.com/Geonhee-LEE/mpc_ros 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | */ 17 | 18 | #ifndef MPC_H 19 | #define MPC_H 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | using namespace std; 26 | 27 | class MPCCore { 28 | public: 29 | MPCCore(); 30 | 31 | // Solve the model given an initial state and polynomial coefficients. 32 | // Return the first actuatotions. 33 | vector Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs); 34 | vector mpc_x; 35 | vector mpc_y; 36 | vector mpc_theta; 37 | 38 | void LoadParams(const std::map ¶ms); 39 | 40 | private: 41 | // Parameters for mpc solver 42 | double _max_angvel, _max_throttle, _bound_value; 43 | int _mpc_steps, _x_start, _y_start, _theta_start, _v_start, _cte_start, 44 | _etheta_start, _angvel_start, _a_start; 45 | std::map _params; 46 | 47 | unsigned int dis_cnt; 48 | }; 49 | 50 | #endif /* MPC_H */ 51 | -------------------------------------------------------------------------------- /nav2_mpc_controller.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | nav2_mpc_controller 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_mpc_controller 5 | 0.0.1 6 | Model Predictive Control Controller 7 | Harderthan 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | nav2_common 13 | nav2_core 14 | nav2_util 15 | nav2_costmap_2d 16 | rclcpp 17 | geometry_msgs 18 | nav2_msgs 19 | pluginlib 20 | tf2 21 | 22 | ament_cmake_gtest 23 | ament_lint_common 24 | ament_lint_auto 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /src/mpc_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Harderthan 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 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "nav2_core/exceptions.hpp" 20 | #include "nav2_mpc_controller/mpc_controller.hpp" 21 | #include "nav2_util/geometry_utils.hpp" 22 | #include "nav2_util/node_utils.hpp" 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | using nav2_util::declare_parameter_if_not_declared; 30 | 31 | namespace nav2_mpc_controller { 32 | void MPCController::configure( 33 | const rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string name, 34 | const std::shared_ptr &tf, 35 | const std::shared_ptr &costmap_ros) { 36 | costmap_ros_ = costmap_ros; 37 | costmap_ = costmap_ros_->getCostmap(); 38 | tf_ = tf; 39 | plugin_name_ = name; 40 | logger_ = node->get_logger(); 41 | 42 | declare_parameter_if_not_declared(node, plugin_name_ + ".delay_mode", 43 | rclcpp::ParameterValue(false)); 44 | declare_parameter_if_not_declared(node, plugin_name_ + ".thread_numbers", 45 | rclcpp::ParameterValue(2)); 46 | declare_parameter_if_not_declared(node, plugin_name_ + ".max_speed", 47 | rclcpp::ParameterValue(0.5)); 48 | declare_parameter_if_not_declared(node, plugin_name_ + ".waypoints_dist", 49 | rclcpp::ParameterValue(-1.0)); 50 | declare_parameter_if_not_declared(node, plugin_name_ + ".pathLength", 51 | rclcpp::ParameterValue(8.0)); // unit: m 52 | declare_parameter_if_not_declared(node, plugin_name_ + ".goal_radius", 53 | rclcpp::ParameterValue(0.5)); // unit: m 54 | declare_parameter_if_not_declared(node, plugin_name_ + ".controller_freq", 55 | rclcpp::ParameterValue(10)); 56 | 57 | node->get_parameter(plugin_name_ + ".delay_mode", delay_mode_); 58 | node->get_parameter(plugin_name_ + ".thread_numbers", thread_numbers_); 59 | node->get_parameter(plugin_name_ + ".max_speed", max_speed_); 60 | node->get_parameter(plugin_name_ + ".waypoints_dist", waypointsDist_); 61 | node->get_parameter(plugin_name_ + ".pathLength", pathLength_); 62 | node->get_parameter(plugin_name_ + ".goal_radius", goalRadius_); 63 | node->get_parameter(plugin_name_ + ".controller_freq", controller_freq_); 64 | 65 | throttle_ = 0.0; 66 | w_ = 0.0; 67 | speed_ = 0.0; 68 | 69 | dt_ = double(1.0 / controller_freq_); // time step duration dt in s 70 | mpc_steps_ = 20.0; 71 | ref_cte_ = 0.0; 72 | ref_vel_ = 1.0; 73 | ref_etheta_ = 0.0; 74 | w_cte_ = 5000.0; 75 | w_etheta_ = 5000.0; 76 | w_vel_ = 1.0; 77 | w_angvel_ = 100.0; 78 | w_angvel_d_ = 10.0; 79 | w_accel_ = 50.0; 80 | w_accel_d_ = 10.0; 81 | max_angvel_ = 3.0; // Maximal angvel radian (~30 deg) 82 | max_throttle_ = 1.0; 83 | bound_value_ = 1.0e3; 84 | 85 | // Init parameters for MPC object 86 | _mpc_params["DT"] = dt_; 87 | _mpc_params["STEPS"] = mpc_steps_; 88 | _mpc_params["REF_CTE"] = ref_cte_; 89 | _mpc_params["REF_ETHETA"] = ref_etheta_; 90 | _mpc_params["REF_V"] = ref_vel_; 91 | _mpc_params["W_CTE"] = w_cte_; 92 | _mpc_params["W_EPSI"] = w_etheta_; 93 | _mpc_params["W_V"] = w_vel_; 94 | _mpc_params["W_ANGVEL"] = w_angvel_; 95 | _mpc_params["W_A"] = w_accel_; 96 | _mpc_params["W_DANGVEL"] = w_angvel_d_; 97 | _mpc_params["W_DA"] = w_accel_d_; 98 | _mpc_params["ANGVEL"] = max_angvel_; 99 | _mpc_params["MAXTHR"] = max_throttle_; 100 | _mpc_params["BOUND"] = bound_value_; 101 | _mpc.LoadParams(_mpc_params); 102 | 103 | double transform_tolerance = 0.1; 104 | double control_frequency = 20.0; 105 | 106 | transform_tolerance_ = tf2::durationFromSec(transform_tolerance); 107 | control_duration_ = 1.0 / control_frequency; 108 | 109 | global_path_pub_ = 110 | node->create_publisher("received_global_plan", 1); 111 | } 112 | 113 | void MPCController::cleanup() { 114 | RCLCPP_INFO(logger_, 115 | "Cleaning up controller: %s of type" 116 | " nav2_mpc_controller::MPCController", 117 | plugin_name_.c_str()); 118 | global_path_pub_.reset(); 119 | } 120 | 121 | void MPCController::activate() { 122 | RCLCPP_INFO(logger_, 123 | "Activating controller: %s of type " 124 | " nav2_mpc_controller::MPCController", 125 | plugin_name_.c_str()); 126 | global_path_pub_->on_activate(); 127 | } 128 | 129 | void MPCController::deactivate() { 130 | RCLCPP_INFO(logger_, 131 | "Deactivating controller: %s of type " 132 | " nav2_mpc_controller::MPCController", 133 | plugin_name_.c_str()); 134 | global_path_pub_->on_deactivate(); 135 | } 136 | 137 | geometry_msgs::msg::TwistStamped MPCController::computeVelocityCommands( 138 | const geometry_msgs::msg::PoseStamped &pose, 139 | const geometry_msgs::msg::Twist &speed) { 140 | // Update system states: X=[x, y, theta, v] 141 | const double px = pose.pose.position.x; 142 | const double py = pose.pose.position.y; 143 | 144 | tf2::Quaternion q; 145 | tf2::fromMsg(pose.pose.orientation, q); 146 | const double theta = tf2::getYaw(q); 147 | const double costheta = cos(theta); 148 | const double sintheta = sin(theta); 149 | const double v = speed.linear.x; 150 | const double w = w_; // steering -> w 151 | const double throttle = throttle_; // accel: >0; brake: <0 152 | const double dt = dt_; 153 | 154 | // Waypoints related parameters 155 | const int N = global_plan_.poses.size(); 156 | 157 | // Convert to the vehicle coordinate system 158 | Eigen::VectorXd x_veh(N); 159 | Eigen::VectorXd y_veh(N); 160 | for (int i = 0; i < N; i++) { 161 | const double dx = global_plan_.poses[i].pose.position.x - px; 162 | const double dy = global_plan_.poses[i].pose.position.y - py; 163 | x_veh[i] = dx * costheta + dy * sintheta; 164 | y_veh[i] = dy * costheta - dx * sintheta; 165 | } 166 | 167 | // Fit waypoints 168 | Eigen::VectorXd coeffs = polyfit(x_veh, y_veh, 3); 169 | const double cte = polyeval(coeffs, 0.0); 170 | cout << "coeffs : " << coeffs[0] << endl; 171 | cout << "pow : " << pow(0.0, 0) << endl; 172 | cout << "cte : " << cte << endl; 173 | 174 | double etheta = impThetaError(theta, coeffs, N, 0.3); 175 | 176 | Eigen::VectorXd state(6); 177 | if (delay_mode_) { 178 | // Kinematic model is used to predict vehicle state at the actual moment of 179 | // control (current time + delay dt) 180 | const double px_act = v * dt; 181 | const double py_act = 0; 182 | const double theta_act = 183 | w * dt; //(steering) theta_act = v * steering * dt / Lf; 184 | const double v_act = v + throttle * dt; // v = v + a * dt 185 | 186 | const double cte_act = cte + v * sin(etheta) * dt; 187 | const double etheta_act = etheta - theta_act; 188 | 189 | state << px_act, py_act, theta_act, v_act, cte_act, etheta_act; 190 | } else { 191 | state << 0, 0, 0, v, cte, etheta; 192 | } 193 | 194 | // Solve MPC Problem 195 | rclcpp::Time begin = rclcpp::Clock().now(); 196 | vector mpc_results = _mpc.Solve(state, coeffs); 197 | rclcpp::Time end = rclcpp::Clock().now(); 198 | cout << "Duration: " << end.seconds() << "." << end.nanoseconds() << endl 199 | << begin.seconds() << "." << begin.nanoseconds() << endl; 200 | 201 | // MPC result (all described in car frame), output = (acceleration, w) 202 | w_ = mpc_results[0]; // radian/sec, angular velocity 203 | throttle_ = mpc_results[1]; // acceleration 204 | 205 | speed_ = v + throttle_ * dt_; // speed 206 | if (speed_ >= max_speed_) 207 | speed_ = max_speed_; 208 | if (speed_ <= 0.0) 209 | speed_ = 0.0; 210 | 211 | if (debug_info_) { 212 | cout << "\n\nDEBUG" << endl; 213 | cout << "theta: " << theta << endl; 214 | cout << "V: " << v << endl; 215 | cout << "coeffs: \n" << coeffs << endl; 216 | cout << "w_: \n" << w_ << endl; 217 | cout << "throttle_: \n" << throttle_ << endl; 218 | cout << "speed_: \n" << speed_ << endl; 219 | } 220 | 221 | // populate and return message 222 | geometry_msgs::msg::TwistStamped cmd_vel; 223 | cmd_vel.header = pose.header; 224 | cmd_vel.twist.linear.x = speed_; 225 | cmd_vel.twist.angular.z = w_; 226 | return cmd_vel; 227 | } 228 | 229 | // Evaluate a polynomial. 230 | double MPCController::polyeval(Eigen::VectorXd coeffs, double x) { 231 | double result = 0.0; 232 | for (int i = 0; i < coeffs.size(); i++) { 233 | result += coeffs[i] * pow(x, i); 234 | } 235 | return result; 236 | } 237 | 238 | // Fit a polynomial. 239 | // Adapted from 240 | // https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716 241 | Eigen::VectorXd MPCController::polyfit(Eigen::VectorXd xvals, 242 | Eigen::VectorXd yvals, int order) { 243 | assert(xvals.size() == yvals.size()); 244 | assert(order >= 1 && order <= xvals.size() - 1); 245 | Eigen::MatrixXd A(xvals.size(), order + 1); 246 | 247 | for (int i = 0; i < xvals.size(); i++) 248 | A(i, 0) = 1.0; 249 | 250 | for (int j = 0; j < xvals.size(); j++) { 251 | for (int i = 0; i < order; i++) 252 | A(j, i + 1) = A(j, i) * xvals(j); 253 | } 254 | auto Q = A.householderQr(); 255 | auto result = Q.solve(yvals); 256 | return result; 257 | } 258 | 259 | void MPCController::setPlan(const nav_msgs::msg::Path &path) { 260 | global_plan_ = path; 261 | } 262 | 263 | /* 264 | * Global coordinate system about theta 265 | */ 266 | double MPCController::impThetaError(double theta, const Eigen::VectorXd& coeffs, 267 | int sample_size, int sample_ratio) { 268 | double etheta = atan(coeffs[1]); 269 | double gx = 0; 270 | double gy = 0; 271 | for (int i = 1; i < sample_size * sample_ratio; i++) { 272 | gx += global_plan_.poses[i].pose.position.x - 273 | global_plan_.poses[i - 1].pose.position.x; 274 | gy += global_plan_.poses[i].pose.position.y - 275 | global_plan_.poses[i - 1].pose.position.y; 276 | } 277 | 278 | double temp_theta = theta; 279 | double traj_deg = atan2(gy, gx); 280 | double PI = 3.141592; 281 | 282 | // Degree conversion -pi~pi -> 0~2pi(ccw) since need a continuity 283 | if (temp_theta <= -PI + traj_deg) 284 | temp_theta = temp_theta + 2 * PI; 285 | 286 | // Implementation about theta error more precisly 287 | if (gx && gy && temp_theta - traj_deg < 1.8 * PI) 288 | etheta = temp_theta - traj_deg; 289 | else 290 | etheta = 0; 291 | cout << "etheta: " << etheta << ", atan2(gy,gx): " << atan2(gy, gx) 292 | << ", temp_theta:" << traj_deg << endl; 293 | 294 | return etheta; 295 | } 296 | 297 | /* 298 | * Implimentation Coefficients 299 | */ 300 | // Eigen::VectorXd& impCoefficients(const geometry_msgs::msg::PoseStamped &pose, const nav_msgs::msg::Path& global_plan){ 301 | // tf2::Quaternion q; 302 | // tf2::fromMsg(pose.pose.orientation, q); 303 | // const double theta = tf2::getYaw(q); 304 | // const double costheta = cos(theta); 305 | // const double sintheta = sin(theta); 306 | // // Update system states: X=[x, y, theta, v] 307 | // const double px = pose.pose.position.x; 308 | // const double py = pose.pose.position.y; 309 | // // Waypoints related parameters 310 | // const int N = global_plan.poses.size(); 311 | 312 | // // Convert to the vehicle coordinate system 313 | // Eigen::VectorXd x_veh(N); 314 | // Eigen::VectorXd y_veh(N); 315 | // for (int i = 0; i < N; i++) { 316 | // const double dx = global_plan.poses[i].pose.position.x - px; 317 | // const double dy = global_plan.poses[i].pose.position.y - py; 318 | // x_veh[i] = dx * costheta + dy * sintheta; 319 | // y_veh[i] = dy * costheta - dx * sintheta; 320 | // } 321 | // } 322 | } // namespace nav2_mpc_controller 323 | 324 | // Register this controller as a nav2_core plugin 325 | PLUGINLIB_EXPORT_CLASS(nav2_mpc_controller::MPCController, 326 | nav2_core::Controller) 327 | -------------------------------------------------------------------------------- /src/mpc_core.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | # Copyright 2021 Harderthan 3 | # Original Code is from https://github.com/Geonhee-LEE/mpc_ros 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | */ 17 | 18 | #include "nav2_mpc_controller/mpc_core.hpp" 19 | 20 | #include 21 | #include 22 | 23 | // The program use fragments of code from 24 | // https://github.com/udacity/CarND-MPC-Quizzes 25 | 26 | using CppAD::AD; 27 | 28 | // ========================================= 29 | // FG_eval class definition implementation. 30 | // ========================================= 31 | class FG_eval { 32 | public: 33 | // Fitted polynomial coefficients 34 | Eigen::VectorXd coeffs; 35 | 36 | double _dt, _ref_cte, _ref_etheta, _ref_vel; 37 | double _w_cte, _w_etheta, _w_vel, _w_angvel, _w_accel, _w_angvel_d, 38 | _w_accel_d; 39 | int _mpc_steps, _x_start, _y_start, _theta_start, _v_start, _cte_start, 40 | _etheta_start, _angvel_start, _a_start; 41 | 42 | AD cost_cte, cost_etheta, cost_vel; 43 | // Constructor 44 | FG_eval(Eigen::VectorXd coeffs) { 45 | this->coeffs = coeffs; 46 | 47 | // Set default value 48 | _dt = 0.1; // in sec 49 | _ref_cte = 0; 50 | _ref_etheta = 0; 51 | _ref_vel = 0.5; // m/s 52 | _w_cte = 100; 53 | _w_etheta = 100; 54 | _w_vel = 1; 55 | _w_angvel = 100; 56 | _w_accel = 50; 57 | _w_angvel_d = 0; 58 | _w_accel_d = 0; 59 | 60 | _mpc_steps = 40; 61 | _x_start = 0; 62 | _y_start = _x_start + _mpc_steps; 63 | _theta_start = _y_start + _mpc_steps; 64 | _v_start = _theta_start + _mpc_steps; 65 | _cte_start = _v_start + _mpc_steps; 66 | _etheta_start = _cte_start + _mpc_steps; 67 | _angvel_start = _etheta_start + _mpc_steps; 68 | _a_start = _angvel_start + _mpc_steps - 1; 69 | } 70 | 71 | // Load parameters for constraints 72 | void LoadParams(const std::map ¶ms) { 73 | _dt = params.find("DT") != params.end() ? params.at("DT") : _dt; 74 | _mpc_steps = 75 | params.find("STEPS") != params.end() ? params.at("STEPS") : _mpc_steps; 76 | _ref_cte = params.find("REF_CTE") != params.end() ? params.at("REF_CTE") 77 | : _ref_cte; 78 | _ref_etheta = params.find("REF_ETHETA") != params.end() 79 | ? params.at("REF_ETHETA") 80 | : _ref_etheta; 81 | _ref_vel = 82 | params.find("REF_V") != params.end() ? params.at("REF_V") : _ref_vel; 83 | 84 | _w_cte = params.find("W_CTE") != params.end() ? params.at("W_CTE") : _w_cte; 85 | _w_etheta = 86 | params.find("W_EPSI") != params.end() ? params.at("W_EPSI") : _w_etheta; 87 | _w_vel = params.find("W_V") != params.end() ? params.at("W_V") : _w_vel; 88 | _w_angvel = params.find("W_ANGVEL") != params.end() ? params.at("W_ANGVEL") 89 | : _w_angvel; 90 | _w_accel = params.find("W_A") != params.end() ? params.at("W_A") : _w_accel; 91 | _w_angvel_d = params.find("W_DANGVEL") != params.end() 92 | ? params.at("W_DANGVEL") 93 | : _w_angvel_d; 94 | _w_accel_d = 95 | params.find("W_DA") != params.end() ? params.at("W_DA") : _w_accel_d; 96 | 97 | _x_start = 0; 98 | _y_start = _x_start + _mpc_steps; 99 | _theta_start = _y_start + _mpc_steps; 100 | _v_start = _theta_start + _mpc_steps; 101 | _cte_start = _v_start + _mpc_steps; 102 | _etheta_start = _cte_start + _mpc_steps; 103 | _angvel_start = _etheta_start + _mpc_steps; 104 | _a_start = _angvel_start + _mpc_steps - 1; 105 | 106 | // cout << "\n!! FG_eval Obj parameters updated !! " << _mpc_steps << endl; 107 | } 108 | 109 | // MPC implementation (cost func & constraints) 110 | typedef CPPAD_TESTVECTOR(AD) ADvector; 111 | // fg: function that evaluates the objective and constraints using the syntax 112 | void operator()(ADvector &fg, const ADvector &vars) { 113 | // fg[0] for cost function 114 | fg[0] = 0; 115 | cost_cte = 0; 116 | cost_etheta = 0; 117 | cost_vel = 0; 118 | 119 | /* 120 | for (int i = 0; i < _mpc_steps; i++) 121 | { 122 | cout << i << endl; 123 | cout << "_x_start" << vars[_x_start + i] < x1 = vars[_x_start + i + 1]; 178 | AD y1 = vars[_y_start + i + 1]; 179 | AD theta1 = vars[_theta_start + i + 1]; 180 | AD v1 = vars[_v_start + i + 1]; 181 | AD cte1 = vars[_cte_start + i + 1]; 182 | AD etheta1 = vars[_etheta_start + i + 1]; 183 | 184 | // The state at time t. 185 | AD x0 = vars[_x_start + i]; 186 | AD y0 = vars[_y_start + i]; 187 | AD theta0 = vars[_theta_start + i]; 188 | AD v0 = vars[_v_start + i]; 189 | AD cte0 = vars[_cte_start + i]; 190 | AD etheta0 = vars[_etheta_start + i]; 191 | 192 | // Only consider the actuation at time t. 193 | // AD angvel0 = vars[_angvel_start + i]; 194 | AD w0 = vars[_angvel_start + i]; 195 | AD a0 = vars[_a_start + i]; 196 | 197 | // AD f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 198 | // 2) + coeffs[3] * CppAD::pow(x0, 3); 199 | AD f0 = 0.0; 200 | for (int i = 0; i < coeffs.size(); i++) { 201 | f0 += coeffs[i] * CppAD::pow(x0, i); // f(0) = y 202 | } 203 | 204 | // AD trj_grad0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * 205 | // coeffs[3] * CppAD::pow(x0, 2)); 206 | AD trj_grad0 = 0.0; 207 | for (int i = 1; i < coeffs.size(); i++) { 208 | trj_grad0 += i * coeffs[i] * CppAD::pow(x0, i - 1); // f'(x0) = f(1)/1 209 | } 210 | trj_grad0 = CppAD::atan(trj_grad0); 211 | 212 | // Here's `x` to get you started. 213 | // The idea here is to constraint this value to be 0. 214 | // 215 | // NOTE: The use of `AD` and use of `CppAD`! 216 | // This is also CppAD can compute derivatives and pass 217 | // these to the solver. 218 | // TODO: Setup the rest of the model constraints 219 | fg[2 + _x_start + i] = x1 - (x0 + v0 * CppAD::cos(theta0) * _dt); 220 | fg[2 + _y_start + i] = y1 - (y0 + v0 * CppAD::sin(theta0) * _dt); 221 | fg[2 + _theta_start + i] = theta1 - (theta0 + w0 * _dt); 222 | fg[2 + _v_start + i] = v1 - (v0 + a0 * _dt); 223 | 224 | fg[2 + _cte_start + i] = 225 | cte1 - ((f0 - y0) + (v0 * CppAD::sin(etheta0) * _dt)); 226 | // fg[2 + _etheta_start + i] = etheta1 - ((theta0 - trj_grad0) + w0 * 227 | // _dt);//theta0-trj_grad0)->etheta : it can have more curvature 228 | // prediction, but its gradient can be only adjust positive plan. 229 | fg[2 + _etheta_start + i] = etheta1 - (etheta0 + w0 * _dt); 230 | } 231 | } 232 | }; 233 | 234 | // ==================================== 235 | // MPCCore class definition implementation. 236 | // ==================================== 237 | MPCCore::MPCCore() { 238 | // Set default value 239 | _mpc_steps = 20; 240 | _max_angvel = 3.0; // Maximal angvel radian (~30 deg) 241 | _max_throttle = 1.0; // Maximal throttle accel 242 | _bound_value = 1.0e3; // Bound value for other variables 243 | 244 | _x_start = 0; 245 | _y_start = _x_start + _mpc_steps; 246 | _theta_start = _y_start + _mpc_steps; 247 | _v_start = _theta_start + _mpc_steps; 248 | _cte_start = _v_start + _mpc_steps; 249 | _etheta_start = _cte_start + _mpc_steps; 250 | _angvel_start = _etheta_start + _mpc_steps; 251 | _a_start = _angvel_start + _mpc_steps - 1; 252 | } 253 | 254 | void MPCCore::LoadParams(const std::map ¶ms) { 255 | _params = params; 256 | // Init parameters for MPC object 257 | _mpc_steps = 258 | _params.find("STEPS") != _params.end() ? _params.at("STEPS") : _mpc_steps; 259 | _max_angvel = _params.find("ANGVEL") != _params.end() ? _params.at("ANGVEL") 260 | : _max_angvel; 261 | _max_throttle = _params.find("MAXTHR") != _params.end() ? _params.at("MAXTHR") 262 | : _max_throttle; 263 | _bound_value = _params.find("BOUND") != _params.end() ? _params.at("BOUND") 264 | : _bound_value; 265 | 266 | _x_start = 0; 267 | _y_start = _x_start + _mpc_steps; 268 | _theta_start = _y_start + _mpc_steps; 269 | _v_start = _theta_start + _mpc_steps; 270 | _cte_start = _v_start + _mpc_steps; 271 | _etheta_start = _cte_start + _mpc_steps; 272 | _angvel_start = _etheta_start + _mpc_steps; 273 | _a_start = _angvel_start + _mpc_steps - 1; 274 | 275 | cout << "\n!! MPC Obj parameters updated !! " << endl; 276 | } 277 | 278 | vector MPCCore::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) { 279 | bool ok = true; 280 | typedef CPPAD_TESTVECTOR(double) Dvector; 281 | const double x = state[0]; 282 | const double y = state[1]; 283 | const double theta = state[2]; 284 | const double v = state[3]; 285 | const double cte = state[4]; 286 | const double etheta = state[5]; 287 | 288 | // Set the number of model variables (includes both states and inputs). 289 | // For example: If the state is a 4 element vector, the actuators is a 2 290 | // element vector and there are 10 timesteps. The number of variables is: 291 | // 4 * 10 + 2 * 9 292 | size_t n_vars = _mpc_steps * 6 + (_mpc_steps - 1) * 2; 293 | 294 | // Set the number of constraints 295 | size_t n_constraints = _mpc_steps * 6; 296 | 297 | // Initial value of the independent variables. 298 | // SHOULD BE 0 besides initial state. 299 | Dvector vars(n_vars); 300 | for (size_t i = 0; i < n_vars; i++) { 301 | vars[i] = 0; 302 | } 303 | 304 | // Set the initial variable values 305 | vars[_x_start] = x; 306 | vars[_y_start] = y; 307 | vars[_theta_start] = theta; 308 | vars[_v_start] = v; 309 | vars[_cte_start] = cte; 310 | vars[_etheta_start] = etheta; 311 | 312 | // Set lower and upper limits for variables. 313 | Dvector vars_lowerbound(n_vars); 314 | Dvector vars_upperbound(n_vars); 315 | 316 | // Set all non-actuators upper and lowerlimits 317 | // to the max negative and positive values. 318 | for (size_t i = 0; i < (size_t)_angvel_start; i++) { 319 | vars_lowerbound[i] = -_bound_value; 320 | vars_upperbound[i] = _bound_value; 321 | } 322 | // The upper and lower limits of angvel are set to -25 and 25 323 | // degrees (values in radians). 324 | for (size_t i = _angvel_start; i < (size_t)_a_start; i++) { 325 | vars_lowerbound[i] = -_max_angvel; 326 | vars_upperbound[i] = _max_angvel; 327 | } 328 | // Acceleration/decceleration upper and lower limits 329 | for (size_t i = _a_start; i < n_vars; i++) { 330 | vars_lowerbound[i] = -_max_throttle; 331 | vars_upperbound[i] = _max_throttle; 332 | } 333 | 334 | // Lower and upper limits for the constraints 335 | // Should be 0 besides initial state. 336 | Dvector constraints_lowerbound(n_constraints); 337 | Dvector constraints_upperbound(n_constraints); 338 | for (size_t i = 0; i < n_constraints; i++) { 339 | constraints_lowerbound[i] = 0; 340 | constraints_upperbound[i] = 0; 341 | } 342 | constraints_lowerbound[_x_start] = x; 343 | constraints_lowerbound[_y_start] = y; 344 | constraints_lowerbound[_theta_start] = theta; 345 | constraints_lowerbound[_v_start] = v; 346 | constraints_lowerbound[_cte_start] = cte; 347 | constraints_lowerbound[_etheta_start] = etheta; 348 | constraints_upperbound[_x_start] = x; 349 | constraints_upperbound[_y_start] = y; 350 | constraints_upperbound[_theta_start] = theta; 351 | constraints_upperbound[_v_start] = v; 352 | constraints_upperbound[_cte_start] = cte; 353 | constraints_upperbound[_etheta_start] = etheta; 354 | 355 | // object that computes objective and constraints 356 | FG_eval fg_eval(coeffs); 357 | fg_eval.LoadParams(_params); 358 | 359 | // options for IPOPT solver 360 | std::string options; 361 | // Uncomment this if you'd like more print information 362 | options += "Integer print_level 0\n"; 363 | // NOTE: Setting sparse to true allows the solver to take advantage 364 | // of sparse routines, this makes the computation MUCH FASTER. If you 365 | // can uncomment 1 of these and see if it makes a difference or not but 366 | // if you uncomment both the computation time should go up in orders of 367 | // magnitude. 368 | options += "Sparse true forward\n"; 369 | options += "Sparse true reverse\n"; 370 | // NOTE: Currently the solver has a maximum time limit of 0.5 seconds. 371 | // Change this as you see fit. 372 | options += "Numeric max_cpu_time 0.5\n"; 373 | 374 | // place to return solution 375 | CppAD::ipopt::solve_result solution; 376 | 377 | // solve the problem 378 | CppAD::ipopt::solve( 379 | options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound, 380 | constraints_upperbound, fg_eval, solution); 381 | 382 | // Check some of the solution values 383 | ok &= solution.status == CppAD::ipopt::solve_result::success; 384 | 385 | // Cost 386 | auto cost = solution.obj_value; 387 | std::cout << "------------ Total Cost(solution): " << cost << "------------" 388 | << std::endl; 389 | cout << "max_angvel:" << _max_angvel << endl; 390 | cout << "max_throttle:" << _max_throttle << endl; 391 | 392 | cout << "-----------------------------------------------" << endl; 393 | 394 | this->mpc_x = {}; 395 | this->mpc_y = {}; 396 | this->mpc_theta = {}; 397 | for (int i = 0; i < _mpc_steps; i++) { 398 | this->mpc_x.push_back(solution.x[_x_start + i]); 399 | this->mpc_y.push_back(solution.x[_y_start + i]); 400 | this->mpc_theta.push_back(solution.x[_theta_start + i]); 401 | } 402 | 403 | vector result; 404 | result.push_back(solution.x[_angvel_start]); 405 | result.push_back(solution.x[_a_start]); 406 | return result; 407 | } 408 | --------------------------------------------------------------------------------