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