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