Repository: Hypha-ROS/hypharos_minicar Branch: master Commit: fef61e1757d3 Files: 55 Total size: 169.5 KB Directory structure: gitextract_x9qft2fw/ ├── CMakeLists.txt ├── LICENSE ├── document/ │ ├── ipopt_install/ │ │ ├── ipopt_arm_install_tutorial.md │ │ └── ipopt_x86_install_tutorial.md │ └── udev/ │ ├── stm32base.rules │ └── udev_install.sh ├── example/ │ ├── CppAD_Ipopt.cpp │ └── CppAD_started.cpp ├── include/ │ └── MPC.h ├── launch/ │ ├── Desktop/ │ │ ├── HyphaROS_Desktop_Mapping.launch │ │ └── HyphaROS_Desktop_Racing.launch │ ├── HyphaROS_MiniCar_Mapping.launch │ ├── HyphaROS_MiniCar_Racing.launch │ ├── Simulation/ │ │ ├── HyphaROS_Simulation_Stage.launch │ │ ├── maps/ │ │ │ └── maze.yaml │ │ ├── params/ │ │ │ ├── amcl_params.yaml │ │ │ ├── carlike/ │ │ │ │ ├── costmap_common_params.yaml │ │ │ │ ├── global_costmap_params.yaml │ │ │ │ ├── global_planner.yaml │ │ │ │ ├── local_costmap_params.yaml │ │ │ │ └── teb_local_planner_params.yaml │ │ │ ├── mpc_params.yaml │ │ │ ├── pure_pursuit_params.yaml │ │ │ └── rviz_navigation.rviz │ │ └── stage/ │ │ ├── maze_carlike.world │ │ └── robots/ │ │ └── carlike_robot.inc │ ├── includes/ │ │ ├── HyphaROS_MiniCar_Drivers.launch.xml │ │ ├── amcl.launch.xml │ │ ├── gmapping.launch.xml │ │ ├── hector_slam.launch.xml │ │ ├── karto_slam.launch.xml │ │ ├── minicar_base.launch.xml │ │ ├── minicar_tf.launch.xml │ │ ├── mrpt_icp.launch.xml │ │ └── ydlidar.launch.xml │ └── params/ │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ ├── global_planner.yaml │ ├── local_costmap_params.yaml │ ├── mpc/ │ │ └── mpc_params.yaml │ ├── pure_pursuit/ │ │ └── pure_pursuit_params.yaml │ └── teb/ │ └── teb_local_planner_params.yaml ├── map/ │ ├── map_amcl.pgm │ ├── map_amcl.yaml │ ├── map_nav.pgm │ └── map_nav.yaml ├── package.xml ├── readme.md ├── rviz/ │ ├── hypharos_minicar_mapping.rviz │ └── hypharos_minicar_racing.rviz ├── script/ │ ├── base_control.py │ └── teleop_keyboard.py └── src/ ├── MPC.cpp ├── MPC_Node.cpp └── Pure_Pursuit.cpp ================================================ FILE CONTENTS ================================================ ================================================ FILE: CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.5) project(hypharos_minicar) ## Compile as C++11, supported in ROS Kinetic and newer add_definitions(-std=c++11 -O3) # options for build configuration option(BUILD_EXAMPLE "Whether or not building the CppAD & Ipopt example" OFF) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS costmap_2d dynamic_reconfigure geometry_msgs nav_msgs ackermann_msgs message_generation move_base roscpp rospy std_msgs tf visualization_msgs ) catkin_package( INCLUDE_DIRS include # LIBRARIES hypharos_minicar # CATKIN_DEPENDS costmap_2d dynamic_reconfigure geometry_msgs message_generation move_base roscpp rospy std_msgs tf visualization_msgs # DEPENDS system_lib ) ########### ## Build ## ########### ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( ${catkin_INCLUDE_DIRS} include ) include_directories(/usr/include) link_directories(/usr/lib) # MPC_Node ADD_EXECUTABLE( MPC_Node src/MPC.cpp src/MPC_Node.cpp ) TARGET_LINK_LIBRARIES(MPC_Node ipopt ${catkin_LIBRARIES} ) # Pure Pursuit Node add_executable(Pure_Pursuit src/Pure_Pursuit.cpp) target_link_libraries(Pure_Pursuit ${catkin_LIBRARIES}) ############# ## Example ## ############# if(BUILD_EXAMPLE) ADD_EXECUTABLE( CppAD_started example/CppAD_started.cpp ) TARGET_LINK_LIBRARIES(CppAD_started) ADD_EXECUTABLE( CppAD_Ipopt example/CppAD_Ipopt.cpp ) TARGET_LINK_LIBRARIES(CppAD_Ipopt ipopt) endif(BUILD_EXAMPLE) ================================================ FILE: LICENSE ================================================ Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright [2018] [HyphaROS Workshop] 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. ================================================ FILE: document/ipopt_install/ipopt_arm_install_tutorial.md ================================================ # How to install Ipopt on arm environment ## Inatall CPPAD & Fortran $ sudo apt-get install cppad gfortran ## Get ipopt source code Download Ipopt-3.12.8.tgz from https://www.coin-or.org/download/source/Ipopt/ Untar the package and cd into the "Ipopt-3.12.8" folder ## Step by step $ cd CUSTOM_PATH/Ipopt-3.12.8/ThirdParty/Blas $ ./get.Blas $ cd ../Lapack $ ./get.Lapack $ cd ../Mumps $ ./get.Mumps $ cd ../Metis $ ./get.Metis $ cd CUSTOM_PATH/Ipopt-3.12.8 $ mkdir build $ cd build $ ../configure --build=arm-linux $ make -j4 $ make install ## Copy install files into specific directory $ cd CUSTOM_PATH/Ipopt-3.12.8/build $ sudo cp -a include/* /usr/include/. $ sudo cp -a lib/* /usr/lib/. ## Trouble Shooting If something wrong like: "redefinition of ‘bool CppAD::is_pod()" Modify the file: $ sudo gedit /usr/include/cppad/configure.hpp Change line from: define CPPAD_SIZE_T_NOT_UNSIGNED_INT 1 to define CPPAD_SIZE_T_NOT_UNSIGNED_INT 0 ================================================ FILE: document/ipopt_install/ipopt_x86_install_tutorial.md ================================================ # How to install Ipopt on x86 environment ## Inatall CPPAD & Fortran $ sudo apt-get install cppad gfortran ## Get ipopt source code Download Ipopt-3.12.8.tgz from https://www.coin-or.org/download/source/Ipopt/ Untar the package and cd into the "Ipopt-3.12.8" folder ## Step by step $ cd CUSTOM_PATH/Ipopt-3.12.8/ThirdParty/Blas $ ./get.Blas $ cd ../Lapack $ ./get.Lapack $ cd ../Mumps $ ./get.Mumps $ cd ../Metis $ ./get.Metis $ cd ../ASL $ ./get.ASL $ cd CUSTOM_PATH/Ipopt-3.12.8 $ mkdir build $ cd build $ ../configure $ make -j4 $ make install ## Copy install files into specific directory $ cd CUSTOM_PATH/Ipopt-3.12.8/build $ sudo cp -a include/* /usr/include/. $ sudo cp -a lib/* /usr/lib/. ================================================ FILE: document/udev/stm32base.rules ================================================ # set the udev rule , make the device_port be fixed by arduino # KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="666", SYMLINK+="stm32base" ================================================ FILE: document/udev/udev_install.sh ================================================ #!/bin/bash echo "=========== HyphaROS ===========" echo "Automatically install udev rules" # YDLidar #echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar.rules #echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-V2.rules #echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="067b", ATTRS{idProduct}=="2303", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-2303.rules # GY85 IMU #echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="666", SYMLINK+="gy85"' >/etc/udev/rules.d/gy85.rules # STM32 Base echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="666", SYMLINK+="stm32base"' >/etc/udev/rules.d/stm32base.rules service udev reload sleep 2 service udev restart echo "=========== Finished ===========" exit ================================================ FILE: example/CppAD_Ipopt.cpp ================================================ // $Id$ /* -------------------------------------------------------------------------- CppAD: C++ Algorithmic Differentiation: Copyright (C) 2003-16 Bradley M. Bell CppAD is distributed under multiple licenses. This distribution is under the terms of the Eclipse Public License Version 1.0. A copy of this license is included in the COPYING file of this distribution. Please visit http://www.coin-or.org/CppAD/ for information on other licenses. -------------------------------------------------------------------------- */ /* $begin ipopt_solve_get_started.cpp$$ $spell cppad_nlp IpoptDir CppAD $$ $section Nonlinear Programming Using CppAD and Ipopt: Example and Test$$ $mindex ipopt AD$$ $head Purpose$$ This example program demonstrates how to use $cref ipopt_solve$$ to solve the example problem in the Ipopt documentation; i.e., the problem $latex \[ \begin{array}{lc} {\rm minimize \; } & x_1 * x_4 * (x_1 + x_2 + x_3) + x_3 \\ {\rm subject \; to \; } & x_1 * x_2 * x_3 * x_4 \geq 25 \\ & x_1^2 + x_2^2 + x_3^2 + x_4^2 = 40 \\ & 1 \leq x_1, x_2, x_3, x_4 \leq 5 \end{array} \] $$ $head Configuration Requirement$$ This example will be compiled and tested provided that $cref ipopt_prefix$$ is specified on the $cref cmake$$ command line. $code $srcfile%example/ipopt_solve/get_started.cpp%0%// BEGIN C++%// END C++%1%$$ $$ $end */ // BEGIN C++ // system include files used for I/O # include // C style asserts # include // ipopt solve include file # include namespace { using CppAD::AD; class FG_eval { public: typedef CPPAD_TESTVECTOR( AD ) ADvector; // fg: function that evaluates the objective and constraints using the syntax void operator()(ADvector& fg, const ADvector& x) { assert( fg.size() == 3 ); assert( x.size() == 4 ); // Fortran style indexing AD x1 = x[0]; AD x2 = x[1]; AD x3 = x[2]; AD x4 = x[3]; // f(x) fg[0] = x1 * x4 * (x1 + x2 + x3) + x3; // g_1 (x) fg[1] = x1 * x2 * x3 * x4; // g_2 (x) fg[2] = x1 * x1 + x2 * x2 + x3 * x3 + x4 * x4; // return; } }; } bool get_started(void) { bool ok = true; size_t i; typedef CPPAD_TESTVECTOR( double ) Dvector; // number of independent variables (domain dimension for f and g) size_t nx = 4; // number of constraints (range dimension for g) size_t ng = 2; // initial value of the independent variables Dvector xi(nx); xi[0] = 1.0; xi[1] = 5.0; xi[2] = 5.0; xi[3] = 1.0; // lower and upper limits for x Dvector xl(nx), xu(nx); for(i = 0; i < nx; i++) { xl[i] = 1.0; xu[i] = 5.0; } // lower and upper limits for g Dvector gl(ng), gu(ng); gl[0] = 25.0; gu[0] = 1.0e19; gl[1] = 40.0; gu[1] = 40.0; // object that computes objective and constraints FG_eval fg_eval; // options std::string options; // turn off any printing options += "Integer print_level 0\n"; options += "String sb yes\n"; // maximum number of iterations options += "Integer max_iter 10\n"; // approximate accuracy in first order necessary conditions; // see Mathematical Programming, Volume 106, Number 1, // Pages 25-57, Equation (6) options += "Numeric tol 1e-6\n"; // derivative testing options += "String derivative_test second-order\n"; // maximum amount of random pertubation; e.g., // when evaluation finite diff options += "Numeric point_perturbation_radius 0.\n"; // place to return solution CppAD::ipopt::solve_result solution; // solve the problem CppAD::ipopt::solve( options, xi, xl, xu, gl, gu, fg_eval, solution ); // // Check some of the solution values // ok &= solution.status == CppAD::ipopt::solve_result::success; // exact solution double check_x[] = { 1.000000, 4.743000, 3.82115, 1.379408 }; double check_zl[] = { 1.087871, 0., 0., 0. }; double check_zu[] = { 0., 0., 0., 0. }; double rel_tol = 1e-6; // relative tolerance double abs_tol = 1e-6; // absolute tolerance for(i = 0; i < nx; i++) { ok &= CppAD::NearEqual( check_x[i], solution.x[i], rel_tol, abs_tol ); ok &= CppAD::NearEqual( check_zl[i], solution.zl[i], rel_tol, abs_tol ); ok &= CppAD::NearEqual( check_zu[i], solution.zu[i], rel_tol, abs_tol ); std::cout << "x[" << i << "] = " << solution.x[i] << std::endl; } return ok; } // main program that runs all the tests int main(void) { std::cout << "===== Ipopt with CppAD Testing =====" << std::endl; bool result = get_started(); std::cout << "Final checking: " << result << std::endl; } // END C++ ================================================ FILE: example/CppAD_started.cpp ================================================ /* -------------------------------------------------------------------------- CppAD: C++ Algorithmic Differentiation: Copyright (C) 2003-17 Bradley M. Bell CppAD is distributed under multiple licenses. This distribution is under the terms of the Eclipse Public License Version 1.0. A copy of this license is included in the COPYING file of this distribution. Please visit http://www.coin-or.org/CppAD/ for information on other licenses. -------------------------------------------------------------------------- */ /* $begin get_started.cpp$$ $spell cppad.hpp http://www.coin-or.org/CppAD/ getstarted namespace iostream const std powx Jacobian jac endl da cout $$ $section Getting Started Using CppAD to Compute Derivatives$$ $mindex simple example start$$ $head Purpose$$ Demonstrate the use of CppAD by computing the derivative of a simple example function. $head Function$$ The example function $latex f : \B{R} \rightarrow \B{R}$$ is defined by $latex \[ f(x) = a_0 + a_1 * x^1 + \cdots + a_{k-1} * x^{k-1} \] $$ where $icode a$$ is a fixed vector of length $icode k$$. $head Derivative$$ The derivative of $latex f(x)$$ is given by $latex \[ f' (x) = a_1 + 2 * a_2 * x + \cdots + (k-1) * a_{k-1} * x^{k-2} \] $$ $head Value$$ For the particular case in this example, $latex k$$ is equal to 5, $latex a = (1, 1, 1, 1, 1)$$, and $latex x = 3$$. If follows that $latex \[ f' ( 3 ) = 1 + 2 * 3 + 3 * 3^2 + 4 * 3^3 = 142 \] $$ $head Poly$$ The routine $code Poly$$ is defined below for this particular application. A general purpose polynomial evaluation routine is documented and distributed with CppAD (see $cref Poly$$). $head Exercises$$ Modify the program below to accomplish the following tasks using CppAD: $list number$$ Compute and print the derivative of $latex f(x) = 1 + x + x^2 + x^3 + x^4$$ at the point $latex x = 2$$. $lnext Compute and print the derivative of $latex f(x) = 1 + x + x^2 / 2$$ at the point $latex x = .5$$. $lnext Compute and print the derivative of $latex f(x) = \exp (x) - 1 - x - x^2 / 2$$ at the point $latex x = .5$$. $lend $head Program$$ $srccode%cpp% */ #include // standard input/output #include // standard vector #include // the CppAD package http://www.coin-or.org/CppAD/ namespace { // define y(x) = Poly(a, x) in the empty namespace template Type Poly(const std::vector &a, const Type &x) { size_t k = a.size(); Type y = 0.; // initialize summation Type x_i = 1.; // initialize x^i size_t i; for(i = 0; i < k; i++) { y += a[i] * x_i; // y = y + a_i * x^i x_i *= x; // x_i = x_i * x } return y; } } // main program int main(void) { std::cout << "===== CppAD Diff Testing =====" << std::endl; using CppAD::AD; // use AD as abbreviation for CppAD::AD using std::vector; // use vector as abbreviation for std::vector size_t i; // a temporary index // vector of polynomial coefficients size_t k = 5; // number of polynomial coefficients vector a(k); // vector of polynomial coefficients for(i = 0; i < k; i++) a[i] = 1.; // value of polynomial coefficients // domain space vector size_t n = 1; // number of domain space variables vector< AD > X(n); // vector of domain space variables X[0] = 3.; // value corresponding to operation sequence // declare independent variables and start recording operation sequence CppAD::Independent(X); // range space vector size_t m = 1; // number of ranges space variables vector< AD > Y(m); // vector of ranges space variables Y[0] = Poly(a, X[0]); // value during recording of operations // store operation sequence in f: X -> Y and stop recording CppAD::ADFun f(X, Y); // compute derivative using operation sequence stored in f vector jac(m * n); // Jacobian of f (m by n matrix) vector x(n); // domain space vector x[0] = 3.; // argument value for derivative jac = f.Jacobian(x); // Jacobian for operation sequence // print the results std::cout << "f'(3) computed by CppAD = " << jac[0] << std::endl; // check if the derivative is correct int error_code; if( jac[0] == 142. ) error_code = 0; // return code for correct case else error_code = 1; // return code for incorrect case return error_code; } /* %$$ $head Output$$ Executing the program above will generate the following output: $codep f'(3) computed by CppAD = 142 $$ $head Running$$ To build and run this program see $cref cmake_check$$. $end */ ================================================ FILE: include/MPC.h ================================================ /* # Copyright 2018 HyphaROS Workshop. # Developer: HaoChih, LIN (hypha.ros@gmail.com) # # 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_H #define MPC_H #include #include #include using namespace std; class MPC { public: MPC(); // Solve the model given an initial state and polynomial coefficients. // Return the first actuatotions. vector Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs); vector mpc_x; vector mpc_y; void LoadParams(const std::map ¶ms); private: // Parameters for mpc solver double _max_steering, _max_throttle, _bound_value; int _mpc_steps, _x_start, _y_start, _psi_start, _v_start, _cte_start, _epsi_start, _delta_start, _a_start; std::map _params; }; #endif /* MPC_H */ ================================================ FILE: launch/Desktop/HyphaROS_Desktop_Mapping.launch ================================================ ================================================ FILE: launch/Desktop/HyphaROS_Desktop_Racing.launch ================================================ ================================================ FILE: launch/HyphaROS_MiniCar_Mapping.launch ================================================ ================================================ FILE: launch/HyphaROS_MiniCar_Racing.launch ================================================ ================================================ FILE: launch/Simulation/HyphaROS_Simulation_Stage.launch ================================================ ================================================ FILE: launch/Simulation/maps/maze.yaml ================================================ image: maze.png resolution: 0.05 origin: [0.0, 0.0, 0.0] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 ================================================ FILE: launch/Simulation/params/amcl_params.yaml ================================================ use_map_topic: true odom_frame_id: "odom" base_frame_id: "base_footprint" global_frame_id: "/map" ## Publish scans from best pose at a max of 10 Hz odom_model_type: "diff" odom_alpha5: 0.1 gui_publish_rate: 10.0 laser_max_beams: 60 laser_max_range: 12.0 min_particles: 500 max_particles: 2000 kld_err: 0.05 kld_z: 0.99 odom_alpha1: 0.2 odom_alpha2: 0.2 ## translation std dev, m odom_alpha3: 0.2 odom_alpha4: 0.2 laser_z_hit: 0.5 aser_z_short: 0.05 laser_z_max: 0.05 laser_z_rand: 0.5 laser_sigma_hit: 0.2 laser_lambda_short: 0.1 laser_model_type: "likelihood_field" # "likelihood_field" or "beam" laser_likelihood_max_dist: 2.0 update_min_d: 0.25 update_min_a: 0.2 resample_interval: 1 ## Increase tolerance because the computer can get quite busy transform_tolerance: 1.0 recovery_alpha_slow: 0.001 recovery_alpha_fast: 0.1 ================================================ FILE: launch/Simulation/params/carlike/costmap_common_params.yaml ================================================ #---standard pioneer footprint--- #---(in meters)--- #footprint: [ [0.254, -0.0508], [0.1778, -0.0508], [0.1778, -0.1778], [-0.1905, -0.1778], [-0.254, 0], [-0.1905, 0.1778], [0.1778, 0.1778], [0.1778, 0.0508], [0.254, 0.0508] ] #footprint: [ [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] ] footprint: [ [-0.6,-0.125], [0.0,-0.125], [0.0,0.125], [-0.6,0.125] ] transform_tolerance: 0.2 map_type: costmap obstacle_layer: enabled: true obstacle_range: 3.0 raytrace_range: 3.5 inflation_radius: 0.2 track_unknown_space: false combination_method: 1 observation_sources: laser_scan_sensor laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true} inflation_layer: enabled: true cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10) inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths. static_layer: enabled: true map_topic: "/map" ================================================ FILE: launch/Simulation/params/carlike/global_costmap_params.yaml ================================================ global_costmap: global_frame: /map robot_base_frame: base_link update_frequency: 1.0 publish_frequency: 0.5 static_map: true transform_tolerance: 0.5 plugins: - {name: static_layer, type: "costmap_2d::StaticLayer"} - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} - {name: inflation_layer, type: "costmap_2d::InflationLayer"} inflation_layer: inflation_radius: 2.5 cost_scaling_factor: 2.0 ================================================ FILE: launch/Simulation/params/carlike/global_planner.yaml ================================================ GlobalPlanner: use_dijkstra: true use_grid_path: false ================================================ FILE: launch/Simulation/params/carlike/local_costmap_params.yaml ================================================ local_costmap: global_frame: /map robot_base_frame: base_link update_frequency: 10.0 publish_frequency: 10.0 static_map: false rolling_window: true width: 5.5 height: 5.5 resolution: 0.2 transform_tolerance: 0.5 plugins: - {name: static_layer, type: "costmap_2d::StaticLayer"} - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} ================================================ FILE: launch/Simulation/params/carlike/teb_local_planner_params.yaml ================================================ TebLocalPlannerROS: odom_topic: odom # Trajectory teb_autosize: True dt_ref: 0.3 dt_hysteresis: 0.1 global_plan_overwrite_orientation: True allow_init_with_backwards_motion: True max_global_plan_lookahead_dist: 3.0 feasibility_check_no_poses: 2 # Robot max_vel_x: 0.4 max_vel_x_backwards: 0.2 max_vel_y: 0.0 max_vel_theta: 0.3 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega) acc_lim_x: 0.5 acc_lim_theta: 0.5 # ********************** Carlike robot parameters ******************** min_turning_radius: 0.5 # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually) wheelbase: 0.4 # Wheelbase of our robot cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message) # ******************************************************************** footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" type: "line" radius: 0.2 # for type "circular" line_start: [0.0, 0.0] # for type "line" line_end: [0.4, 0.0] # for type "line" front_offset: 0.2 # for type "two_circles" front_radius: 0.2 # for type "two_circles" rear_offset: 0.2 # for type "two_circles" rear_radius: 0.2 # for type "two_circles" vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon" # GoalTolerance xy_goal_tolerance: 0.2 yaw_goal_tolerance: 0.1 free_goal_vel: False # Obstacles min_obstacle_dist: 0.27 # This value must also include our robot's expansion, since footprint_model is set to "line". include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 1.0 obstacle_poses_affected: 30 costmap_converter_plugin: "" costmap_converter_spin_thread: True costmap_converter_rate: 5 # Optimization no_inner_iterations: 5 no_outer_iterations: 4 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.1 weight_max_vel_x: 2 weight_max_vel_theta: 1 weight_acc_lim_x: 1 weight_acc_lim_theta: 1 weight_kinematics_nh: 1000 weight_kinematics_forward_drive: 1 weight_kinematics_turning_radius: 1 weight_optimaltime: 1 weight_obstacle: 50 weight_dynamic_obstacle: 10 # not in use yet # Homotopy Class Planner enable_homotopy_class_planning: True enable_multithreading: True simple_exploration: False max_number_classes: 4 selection_cost_hysteresis: 1.0 selection_obst_cost_scale: 1.0 selection_alternative_time_cost: False roadmap_graph_no_samples: 15 roadmap_graph_area_width: 5 h_signature_prescaler: 0.5 h_signature_threshold: 0.1 obstacle_keypoint_offset: 0.1 obstacle_heading_threshold: 0.45 visualize_hc_graph: False ================================================ FILE: launch/Simulation/params/mpc_params.yaml ================================================ # Parameters for control loop pub_twist_cmd: true debug_info: false delay_mode: true max_speed: 2.0 # unit: m/s waypoints_dist: -1.0 # unit: m, set < 0 means computed by node path_length: 8.0 # unit: m goal_radius: 0.5 # unit: m controller_freq: 10 vehicle_Lf: 0.25 # distance between the front of the vehicle and its center of gravity # Parameter for MPC solver mpc_steps: 20.0 mpc_ref_cte: 0.0 mpc_ref_epsi: 0.0 mpc_ref_vel: 1.5 mpc_w_cte: 100.0 mpc_w_epsi: 100.0 mpc_w_vel: 100.0 mpc_w_delta: 100.0 mpc_w_accel: 50.0 mpc_w_delta_d: 0.0 mpc_w_accel_d: 0.0 mpc_max_steering: 0.523 # Maximal steering radian (~30 deg) mpc_max_throttle: 1.0 # Maximal throttle accel mpc_bound_value: 1.0e3 # Bound value for other variables ================================================ FILE: launch/Simulation/params/pure_pursuit_params.yaml ================================================ # Parameters for pure pursuit controller L: 0.5 # Length of car (m) lfw: 0.25 # distance between the front of the vehicle and its center of gravity Vcmd: 2.0 # reference speed (m/s) Lfw: 1.0 # forward look ahead distance (m) goal_radius: 1.0 # unit: m controller_freq: 20 # unit: Hz steering_gain: 1.0 base_angle: 0.0 # neutral point of servo (rad) cmd_vel_mode: true # whether or not publishing cmd_vel smooth_accel: true # whether or not smoothing the acceleration of car speed_incremental: 0.05 # speed incremental value (discrete acceleraton), unit: m/s debug_mode: true ================================================ FILE: launch/Simulation/params/rviz_navigation.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 81 Name: Displays Property Tree Widget: Expanded: - /Global Planner1/Potential map1 - /Local Planner1/Local costmap1 Splitter Ratio: 0.605095983 Tree Height: 562 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" Visualization Manager: Class: "" Displays: - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true base_footprint: Value: true base_laser_link: Value: true base_link: Value: true map: Value: true odom: Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: map: odom: base_footprint: base_link: base_laser_link: {} Update Interval: 0 Value: true - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 249; 249; 255 Enabled: true Line Style: Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 50 Reference Frame: Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.100000001 Class: rviz/Pose Color: 255; 25; 0 Enabled: true Head Length: 0.300000012 Head Radius: 0.100000001 Name: Pose Shaft Length: 1 Shaft Radius: 0.0500000007 Shape: Arrow Topic: /move_base/current_goal Unreliable: false Value: true - Alpha: 0.400000006 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /map Unreliable: false Use Timestamp: false Value: true - Class: rviz/Group Displays: - Alpha: 1 Class: rviz/Polygon Color: 85; 0; 255 Enabled: true Name: Robot Footprint Topic: /move_base/local_costmap/footprint Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 1 Min Color: 0; 0; 0 Min Intensity: 1 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.00999999978 Style: Flat Squares Topic: /scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Arrow Length: 0.100000001 Axes Length: 0.300000012 Axes Radius: 0.00999999978 Class: rviz/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.0700000003 Head Radius: 0.0299999993 Name: AMCL Particles Shaft Length: 0.230000004 Shaft Radius: 0.00999999978 Shape: Arrow (Flat) Topic: /particlecloud Unreliable: false Value: false - Class: rviz/TF Enabled: false Frame Timeout: 15 Frames: All Enabled: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: {} Update Interval: 0 Value: false - Angle Tolerance: 0.100000001 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.300000012 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 100 Name: Odometry Position Tolerance: 0.100000001 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.100000001 Color: 255; 25; 0 Head Length: 0.300000012 Head Radius: 0.100000001 Shaft Length: 1 Shaft Radius: 0.0500000007 Value: Arrow Topic: /odom Unreliable: false Value: false Enabled: true Name: Robot - Class: rviz/Group Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.300000012 Head Length: 0.200000003 Length: 0.300000012 Line Style: Lines Line Width: 0.0299999993 Name: GlobalPath Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.0299999993 Shaft Diameter: 0.100000001 Shaft Length: 0.100000001 Topic: /move_base/GlobalPlanner/plan Unreliable: false Value: true - Alpha: 0.400000006 Class: rviz/Map Color Scheme: costmap Draw Behind: false Enabled: true Name: Potential map Topic: /move_base/GlobalPlanner/potential Unreliable: false Use Timestamp: false Value: true Enabled: true Name: Global Planner - Class: rviz/Group Displays: - Alpha: 1 Arrow Length: 0.300000012 Axes Length: 0.300000012 Axes Radius: 0.00999999978 Class: rviz/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.0700000003 Head Radius: 0.0299999993 Name: TebPoses Shaft Length: 0.230000004 Shaft Radius: 0.00999999978 Shape: Arrow (Flat) Topic: /move_base/TebLocalPlannerROS/teb_poses Unreliable: false Value: false - Class: rviz/Marker Enabled: false Marker Topic: /move_base/TebLocalPlannerROS/teb_markers Name: TebMarker Namespaces: {} Queue Size: 100 Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: false Head Diameter: 0.300000012 Head Length: 0.200000003 Length: 0.300000012 Line Style: Lines Line Width: 0.0299999993 Name: LocalPath Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.0299999993 Shaft Diameter: 0.100000001 Shaft Length: 0.100000001 Topic: /move_base/TrajectoryPlannerROS/global_plan Unreliable: false Value: false - Alpha: 0.800000012 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Local costmap Topic: /move_base/local_costmap/costmap Unreliable: false Use Timestamp: false Value: true Enabled: true Name: Local Planner - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 85; 85; 255 Enabled: true Head Diameter: 0.300000012 Head Length: 0.200000003 Length: 0.300000012 Line Style: Lines Line Width: 0.0299999993 Name: MPC_Reference Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.0299999993 Shaft Diameter: 0.100000001 Shaft Length: 0.100000001 Topic: /mpc_reference Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true Head Diameter: 0.300000012 Head Length: 0.00999999978 Length: 0.200000003 Line Style: Lines Line Width: 0.0299999993 Name: MPC_Trajectory Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: Axes Radius: 0.00999999978 Shaft Diameter: 0.100000001 Shaft Length: 0.00999999978 Topic: /mpc_trajectory Unreliable: false Value: true - Class: rviz/Marker Enabled: true Marker Topic: /pure_pursuit/path_marker Name: Pure_Pursuit Namespaces: Markers: true Queue Size: 100 Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 13.6532335 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 4.77877808 Y: 4.80319309 Z: -1.40279174 Focal Shape Fixed Size: true Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 Pitch: 1.56480002 Target Frame: Value: Orbit (rviz) Yaw: 4.71043015 Saved: ~ Window Geometry: Displays: collapsed: false Height: 846 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000034f000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1215 X: 65 Y: 25 ================================================ FILE: launch/Simulation/stage/maze_carlike.world ================================================ include "robots/carlike_robot.inc" define floorplan model ( # sombre, sensible, artistic color "gray30" # most maps will need a bounding box boundary 1 gui_nose 0 gui_grid 0 gui_outline 0 gripper_return 0 fiducial_return 0 laser_return 1 ) resolution 0.02 interval_sim 100 # simulation timestep in milliseconds window ( size [ 600.0 700.0 ] center [ 0.0 0.0 ] rotate [ 0.0 0.0 ] scale 60 ) floorplan ( name "maze" bitmap "../maps/maze.png" size [ 10.0 10.0 2.0 ] pose [ 5.0 5.0 0.0 0.0 ] ) # throw in a robot carlike_robot ( pose [ 1.0 1.0 0.0 0.0 ] name "robot" ) ================================================ FILE: launch/Simulation/stage/robots/carlike_robot.inc ================================================ define laser ranger ( sensor ( range_max 6.5 fov 180.0 samples 640 ) # generic model properties color "black" size [ 0.06 0.15 0.03 ] ) # # Robot model: # footprint (counter-clockwise): [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] # center of rotation: [0,0] # wheelbase: 0.4 define carlike_robot position ( pose [ 0.0 0.0 0.0 0.0 ] odom_error [0.03 0.03 999999 999999 999999 0.02] size [ 0.6 0.25 0.40 ] # this models the footprint (rectangular), but shifted such that the bottom-left corner is in [0,0]. The center of rotation now here at [0.3, 0.125, 0.2] # correct center of rotation: origin [ 0.3 0.0 0.0 0.0] gui_nose 1 color "red" # kinematics drive "car" wheelbase 0.4 # distance between both axles # spawn sensors laser(pose [ -0.1 0.0 -0.11 0.0 ]) ) ================================================ FILE: launch/includes/HyphaROS_MiniCar_Drivers.launch.xml ================================================ ================================================ FILE: launch/includes/amcl.launch.xml ================================================ ================================================ FILE: launch/includes/gmapping.launch.xml ================================================ ================================================ FILE: launch/includes/hector_slam.launch.xml ================================================ ================================================ FILE: launch/includes/karto_slam.launch.xml ================================================ ================================================ FILE: launch/includes/minicar_base.launch.xml ================================================ ================================================ FILE: launch/includes/minicar_tf.launch.xml ================================================ ================================================ FILE: launch/includes/mrpt_icp.launch.xml ================================================ ================================================ FILE: launch/includes/ydlidar.launch.xml ================================================ ================================================ FILE: launch/params/costmap_common_params.yaml ================================================ #---standard pioneer footprint--- #---(in meters)--- #footprint: [ [0.254, -0.0508], [0.1778, -0.0508], [0.1778, -0.1778], [-0.1905, -0.1778], [-0.254, 0], [-0.1905, 0.1778], [0.1778, 0.1778], [0.1778, 0.0508], [0.254, 0.0508] ] #footprint: [ [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] ] footprint: [ [-0.145,-0.08], [0.0,-0.08], [0.0,0.08], [-0.145,0.08] ] transform_tolerance: 0.2 map_type: costmap obstacle_layer: enabled: true obstacle_range: 3.0 raytrace_range: 3.5 inflation_radius: 0.2 track_unknown_space: false combination_method: 1 observation_sources: base_scan base_scan: {sensor_frame: laser_link, data_type: LaserScan, topic: scan, marking: true, clearing: true} inflation_layer: enabled: true cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10) inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths. static_layer: enabled: true map_topic: "/map" ================================================ FILE: launch/params/global_costmap_params.yaml ================================================ global_costmap: global_frame: /map robot_base_frame: base_link update_frequency: 1.0 publish_frequency: 0.5 static_map: true transform_tolerance: 0.5 plugins: - {name: static_layer, type: "costmap_2d::StaticLayer"} - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} - {name: inflation_layer, type: "costmap_2d::InflationLayer"} inflation_layer: inflation_radius: 1.0 cost_scaling_factor: 6.0 ================================================ FILE: launch/params/global_planner.yaml ================================================ GlobalPlanner: use_dijkstra: true use_grid_path: false ================================================ FILE: launch/params/local_costmap_params.yaml ================================================ local_costmap: global_frame: /map robot_base_frame: base_link update_frequency: 10.0 publish_frequency: 10.0 static_map: false rolling_window: true width: 1.5 height: 1.5 resolution: 0.2 transform_tolerance: 0.5 plugins: - {name: static_layer, type: "costmap_2d::StaticLayer"} - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} ================================================ FILE: launch/params/mpc/mpc_params.yaml ================================================ # Parameters for control loop pub_twist_cmd: true debug_info: false delay_mode: true max_speed: 1.5 # unit: m/s waypoints_dist: -1.0 # unit: m, set < 0 means computed by node path_length: 3.0 # unit: m goal_radius: 0.2 # unit: m controller_freq: 10 vehicle_Lf: 0.0723 # distance between the front of the vehicle and its center of gravity # Parameter for MPC solver mpc_steps: 20.0 mpc_ref_cte: 0.0 mpc_ref_epsi: 0.0 mpc_ref_vel: 1.0 mpc_w_cte: 100.0 mpc_w_epsi: 100.0 mpc_w_vel: 100.0 mpc_w_delta: 100.0 mpc_w_accel: 50.0 mpc_w_delta_d: 0.0 mpc_w_accel_d: 0.0 mpc_max_steering: 0.436 # Maximal steering radian (~25 deg) mpc_max_throttle: 0.8 # Maximal throttle accel mpc_bound_value: 1.0e3 # Bound value for other variables ================================================ FILE: launch/params/pure_pursuit/pure_pursuit_params.yaml ================================================ # Parameters for pure pursuit controller L: 0.1446 # Length of car (m) lfw: 0.0723 # distance between the front of the vehicle and its center of gravity Vcmd: 1.5 # reference speed (m/s) Lfw: 1.0 # forward look ahead distance (m) goal_radius: 1.0 # unit: m controller_freq: 20 # unit: Hz steering_gain: 1.2 base_angle: 0.0 # neutral point of servo (rad) cmd_vel_mode: false # whether or not publishing cmd_vel smooth_accel: true # whether or not smoothing the acceleration of car speed_incremental: 0.05 # speed incremental value (discrete acceleraton), unit: m/s debug_mode: false ================================================ FILE: launch/params/teb/teb_local_planner_params.yaml ================================================ TebLocalPlannerROS: odom_topic: odom # Trajectory teb_autosize: True dt_ref: 0.3 dt_hysteresis: 0.1 global_plan_overwrite_orientation: True allow_init_with_backwards_motion: True max_global_plan_lookahead_dist: 3.0 feasibility_check_no_poses: 2 # Robot max_vel_x: 0.4 max_vel_x_backwards: 0.2 max_vel_y: 0.0 max_vel_theta: 0.3 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega) acc_lim_x: 0.5 acc_lim_theta: 0.5 # ********************** Carlike robot parameters ******************** min_turning_radius: 0.5 # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually) wheelbase: 0.4 # Wheelbase of our robot cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message) # ******************************************************************** footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" type: "line" radius: 0.2 # for type "circular" line_start: [0.0, 0.0] # for type "line" line_end: [0.4, 0.0] # for type "line" front_offset: 0.2 # for type "two_circles" front_radius: 0.2 # for type "two_circles" rear_offset: 0.2 # for type "two_circles" rear_radius: 0.2 # for type "two_circles" vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon" # GoalTolerance xy_goal_tolerance: 0.2 yaw_goal_tolerance: 0.1 free_goal_vel: False # Obstacles min_obstacle_dist: 0.27 # This value must also include our robot's expansion, since footprint_model is set to "line". include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 1.0 obstacle_poses_affected: 30 costmap_converter_plugin: "" costmap_converter_spin_thread: True costmap_converter_rate: 5 # Optimization no_inner_iterations: 5 no_outer_iterations: 4 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.1 weight_max_vel_x: 2 weight_max_vel_theta: 1 weight_acc_lim_x: 1 weight_acc_lim_theta: 1 weight_kinematics_nh: 1000 weight_kinematics_forward_drive: 1 weight_kinematics_turning_radius: 1 weight_optimaltime: 1 weight_obstacle: 50 weight_dynamic_obstacle: 10 # not in use yet # Homotopy Class Planner enable_homotopy_class_planning: True enable_multithreading: True simple_exploration: False max_number_classes: 4 selection_cost_hysteresis: 1.0 selection_obst_cost_scale: 1.0 selection_alternative_time_cost: False roadmap_graph_no_samples: 15 roadmap_graph_area_width: 5 h_signature_prescaler: 0.5 h_signature_threshold: 0.1 obstacle_keypoint_offset: 0.1 obstacle_heading_threshold: 0.45 visualize_hc_graph: False ================================================ FILE: map/map_amcl.pgm ================================================ P5 # CREATOR: Map_generator.cpp 0.050 m/pix 1984 1984 255 ================================================ FILE: map/map_amcl.yaml ================================================ image: map_amcl.pgm resolution: 0.050000 origin: [-50.000000, -50.000000, 0.000000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 ================================================ FILE: map/map_nav.pgm ================================================ P5 # CREATOR: GIMP PNM Filter Version 1.1 1984 1984 255 ================================================ FILE: map/map_nav.yaml ================================================ image: map_nav.pgm resolution: 0.050000 origin: [-50.000000, -50.000000, 0.000000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 ================================================ FILE: package.xml ================================================ hypharos_minicar 0.0.0 The hypharos_minicar package HaoChih, Lin Apache 2.0 https://hypharosworkshop.wordpress.com/ catkin costmap_2d dynamic_reconfigure geometry_msgs message_generation move_base nav_msgs ackermann_msgs roscpp rospy std_msgs tf visualization_msgs costmap_2d dynamic_reconfigure geometry_msgs move_base nav_msgs ackermann_msgs roscpp rospy std_msgs tf visualization_msgs costmap_2d dynamic_reconfigure geometry_msgs move_base nav_msgs ackermann_msgs roscpp rospy std_msgs tf visualization_msgs ================================================ FILE: readme.md ================================================ # HyphaROS MiniCar (1/20 Scale MPC Racing Car) ![alt text](https://github.com/Hypha-ROS/hypharos_minicar/blob/master/document/photo/HyphaROS_logo.png) ## Abstract Low cost, High speed 1/20 Racing Car for control laws evaluation ! Fully open-sourced (hardware & software), total cost <300USD. Currently supports: Pure-Pursuit, Model-Predictive-Control (Nonlinear) ![alt text](https://github.com/Hypha-ROS/hypharos_minicar/blob/master/document/photo/HyphaROS_MiniCar_photo.jpg) ## About us FB Page: https://www.facebook.com/HyphaROS/ Website: https://hypharosworkshop.wordpress.com/ Contact: hypha.ros@gmail.com Developer: * HaoChih, LIN Date: 2018/08/16 License: Apache 2.0 ## Features * Nonlinear Bicycle Model Based MPC (through ipopt solver) * Pure-Pursuit Controller * Onboard mapping (Gmapping, Hector-SLAM, Karto-SLAM, MRPT-ICP) * STM32 for motor speed closed-loop control * AMCL localization (encoder-odometry based) * Dynamic obstacle avoidance * Stage Simulation (supports: MPC & Pure-Pursuit) ## Roadmap * Add EKF supports (odometry with mpu6050) * MPC for obstacle avoidance * Implement MPC on different solvers (ACADO, OSQP, etc) * Multi-cars racing through ROS 2.0 layer * High Speed Drifting ## Hardware * Odroid XU4 (Ref: https://www.hardkernel.com/main/products/prdt_info.php) * YDLidar X4 (Ref: http://www.ydlidar.com/product/X4) * STM32(F103) MCU (with OLED display) * Diff Motor with A/B encoder(res: 340) * Ackermann Based 1/20 car chassis (Ref: https://item.taobao.com/item.htm?spm=a312a.7700824.w4004-15726392027.68.79503c88Rqwzb9&id=554619475840) Total Cost: < 300 USD ## Document HyphaROS MPC MiniCar 1-Day Workshop: https://drive.google.com/open?id=1yX0aeA4spf_szpxXFpIlH0EQKIgiwJx7 ROS Summer School in China 2018 Slides: https://goo.gl/RpVBDH ## Software ### VirtualBox Image ### OVA image file (Kinetic, password: hypharos, 20180816) Link: https://drive.google.com/open?id=1uRvXGakvQrbynmPHX_KIFJxPm8o6MWPb ### Odroid Image Image file for Odroid XU4.(with SD card >=16G, password: hypharos) Link: https://goo.gl/87vrNk (if your SD card is around 13GB, it's OK to force Win32DiskImager to write the file!) The default ethernet IP address is 10.0.0.1 ### STM32 (MCU) Source codes: https://drive.google.com/open?id=19rjjpJmz85lTSxCyu-9CtvZhUW37c2LS [WARNING!] Since the original STM32 codes came from third-paries, currently, the quality of codes are not guaranteed by us. For MCUISP Driver: http://www.mcuisp.com/English%20mcuisp%20web/ruanjianxiazai-english.htm ### Install from source (16.04) 1. Install ROS Kinetic (Desktop-Full) (http://wiki.ros.org/kinetic/Installation/Ubuntu) 2. Install dependencies: $ sudo apt-get install remmina synaptic gimp git ros-kinetic-navigation* ros-kinetic-gmapping ros-kinetic-hector-slam ros-kinetic-mrpt-icp-slam-2d ros-kinetic-slam-karto ros-kinetic-ackermann-msgs -y 3. Install Ipopt: Please refer the tutorial in "document/ipopt_install". 4. create your own catkin_ws (http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment#Create_a_ROS_Workspace) $ cd catkin_ws/src $ git clone https://github.com/EAIBOT/ydlidar $ git clone https://github.com/Hypha-ROS/hypharos_minicar $ cd .. $ catkin_make ## Operation ### Simulation $ roslaunch hypharos_minicar HyphaROS_Simulation_Stage.launch The default controller is mpc, you can switch to pure-pursuit or DWA through param: "controller" ![alt text](https://github.com/Hypha-ROS/hypharos_minicar/blob/master/document/photo/HyphaROS_MPC_MiniCar_Simulation.jpg) ### Ethernet Connection The default static eth IP on Odroid image is 10.0.0.1, hence, to connect to your Odroid through cable, please set your host IP as 10.0.0.X Notice: for the first bootup, you have to update Odroid MAC address through HDMI Dispaly! ### Wifi Connection Use ethernet or display connection to make Odroid connect to your local Wifi AP. Remember to set ROS_MASTER_URI and ROS_IP in ".bashrc" file on both Odroid & host machine. ### Mapping * MiniCar (Odroid) side: $ roslaunch hypharos_minicar HyphaROS_MiniCar_Mapping.launch The default mapping algorithm is gmapping, you can swith to other slam method through param: "slam_type" (crrently supports: gmapping, karto_slam, mrpt_icp and hector_slam) * Host (Desktop) side: $ roslaunch hypharos_minicar HyphaROS_Desktop_Mapping.launch Use keyboard to control the minicar. After mapping, remember to save two maps, one for amcl and the other for move_base! ### Racing * MiniCar (Odroid) side: $ roslaunch hypharos_minicar HyphaROS_MiniCar_Racing.launch The default controller is mpc, you can swith to other slam method through param: "controller" (crrently supports: mpc and pure_pursuit) * Host (Desktop) side: $ roslaunch hypharos_minicar HyphaROS_Desktop_Racing.launch Use keyboard to interrupt controller's behavior. ================================================ FILE: rviz/hypharos_minicar_mapping.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 Tree Height: 775 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: LaserScan Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Alpha: 0.699999988 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /map Unreliable: false Use Timestamp: false Value: true - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true base_footprint: Value: true base_link: Value: true imu_link: Value: true laser_link: Value: true map: Value: true odom: Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: map: odom: base_footprint: base_link: imu_link: {} laser_link: {} Update Interval: 0 Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 10 Min Color: 0; 0; 0 Min Intensity: 10 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.00999999978 Style: Flat Squares Topic: /scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Angle Tolerance: 0.100000001 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.300000012 Color: 204; 51; 204 Scale: 1 Value: true Value: false Enabled: true Keep: 100 Name: Odometry Position Tolerance: 0.100000001 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.100000001 Color: 255; 25; 0 Head Length: 0.100000001 Head Radius: 0.0500000007 Shaft Length: 0.100000001 Shaft Radius: 0.0199999996 Value: Arrow Topic: /odom Unreliable: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 5.40849447 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 0.00899724383 Y: -0.074087061 Z: 0.856899977 Focal Shape Fixed Size: true Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 Pitch: 1.56979632 Target Frame: Value: Orbit (rviz) Yaw: 3.09352851 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1056 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002c7000000f70000000000000000000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1855 X: 65 Y: 24 ================================================ FILE: rviz/hypharos_minicar_racing.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 81 Name: Displays Property Tree Widget: Expanded: - /Global Planner1/Potential map1 - /Local Planner1/Local costmap1 Splitter Ratio: 0.605095983 Tree Height: 562 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: LaserScan Visualization Manager: Class: "" Displays: - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true base_footprint: Value: true base_link: Value: true imu_link: Value: true laser_link: Value: true map: Value: true odom: Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: map: odom: base_footprint: base_link: imu_link: {} laser_link: {} Update Interval: 0 Value: true - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 249; 249; 255 Enabled: true Line Style: Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 50 Reference Frame: Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.100000001 Class: rviz/Pose Color: 255; 25; 0 Enabled: true Head Length: 0.300000012 Head Radius: 0.100000001 Name: Pose Shaft Length: 1 Shaft Radius: 0.0500000007 Shape: Arrow Topic: /move_base/current_goal Unreliable: false Value: true - Alpha: 0.400000006 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /map Unreliable: false Use Timestamp: false Value: true - Class: rviz/Group Displays: - Alpha: 1 Class: rviz/Polygon Color: 85; 0; 255 Enabled: true Name: Robot Footprint Topic: /move_base/local_costmap/footprint Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 1 Min Color: 0; 0; 0 Min Intensity: 1 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.00999999978 Style: Flat Squares Topic: /scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Arrow Length: 0.100000001 Axes Length: 0.300000012 Axes Radius: 0.00999999978 Class: rviz/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.0700000003 Head Radius: 0.0299999993 Name: AMCL Particles Shaft Length: 0.230000004 Shaft Radius: 0.00999999978 Shape: Arrow (Flat) Topic: /particlecloud Unreliable: false Value: false - Class: rviz/TF Enabled: false Frame Timeout: 15 Frames: All Enabled: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: {} Update Interval: 0 Value: false - Angle Tolerance: 0.100000001 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.300000012 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 100 Name: Odometry Position Tolerance: 0.100000001 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.100000001 Color: 255; 25; 0 Head Length: 0.300000012 Head Radius: 0.100000001 Shaft Length: 1 Shaft Radius: 0.0500000007 Value: Arrow Topic: /odom Unreliable: false Value: false Enabled: true Name: Robot - Class: rviz/Group Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.300000012 Head Length: 0.200000003 Length: 0.300000012 Line Style: Lines Line Width: 0.0299999993 Name: GlobalPath Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.0299999993 Shaft Diameter: 0.100000001 Shaft Length: 0.100000001 Topic: /move_base/GlobalPlanner/plan Unreliable: false Value: true - Alpha: 0.400000006 Class: rviz/Map Color Scheme: costmap Draw Behind: false Enabled: true Name: Potential map Topic: /move_base/GlobalPlanner/potential Unreliable: false Use Timestamp: false Value: true Enabled: true Name: Global Planner - Class: rviz/Group Displays: - Alpha: 1 Arrow Length: 0.300000012 Axes Length: 0.300000012 Axes Radius: 0.00999999978 Class: rviz/PoseArray Color: 255; 25; 0 Enabled: false Head Length: 0.0700000003 Head Radius: 0.0299999993 Name: TebPoses Shaft Length: 0.230000004 Shaft Radius: 0.00999999978 Shape: Arrow (Flat) Topic: /move_base/TebLocalPlannerROS/teb_poses Unreliable: false Value: false - Class: rviz/Marker Enabled: false Marker Topic: /move_base/TebLocalPlannerROS/teb_markers Name: TebMarker Namespaces: {} Queue Size: 100 Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: false Head Diameter: 0.300000012 Head Length: 0.200000003 Length: 0.300000012 Line Style: Lines Line Width: 0.0299999993 Name: LocalPath Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.0299999993 Shaft Diameter: 0.100000001 Shaft Length: 0.100000001 Topic: /move_base/TrajectoryPlannerROS/global_plan Unreliable: false Value: false - Alpha: 0.200000003 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Local costmap Topic: /move_base/local_costmap/costmap Unreliable: false Use Timestamp: false Value: true Enabled: true Name: Local Planner - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 85; 85; 255 Enabled: true Head Diameter: 0.300000012 Head Length: 0.200000003 Length: 0.300000012 Line Style: Lines Line Width: 0.0299999993 Name: MPC_Reference Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.0299999993 Shaft Diameter: 0.100000001 Shaft Length: 0.100000001 Topic: /mpc_reference Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true Head Diameter: 0.300000012 Head Length: 0.00999999978 Length: 0.200000003 Line Style: Lines Line Width: 0.0299999993 Name: MPC_Trajectory Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: Axes Radius: 0.00999999978 Shaft Diameter: 0.100000001 Shaft Length: 0.00999999978 Topic: /mpc_trajectory Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 10 Min Color: 0; 0; 0 Min Intensity: 10 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.00999999978 Style: Flat Squares Topic: /scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.5 Arrow Length: 0.0500000007 Axes Length: 0.300000012 Axes Radius: 0.00999999978 Class: rviz/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.0700000003 Head Radius: 0.0299999993 Name: PoseArray Shaft Length: 0.230000004 Shaft Radius: 0.00999999978 Shape: Arrow (Flat) Topic: /particlecloud Unreliable: false Value: true - Class: rviz/Marker Enabled: true Marker Topic: /pure_pursuit/path_marker Name: Pure_Pursuit_Marker Namespaces: {} Queue Size: 100 Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 10.2806225 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 1.29834783 Y: 1.0163064 Z: -1.42960763 Focal Shape Fixed Size: true Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 Pitch: 1.56979632 Target Frame: Value: Orbit (rviz) Yaw: 3.14542842 Saved: ~ Window Geometry: Displays: collapsed: false Height: 846 Hide Left Dock: false Hide Right Dock: true QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000034f000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1215 X: 65 Y: 25 ================================================ FILE: script/base_control.py ================================================ #!/usr/bin/python # Copyright 2018 HyphaROS Workshop. # Developer: HaoChih, LIN (hypha.ros@gmail.com) # # 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. import rospy import tf import time import sys import math import serial import string from geometry_msgs.msg import Twist from ackermann_msgs.msg import AckermannDriveStamped from nav_msgs.msg import Odometry class BaseControl: def __init__(self): # Get params self.baseId = rospy.get_param('~base_id', 'base_footprint') # base link self.odomId = rospy.get_param('~odom_id', 'odom') # odom link self.devicePort = rospy.get_param('~port', '/dev/stm32base') # device port self.baudrate = float( rospy.get_param('~baudrate', '115200') ) # baudrate self.odom_freq = float( rospy.get_param('~odom_freq', '50') ) # hz of odom pub self.pwmRadian = float( rospy.get_param('~pwm_radian', '450') ) # non scaled factor pwm/radian self.neutralPt = float( rospy.get_param('~neutral_pt', '0') ) # steering neutral point (degree) self.carLength = float( rospy.get_param('~car_length', '0.1445') ) # unit: meter self.wheelSep = float( rospy.get_param('~wheel_separation', '0.156') ) # unit: meter self.wheelRad = float( rospy.get_param('~wheel_radius', '0.032') ) # unit: meter self.timeOut = float( rospy.get_param('~time_out', '0.5') ) # unit: sec self.VxCov = float( rospy.get_param('~vx_cov', '1.0') ) # covariance for Vx measurement self.VyawCov = float( rospy.get_param('~vyaw_cov', '1.0') ) # covariance for Vyaw measurement self.odom_topic = rospy.get_param('~odom_topic', '/odom') # topic name self.pub_tf = bool(rospy.get_param('~pub_tf', True)) # whether publishes TF or not self.debug_mode = bool(rospy.get_param('~debug_mode', False)) # true for detail info self.steering_pwm_bound = float(rospy.get_param('~steering_pwm_bound', '240.0')) # unit: pwm self.throttle_pwm_bound = float(rospy.get_param('~throttle_pwm_bound', '128.0')) # unit: pwm, 93: 7.4v(~1.0 m/s), 131: 11.1v(~1.6 m/s) # Serial Communication try: self.serial = serial.Serial(self.devicePort, self.baudrate, timeout=10) rospy.loginfo("Flusing first 50 data readings ...") for x in range(0, 50): data = self.serial.read() time.sleep(0.01) except serial.serialutil.SerialException: rospy.logerr("Can not receive data from the port: "+ self.devicePort + ". Did you specify the correct port ?") self.serial.close sys.exit(0) rospy.loginfo("Communication success !") # ROS handler self.sub_cmd = rospy.Subscriber('ackermann_cmd', AckermannDriveStamped, self.ackermannCmdCB, queue_size=10) self.sub_safe = rospy.Subscriber('ackermann_safe', AckermannDriveStamped, self.ackermannSafeCB, queue_size=10) self.pub = rospy.Publisher(self.odom_topic, Odometry, queue_size=10) self.timer_odom = rospy.Timer(rospy.Duration(1.0/self.odom_freq), self.timerOdomCB) self.timer_cmd = rospy.Timer(rospy.Duration(0.05), self.timerCmdCB) # 10Hz self.tf_broadcaster = tf.TransformBroadcaster() # TF # variable self.neutralPt_radian = self.neutralPt*math.pi/180.0 #self.steering_pwm_bound = 240.0 # unit: pwm #self.throttle_pwm_bound = 93.0 self.cmd_steering_bound = self.steering_pwm_bound/self.pwmRadian # (pwm limit)/pwmRadian, unit: radian self.cmd_steering = 0.0 # radian self.cmd_speed_bound = 2*math.pi*self.wheelRad*100.0/1560.0*self.throttle_pwm_bound # pwm to m/s formula (hardware spec), unit: m/s self.max_speed_compensate = self.cmd_speed_bound*self.wheelSep*math.tan(self.cmd_steering_bound - abs(self.neutralPt_radian))/(2.0*self.carLength) # find max rear wheel difference (m/s) self.compensate_factor = 0.75; # since the real compensation can only be found by iteration, we use a factor to approximate self.cmd_speed = 0.0 # m/s self.cmd_time = rospy.Time.now() self.safe_steering = 0.0 self.safe_speed = 0.0 self.safe_time = rospy.Time.now() self.current_time = rospy.Time.now() self.previous_time = rospy.Time.now() self.timeOut_flag = 0 self.pose_x = 0.0 # SI self.pose_y = 0.0 self.pose_yaw = 0.0 # reading loop while True: reading = self.serial.read(6) if int(reading[0].encode('hex'),16) == 255 and int(reading[1].encode('hex'),16) == 254: self.data = reading else: self.serial.read(1) def ackermannCmdCB(self, data): self.cmd_steering = data.drive.steering_angle # radian if self.cmd_steering > (self.cmd_steering_bound - abs(self.neutralPt_radian)): # shrink the bound range by subtracting the neutral point self.cmd_steering = self.cmd_steering_bound - abs(self.neutralPt_radian) if self.cmd_steering < -(self.cmd_steering_bound - abs(self.neutralPt_radian)): self.cmd_steering = -(self.cmd_steering_bound - abs(self.neutralPt_radian)) self.cmd_speed = data.drive.speed if self.cmd_speed > (self.cmd_speed_bound - self.compensate_factor*self.max_speed_compensate): # decrease speed if compensation added value is higher than the bound self.cmd_speed = self.cmd_speed_bound - self.compensate_factor*self.max_speed_compensate if self.cmd_speed < -(self.cmd_speed_bound- self.compensate_factor*self.max_speed_compensate): self.cmd_speed = -(self.cmd_speed_bound - self.compensate_factor*self.max_speed_compensate) self.cmd_time = rospy.Time.now() self.timeOut_flag = 0 # reset # Relay: Safety commands def ackermannSafeCB(self, data): self.safe_steering = data.drive.steering_angle # radian if self.safe_steering > (self.cmd_steering_bound - abs(self.neutralPt_radian)): # shrink the bound range by subtracting the neutral point self.safe_steering = self.cmd_steering_bound - abs(self.neutralPt_radian) if self.safe_steering < -(self.cmd_steering_bound - abs(self.neutralPt_radian)): self.safe_steering = -(self.cmd_steering_bound - abs(self.neutralPt_radian)) self.safe_speed = data.drive.speed if self.safe_speed > (self.cmd_speed_bound - self.compensate_factor*self.max_speed_compensate): # decrease speed if compensation added value is higher than the bound self.safe_speed = self.cmd_speed_bound - self.compensate_factor*self.max_speed_compensate if self.safe_speed < -(self.cmd_speed_bound- self.compensate_factor*self.max_speed_compensate): self.safe_speed = -(self.cmd_speed_bound - self.compensate_factor*self.max_speed_compensate) self.safe_time = rospy.Time.now() self.timeOut_flag = 0 # reset def timerOdomCB(self, event): # Serial read & publish try: data = self.data # Normal mode if len(data) == 6: WL = -float( (int(data[2].encode('hex'),16)*256 + int(data[3].encode('hex'),16) -500)*100.0/1560.0*2*math.pi ) # unit: rad/sec WR = -float( (int(data[4].encode('hex'),16)*256 + int(data[5].encode('hex'),16) -500)*100.0/1560.0*2*math.pi ) else: print 'Error Value! header1: ' + str(int(data[0].encode('hex'),16)) + ', header2: ' + str(int(data[1].encode('hex'),16)) # Twist VL = WL * self.wheelRad # V = omega * radius, unit: m/s VR = WR * self.wheelRad Vx = (VR+VL)/2.0 Vyaw = Vx * math.tan(self.cmd_steering) / self.wheelSep; # Pose self.current_time = rospy.Time.now() dt = (self.current_time - self.previous_time).to_sec() self.previous_time = self.current_time self.pose_x = self.pose_x + Vx * math.cos(self.pose_yaw) * dt self.pose_y = self.pose_y + Vx * math.sin(self.pose_yaw) * dt self.pose_yaw = self.pose_yaw + Vyaw * dt pose_quat = tf.transformations.quaternion_from_euler(0,0,self.pose_yaw) # Publish Odometry msg = Odometry() msg.header.stamp = self.current_time msg.header.frame_id = self.odomId msg.child_frame_id = self.baseId msg.pose.pose.position.x = self.pose_x msg.pose.pose.position.y = self.pose_y msg.pose.pose.position.z = 0.0 msg.pose.pose.orientation.x = pose_quat[0] msg.pose.pose.orientation.y = pose_quat[1] msg.pose.pose.orientation.z = pose_quat[2] msg.pose.pose.orientation.w = pose_quat[3] msg.twist.twist.linear.x = Vx msg.twist.twist.angular.z = Vyaw for i in range(36): msg.twist.covariance[i] = 0 msg.twist.covariance[0] = self.VxCov msg.twist.covariance[35] = self.VyawCov msg.pose.covariance = msg.twist.covariance self.pub.publish(msg) # TF Broadcaster if self.pub_tf: self.tf_broadcaster.sendTransform( (self.pose_x, self.pose_y, 0.0), pose_quat, self.current_time, self.baseId, self.odomId) # Debug mode if self.debug_mode: if len(data) == 6: header_1 = int(data[0].encode('hex'),16) header_2 = int(data[1].encode('hex'),16) tx_1 = int(data[2].encode('hex'),16) tx_2 = int(data[3].encode('hex'),16) tx_3 = int(data[4].encode('hex'),16) tx_4 = int(data[5].encode('hex'),16) rospy.loginfo("[Debug] header_1:%4d, header_2:%4d, tx_1:%4d, tx_2:%4d, tx_3:%4d, tx_4:%4d", header_1, header_2, tx_1, tx_2, tx_3, tx_4 ) except: #rospy.loginfo("Error in sensor value !") pass def timerCmdCB(self, event): # time out thresholding if (rospy.Time.now() - self.cmd_time).to_sec() > self.timeOut and (rospy.Time.now() - self.safe_time).to_sec() > self.timeOut and self.timeOut_flag == 0: rospy.loginfo("Commands time out!") output = chr(255) + chr(254) + chr(0) + chr(0) + chr(0) + chr(0) + chr(0) #Vel_L, Vel_R, forward, steering, leftside self.serial.write(output) self.timeOut_flag = 1 return if self.timeOut_flag == 0: # check safety cmds cmd_steering = self.cmd_steering # local copy cmd_speed = self.cmd_speed if (rospy.Time.now() - self.safe_time).to_sec() <= self.timeOut: cmd_steering = self.safe_steering cmd_speed = self.safe_speed # compute steering cmd cmd_steering_adjust = cmd_steering + self.neutralPt_radian # add neutral point compensation (rad) steering_send = int(cmd_steering_adjust*self.pwmRadian) # convert pwm from radian leftside = 0 # 0: turn left, 1: turn right if steering_send < 0: leftside = 1 steering_send = -steering_send # steering limit if steering_send > int(self.steering_pwm_bound): steering_send = int(self.steering_pwm_bound) # compute speed cmd omega = cmd_speed/self.wheelRad # w = V/r*i (i is gear ratio), unit: rad/sec # calculate rear-wheel differential speed (ackermann effect) omega_left = omega*(1 + self.wheelSep*math.tan(cmd_steering)/(2.0*self.carLength)) # w_L = omega(1+d/(2R)) R = L/tan(theta), unit: rad/s omega_right = omega*(1 - self.wheelSep*math.tan(cmd_steering)/(2.0*self.carLength)) # w_R = omega(1+d/(2R)) R = L/tan(theta), unit: rad/s pwm_left = abs(int(omega_left/100.0*1560.0/2.0/math.pi)) # rad/s to pwm formula (hardware spec) pwm_right = abs(int(omega_right/100.0*1560.0/2.0/math.pi)) if pwm_left > int(self.throttle_pwm_bound): # [final check: shouldn't be called if everything is ok] if achieve max pwm, decrease both speed pwm_right = pwm_right - (pwm_left - int(self.throttle_pwm_bound)) pwm_left = int(self.throttle_pwm_bound) if pwm_right > int(self.throttle_pwm_bound): # [final check: shouldn't be called if everything is ok] if achieve max pwm, decrease both speed pwm_left = pwm_left - (pwm_right - int(self.throttle_pwm_bound)) pwm_right = int(self.throttle_pwm_bound) forward = 0 # 0: forward, 1: reverse if cmd_speed < 0.0: forward = 1 #Protocal: Vel_L, Vel_R, forward, steering, leftside output = chr(255) + chr(254) + chr(pwm_left) + chr(pwm_right) + chr(forward) + chr(steering_send) + chr(leftside) #print 'left: ' + str(pwm_left) + ', right: ' + str(pwm_right) self.serial.write(output) if __name__ == "__main__": try: # ROS Init rospy.init_node('base_control', anonymous=True) # Constract BaseControl Obj rospy.loginfo("HyphaROS MiniCar Base Control ...") bc = BaseControl() rospy.spin() except KeyboardInterrupt: bc.serial.close print("Shutting down") ================================================ FILE: script/teleop_keyboard.py ================================================ #!/usr/bin/python # This is a modified verison of turtlebot_teleop.py # to fullfill the needs of HyphaROS MiniCar use case # Copyright (c) 2018, HyphaROS Workshop # # The original license info are as below: # Copyright (c) 2011, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import sys, select, termios, tty, math import rospy from ackermann_msgs.msg import AckermannDriveStamped header_msg = """ Control HyphaROS Minicar! ------------------------- Moving around: i j k l , w/x : increase/decrease throttle bounds by 10% e/c : increase/decrease steering bounds by 10% s : safety mode space key, k : force stop anything else : keep previous commands CTRL-C to quit """ # Func for getting keyboard value def getKey(safety_mode): if safety_mode: # wait unit keyboard interrupt tty.setraw(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) key = sys.stdin.read(1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) return key else: # pass if not detected tty.setraw(sys.stdin.fileno()) rlist, _, _ = select.select([sys.stdin], [], [], 0.1) if rlist: key = sys.stdin.read(1) else: key = '' termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) return key # Func for showing current bounds def showInfo(speed_bound, angle_bound): return "current bounds:\tspeed %s\tangle %s " % (speed_bound, angle_bound) # Main Func if __name__=="__main__": settings = termios.tcgetattr(sys.stdin) rospy.init_node('minicar_teleop') pub_cmd = rospy.Publisher('/ackermann_cmd', AckermannDriveStamped, queue_size=5) pub_safe = rospy.Publisher('/ackermann_safe', AckermannDriveStamped, queue_size=5) safe_mode = bool(rospy.get_param('~safety_mode', False)) # true for safety cmds speed_i = float(rospy.get_param('~speed_incremental', 0.1)) # m/s angle_i = float(rospy.get_param('~angle_incremental', 5.0*math.pi/180.0)) # rad (=5 degree) speed_bound = float(rospy.get_param('~speed_bound', 2.0)) angle_bound = float(rospy.get_param('~angle_bound', 30.0*math.pi/180.0)) if safe_mode: print "Switched to Safety Mode !" moveBindings = { 'i':(speed_i,0.0), 'j':(0.0,angle_i), 'l':(0.0,-angle_i), ',':(-speed_i,0.0), } boundBindings={ 'w':(1.1,1), 'x':(.9,1), 'e':(1,1.1), 'c':(1,.9), } status = 0 acc = 0.1 target_speed = 0.0 # m/s target_angle = 0.0 # rad # Create AckermannDriveStamped msg object ackermann_msg = AckermannDriveStamped() #ackermann_msg.header.frame_id = 'car_id' # for future multi-cars applicaton try: print(header_msg) print(showInfo(speed_bound, angle_bound)) while(1): key = getKey(safe_mode) if key in moveBindings.keys(): target_speed = target_speed + moveBindings[key][0] target_angle = target_angle + moveBindings[key][1] elif key in boundBindings.keys(): speed_bound = speed_bound * boundBindings[key][0] angle_bound = angle_bound * boundBindings[key][1] print(showInfo(speed_bound, angle_bound)) if (status == 14): print(header_msg) status = (status + 1) % 15 elif key == ' ' or key == 'k' : target_speed = 0.0 target_angle = 0.0 elif key == 's' : # switch safety mode safe_mode = not safe_mode if safe_mode: print "Switched to Safety Mode !" else: print "Back to Standard Mode !" elif key == '\x03': # cltr + C break # Command constraints if target_speed > speed_bound: target_speed = speed_bound if target_speed < -speed_bound: target_speed = -speed_bound if target_angle > angle_bound: target_angle = angle_bound if target_angle < -angle_bound: target_angle = -angle_bound # Publishing command #ackermann_msg.header.stamp = rospy.Time.now() # for future multi-cars applicaton ackermann_msg.drive.speed = target_speed ackermann_msg.drive.steering_angle = target_angle if safe_mode: pub_safe.publish(ackermann_msg) else: pub_cmd.publish(ackermann_msg) except Exception as e: print(e) finally: ackermann_msg.drive.speed = 0 ackermann_msg.drive.steering_angle = 0 pub_cmd.publish(ackermann_msg) pub_safe.publish(ackermann_msg) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) ================================================ FILE: src/MPC.cpp ================================================ /* # Copyright 2018 HyphaROS Workshop. # Developer: HaoChih, LIN (hypha.ros@gmail.com) # # 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. */ #include "MPC.h" #include #include #include // The program use fragments of code from // https://github.com/udacity/CarND-MPC-Quizzes using CppAD::AD; // ========================================= // FG_eval class definition implementation. // ========================================= class FG_eval { public: // Fitted polynomial coefficients Eigen::VectorXd coeffs; double _Lf, _dt, _ref_cte, _ref_epsi, _ref_vel; double _w_cte, _w_epsi, _w_vel, _w_delta, _w_accel, _w_delta_d, _w_accel_d; int _mpc_steps, _x_start, _y_start, _psi_start, _v_start, _cte_start, _epsi_start, _delta_start, _a_start; // Constructor FG_eval(Eigen::VectorXd coeffs) { this->coeffs = coeffs; // Set default value _Lf = 0.25; // distance between the front of the vehicle and its center of gravity _dt = 0.1; // in sec _ref_cte = 0; _ref_epsi = 0; _ref_vel = 1.0; // m/s _w_cte = 100; _w_epsi = 100; _w_vel = 100; _w_delta = 100; _w_accel = 50; _w_delta_d = 0; _w_accel_d = 0; _mpc_steps = 40; _x_start = 0; _y_start = _x_start + _mpc_steps; _psi_start = _y_start + _mpc_steps; _v_start = _psi_start + _mpc_steps; _cte_start = _v_start + _mpc_steps; _epsi_start = _cte_start + _mpc_steps; _delta_start = _epsi_start + _mpc_steps; _a_start = _delta_start + _mpc_steps - 1; } // Load parameters for constraints void LoadParams(const std::map ¶ms) { _dt = params.find("DT") != params.end() ? params.at("DT") : _dt; _Lf = params.find("LF") != params.end() ? params.at("LF") : _Lf; _mpc_steps = params.find("STEPS") != params.end() ? params.at("STEPS") : _mpc_steps; _ref_cte = params.find("REF_CTE") != params.end() ? params.at("REF_CTE") : _ref_cte; _ref_epsi = params.find("REF_EPSI") != params.end() ? params.at("REF_EPSI") : _ref_epsi; _ref_vel = params.find("REF_V") != params.end() ? params.at("REF_V") : _ref_vel; _w_cte = params.find("W_CTE") != params.end() ? params.at("W_CTE") : _w_cte; _w_epsi = params.find("W_EPSI") != params.end() ? params.at("W_EPSI") : _w_epsi; _w_vel = params.find("W_V") != params.end() ? params.at("W_V") : _w_vel; _w_delta = params.find("W_DELTA") != params.end() ? params.at("W_DELTA") : _w_delta; _w_accel = params.find("W_A") != params.end() ? params.at("W_A") : _w_accel; _w_delta_d = params.find("W_DDELTA") != params.end() ? params.at("W_DDELTA") : _w_delta_d; _w_accel_d = params.find("W_DA") != params.end() ? params.at("W_DA") : _w_accel_d; _x_start = 0; _y_start = _x_start + _mpc_steps; _psi_start = _y_start + _mpc_steps; _v_start = _psi_start + _mpc_steps; _cte_start = _v_start + _mpc_steps; _epsi_start = _cte_start + _mpc_steps; _delta_start = _epsi_start + _mpc_steps; _a_start = _delta_start + _mpc_steps - 1; //cout << "\n!! FG_eval Obj parameters updated !! " << _mpc_steps << endl; } // MPC implementation (cost func & constraints) typedef CPPAD_TESTVECTOR(AD) ADvector; // fg: function that evaluates the objective and constraints using the syntax void operator()(ADvector& fg, const ADvector& vars) { // fg[0] for cost function fg[0] = 0; for (int i = 0; i < _mpc_steps; i++) { fg[0] += _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2); // cross deviation error fg[0] += _w_epsi * CppAD::pow(vars[_epsi_start + i] - _ref_epsi, 2); // heading error fg[0] += _w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2); // speed error } // Minimize the use of actuators. for (int i = 0; i < _mpc_steps - 1; i++) { fg[0] += _w_delta * CppAD::pow(vars[_delta_start + i], 2); fg[0] += _w_accel * CppAD::pow(vars[_a_start + i], 2); } // Minimize the value gap between sequential actuations. for (int i = 0; i < _mpc_steps - 2; i++) { fg[0] += _w_delta_d * CppAD::pow(vars[_delta_start + i + 1] - vars[_delta_start + i], 2); fg[0] += _w_accel_d * CppAD::pow(vars[_a_start + i + 1] - vars[_a_start + i], 2); } // fg[x] for constraints // Initial constraints fg[1 + _x_start] = vars[_x_start]; fg[1 + _y_start] = vars[_y_start]; fg[1 + _psi_start] = vars[_psi_start]; fg[1 + _v_start] = vars[_v_start]; fg[1 + _cte_start] = vars[_cte_start]; fg[1 + _epsi_start] = vars[_epsi_start]; // Add system dynamic model constraint for (int i = 0; i < _mpc_steps - 1; i++) { // The state at time t+1 . AD x1 = vars[_x_start + i + 1]; AD y1 = vars[_y_start + i + 1]; AD psi1 = vars[_psi_start + i + 1]; AD v1 = vars[_v_start + i + 1]; AD cte1 = vars[_cte_start + i + 1]; AD epsi1 = vars[_epsi_start + i + 1]; // The state at time t. AD x0 = vars[_x_start + i]; AD y0 = vars[_y_start + i]; AD psi0 = vars[_psi_start + i]; AD v0 = vars[_v_start + i]; AD cte0 = vars[_cte_start + i]; AD epsi0 = vars[_epsi_start + i]; // Only consider the actuation at time t. AD delta0 = vars[_delta_start + i]; AD a0 = vars[_a_start + i]; AD f0 = 0.0; for (int i = 0; i < coeffs.size(); i++) { f0 += coeffs[i] * CppAD::pow(x0, i); } AD psides0 = 0.0; for (int i = 1; i < coeffs.size(); i++) { psides0 += i*coeffs[i] * CppAD::pow(x0, i-1); // f'(x0) } psides0 = CppAD::atan(psides0); fg[2 + _x_start + i] = x1 - (x0 + v0 * CppAD::cos(psi0) * _dt); fg[2 + _y_start + i] = y1 - (y0 + v0 * CppAD::sin(psi0) * _dt); fg[2 + _psi_start + i] = psi1 - (psi0 + v0 * delta0 / _Lf * _dt); fg[2 + _v_start + i] = v1 - (v0 + a0 * _dt); fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * _dt)); fg[2 + _epsi_start + i] = epsi1 - ((psi0 - psides0) + v0 * delta0 / _Lf * _dt); } } }; // ==================================== // MPC class definition implementation. // ==================================== MPC::MPC() { // Set default value _mpc_steps = 40; _max_steering = 0.523; // Maximal steering radian (~30 deg) _max_throttle = 1.0; // Maximal throttle accel _bound_value = 1.0e3; // Bound value for other variables _x_start = 0; _y_start = _x_start + _mpc_steps; _psi_start = _y_start + _mpc_steps; _v_start = _psi_start + _mpc_steps; _cte_start = _v_start + _mpc_steps; _epsi_start = _cte_start + _mpc_steps; _delta_start = _epsi_start + _mpc_steps; _a_start = _delta_start + _mpc_steps - 1; } void MPC::LoadParams(const std::map ¶ms) { _params = params; //Init parameters for MPC object _mpc_steps = _params.find("STEPS") != _params.end() ? _params.at("STEPS") : _mpc_steps; _max_steering = _params.find("MAXSTR") != _params.end() ? _params.at("MAXSTR") : _max_steering; _max_throttle = _params.find("MAXTHR") != _params.end() ? _params.at("MAXTHR") : _max_throttle; _bound_value = _params.find("BOUND") != _params.end() ? _params.at("BOUND") : _bound_value; _x_start = 0; _y_start = _x_start + _mpc_steps; _psi_start = _y_start + _mpc_steps; _v_start = _psi_start + _mpc_steps; _cte_start = _v_start + _mpc_steps; _epsi_start = _cte_start + _mpc_steps; _delta_start = _epsi_start + _mpc_steps; _a_start = _delta_start + _mpc_steps - 1; cout << "\n!! MPC Obj parameters updated !! " << endl; } vector MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) { bool ok = true; size_t i; typedef CPPAD_TESTVECTOR(double) Dvector; const double x = state[0]; const double y = state[1]; const double psi = state[2]; const double v = state[3]; const double cte = state[4]; const double epsi = state[5]; // Set the number of model variables (includes both states and inputs). // For example: If the state is a 4 element vector, the actuators is a 2 // element vector and there are 10 timesteps. The number of variables is: size_t n_vars = _mpc_steps * 6 + (_mpc_steps - 1) * 2; // Set the number of constraints size_t n_constraints = _mpc_steps * 6; // Initial value of the independent variables. // SHOULD BE 0 besides initial state. Dvector vars(n_vars); for (int i = 0; i < n_vars; i++) { vars[i] = 0; } Dvector vars_lowerbound(n_vars); Dvector vars_upperbound(n_vars); // Set lower and upper limits for variables. for (int i = 0; i < _delta_start; i++) { vars_lowerbound[i] = -_bound_value; vars_upperbound[i] = _bound_value; } // The upper and lower limits of delta are set to -25 and 25 // degrees (values in radians). for (int i = _delta_start; i < _a_start; i++) { vars_lowerbound[i] = -_max_steering; vars_upperbound[i] = _max_steering; } // Acceleration/decceleration upper and lower limits for (int i = _a_start; i < n_vars; i++) { vars_lowerbound[i] = -_max_throttle; vars_upperbound[i] = _max_throttle; } // Lower and upper limits for the constraints // Should be 0 besides initial state. Dvector constraints_lowerbound(n_constraints); Dvector constraints_upperbound(n_constraints); for (int i = 0; i < n_constraints; i++) { constraints_lowerbound[i] = 0; constraints_upperbound[i] = 0; } constraints_lowerbound[_x_start] = x; constraints_lowerbound[_y_start] = y; constraints_lowerbound[_psi_start] = psi; constraints_lowerbound[_v_start] = v; constraints_lowerbound[_cte_start] = cte; constraints_lowerbound[_epsi_start] = epsi; constraints_upperbound[_x_start] = x; constraints_upperbound[_y_start] = y; constraints_upperbound[_psi_start] = psi; constraints_upperbound[_v_start] = v; constraints_upperbound[_cte_start] = cte; constraints_upperbound[_epsi_start] = epsi; // object that computes objective and constraints FG_eval fg_eval(coeffs); fg_eval.LoadParams(_params); // options for IPOPT solver std::string options; // Uncomment this if you'd like more print information options += "Integer print_level 0\n"; // NOTE: Setting sparse to true allows the solver to take advantage // of sparse routines, this makes the computation MUCH FASTER. If you // can uncomment 1 of these and see if it makes a difference or not but // if you uncomment both the computation time should go up in orders of // magnitude. options += "Sparse true forward\n"; options += "Sparse true reverse\n"; // NOTE: Currently the solver has a maximum time limit of 0.5 seconds. // Change this as you see fit. options += "Numeric max_cpu_time 0.5\n"; // place to return solution CppAD::ipopt::solve_result solution; // solve the problem CppAD::ipopt::solve( options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound, constraints_upperbound, fg_eval, solution); // Check some of the solution values ok &= solution.status == CppAD::ipopt::solve_result::success; // Cost auto cost = solution.obj_value; //std::cout << "Cost " << cost << std::endl; this->mpc_x = {}; this->mpc_y = {}; for (int i = 0; i < _mpc_steps; i++) { this->mpc_x.push_back(solution.x[_x_start + i]); this->mpc_y.push_back(solution.x[_y_start + i]); } vector result; result.push_back(solution.x[_delta_start]); result.push_back(solution.x[_a_start]); return result; } ================================================ FILE: src/MPC_Node.cpp ================================================ /* # Copyright 2018 HyphaROS Workshop. # Developer: HaoChih, LIN (hypha.ros@gmail.com) # # 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. */ #include #include #include #include "ros/ros.h" #include #include #include #include #include #include #include #include #include #include "MPC.h" #include #include using namespace std; using namespace Eigen; /********************/ /* CLASS DEFINITION */ /********************/ class MPCNode { public: MPCNode(); int get_thread_numbers(); private: ros::NodeHandle _nh; ros::Subscriber _sub_odom, _sub_path, _sub_goal, _sub_amcl; ros::Publisher _pub_odompath, _pub_twist, _pub_ackermann, _pub_mpctraj; ros::Timer _timer1; tf::TransformListener _tf_listener; geometry_msgs::Point _goal_pos; nav_msgs::Odometry _odom; nav_msgs::Path _odom_path, _mpc_traj; ackermann_msgs::AckermannDriveStamped _ackermann_msg; geometry_msgs::Twist _twist_msg; string _globalPath_topic, _goal_topic; string _map_frame, _odom_frame, _car_frame; MPC _mpc; map _mpc_params; double _mpc_steps, _ref_cte, _ref_epsi, _ref_vel, _w_cte, _w_epsi, _w_vel, _w_delta, _w_accel, _w_delta_d, _w_accel_d, _max_steering, _max_throttle, _bound_value; double _Lf, _dt, _steering, _throttle, _speed, _max_speed; double _pathLength, _goalRadius, _waypointsDist; int _controller_freq, _downSampling, _thread_numbers; bool _goal_received, _goal_reached, _path_computed, _pub_twist_flag, _debug_info, _delay_mode; double polyeval(Eigen::VectorXd coeffs, double x); Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order); void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg); void pathCB(const nav_msgs::Path::ConstPtr& pathMsg); void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg); void amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg); void controlLoopCB(const ros::TimerEvent&); }; // end of class MPCNode::MPCNode() { //Private parameters handler ros::NodeHandle pn("~"); //Parameters for control loop pn.param("thread_numbers", _thread_numbers, 2); // number of threads for this ROS node pn.param("pub_twist_cmd", _pub_twist_flag, true); pn.param("debug_info", _debug_info, false); pn.param("delay_mode", _delay_mode, true); pn.param("max_speed", _max_speed, 2.0); // unit: m/s pn.param("waypoints_dist", _waypointsDist, -1.0); // unit: m pn.param("path_length", _pathLength, 8.0); // unit: m pn.param("goal_radius", _goalRadius, 0.5); // unit: m pn.param("controller_freq", _controller_freq, 10); pn.param("vehicle_Lf", _Lf, 0.25); // distance between the front of the vehicle and its center of gravity _dt = double(1.0/_controller_freq); // time step duration dt in s //Parameter for MPC solver pn.param("mpc_steps", _mpc_steps, 20.0); pn.param("mpc_ref_cte", _ref_cte, 0.0); pn.param("mpc_ref_epsi", _ref_epsi, 0.0); pn.param("mpc_ref_vel", _ref_vel, 1.5); pn.param("mpc_w_cte", _w_cte, 100.0); pn.param("mpc_w_epsi", _w_epsi, 100.0); pn.param("mpc_w_vel", _w_vel, 100.0); pn.param("mpc_w_delta", _w_delta, 100.0); pn.param("mpc_w_accel", _w_accel, 50.0); pn.param("mpc_w_delta_d", _w_delta_d, 0.0); pn.param("mpc_w_accel_d", _w_accel_d, 0.0); pn.param("mpc_max_steering", _max_steering, 0.523); // Maximal steering radian (~30 deg) pn.param("mpc_max_throttle", _max_throttle, 1.0); // Maximal throttle accel pn.param("mpc_bound_value", _bound_value, 1.0e3); // Bound value for other variables //Parameter for topics & Frame name pn.param("global_path_topic", _globalPath_topic, "/move_base/TrajectoryPlannerROS/global_plan" ); pn.param("goal_topic", _goal_topic, "/move_base_simple/goal" ); pn.param("map_frame", _map_frame, "map" ); pn.param("odom_frame", _odom_frame, "odom"); pn.param("car_frame", _car_frame, "base_link" ); //Display the parameters cout << "\n===== Parameters =====" << endl; cout << "pub_twist_cmd: " << _pub_twist_flag << endl; cout << "debug_info: " << _debug_info << endl; cout << "delay_mode: " << _delay_mode << endl; cout << "vehicle_Lf: " << _Lf << endl; cout << "frequency: " << _dt << endl; cout << "mpc_steps: " << _mpc_steps << endl; cout << "mpc_ref_vel: " << _ref_vel << endl; cout << "mpc_w_cte: " << _w_cte << endl; cout << "mpc_w_epsi: " << _w_epsi << endl; cout << "mpc_max_steering: " << _max_steering << endl; //Publishers and Subscribers _sub_odom = _nh.subscribe("/odom", 1, &MPCNode::odomCB, this); _sub_path = _nh.subscribe( _globalPath_topic, 1, &MPCNode::pathCB, this); _sub_goal = _nh.subscribe( _goal_topic, 1, &MPCNode::goalCB, this); _sub_amcl = _nh.subscribe("/amcl_pose", 5, &MPCNode::amclCB, this); _pub_odompath = _nh.advertise("/mpc_reference", 1); // reference path for MPC _pub_mpctraj = _nh.advertise("/mpc_trajectory", 1);// MPC trajectory output _pub_ackermann = _nh.advertise("/ackermann_cmd", 1); if(_pub_twist_flag) _pub_twist = _nh.advertise("/cmd_vel", 1); //for stage (Ackermann msg non-supported) //Timer _timer1 = _nh.createTimer(ros::Duration((1.0)/_controller_freq), &MPCNode::controlLoopCB, this); // 10Hz //Init variables _goal_received = false; _goal_reached = false; _path_computed = false; _throttle = 0.0; _steering = 0.0; _speed = 0.0; _ackermann_msg = ackermann_msgs::AckermannDriveStamped(); _twist_msg = geometry_msgs::Twist(); _mpc_traj = nav_msgs::Path(); //Init parameters for MPC object _mpc_params["DT"] = _dt; _mpc_params["LF"] = _Lf; _mpc_params["STEPS"] = _mpc_steps; _mpc_params["REF_CTE"] = _ref_cte; _mpc_params["REF_EPSI"] = _ref_epsi; _mpc_params["REF_V"] = _ref_vel; _mpc_params["W_CTE"] = _w_cte; _mpc_params["W_EPSI"] = _w_epsi; _mpc_params["W_V"] = _w_vel; _mpc_params["W_DELTA"] = _w_delta; _mpc_params["W_A"] = _w_accel; _mpc_params["W_DDELTA"] = _w_delta_d; _mpc_params["W_DA"] = _w_accel_d; _mpc_params["MAXSTR"] = _max_steering; _mpc_params["MAXTHR"] = _max_throttle; _mpc_params["BOUND"] = _bound_value; _mpc.LoadParams(_mpc_params); } // Public: return _thread_numbers int MPCNode::get_thread_numbers() { return _thread_numbers; } // Evaluate a polynomial. double MPCNode::polyeval(Eigen::VectorXd coeffs, double x) { double result = 0.0; for (int i = 0; i < coeffs.size(); i++) { result += coeffs[i] * pow(x, i); } return result; } // Fit a polynomial. // Adapted from // https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716 Eigen::VectorXd MPCNode::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) { assert(xvals.size() == yvals.size()); assert(order >= 1 && order <= xvals.size() - 1); Eigen::MatrixXd A(xvals.size(), order + 1); for (int i = 0; i < xvals.size(); i++) A(i, 0) = 1.0; for (int j = 0; j < xvals.size(); j++) { for (int i = 0; i < order; i++) A(j, i + 1) = A(j, i) * xvals(j); } auto Q = A.householderQr(); auto result = Q.solve(yvals); return result; } // CallBack: Update odometry void MPCNode::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg) { _odom = *odomMsg; } // CallBack: Update path waypoints (conversion to odom frame) void MPCNode::pathCB(const nav_msgs::Path::ConstPtr& pathMsg) { if(_goal_received && !_goal_reached) { nav_msgs::Path odom_path = nav_msgs::Path(); try { //find waypoints distance if(_waypointsDist <=0.0) { double dx = pathMsg->poses[1].pose.position.x - pathMsg->poses[0].pose.position.x; double dy = pathMsg->poses[1].pose.position.y - pathMsg->poses[0].pose.position.y; _waypointsDist = sqrt(dx*dx + dy*dy); _downSampling = int(_pathLength/10.0/_waypointsDist); } double total_length = 0.0; int sampling = _downSampling; // Cut and downsampling the path for(int i =0; i< pathMsg->poses.size(); i++) { if(total_length > _pathLength) break; if(sampling == _downSampling) { geometry_msgs::PoseStamped tempPose; _tf_listener.transformPose(_odom_frame, ros::Time(0) , pathMsg->poses[i], _map_frame, tempPose); odom_path.poses.push_back(tempPose); sampling = 0; } total_length = total_length + _waypointsDist; sampling = sampling + 1; } if(odom_path.poses.size() >= 6 ) { _odom_path = odom_path; // Path waypoints in odom frame _path_computed = true; // publish odom path odom_path.header.frame_id = _odom_frame; odom_path.header.stamp = ros::Time::now(); _pub_odompath.publish(odom_path); } //DEBUG //cout << endl << "N: " << odom_path.poses.size() << endl << "Car path[0]: " << odom_path.poses[0] << ", path[N]: " << _odom_path.poses[_odom_path.poses.size()-1] << endl; } catch(tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); } } } // CallBack: Update goal status void MPCNode::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg) { _goal_pos = goalMsg->pose.position; _goal_received = true; _goal_reached = false; ROS_INFO("Goal Received !"); } // Callback: Check if the car is inside the goal area or not void MPCNode::amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg) { if(_goal_received) { double car2goal_x = _goal_pos.x - amclMsg->pose.pose.position.x; double car2goal_y = _goal_pos.y - amclMsg->pose.pose.position.y; double dist2goal = sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y); if(dist2goal < _goalRadius) { _goal_reached = true; _goal_received = false; _path_computed = false; ROS_INFO("Goal Reached !"); } } } // Timer: Control Loop (closed loop nonlinear MPC) void MPCNode::controlLoopCB(const ros::TimerEvent&) { if(_goal_received && !_goal_reached && _path_computed ) //received goal & goal not reached { nav_msgs::Odometry odom = _odom; nav_msgs::Path odom_path = _odom_path; // Update system states: X=[x, y, psi, v] const double px = odom.pose.pose.position.x; //pose: odom frame const double py = odom.pose.pose.position.y; tf::Pose pose; tf::poseMsgToTF(odom.pose.pose, pose); const double psi = tf::getYaw(pose.getRotation()); const double v = odom.twist.twist.linear.x; //twist: body fixed frame // Update system inputs: U=[steering, throttle] const double steering = _steering; // radian const double throttle = _throttle; // accel: >0; brake: <0 const double dt = _dt; const double Lf = _Lf; // Waypoints related parameters const int N = odom_path.poses.size(); // Number of waypoints const double cospsi = cos(psi); const double sinpsi = sin(psi); // Convert to the vehicle coordinate system VectorXd x_veh(N); VectorXd y_veh(N); for(int i = 0; i < N; i++) { const double dx = odom_path.poses[i].pose.position.x - px; const double dy = odom_path.poses[i].pose.position.y - py; x_veh[i] = dx * cospsi + dy * sinpsi; y_veh[i] = dy * cospsi - dx * sinpsi; } // Fit waypoints auto coeffs = polyfit(x_veh, y_veh, 3); const double cte = polyeval(coeffs, 0.0); const double epsi = atan(coeffs[1]); VectorXd state(6); if(_delay_mode) { // Kinematic model is used to predict vehicle state at the actual // moment of control (current time + delay dt) const double px_act = v * dt; const double py_act = 0; const double psi_act = v * steering * dt / Lf; const double v_act = v + throttle * dt; const double cte_act = cte + v * sin(epsi) * dt; const double epsi_act = -epsi + psi_act; state << px_act, py_act, psi_act, v_act, cte_act, epsi_act; } else { state << 0, 0, 0, v, cte, epsi; } // Solve MPC Problem vector mpc_results = _mpc.Solve(state, coeffs); // MPC result (all described in car frame) _steering = mpc_results[0]; // radian _throttle = mpc_results[1]; // acceleration _speed = v + _throttle*dt; // speed if (_speed >= _max_speed) _speed = _max_speed; if(_speed <= 0.0) _speed = 0.0; if(_debug_info) { cout << "\n\nDEBUG" << endl; cout << "psi: " << psi << endl; cout << "V: " << v << endl; //cout << "odom_path: \n" << odom_path << endl; //cout << "x_points: \n" << x_veh << endl; //cout << "y_points: \n" << y_veh << endl; cout << "coeffs: \n" << coeffs << endl; cout << "_steering: \n" << _steering << endl; cout << "_throttle: \n" << _throttle << endl; cout << "_speed: \n" << _speed << endl; } // Display the MPC predicted trajectory _mpc_traj = nav_msgs::Path(); _mpc_traj.header.frame_id = _car_frame; // points in car coordinate _mpc_traj.header.stamp = ros::Time::now(); for(int i=0; i<_mpc.mpc_x.size(); i++) { geometry_msgs::PoseStamped tempPose; tempPose.header = _mpc_traj.header; tempPose.pose.position.x = _mpc.mpc_x[i]; tempPose.pose.position.y = _mpc.mpc_y[i]; tempPose.pose.orientation.w = 1.0; _mpc_traj.poses.push_back(tempPose); } // publish the mpc trajectory _pub_mpctraj.publish(_mpc_traj); } else { _steering = 0.0; _throttle = 0.0; _speed = 0.0; if(_goal_reached && _goal_received) cout << "Goal Reached !" << endl; } // publish cmd _ackermann_msg.header.frame_id = _car_frame; _ackermann_msg.header.stamp = ros::Time::now(); _ackermann_msg.drive.steering_angle = _steering; _ackermann_msg.drive.speed = _speed; _ackermann_msg.drive.acceleration = _throttle; _pub_ackermann.publish(_ackermann_msg); if(_pub_twist_flag) { _twist_msg.linear.x = _speed; _twist_msg.angular.z = _steering; _pub_twist.publish(_twist_msg); } } /*****************/ /* MAIN FUNCTION */ /*****************/ int main(int argc, char **argv) { //Initiate ROS ros::init(argc, argv, "MPC_Node"); MPCNode mpc_node; ROS_INFO("Waiting for global path msgs ~"); ros::AsyncSpinner spinner(mpc_node.get_thread_numbers()); // Use multi threads spinner.start(); ros::waitForShutdown(); return 0; } ================================================ FILE: src/Pure_Pursuit.cpp ================================================ /* # Copyright 2018 HyphaROS Workshop. # Latest Modifier: HaoChih, LIN (hypha.ros@gmail.com) # Original Author: ChanYuan KUO & YoRu LU # # 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. */ #include #include "ros/ros.h" #include #include #include #include #include #include #include #include #include /********************/ /* CLASS DEFINITION */ /********************/ class PurePursuit { public: PurePursuit(); void initMarker(); bool isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose); bool isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos); double getYawFromPose(const geometry_msgs::Pose& carPose); double getEta(const geometry_msgs::Pose& carPose); double getCar2GoalDist(); double getSteering(double eta); geometry_msgs::Point get_odom_car2WayPtVec(const geometry_msgs::Pose& carPose); private: ros::NodeHandle n_; ros::Subscriber odom_sub, path_sub, goal_sub, amcl_sub; ros::Publisher ackermann_pub, cmdvel_pub, marker_pub; ros::Timer timer1, timer2; tf::TransformListener tf_listener; visualization_msgs::Marker points, line_strip, goal_circle; geometry_msgs::Point odom_goal_pos, goal_pos; geometry_msgs::Twist cmd_vel; ackermann_msgs::AckermannDriveStamped ackermann_cmd; nav_msgs::Odometry odom; nav_msgs::Path map_path, odom_path; double L, Lfw, Vcmd, lfw, steering, velocity; double steering_gain, base_angle, goal_radius, speed_incremental; int controller_freq; bool foundForwardPt, goal_received, goal_reached, cmd_vel_mode, debug_mode, smooth_accel; void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg); void pathCB(const nav_msgs::Path::ConstPtr& pathMsg); void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg); void amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg); void controlLoopCB(const ros::TimerEvent&); }; // end of class PurePursuit::PurePursuit() { //Private parameters handler ros::NodeHandle pn("~"); //Car parameter pn.param("L", L, 0.26); // length of car pn.param("Vcmd", Vcmd, 1.0);// reference speed (m/s) pn.param("Lfw", Lfw, 3.0); // forward look ahead distance (m) pn.param("lfw", lfw, 0.13); // distance between front the center of car //Controller parameter pn.param("controller_freq", controller_freq, 20); pn.param("steering_gain", steering_gain, 1.0); pn.param("goal_radius", goal_radius, 0.5); // goal radius (m) pn.param("base_angle", base_angle, 0.0); // neutral point of servo (rad) pn.param("cmd_vel_mode", cmd_vel_mode, false); // whether or not publishing cmd_vel pn.param("debug_mode", debug_mode, false); // debug mode pn.param("smooth_accel", smooth_accel, true); // smooth the acceleration of car pn.param("speed_incremental", speed_incremental, 0.5); // speed incremental value (discrete acceleraton), unit: m/s //Publishers and Subscribers odom_sub = n_.subscribe("/pure_pursuit/odom", 1, &PurePursuit::odomCB, this); path_sub = n_.subscribe("/pure_pursuit/global_planner", 1, &PurePursuit::pathCB, this); goal_sub = n_.subscribe("/pure_pursuit/goal", 1, &PurePursuit::goalCB, this); amcl_sub = n_.subscribe("/amcl_pose", 5, &PurePursuit::amclCB, this); marker_pub = n_.advertise("/pure_pursuit/path_marker", 10); ackermann_pub = n_.advertise("/pure_pursuit/ackermann_cmd", 1); if(cmd_vel_mode) cmdvel_pub = n_.advertise("/pure_pursuit/cmd_vel", 1); //Timer timer1 = n_.createTimer(ros::Duration((1.0)/controller_freq), &PurePursuit::controlLoopCB, this); // Duration(0.05) -> 20Hz //Init variables foundForwardPt = false; goal_received = false; goal_reached = false; velocity = 0.0; steering = base_angle; //Show info ROS_INFO("[param] base_angle: %f", base_angle); ROS_INFO("[param] Vcmd: %f", Vcmd); ROS_INFO("[param] Lfw: %f", Lfw); //Visualization Marker Settings initMarker(); cmd_vel = geometry_msgs::Twist(); ackermann_cmd = ackermann_msgs::AckermannDriveStamped(); } void PurePursuit::initMarker() { points.header.frame_id = line_strip.header.frame_id = goal_circle.header.frame_id = "odom"; points.ns = line_strip.ns = goal_circle.ns = "Markers"; points.action = line_strip.action = goal_circle.action = visualization_msgs::Marker::ADD; points.pose.orientation.w = line_strip.pose.orientation.w = goal_circle.pose.orientation.w = 1.0; points.id = 0; line_strip.id = 1; goal_circle.id = 2; points.type = visualization_msgs::Marker::POINTS; line_strip.type = visualization_msgs::Marker::LINE_STRIP; goal_circle.type = visualization_msgs::Marker::CYLINDER; // POINTS markers use x and y scale for width/height respectively points.scale.x = 0.2; points.scale.y = 0.2; //LINE_STRIP markers use only the x component of scale, for the line width line_strip.scale.x = 0.1; goal_circle.scale.x = goal_radius; goal_circle.scale.y = goal_radius; goal_circle.scale.z = 0.1; // Points are green points.color.g = 1.0f; points.color.a = 1.0; // Line strip is blue line_strip.color.b = 1.0; line_strip.color.a = 1.0; //goal_circle is yellow goal_circle.color.r = 1.0; goal_circle.color.g = 1.0; goal_circle.color.b = 0.0; goal_circle.color.a = 0.5; } void PurePursuit::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg) { this->odom = *odomMsg; } void PurePursuit::pathCB(const nav_msgs::Path::ConstPtr& pathMsg) { this->map_path = *pathMsg; } void PurePursuit::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg) { this->goal_pos = goalMsg->pose.position; try { geometry_msgs::PoseStamped odom_goal; tf_listener.transformPose("odom", ros::Time(0) , *goalMsg, "map" ,odom_goal); odom_goal_pos = odom_goal.pose.position; goal_received = true; goal_reached = false; /*Draw Goal on RVIZ*/ goal_circle.pose = odom_goal.pose; marker_pub.publish(goal_circle); } catch(tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); } } double PurePursuit::getYawFromPose(const geometry_msgs::Pose& carPose) { float x = carPose.orientation.x; float y = carPose.orientation.y; float z = carPose.orientation.z; float w = carPose.orientation.w; double tmp,yaw; tf::Quaternion q(x,y,z,w); tf::Matrix3x3 quaternion(q); quaternion.getRPY(tmp,tmp, yaw); return yaw; } bool PurePursuit::isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose) { float car2wayPt_x = wayPt.x - carPose.position.x; float car2wayPt_y = wayPt.y - carPose.position.y; double car_theta = getYawFromPose(carPose); float car_car2wayPt_x = cos(car_theta)*car2wayPt_x + sin(car_theta)*car2wayPt_y; float car_car2wayPt_y = -sin(car_theta)*car2wayPt_x + cos(car_theta)*car2wayPt_y; if(car_car2wayPt_x >0) /*is Forward WayPt*/ return true; else return false; } bool PurePursuit::isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos) { double dx = wayPt.x - car_pos.x; double dy = wayPt.y - car_pos.y; double dist = sqrt(dx*dx + dy*dy); if(dist < Lfw) return false; else if(dist >= Lfw) return true; } geometry_msgs::Point PurePursuit::get_odom_car2WayPtVec(const geometry_msgs::Pose& carPose) { geometry_msgs::Point carPose_pos = carPose.position; double carPose_yaw = getYawFromPose(carPose); geometry_msgs::Point forwardPt; geometry_msgs::Point odom_car2WayPtVec; foundForwardPt = false; if(!goal_reached){ for(int i =0; i< map_path.poses.size(); i++) { geometry_msgs::PoseStamped map_path_pose = map_path.poses[i]; geometry_msgs::PoseStamped odom_path_pose; try { tf_listener.transformPose("odom", ros::Time(0) , map_path_pose, "map" ,odom_path_pose); geometry_msgs::Point odom_path_wayPt = odom_path_pose.pose.position; bool _isForwardWayPt = isForwardWayPt(odom_path_wayPt,carPose); if(_isForwardWayPt) { bool _isWayPtAwayFromLfwDist = isWayPtAwayFromLfwDist(odom_path_wayPt,carPose_pos); if(_isWayPtAwayFromLfwDist) { forwardPt = odom_path_wayPt; foundForwardPt = true; break; } } } catch(tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); } } } else if(goal_reached) { forwardPt = odom_goal_pos; foundForwardPt = false; //ROS_INFO("goal REACHED!"); } /*Visualized Target Point on RVIZ*/ /*Clear former target point Marker*/ points.points.clear(); line_strip.points.clear(); if(foundForwardPt && !goal_reached) { points.points.push_back(carPose_pos); points.points.push_back(forwardPt); line_strip.points.push_back(carPose_pos); line_strip.points.push_back(forwardPt); } marker_pub.publish(points); marker_pub.publish(line_strip); odom_car2WayPtVec.x = cos(carPose_yaw)*(forwardPt.x - carPose_pos.x) + sin(carPose_yaw)*(forwardPt.y - carPose_pos.y); odom_car2WayPtVec.y = -sin(carPose_yaw)*(forwardPt.x - carPose_pos.x) + cos(carPose_yaw)*(forwardPt.y - carPose_pos.y); return odom_car2WayPtVec; } double PurePursuit::getEta(const geometry_msgs::Pose& carPose) { geometry_msgs::Point odom_car2WayPtVec = get_odom_car2WayPtVec(carPose); return atan2(odom_car2WayPtVec.y,odom_car2WayPtVec.x); } double PurePursuit::getCar2GoalDist() { geometry_msgs::Point car_pose = this->odom.pose.pose.position; double car2goal_x = this->odom_goal_pos.x - car_pose.x; double car2goal_y = this->odom_goal_pos.y - car_pose.y; return sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y); } double PurePursuit::getSteering(double eta) { return atan2((this->L*sin(eta)),(this->Lfw/2 + this->lfw*cos(eta))); } void PurePursuit::amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg) { if(this->goal_received) { double car2goal_x = this->goal_pos.x - amclMsg->pose.pose.position.x; double car2goal_y = this->goal_pos.y - amclMsg->pose.pose.position.y; double dist2goal = sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y); if(dist2goal < this->goal_radius) { this->goal_reached = true; this->goal_received = false; ROS_INFO("Goal Reached !"); } } } void PurePursuit::controlLoopCB(const ros::TimerEvent&) { geometry_msgs::Pose carPose = this->odom.pose.pose; geometry_msgs::Twist carVel = this->odom.twist.twist; if(this->goal_received) { /*Estimate Steering Angle*/ double eta = getEta(carPose); if(foundForwardPt) { this->steering = this->base_angle + getSteering(eta)*this->steering_gain; /*Estimate Gas Input*/ if(!this->goal_reached) { if(this->smooth_accel) this->velocity = std::min(this->velocity + this->speed_incremental, this->Vcmd); else this->velocity = this->Vcmd; if(debug_mode) ROS_INFO("Velocity = %.2f, Steering = %.2f", this->velocity, this->steering); } } } if(this->goal_reached) { this->velocity = 0.0; this->steering = this->base_angle; } this->ackermann_cmd.drive.steering_angle = this->steering; this->ackermann_cmd.drive.speed = this->velocity; this->ackermann_pub.publish(this->ackermann_cmd); if(this->cmd_vel_mode) { this->cmd_vel.linear.x = this->velocity; this->cmd_vel.angular.z = this->steering; this->cmdvel_pub.publish(this->cmd_vel); } } /*****************/ /* MAIN FUNCTION */ /*****************/ int main(int argc, char **argv) { //Initiate ROS ros::init(argc, argv, "PurePursuit"); PurePursuit controller; ros::AsyncSpinner spinner(2); // Use multi threads spinner.start(); ros::waitForShutdown(); return 0; }