add backend
This commit is contained in:
Han-Sin
29
trajectoryOpt/src/utils/DecompROS/LICENSE
Normal file
29
trajectoryOpt/src/utils/DecompROS/LICENSE
Normal file
@@ -0,0 +1,29 @@
|
||||
BSD 3-Clause License
|
||||
|
||||
Copyright (c) 2018, sikang
|
||||
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 copyright holder 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 HOLDER 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.
|
||||
@@ -0,0 +1,10 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(decomp_ros_msgs)
|
||||
|
||||
find_package(catkin_simple REQUIRED)
|
||||
|
||||
catkin_simple()
|
||||
|
||||
cs_install()
|
||||
|
||||
cs_export()
|
||||
@@ -0,0 +1,2 @@
|
||||
float64[3] d
|
||||
float64[9] E
|
||||
@@ -0,0 +1,2 @@
|
||||
Header header
|
||||
Ellipsoid[] ellipsoids
|
||||
@@ -0,0 +1,2 @@
|
||||
geometry_msgs/Point[] points
|
||||
geometry_msgs/Point[] normals #norm is an outer vector
|
||||
@@ -0,0 +1,2 @@
|
||||
Header header
|
||||
Polyhedron[] polyhedrons
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>decomp_ros_msgs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The decomp_ros_msgs package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="sikang@seas.upenn.edu">sikang</maintainer>
|
||||
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>catkin_simple</build_depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>message_generation</depend>
|
||||
<depend>message_runtime</depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,47 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(decomp_ros_utils)
|
||||
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3 -Wall -Wno-deprecated-declarations")
|
||||
find_package(catkin REQUIRED COMPONENTS rviz roscpp)
|
||||
find_package(catkin_simple REQUIRED)
|
||||
find_package(cmake_modules)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
|
||||
|
||||
include_directories(${EIGEN3_INCLUDE_DIRS} ${DECOMP_UTIL_INCLUDE_DIRS})
|
||||
|
||||
add_definitions(-DQT_NO_KEYWORDS)
|
||||
|
||||
## This setting causes Qt's "MOC" generation to happen automatically.
|
||||
## this does not moc things in include!!!!!!! only in src
|
||||
set(CMAKE_AUTOMOC ON)
|
||||
|
||||
## We'll use the version that rviz used so they are compatible.
|
||||
if(rviz_QT_VERSION VERSION_LESS "5")
|
||||
message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
|
||||
find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui)
|
||||
include(${QT_USE_FILE})
|
||||
else()
|
||||
message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
|
||||
find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
|
||||
set(QT_LIBRARIES Qt5::Widgets)
|
||||
endif()
|
||||
|
||||
catkin_simple()
|
||||
|
||||
set(SOURCE_FILES
|
||||
src/bound_visual.cpp
|
||||
src/mesh_visual.cpp
|
||||
src/vector_visual.cpp
|
||||
src/ellipsoid_array_visual.cpp
|
||||
src/ellipsoid_array_display.cpp
|
||||
src/polyhedron_array_display.cpp
|
||||
${MOC_FILES})
|
||||
|
||||
cs_add_library(decomp_rviz_plugins ${SOURCE_FILES})
|
||||
target_link_libraries(decomp_rviz_plugins ${QT_LIBRARIES} ${catkin_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES})
|
||||
|
||||
cs_install()
|
||||
|
||||
cs_export()
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 145 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 259 KiB |
@@ -0,0 +1,130 @@
|
||||
/**
|
||||
* @file data_type.h
|
||||
* @brief Defines all data types used in this lib
|
||||
|
||||
* Mostly alias from Eigen Library.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
#include <Eigen/Geometry>
|
||||
#include <Eigen/StdVector>
|
||||
|
||||
///Set red font in printf funtion
|
||||
#ifndef ANSI_COLOR_RED
|
||||
#define ANSI_COLOR_RED "\x1b[1;31m"
|
||||
#endif
|
||||
///Set green font in printf funtion
|
||||
#ifndef ANSI_COLOR_GREEN
|
||||
#define ANSI_COLOR_GREEN "\x1b[1;32m"
|
||||
#endif
|
||||
///Set yellow font in printf funtion
|
||||
#ifndef ANSI_COLOR_YELLOW
|
||||
#define ANSI_COLOR_YELLOW "\x1b[1;33m"
|
||||
#endif
|
||||
///Set blue font in printf funtion
|
||||
#ifndef ANSI_COLOR_BLUE
|
||||
#define ANSI_COLOR_BLUE "\x1b[1;34m"
|
||||
#endif
|
||||
///Set magenta font in printf funtion
|
||||
#ifndef ANSI_COLOR_MAGENTA
|
||||
#define ANSI_COLOR_MAGENTA "\x1b[1;35m"
|
||||
#endif
|
||||
///Set cyan font in printf funtion
|
||||
#ifndef ANSI_COLOR_CYAN
|
||||
#define ANSI_COLOR_CYAN "\x1b[1;36m"
|
||||
#endif
|
||||
///Reset font color in printf funtion
|
||||
#ifndef ANSI_COLOR_RESET
|
||||
#define ANSI_COLOR_RESET "\x1b[0m"
|
||||
#endif
|
||||
|
||||
#ifndef DATA_TYPE_H
|
||||
#define DATA_TYPE_H
|
||||
/*! \brief Rename the float type used in lib
|
||||
|
||||
Default is set to be double, but user can change it to float.
|
||||
*/
|
||||
typedef double decimal_t;
|
||||
|
||||
///Pre-allocated std::vector for Eigen using vec_E
|
||||
template <typename T>
|
||||
using vec_E = std::vector<T, Eigen::aligned_allocator<T>>;
|
||||
///Eigen 1D float vector
|
||||
template <int N>
|
||||
using Vecf = Eigen::Matrix<decimal_t, N, 1>;
|
||||
///Eigen 1D int vector
|
||||
template <int N>
|
||||
using Veci = Eigen::Matrix<int, N, 1>;
|
||||
///MxN Eigen matrix
|
||||
template <int M, int N>
|
||||
using Matf = Eigen::Matrix<decimal_t, M, N>;
|
||||
///MxN Eigen matrix with M unknown
|
||||
template <int N>
|
||||
using MatDNf = Eigen::Matrix<decimal_t, Eigen::Dynamic, N>;
|
||||
///Vector of Eigen 1D float vector
|
||||
template <int N>
|
||||
using vec_Vecf = vec_E<Vecf<N>>;
|
||||
///Vector of Eigen 1D int vector
|
||||
template <int N>
|
||||
using vec_Veci = vec_E<Veci<N>>;
|
||||
|
||||
///Eigen 1D float vector of size 2
|
||||
typedef Vecf<2> Vec2f;
|
||||
///Eigen 1D int vector of size 2
|
||||
typedef Veci<2> Vec2i;
|
||||
///Eigen 1D float vector of size 3
|
||||
typedef Vecf<3> Vec3f;
|
||||
///Eigen 1D int vector of size 3
|
||||
typedef Veci<3> Vec3i;
|
||||
///Eigen 1D float vector of size 4
|
||||
typedef Vecf<4> Vec4f;
|
||||
///Column vector in float of size 6
|
||||
typedef Vecf<6> Vec6f;
|
||||
|
||||
///Vector of type Vec2f.
|
||||
typedef vec_E<Vec2f> vec_Vec2f;
|
||||
///Vector of type Vec2i.
|
||||
typedef vec_E<Vec2i> vec_Vec2i;
|
||||
///Vector of type Vec3f.
|
||||
typedef vec_E<Vec3f> vec_Vec3f;
|
||||
///Vector of type Vec3i.
|
||||
typedef vec_E<Vec3i> vec_Vec3i;
|
||||
|
||||
///2x2 Matrix in float
|
||||
typedef Matf<2, 2> Mat2f;
|
||||
///3x3 Matrix in float
|
||||
typedef Matf<3, 3> Mat3f;
|
||||
///4x4 Matrix in float
|
||||
typedef Matf<4, 4> Mat4f;
|
||||
///6x6 Matrix in float
|
||||
typedef Matf<6, 6> Mat6f;
|
||||
|
||||
///Dynamic Nx1 Eigen float vector
|
||||
typedef Vecf<Eigen::Dynamic> VecDf;
|
||||
///Nx2 Eigen float matrix
|
||||
typedef MatDNf<2> MatD2f;
|
||||
///Nx3 Eigen float matrix
|
||||
typedef MatDNf<3> MatD3f;
|
||||
///Dynamic MxN Eigen float matrix
|
||||
typedef Matf<Eigen::Dynamic, Eigen::Dynamic> MatDf;
|
||||
|
||||
///Allias of Eigen::Affine2d
|
||||
typedef Eigen::Transform<decimal_t, 2, Eigen::Affine> Aff2f;
|
||||
///Allias of Eigen::Affine3d
|
||||
typedef Eigen::Transform<decimal_t, 3, Eigen::Affine> Aff3f;
|
||||
#endif
|
||||
|
||||
#ifndef EIGEN_QUAT
|
||||
#define EIGEN_QUAT
|
||||
///Allias of Eigen::Quaterniond
|
||||
typedef Eigen::Quaternion<decimal_t> Quatf;
|
||||
#endif
|
||||
|
||||
#ifndef EIGEN_EPSILON
|
||||
#define EIGEN_EPSILON
|
||||
///Compensate for numerical error
|
||||
constexpr decimal_t epsilon_ = 1e-10; // numerical calculation error
|
||||
#endif
|
||||
@@ -0,0 +1,36 @@
|
||||
/**
|
||||
* @file data_utils.h
|
||||
* @brief Provide a few widely used function for basic type
|
||||
*/
|
||||
#ifndef DATA_UTILS_H
|
||||
#define DATA_UTILS_H
|
||||
|
||||
#include <decomp_basis/data_type.h>
|
||||
|
||||
///Template for transforming a vector
|
||||
template <class T, class TF>
|
||||
vec_E<T> transform_vec(const vec_E<T> &t, const TF &tf) {
|
||||
vec_E<T> new_t;
|
||||
for (const auto &it : t)
|
||||
new_t.push_back(tf * it);
|
||||
return new_t;
|
||||
}
|
||||
|
||||
///Template for calculating distance
|
||||
template <class T>
|
||||
decimal_t total_distance(const vec_E<T>& vs){
|
||||
decimal_t dist = 0;
|
||||
for(unsigned int i = 1; i < vs.size(); i++)
|
||||
dist += (vs[i] - vs[i-1]).norm();
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
|
||||
///Transform all entries in a vector using given TF
|
||||
#define transform_vec3 transform_vec<Vec3f, Aff3f>
|
||||
///Sum up total distance for Vec3f
|
||||
#define total_distance3f total_distance<Vec3f>
|
||||
///Sum up total distance for Vec3i
|
||||
#define total_distance3i total_distance<Vec3i>
|
||||
#endif
|
||||
@@ -0,0 +1,102 @@
|
||||
/**
|
||||
* @file ellipsoid.h
|
||||
* @brief Ellipsoid class
|
||||
*/
|
||||
|
||||
#ifndef DECOMP_ELLIPSOID_H
|
||||
#define DECOMP_ELLIPSOID_H
|
||||
|
||||
#include <iostream>
|
||||
#include <decomp_basis/data_type.h>
|
||||
#include <decomp_geometry/polyhedron.h>
|
||||
|
||||
template <int Dim>
|
||||
struct Ellipsoid {
|
||||
Ellipsoid() {}
|
||||
Ellipsoid(const Matf<Dim, Dim>& C, const Vecf<Dim>& d) : C_(C), d_(d) {}
|
||||
|
||||
/// Calculate distance to the center
|
||||
decimal_t dist(const Vecf<Dim>& pt) const {
|
||||
return (C_.inverse() * (pt - d_)).norm();
|
||||
}
|
||||
|
||||
/// Check if the point is inside, non-exclusive
|
||||
bool inside(const Vecf<Dim>& pt) const {
|
||||
return dist(pt) <= 1;
|
||||
}
|
||||
|
||||
/// Calculate points inside ellipsoid, non-exclusive
|
||||
vec_Vecf<Dim> points_inside(const vec_Vecf<Dim> &O) const {
|
||||
vec_Vecf<Dim> new_O;
|
||||
for (const auto &it : O) {
|
||||
if (inside(it))
|
||||
new_O.push_back(it);
|
||||
}
|
||||
return new_O;
|
||||
}
|
||||
|
||||
///Find the closest point
|
||||
Vecf<Dim> closest_point(const vec_Vecf<Dim> &O) const {
|
||||
Vecf<Dim> pt = Vecf<Dim>::Zero();
|
||||
decimal_t min_dist = std::numeric_limits<decimal_t>::max();
|
||||
for (const auto &it : O) {
|
||||
decimal_t d = dist(it);
|
||||
if (d < min_dist) {
|
||||
min_dist = d;
|
||||
pt = it;
|
||||
}
|
||||
}
|
||||
return pt;
|
||||
}
|
||||
|
||||
///Find the closest hyperplane from the closest point
|
||||
Hyperplane<Dim> closest_hyperplane(const vec_Vecf<Dim> &O) const {
|
||||
const auto closest_pt = closest_point(O);
|
||||
const auto n = C_.inverse() * C_.inverse().transpose() *
|
||||
(closest_pt - d_);
|
||||
return Hyperplane<Dim>(closest_pt, n.normalized());
|
||||
}
|
||||
|
||||
/// Sample n points along the contour
|
||||
template<int U = Dim>
|
||||
typename std::enable_if<U == 2, vec_Vecf<U>>::type
|
||||
sample(int num) const {
|
||||
vec_Vecf<Dim> pts;
|
||||
decimal_t dyaw = M_PI*2/num;
|
||||
for(decimal_t yaw = 0; yaw < M_PI*2; yaw+=dyaw) {
|
||||
Vecf<Dim> pt;
|
||||
pt << cos(yaw), sin(yaw);
|
||||
pts.push_back(C_ * pt + d_);
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
void print() const {
|
||||
std::cout << "C: " << C_ << std::endl;
|
||||
std::cout << "d: " << d_ << std::endl;
|
||||
}
|
||||
|
||||
/// Get ellipsoid volume
|
||||
decimal_t volume() const {
|
||||
return C_.determinant();
|
||||
}
|
||||
|
||||
/// Get C matrix
|
||||
Matf<Dim, Dim> C() const {
|
||||
return C_;
|
||||
}
|
||||
|
||||
/// Get center
|
||||
Vecf<Dim> d() const {
|
||||
return d_;
|
||||
}
|
||||
|
||||
Matf<Dim, Dim> C_;
|
||||
Vecf<Dim> d_;
|
||||
};
|
||||
|
||||
typedef Ellipsoid<2> Ellipsoid2D;
|
||||
|
||||
typedef Ellipsoid<3> Ellipsoid3D;
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,258 @@
|
||||
/**
|
||||
* @file geometric_utils.h
|
||||
* @brief basic geometry utils
|
||||
*/
|
||||
#ifndef DECOMP_GEOMETRIC_UTILS_H
|
||||
#define DECOMP_GEOMETRIC_UTILS_H
|
||||
|
||||
#include <Eigen/Eigenvalues>
|
||||
#include <decomp_basis/data_utils.h>
|
||||
#include <decomp_geometry/polyhedron.h>
|
||||
#include <iostream>
|
||||
|
||||
/// Calculate eigen values
|
||||
template <int Dim> Vecf<Dim> eigen_value(const Matf<Dim, Dim> &A) {
|
||||
Eigen::SelfAdjointEigenSolver<Matf<Dim, Dim>> es(A);
|
||||
return es.eigenvalues();
|
||||
}
|
||||
|
||||
/// Calculate rotation matrix from a vector (aligned with x-axis)
|
||||
inline Mat2f vec2_to_rotation(const Vec2f &v) {
|
||||
decimal_t yaw = std::atan2(v(1), v(0));
|
||||
Mat2f R;
|
||||
R << cos(yaw), -sin(yaw), sin(yaw), cos(yaw);
|
||||
return R;
|
||||
}
|
||||
|
||||
inline Mat3f vec3_to_rotation(const Vec3f &v) {
|
||||
// zero roll
|
||||
Vec3f rpy(0, std::atan2(-v(2), v.topRows<2>().norm()),
|
||||
std::atan2(v(1), v(0)));
|
||||
Quatf qx(cos(rpy(0) / 2), sin(rpy(0) / 2), 0, 0);
|
||||
Quatf qy(cos(rpy(1) / 2), 0, sin(rpy(1) / 2), 0);
|
||||
Quatf qz(cos(rpy(2) / 2), 0, 0, sin(rpy(2) / 2));
|
||||
return Mat3f(qz * qy * qx);
|
||||
}
|
||||
|
||||
/// Sort plannar points in the counter-clockwise order
|
||||
inline vec_Vec2f sort_pts(const vec_Vec2f &pts) {
|
||||
/// if empty, dont sort
|
||||
if (pts.empty())
|
||||
return pts;
|
||||
/// calculate center point
|
||||
Vec2f avg = Vec2f::Zero();
|
||||
for (const auto &pt : pts)
|
||||
avg += pt;
|
||||
avg /= pts.size();
|
||||
|
||||
/// sort in body frame
|
||||
vec_E<std::pair<decimal_t, Vec2f>> pts_valued;
|
||||
pts_valued.resize(pts.size());
|
||||
for (unsigned int i = 0; i < pts.size(); i++) {
|
||||
decimal_t theta = atan2(pts[i](1) - avg(1), pts[i](0) - avg(0));
|
||||
pts_valued[i] = std::make_pair(theta, pts[i]);
|
||||
}
|
||||
|
||||
std::sort(
|
||||
pts_valued.begin(), pts_valued.end(),
|
||||
[](const std::pair<decimal_t, Vec2f> &i,
|
||||
const std::pair<decimal_t, Vec2f> &j) { return i.first < j.first; });
|
||||
vec_Vec2f pts_sorted(pts_valued.size());
|
||||
for (size_t i = 0; i < pts_valued.size(); i++)
|
||||
pts_sorted[i] = pts_valued[i].second;
|
||||
return pts_sorted;
|
||||
}
|
||||
|
||||
/// Find intersection between two lines on the same plane, return false if they
|
||||
/// are not intersected
|
||||
inline bool line_intersect(const std::pair<Vec2f, Vec2f> &v1,
|
||||
const std::pair<Vec2f, Vec2f> &v2, Vec2f &pi) {
|
||||
decimal_t a1 = -v1.first(1);
|
||||
decimal_t b1 = v1.first(0);
|
||||
decimal_t c1 = a1 * v1.second(0) + b1 * v1.second(1);
|
||||
|
||||
decimal_t a2 = -v2.first(1);
|
||||
decimal_t b2 = v2.first(0);
|
||||
decimal_t c2 = a2 * v2.second(0) + b2 * v2.second(1);
|
||||
|
||||
decimal_t x = (c1 * b2 - c2 * b1) / (a1 * b2 - a2 * b1);
|
||||
decimal_t y = (c1 * a2 - c2 * a1) / (a2 * b1 - a1 * b2);
|
||||
|
||||
if (std::isnan(x) || std::isnan(y) || std::isinf(x) || std::isinf(y))
|
||||
return false;
|
||||
else {
|
||||
pi << x, y;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
/// Find intersection between multiple lines
|
||||
inline vec_Vec2f line_intersects(const vec_E<std::pair<Vec2f, Vec2f>> &lines) {
|
||||
vec_Vec2f pts;
|
||||
for (unsigned int i = 0; i < lines.size(); i++) {
|
||||
for (unsigned int j = i + 1; j < lines.size(); j++) {
|
||||
Vec2f pi;
|
||||
if (line_intersect(lines[i], lines[j], pi)) {
|
||||
pts.push_back(pi);
|
||||
}
|
||||
}
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
/// Find extreme points of Polyhedron2D
|
||||
inline vec_Vec2f cal_vertices(const Polyhedron2D &poly) {
|
||||
vec_E<std::pair<Vec2f, Vec2f>> lines;
|
||||
const auto vs = poly.hyperplanes();
|
||||
for (unsigned int i = 0; i < vs.size(); i++) {
|
||||
Vec2f n = vs[i].n_;
|
||||
Vec2f v(-n(1), n(0));
|
||||
v = v.normalized();
|
||||
|
||||
lines.push_back(std::make_pair(v, vs[i].p_));
|
||||
/*
|
||||
std::cout << "add p: " << lines.back().second.transpose() <<
|
||||
" v: " << lines.back().first.transpose() << std::endl;
|
||||
*/
|
||||
}
|
||||
|
||||
auto vts = line_intersects(lines);
|
||||
// for(const auto& it: vts)
|
||||
// std::cout << "vertice: " << it.transpose() << std::endl;
|
||||
|
||||
vec_Vec2f vts_inside = poly.points_inside(vts);
|
||||
vts_inside = sort_pts(vts_inside);
|
||||
|
||||
return vts_inside;
|
||||
}
|
||||
|
||||
/// Find extreme points of Polyhedron3D
|
||||
inline vec_E<vec_Vec3f> cal_vertices(const Polyhedron3D &poly) {
|
||||
vec_E<vec_Vec3f> bds;
|
||||
const auto vts = poly.hyperplanes();
|
||||
//**** for each plane, find lines on it
|
||||
for (unsigned int i = 0; i < vts.size(); i++) {
|
||||
const Vec3f t = vts[i].p_;
|
||||
const Vec3f n = vts[i].n_;
|
||||
const Quatf q = Quatf::FromTwoVectors(Vec3f(0, 0, 1), n);
|
||||
const Mat3f R(q); // body to world
|
||||
vec_E<std::pair<Vec2f, Vec2f>> lines;
|
||||
for (unsigned int j = 0; j < vts.size(); j++) {
|
||||
if (j == i)
|
||||
continue;
|
||||
Vec3f nw = vts[j].n_;
|
||||
Vec3f nb = R.transpose() * nw;
|
||||
decimal_t bb = vts[j].p_.dot(nw) - nw.dot(t);
|
||||
Vec2f v = Vec3f(0, 0, 1).cross(nb).topRows<2>(); // line direction
|
||||
Vec2f p; // point on the line
|
||||
if (nb(1) != 0)
|
||||
p << 0, bb / nb(1);
|
||||
else if (nb(0) != 0)
|
||||
p << bb / nb(0), 0;
|
||||
else
|
||||
continue;
|
||||
lines.push_back(std::make_pair(v, p));
|
||||
}
|
||||
|
||||
//**** find all intersect points
|
||||
vec_Vec2f pts = line_intersects(lines);
|
||||
//**** filter out points inside polytope
|
||||
vec_Vec2f pts_inside;
|
||||
for (const auto &it : pts) {
|
||||
Vec3f p = R * Vec3f(it(0), it(1), 0) + t; // convert to world frame
|
||||
if (poly.inside(p))
|
||||
pts_inside.push_back(it);
|
||||
}
|
||||
|
||||
if (pts_inside.size() > 2) {
|
||||
//**** sort in plane frame
|
||||
pts_inside = sort_pts(pts_inside);
|
||||
|
||||
//**** transform to world frame
|
||||
vec_Vec3f points_valid;
|
||||
for (auto &it : pts_inside)
|
||||
points_valid.push_back(R * Vec3f(it(0), it(1), 0) + t);
|
||||
|
||||
//**** insert resulting polygon
|
||||
bds.push_back(points_valid);
|
||||
}
|
||||
}
|
||||
return bds;
|
||||
}
|
||||
|
||||
/// Get the convex hull of a 2D points array, use wrapping method
|
||||
inline vec_Vec2f cal_convex_hull(const vec_Vec2f &pts) {
|
||||
/// find left most point
|
||||
Vec2f p0;
|
||||
decimal_t min_x = std::numeric_limits<decimal_t>::infinity();
|
||||
for (const auto &it : pts) {
|
||||
if (min_x > it(0) || (min_x == it(0) && it(1) < p0(1))) {
|
||||
min_x = it(0);
|
||||
p0 = it;
|
||||
}
|
||||
}
|
||||
|
||||
vec_Vec2f vs;
|
||||
vs.push_back(p0);
|
||||
|
||||
while (vs.back() != p0 || vs.size() == 1) {
|
||||
const auto ref_pt = vs.back();
|
||||
Vec2f end_pt = p0;
|
||||
for (size_t i = 0; i < pts.size(); i++) {
|
||||
if (pts[i] == ref_pt)
|
||||
continue;
|
||||
Vec2f dir = (pts[i] - ref_pt).normalized();
|
||||
Hyperplane2D hp(ref_pt, Vec2f(-dir(1), dir(0)));
|
||||
bool most_left_hp = true;
|
||||
for (size_t j = 0; j < pts.size(); j++) {
|
||||
if (hp.signed_dist(pts[j]) > 0 && pts[j] != pts[i] &&
|
||||
pts[j] != ref_pt) {
|
||||
// if(hp.signed_dist(pts[j]) > 0) {
|
||||
most_left_hp = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (most_left_hp) {
|
||||
end_pt = pts[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
// std::cout << "add: " << end_pt.transpose() << std::endl;
|
||||
vs.push_back(end_pt);
|
||||
}
|
||||
|
||||
return vs;
|
||||
}
|
||||
|
||||
inline Polyhedron2D get_convex_hull(const vec_Vec2f &pts) {
|
||||
Polyhedron2D poly;
|
||||
Vec2f prev_dir(-1, -1);
|
||||
for (size_t i = 0; i < pts.size() - 1; i++) {
|
||||
size_t j = i + 1;
|
||||
Vec2f dir = (pts[j] - pts[i]).normalized();
|
||||
if (dir != prev_dir) {
|
||||
poly.add(Hyperplane2D((pts[i] + pts[j]) / 2, Vec2f(-dir(1), dir(0))));
|
||||
prev_dir = dir;
|
||||
}
|
||||
}
|
||||
|
||||
return poly;
|
||||
}
|
||||
|
||||
/// Minkowski sum, add B to A with center Bc
|
||||
inline Polyhedron2D minkowski_sum(const Polyhedron2D &A, const Polyhedron2D &B,
|
||||
const Vec2f &Bc) {
|
||||
const auto A_vertices = cal_vertices(A);
|
||||
const auto B_vertices = cal_vertices(B);
|
||||
|
||||
vec_Vec2f C_vertices;
|
||||
for (const auto &it : A_vertices) {
|
||||
for (const auto &itt : B_vertices)
|
||||
C_vertices.push_back(it + itt - Bc);
|
||||
}
|
||||
|
||||
return get_convex_hull(cal_convex_hull(C_vertices));
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,154 @@
|
||||
/**
|
||||
* @file polygon.h
|
||||
* @brief Polygon class
|
||||
*/
|
||||
|
||||
#ifndef DECOMP_POLYGON_H
|
||||
#define DECOMP_POLYGON_H
|
||||
|
||||
#include <decomp_basis/data_type.h>
|
||||
|
||||
///Hyperplane class
|
||||
template <int Dim>
|
||||
struct Hyperplane {
|
||||
Hyperplane() {}
|
||||
Hyperplane(const Vecf<Dim>& p, const Vecf<Dim>& n) : p_(p), n_(n) {}
|
||||
|
||||
/// Calculate the signed distance from point
|
||||
decimal_t signed_dist(const Vecf<Dim>& pt) const {
|
||||
return n_.dot(pt - p_);
|
||||
}
|
||||
|
||||
/// Calculate the distance from point
|
||||
decimal_t dist(const Vecf<Dim>& pt) const {
|
||||
return std::abs(signed_dist(pt));
|
||||
}
|
||||
|
||||
/// Point on the plane
|
||||
Vecf<Dim> p_;
|
||||
/// Normal of the plane, directional
|
||||
Vecf<Dim> n_;
|
||||
};
|
||||
|
||||
///Hyperplane2D: first is the point on the hyperplane, second is the normal
|
||||
typedef Hyperplane<2> Hyperplane2D;
|
||||
///Hyperplane3D: first is the point on the hyperplane, second is the normal
|
||||
typedef Hyperplane<3> Hyperplane3D;
|
||||
|
||||
|
||||
///Polyhedron class
|
||||
template <int Dim>
|
||||
struct Polyhedron {
|
||||
///Null constructor
|
||||
Polyhedron() {}
|
||||
///Construct from Hyperplane array
|
||||
Polyhedron(const vec_E<Hyperplane<Dim>>& vs) : vs_(vs) {}
|
||||
|
||||
|
||||
///Append Hyperplane
|
||||
void add(const Hyperplane<Dim>& v) {
|
||||
vs_.push_back(v);
|
||||
}
|
||||
|
||||
/// Check if the point is inside polyhedron, non-exclusive
|
||||
bool inside(const Vecf<Dim>& pt) const {
|
||||
for (const auto& v : vs_) {
|
||||
if (v.signed_dist(pt) > epsilon_) {
|
||||
//printf("rejected pt: (%f, %f), d: %f\n",pt(0), pt(1), v.signed_dist(pt));
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/// Calculate points inside polyhedron, non-exclusive
|
||||
vec_Vecf<Dim> points_inside(const vec_Vecf<Dim> &O) const {
|
||||
vec_Vecf<Dim> new_O;
|
||||
for (const auto &it : O) {
|
||||
if (inside(it))
|
||||
new_O.push_back(it);
|
||||
}
|
||||
return new_O;
|
||||
}
|
||||
|
||||
/// Calculate normals, used for visualization
|
||||
vec_E<std::pair<Vecf<Dim>, Vecf<Dim>>> cal_normals() const {
|
||||
vec_E<std::pair<Vecf<Dim>, Vecf<Dim>>> ns(vs_.size());
|
||||
for (size_t i = 0; i < vs_.size(); i++)
|
||||
ns[i] = std::make_pair(vs_[i].p_, vs_[i].n_); // fist is point, second is normal
|
||||
return ns;
|
||||
}
|
||||
|
||||
/// Get the hyperplane array
|
||||
vec_E<Hyperplane<Dim>> hyperplanes() const {
|
||||
return vs_;
|
||||
}
|
||||
|
||||
/// Hyperplane array
|
||||
vec_E<Hyperplane<Dim>> vs_; // normal must go outside
|
||||
|
||||
};
|
||||
|
||||
///Polyhedron2D, consists of 2D hyperplane
|
||||
typedef Polyhedron<2> Polyhedron2D;
|
||||
///Polyhedron3D, consists of 3D hyperplane
|
||||
typedef Polyhedron<3> Polyhedron3D;
|
||||
|
||||
///[A, b] for \f$Ax < b\f$
|
||||
template <int Dim>
|
||||
struct LinearConstraint {
|
||||
///Null constructor
|
||||
LinearConstraint() {}
|
||||
/// Construct from \f$A, b\f$ directly, s.t \f$Ax < b\f$
|
||||
LinearConstraint(const MatDNf<Dim>& A, const VecDf& b) : A_(A), b_(b) {}
|
||||
/**
|
||||
* @brief Construct from a inside point and hyperplane array
|
||||
* @param p0 point that is inside
|
||||
* @param vs hyperplane array, normal should go outside
|
||||
*/
|
||||
LinearConstraint(const Vecf<Dim> p0, const vec_E<Hyperplane<Dim>>& vs) {
|
||||
const unsigned int size = vs.size();
|
||||
MatDNf<Dim> A(size, Dim);
|
||||
VecDf b(size);
|
||||
|
||||
for (unsigned int i = 0; i < size; i++) {
|
||||
auto n = vs[i].n_;
|
||||
decimal_t c = vs[i].p_.dot(n);
|
||||
if (n.dot(p0) - c > 0) {
|
||||
n = -n;
|
||||
c = -c;
|
||||
}
|
||||
A.row(i) = n;
|
||||
b(i) = c;
|
||||
}
|
||||
|
||||
A_ = A;
|
||||
b_ = b;
|
||||
}
|
||||
|
||||
/// Check if the point is inside polyhedron using linear constraint
|
||||
bool inside(const Vecf<Dim> &pt) {
|
||||
VecDf d = A_ * pt - b_;
|
||||
for (unsigned int i = 0; i < d.rows(); i++) {
|
||||
if (d(i) > 0)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/// Get \f$A\f$ matrix
|
||||
MatDNf<Dim> A() const { return A_; }
|
||||
|
||||
/// Get \f$b\f$ matrix
|
||||
VecDf b() const { return b_; }
|
||||
|
||||
MatDNf<Dim> A_;
|
||||
VecDf b_;
|
||||
};
|
||||
|
||||
///LinearConstraint 2D
|
||||
typedef LinearConstraint<2> LinearConstraint2D;
|
||||
///LinearConstraint 3D
|
||||
typedef LinearConstraint<3> LinearConstraint3D;
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,160 @@
|
||||
#ifndef DECOMP_ROS_UTILS_H
|
||||
#define DECOMP_ROS_UTILS_H
|
||||
|
||||
#include <decomp_geometry/ellipsoid.h>
|
||||
#include <decomp_geometry/polyhedron.h>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <decomp_ros_msgs/PolyhedronArray.h>
|
||||
#include <decomp_ros_msgs/EllipsoidArray.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
|
||||
namespace DecompROS {
|
||||
|
||||
template <int Dim> nav_msgs::Path vec_to_path(const vec_Vecf<Dim> &vs) {
|
||||
nav_msgs::Path path;
|
||||
for (const auto& it : vs) {
|
||||
geometry_msgs::PoseStamped pose;
|
||||
pose.pose.position.x = it(0);
|
||||
pose.pose.position.y = it(1);
|
||||
pose.pose.position.z = Dim == 2 ? 0 : it(2);
|
||||
pose.pose.orientation.w = 1.0;
|
||||
pose.pose.orientation.x = 0.0;
|
||||
pose.pose.orientation.y = 0.0;
|
||||
pose.pose.orientation.z = 0.0;
|
||||
|
||||
path.poses.push_back(pose);
|
||||
}
|
||||
|
||||
return path;
|
||||
}
|
||||
|
||||
inline sensor_msgs::PointCloud vec_to_cloud(const vec_Vec3f &pts) {
|
||||
sensor_msgs::PointCloud cloud;
|
||||
cloud.points.resize(pts.size());
|
||||
|
||||
for (unsigned int i = 0; i < pts.size(); i++) {
|
||||
cloud.points[i].x = pts[i](0);
|
||||
cloud.points[i].y = pts[i](1);
|
||||
cloud.points[i].z = pts[i](2);
|
||||
}
|
||||
return cloud;
|
||||
}
|
||||
|
||||
inline vec_Vec3f cloud_to_vec(const sensor_msgs::PointCloud &cloud) {
|
||||
vec_Vec3f pts;
|
||||
pts.resize(cloud.points.size());
|
||||
for (unsigned int i = 0; i < cloud.points.size(); i++) {
|
||||
pts[i](0) = cloud.points[i].x;
|
||||
pts[i](1) = cloud.points[i].y;
|
||||
pts[i](2) = cloud.points[i].z;
|
||||
}
|
||||
|
||||
return pts;
|
||||
}
|
||||
|
||||
inline Polyhedron3D ros_to_polyhedron(const decomp_ros_msgs::Polyhedron& msg){
|
||||
Polyhedron3D poly;
|
||||
for(unsigned int i = 0; i < msg.points.size(); i++){
|
||||
Vec3f pt(msg.points[i].x,
|
||||
msg.points[i].y,
|
||||
msg.points[i].z);
|
||||
Vec3f n(msg.normals[i].x,
|
||||
msg.normals[i].y,
|
||||
msg.normals[i].z);
|
||||
poly.add(Hyperplane3D(pt, n));
|
||||
}
|
||||
return poly;
|
||||
}
|
||||
|
||||
inline vec_E<Polyhedron3D> ros_to_polyhedron_array(const decomp_ros_msgs::PolyhedronArray& msg) {
|
||||
vec_E<Polyhedron3D> polys(msg.polyhedrons.size());
|
||||
|
||||
for(size_t i = 0; i < msg.polyhedrons.size(); i++)
|
||||
polys[i] = ros_to_polyhedron(msg.polyhedrons[i]);
|
||||
|
||||
return polys;
|
||||
}
|
||||
|
||||
inline decomp_ros_msgs::Polyhedron polyhedron_to_ros(const Polyhedron2D& poly){
|
||||
decomp_ros_msgs::Polyhedron msg;
|
||||
for (const auto &p : poly.hyperplanes()) {
|
||||
geometry_msgs::Point pt, n;
|
||||
pt.x = p.p_(0);
|
||||
pt.y = p.p_(1);
|
||||
pt.z = 0;
|
||||
n.x = p.n_(0);
|
||||
n.y = p.n_(1);
|
||||
n.z = 0;
|
||||
msg.points.push_back(pt);
|
||||
msg.normals.push_back(n);
|
||||
}
|
||||
|
||||
geometry_msgs::Point pt1, n1;
|
||||
pt1.x = 0, pt1.y = 0, pt1.z = 0.01;
|
||||
n1.x = 0, n1.y = 0, n1.z = 1;
|
||||
msg.points.push_back(pt1);
|
||||
msg.normals.push_back(n1);
|
||||
|
||||
geometry_msgs::Point pt2, n2;
|
||||
pt2.x = 0, pt2.y = 0, pt2.z = -0.01;
|
||||
n2.x = 0, n2.y = 0, n2.z = -1;
|
||||
msg.points.push_back(pt2);
|
||||
msg.normals.push_back(n2);
|
||||
|
||||
return msg;
|
||||
}
|
||||
|
||||
inline decomp_ros_msgs::Polyhedron polyhedron_to_ros(const Polyhedron3D& poly){
|
||||
decomp_ros_msgs::Polyhedron msg;
|
||||
for (const auto &p : poly.hyperplanes()) {
|
||||
geometry_msgs::Point pt, n;
|
||||
pt.x = p.p_(0);
|
||||
pt.y = p.p_(1);
|
||||
pt.z = p.p_(2);
|
||||
n.x = p.n_(0);
|
||||
n.y = p.n_(1);
|
||||
n.z = p.n_(2);
|
||||
msg.points.push_back(pt);
|
||||
msg.normals.push_back(n);
|
||||
}
|
||||
|
||||
return msg;
|
||||
}
|
||||
|
||||
|
||||
template <int Dim>
|
||||
decomp_ros_msgs::PolyhedronArray polyhedron_array_to_ros(const vec_E<Polyhedron<Dim>>& vs){
|
||||
decomp_ros_msgs::PolyhedronArray msg;
|
||||
for (const auto &v : vs)
|
||||
msg.polyhedrons.push_back(polyhedron_to_ros(v));
|
||||
return msg;
|
||||
}
|
||||
|
||||
template <int Dim>
|
||||
decomp_ros_msgs::EllipsoidArray ellipsoid_array_to_ros(const vec_E<Ellipsoid<Dim>>& Es) {
|
||||
decomp_ros_msgs::EllipsoidArray ellipsoids;
|
||||
for (unsigned int i = 0; i < Es.size(); i++) {
|
||||
decomp_ros_msgs::Ellipsoid ellipsoid;
|
||||
auto d = Es[i].d();
|
||||
ellipsoid.d[0] = d(0);
|
||||
ellipsoid.d[1] = d(1);
|
||||
ellipsoid.d[2] = Dim == 2 ? 0:d(2);
|
||||
|
||||
auto C = Es[i].C();
|
||||
for (int x = 0; x < 3; x++) {
|
||||
for (int y = 0; y < 3; y++) {
|
||||
if(x < Dim && y < Dim)
|
||||
ellipsoid.E[3 * x + y] = C(x, y);
|
||||
else
|
||||
ellipsoid.E[3 * x + y] = 0;
|
||||
}
|
||||
}
|
||||
ellipsoids.ellipsoids.push_back(ellipsoid);
|
||||
}
|
||||
|
||||
return ellipsoids;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,96 @@
|
||||
/**
|
||||
* @file decomp_base.h
|
||||
* @brief Decomp Base Class
|
||||
*/
|
||||
#ifndef DECOMP_BASE_H
|
||||
#define DECOMP_BASE_H
|
||||
|
||||
#include <decomp_geometry/ellipsoid.h>
|
||||
#include <decomp_geometry/polyhedron.h>
|
||||
//#include <decomp_geometry/geometry_utils.h>
|
||||
|
||||
/**
|
||||
* @brief Line Segment Class
|
||||
*
|
||||
* The basic element in EllipsoidDecomp
|
||||
*/
|
||||
template <int Dim>
|
||||
class DecompBase {
|
||||
public:
|
||||
///Null constructor
|
||||
DecompBase() {}
|
||||
/**
|
||||
* @brief Adding local bounding box around line seg
|
||||
* @param Dim Distance in corresponding axis
|
||||
*
|
||||
* This virtual bounding box is parallel to the line segment, the x,y,z axes are not w.r.t the world coordinate system, but instead, x-axis is parallel to the line, y-axis is perpendicular to the line and world z-axis, z-axis is perpendiculat to the line and y-axis
|
||||
*/
|
||||
void set_local_bbox(const Vecf<Dim>& bbox) {
|
||||
local_bbox_ = bbox;
|
||||
}
|
||||
|
||||
///Import obstacle points
|
||||
void set_obs(const vec_Vecf<Dim> &obs) {
|
||||
// only consider points inside local bbox
|
||||
Polyhedron<Dim> vs;
|
||||
add_local_bbox(vs);
|
||||
obs_ = vs.points_inside(obs);
|
||||
}
|
||||
|
||||
///Get obstacel points
|
||||
vec_Vecf<Dim> get_obs() const { return obs_; }
|
||||
|
||||
///Get ellipsoid
|
||||
Ellipsoid<Dim> get_ellipsoid() const { return ellipsoid_; }
|
||||
|
||||
///Get polyhedron
|
||||
Polyhedron<Dim> get_polyhedron() const { return polyhedron_; }
|
||||
|
||||
/**
|
||||
* @brief Inflate the line segment
|
||||
* @param radius the offset added to the long semi-axis
|
||||
*/
|
||||
virtual void dilate(decimal_t radius = 0) = 0;
|
||||
|
||||
/**
|
||||
* @brief Shrink the polyhedron
|
||||
* @param shrink_distance Shrink distance
|
||||
*/
|
||||
virtual void shrink(double shrink_distance) {}
|
||||
protected:
|
||||
virtual void add_local_bbox(Polyhedron<Dim> &Vs) = 0;
|
||||
|
||||
void find_polyhedron() {
|
||||
//**** find half-space
|
||||
Polyhedron<Dim> Vs;
|
||||
vec_Vecf<Dim> obs_remain = obs_;
|
||||
while (!obs_remain.empty()) {
|
||||
const auto v = ellipsoid_.closest_hyperplane(obs_remain);
|
||||
Vs.add(v);
|
||||
vec_Vecf<Dim> obs_tmp;
|
||||
for (const auto &it : obs_remain) {
|
||||
if (v.signed_dist(it) < 0)
|
||||
obs_tmp.push_back(it);
|
||||
}
|
||||
obs_remain = obs_tmp;
|
||||
/*
|
||||
std::cout << "a: " << a.transpose() << std::endl;
|
||||
std::cout << "b: " << b << std::endl;
|
||||
*/
|
||||
}
|
||||
|
||||
polyhedron_ = Vs;
|
||||
}
|
||||
|
||||
/// Obstacles, input
|
||||
vec_Vecf<Dim> obs_;
|
||||
|
||||
/// Output ellipsoid
|
||||
Ellipsoid<Dim> ellipsoid_;
|
||||
/// Output polyhedron
|
||||
Polyhedron<Dim> polyhedron_;
|
||||
|
||||
/// Local bounding box along the line segment
|
||||
Vecf<Dim> local_bbox_{Vecf<Dim>::Zero()};
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,134 @@
|
||||
/**
|
||||
* @file ellipsoid_decomp.h
|
||||
* @brief EllipsoidDecomp Class
|
||||
*/
|
||||
#ifndef ELLIPSOID_DECOMP_H
|
||||
#define ELLIPSOID_DECOMP_H
|
||||
|
||||
#include <memory>
|
||||
#include <decomp_util/line_segment.h>
|
||||
|
||||
/**
|
||||
* @brief EllipsoidDecomp Class
|
||||
*
|
||||
* EllipsoidDecomp takes input as a given path and find the Safe Flight Corridor around it using Ellipsoids
|
||||
*/
|
||||
template <int Dim>
|
||||
class EllipsoidDecomp {
|
||||
public:
|
||||
///Simple constructor
|
||||
EllipsoidDecomp() {}
|
||||
/**
|
||||
* @brief Basic constructor
|
||||
* @param origin The origin of the global bounding box
|
||||
* @param dim The dimension of the global bounding box
|
||||
*/
|
||||
EllipsoidDecomp(const Vecf<Dim> &origin, const Vecf<Dim> &dim) {
|
||||
global_bbox_min_ = origin;
|
||||
global_bbox_max_ = origin + dim;
|
||||
}
|
||||
|
||||
///Set obstacle points
|
||||
void set_obs(const vec_Vecf<Dim> &obs) { obs_ = obs; }
|
||||
|
||||
///Set dimension of bounding box
|
||||
void set_local_bbox(const Vecf<Dim>& bbox) { local_bbox_ = bbox; }
|
||||
|
||||
///Get the path that is used for dilation
|
||||
vec_Vecf<Dim> get_path() const { return path_; }
|
||||
|
||||
///Get the Safe Flight Corridor
|
||||
vec_E<Polyhedron<Dim>> get_polyhedrons() const { return polyhedrons_; }
|
||||
|
||||
///Get the ellipsoids
|
||||
vec_E<Ellipsoid<Dim>> get_ellipsoids() const { return ellipsoids_; }
|
||||
|
||||
///Get the constraints of SFC as \f$Ax\leq b \f$
|
||||
vec_E<LinearConstraint<Dim>> get_constraints() const {
|
||||
vec_E<LinearConstraint<Dim>> constraints;
|
||||
constraints.resize(polyhedrons_.size());
|
||||
for (unsigned int i = 0; i < polyhedrons_.size(); i++){
|
||||
const Vecf<Dim> pt = (path_[i] + path_[i+1])/2;
|
||||
constraints[i] = LinearConstraint<Dim>(pt, polyhedrons_[i].hyperplanes());
|
||||
}
|
||||
return constraints;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Decomposition thread
|
||||
* @param path The path to dilate
|
||||
* @param offset_x offset added to the long semi-axis, default is 0
|
||||
*/
|
||||
void dilate(const vec_Vecf<Dim> &path, double offset_x = 0) {
|
||||
const unsigned int N = path.size() - 1;
|
||||
lines_.resize(N);
|
||||
ellipsoids_.resize(N);
|
||||
polyhedrons_.resize(N);
|
||||
|
||||
for (unsigned int i = 0; i < N; i++) {
|
||||
lines_[i] = std::make_shared<LineSegment<Dim>>(path[i], path[i+1]);
|
||||
lines_[i]->set_local_bbox(local_bbox_);
|
||||
lines_[i]->set_obs(obs_);
|
||||
lines_[i]->dilate(offset_x);
|
||||
|
||||
ellipsoids_[i] = lines_[i]->get_ellipsoid();
|
||||
polyhedrons_[i] = lines_[i]->get_polyhedron();
|
||||
}
|
||||
|
||||
|
||||
path_ = path;
|
||||
|
||||
if(global_bbox_min_.norm() != 0 || global_bbox_max_.norm() != 0) {
|
||||
for(auto& it: polyhedrons_)
|
||||
add_global_bbox(it);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
protected:
|
||||
template<int U = Dim>
|
||||
typename std::enable_if<U == 2>::type
|
||||
add_global_bbox(Polyhedron<Dim> &Vs) {
|
||||
//**** add bound along X, Y axis
|
||||
|
||||
//*** X
|
||||
Vs.add(Hyperplane2D(Vec2f(global_bbox_max_(0), 0), Vec2f(1, 0)));
|
||||
Vs.add(Hyperplane2D(Vec2f(global_bbox_min_(0), 0), Vec2f(-1, 0)));
|
||||
//*** Y
|
||||
Vs.add(Hyperplane2D(Vec2f(0, global_bbox_max_(1)), Vec2f(0, 1)));
|
||||
Vs.add(Hyperplane2D(Vec2f(0, global_bbox_min_(1)), Vec2f(0, -1)));
|
||||
}
|
||||
|
||||
template<int U = Dim>
|
||||
typename std::enable_if<U == 3>::type
|
||||
add_global_bbox(Polyhedron<Dim> &Vs) {
|
||||
//**** add bound along X, Y, Z axis
|
||||
//*** Z
|
||||
Vs.add(Hyperplane3D(Vec3f(0, 0, global_bbox_max_(2)), Vec3f(0, 0, 1)));
|
||||
Vs.add(Hyperplane3D(Vec3f(0, 0, global_bbox_min_(2)), Vec3f(0, 0, -1)));
|
||||
|
||||
//*** X
|
||||
Vs.add(Hyperplane3D(Vec3f(global_bbox_max_(0), 0, 0), Vec3f(1, 0, 0)));
|
||||
Vs.add(Hyperplane3D(Vec3f(global_bbox_min_(0), 0, 0), Vec3f(-1, 0, 0)));
|
||||
//*** Y
|
||||
Vs.add(Hyperplane3D(Vec3f(0, global_bbox_max_(1), 0), Vec3f(0, 1, 0)));
|
||||
Vs.add(Hyperplane3D(Vec3f(0, global_bbox_max_(1), 0), Vec3f(0, -1, 0)));
|
||||
}
|
||||
|
||||
vec_Vecf<Dim> path_;
|
||||
vec_Vecf<Dim> obs_;
|
||||
|
||||
vec_E<Ellipsoid<Dim>> ellipsoids_;
|
||||
vec_E<Polyhedron<Dim>> polyhedrons_;
|
||||
std::vector<std::shared_ptr<LineSegment<Dim>>> lines_;
|
||||
|
||||
Vecf<Dim> local_bbox_{Vecf<Dim>::Zero()};
|
||||
Vecf<Dim> global_bbox_min_{Vecf<Dim>::Zero()}; // bounding box params
|
||||
Vecf<Dim> global_bbox_max_{Vecf<Dim>::Zero()};
|
||||
|
||||
};
|
||||
|
||||
typedef EllipsoidDecomp<2> EllipsoidDecomp2D;
|
||||
|
||||
typedef EllipsoidDecomp<3> EllipsoidDecomp3D;
|
||||
#endif
|
||||
@@ -0,0 +1,105 @@
|
||||
/**
|
||||
* @file iterative_decomp.h
|
||||
* @brief IterativeDecomp Class
|
||||
*/
|
||||
#ifndef ITERATIVE_DECOMP_H
|
||||
#define ITERATIVE_DECOMP_H
|
||||
|
||||
#include <decomp_util/ellipsoid_decomp.h>
|
||||
|
||||
/**
|
||||
* @brief IterativeDecomp Class
|
||||
*
|
||||
* Iteratively calls ElliseDecomp to form a safer Safe Flight Corridor that is away from obstacles
|
||||
*/
|
||||
template <int Dim>
|
||||
class IterativeDecomp : public EllipsoidDecomp<Dim>
|
||||
{
|
||||
public:
|
||||
///Simple constructor
|
||||
IterativeDecomp() {}
|
||||
/**
|
||||
* @brief Basic constructor
|
||||
* @param origin The origin of the global bounding box
|
||||
* @param dim The dimension of the global bounding box
|
||||
*/
|
||||
IterativeDecomp(const Vecf<Dim> &origin, const Vecf<Dim> &dim) :
|
||||
EllipsoidDecomp<Dim>(origin, dim) {}
|
||||
/**
|
||||
* @brief Decomposition thread
|
||||
* @param path_raw The path to dilate
|
||||
* @param iter_num Max iteration number
|
||||
* @param offset_x offset added to the long semi-axis, default is 0
|
||||
* @param res Resolution to downsample the path
|
||||
*/
|
||||
void dilate_iter(const vec_Vecf<Dim> &path_raw, int iter_num = 5,
|
||||
decimal_t res = 0, decimal_t offset_x = 0) {
|
||||
vec_Vecf<Dim> path = res > 0 ? downsample(path_raw, res) : path_raw;
|
||||
this->dilate(path, offset_x);
|
||||
vec_Vecf<Dim> new_path = simplify(path);
|
||||
for (int i = 0; i < iter_num; i++) {
|
||||
if (new_path.size() == path.size())
|
||||
break;
|
||||
else {
|
||||
path = new_path;
|
||||
this->dilate(path, offset_x);
|
||||
new_path = simplify(path);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
/// Uniformly sample path into many segments
|
||||
vec_Vecf<Dim> downsample(const vec_Vecf<Dim> &ps, decimal_t d) {
|
||||
// subdivide according to length
|
||||
if (ps.size() < 2)
|
||||
return ps;
|
||||
vec_Vecf<Dim> path;
|
||||
for (unsigned int i = 1; i < ps.size(); i++) {
|
||||
decimal_t dist = (ps[i] - ps[i - 1]).norm();
|
||||
int cnt = std::ceil(dist / d);
|
||||
for (int j = 0; j < cnt; j++)
|
||||
path.push_back(ps[i - 1] + j * (ps[i] - ps[i - 1]) / cnt);
|
||||
}
|
||||
path.push_back(ps.back());
|
||||
return path;
|
||||
}
|
||||
|
||||
/// Get closest distance
|
||||
decimal_t cal_closest_dist(const Vecf<Dim>& pt, const Polyhedron<Dim>& vs){
|
||||
decimal_t dist = std::numeric_limits<decimal_t>::infinity();
|
||||
for(const auto& it: vs.hyperplanes()){
|
||||
decimal_t d = std::abs(it.n_.dot(pt - it.p_));
|
||||
if(d < dist)
|
||||
dist = d;
|
||||
}
|
||||
return dist;
|
||||
}
|
||||
|
||||
/// Remove redundant waypoints
|
||||
vec_Vecf<Dim> simplify(const vec_Vecf<Dim>& path) {
|
||||
if(path.size() <= 2)
|
||||
return path;
|
||||
|
||||
Vecf<Dim> ref_pt = path.front();
|
||||
vec_Vecf<Dim> new_path;
|
||||
new_path.push_back(ref_pt);
|
||||
|
||||
for(size_t i = 2; i < path.size(); i ++){
|
||||
if(this->polyhedrons_[i-1].inside(ref_pt) &&
|
||||
cal_closest_dist(ref_pt, this->polyhedrons_[i-1]) > 0.1) {
|
||||
}
|
||||
else{
|
||||
ref_pt = path[i-1];
|
||||
new_path.push_back(ref_pt);
|
||||
}
|
||||
}
|
||||
new_path.push_back(path.back());
|
||||
return new_path;
|
||||
}
|
||||
};
|
||||
|
||||
typedef IterativeDecomp<2> IterativeDecomp2D;
|
||||
|
||||
typedef IterativeDecomp<3> IterativeDecomp3D;
|
||||
#endif
|
||||
@@ -0,0 +1,222 @@
|
||||
/**
|
||||
* @file line_segment.h
|
||||
* @brief LineSegment Class
|
||||
*/
|
||||
#ifndef LINE_SEGMENT_H
|
||||
#define LINE_SEGMENT_H
|
||||
|
||||
#include <decomp_util/decomp_base.h>
|
||||
#include <decomp_geometry/geometric_utils.h>
|
||||
|
||||
/**
|
||||
* @brief Line Segment Class
|
||||
*
|
||||
* The basic element in EllipsoidDecomp
|
||||
*/
|
||||
template <int Dim>
|
||||
class LineSegment : public DecompBase<Dim> {
|
||||
public:
|
||||
///Simple constructor
|
||||
LineSegment() {};
|
||||
/**
|
||||
* @brief Basic constructor
|
||||
* @param p1 One end of the line seg
|
||||
* @param p2 The other end of the line seg
|
||||
*/
|
||||
LineSegment(const Vecf<Dim> &p1, const Vecf<Dim> &p2) : p1_(p1), p2_(p2) {}
|
||||
/**
|
||||
* @brief Infalte the line segment
|
||||
* @param radius the offset added to the long semi-axis
|
||||
*/
|
||||
void dilate(decimal_t radius) {
|
||||
find_ellipsoid(radius);
|
||||
this->find_polyhedron();
|
||||
add_local_bbox(this->polyhedron_);
|
||||
}
|
||||
|
||||
/// Get the line
|
||||
vec_Vecf<Dim> get_line_segment() const {
|
||||
vec_Vecf<Dim> line;
|
||||
line.push_back(p1_);
|
||||
line.push_back(p2_);
|
||||
return line;
|
||||
}
|
||||
|
||||
protected:
|
||||
///Add the bounding box
|
||||
void add_local_bbox(Polyhedron<Dim> &Vs) {
|
||||
if(this->local_bbox_.norm() == 0)
|
||||
return;
|
||||
//**** virtual walls parallel to path p1->p2
|
||||
Vecf<Dim> dir = (p2_ - p1_).normalized();
|
||||
Vecf<Dim> dir_h = Vecf<Dim>::Zero();
|
||||
dir_h(0) = dir(1), dir_h(1) = -dir(0);
|
||||
if (dir_h.norm() == 0) {
|
||||
if(Dim == 2)
|
||||
dir_h << -1, 0;
|
||||
else
|
||||
dir_h << -1, 0, 0;
|
||||
}
|
||||
dir_h = dir_h.normalized();
|
||||
|
||||
// along x
|
||||
Vecf<Dim> pp1 = p1_ + dir_h * this->local_bbox_(1);
|
||||
Vecf<Dim> pp2 = p1_ - dir_h * this->local_bbox_(1);
|
||||
Vs.add(Hyperplane<Dim>(pp1, dir_h));
|
||||
Vs.add(Hyperplane<Dim>(pp2, -dir_h));
|
||||
|
||||
// along y
|
||||
Vecf<Dim> pp3 = p2_ + dir * this->local_bbox_(0);
|
||||
Vecf<Dim> pp4 = p1_ - dir * this->local_bbox_(0);
|
||||
Vs.add(Hyperplane<Dim>(pp3, dir));
|
||||
Vs.add(Hyperplane<Dim>(pp4, -dir));
|
||||
|
||||
// along z
|
||||
if(Dim > 2) {
|
||||
Vecf<Dim> dir_v;
|
||||
dir_v(0) = dir(1) * dir_h(2) - dir(2) * dir_h(1);
|
||||
dir_v(1) = dir(2) * dir_h(0) - dir(0) * dir_h(2);
|
||||
dir_v(2) = dir(0) * dir_h(1) - dir(1) * dir_h(0);
|
||||
Vecf<Dim> pp5 = p1_ + dir_v * this->local_bbox_(2);
|
||||
Vecf<Dim> pp6 = p1_ - dir_v * this->local_bbox_(2);
|
||||
Vs.add(Hyperplane<Dim>(pp5, dir_v));
|
||||
Vs.add(Hyperplane<Dim>(pp6, -dir_v));
|
||||
}
|
||||
}
|
||||
|
||||
/// Find ellipsoid in 2D
|
||||
template<int U = Dim>
|
||||
typename std::enable_if<U == 2>::type
|
||||
find_ellipsoid(double offset_x) {
|
||||
const decimal_t f = (p1_ - p2_).norm() / 2;
|
||||
Matf<Dim, Dim> C = f * Matf<Dim, Dim>::Identity();
|
||||
Vecf<Dim> axes = Vecf<Dim>::Constant(f);
|
||||
C(0, 0) += offset_x;
|
||||
axes(0) += offset_x;
|
||||
|
||||
if(axes(0) > 0) {
|
||||
double ratio = axes(1) / axes(0);
|
||||
axes *= ratio;
|
||||
C *= ratio;
|
||||
}
|
||||
|
||||
const auto Ri = vec2_to_rotation(p2_ - p1_);
|
||||
C = Ri * C * Ri.transpose();
|
||||
|
||||
Ellipsoid<Dim> E(C, (p1_ + p2_) / 2);
|
||||
|
||||
auto obs = E.points_inside(this->obs_);
|
||||
|
||||
auto obs_inside = obs;
|
||||
//**** decide short axes
|
||||
while (!obs_inside.empty()) {
|
||||
const auto pw = E.closest_point(obs_inside);
|
||||
Vecf<Dim> p = Ri.transpose() * (pw - E.d()); // to ellipsoid frame
|
||||
if(p(0) < axes(0))
|
||||
axes(1) = std::abs(p(1)) / std::sqrt(1 - std::pow(p(0) / axes(0), 2));
|
||||
Matf<Dim, Dim> new_C = Matf<Dim, Dim>::Identity();
|
||||
new_C(0, 0) = axes(0);
|
||||
new_C(1, 1) = axes(1);
|
||||
E.C_ = Ri * new_C * Ri.transpose();
|
||||
|
||||
vec_Vecf<Dim> obs_new;
|
||||
for(const auto &it: obs_inside) {
|
||||
if(1 - E.dist(it) > epsilon_)
|
||||
obs_new.push_back(it);
|
||||
}
|
||||
obs_inside = obs_new;
|
||||
}
|
||||
|
||||
this->ellipsoid_ = E;
|
||||
}
|
||||
|
||||
/// Find ellipsoid in 3D
|
||||
template<int U = Dim>
|
||||
typename std::enable_if<U == 3>::type
|
||||
find_ellipsoid(double offset_x) {
|
||||
const decimal_t f = (p1_ - p2_).norm() / 2;
|
||||
Matf<Dim, Dim> C = f * Matf<Dim, Dim>::Identity();
|
||||
Vecf<Dim> axes = Vecf<Dim>::Constant(f);
|
||||
C(0, 0) += offset_x;
|
||||
axes(0) += offset_x;
|
||||
|
||||
if(axes(0) > 0) {
|
||||
double ratio = axes(1) / axes(0);
|
||||
axes *= ratio;
|
||||
C *= ratio;
|
||||
}
|
||||
|
||||
const auto Ri = vec3_to_rotation(p2_ - p1_);
|
||||
C = Ri * C * Ri.transpose();
|
||||
|
||||
Ellipsoid<Dim> E(C, (p1_ + p2_) / 2);
|
||||
auto Rf = Ri;
|
||||
|
||||
auto obs = E.points_inside(this->obs_);
|
||||
auto obs_inside = obs;
|
||||
//**** decide short axes
|
||||
while (!obs_inside.empty()) {
|
||||
const auto pw = E.closest_point(obs_inside);
|
||||
Vecf<Dim> p = Ri.transpose() * (pw - E.d()); // to ellipsoid frame
|
||||
const decimal_t roll = atan2(p(2), p(1));
|
||||
Rf = Ri * Quatf(cos(roll / 2), sin(roll / 2), 0, 0);
|
||||
p = Rf.transpose() * (pw - E.d());
|
||||
|
||||
if(p(0) < axes(0))
|
||||
axes(1) = std::abs(p(1)) / std::sqrt(1 - std::pow(p(0) / axes(0), 2));
|
||||
Matf<Dim, Dim> new_C = Matf<Dim, Dim>::Identity();
|
||||
new_C(0, 0) = axes(0);
|
||||
new_C(1, 1) = axes(1);
|
||||
new_C(2, 2) = axes(1);
|
||||
E.C_ = Rf * new_C * Rf.transpose();
|
||||
|
||||
vec_Vecf<Dim> obs_new;
|
||||
for(const auto &it: obs_inside) {
|
||||
if(1 - E.dist(it) > epsilon_)
|
||||
obs_new.push_back(it);
|
||||
}
|
||||
obs_inside = obs_new;
|
||||
}
|
||||
|
||||
//**** reset ellipsoid with old axes(2)
|
||||
C = f * Matf<Dim, Dim>::Identity();
|
||||
C(0, 0) = axes(0);
|
||||
C(1, 1) = axes(1);
|
||||
C(2, 2) = axes(2);
|
||||
E.C_ = Rf * C * Rf.transpose();
|
||||
obs_inside = E.points_inside(obs);
|
||||
|
||||
while (!obs_inside.empty()) {
|
||||
const auto pw = E.closest_point(obs_inside);
|
||||
Vec3f p = Rf.transpose() * (pw - E.d());
|
||||
decimal_t dd = 1 - std::pow(p(0) / axes(0), 2) -
|
||||
std::pow(p(1) / axes(1), 2);
|
||||
if(dd > epsilon_)
|
||||
axes(2) = std::abs(p(2)) / std::sqrt(dd);
|
||||
Matf<Dim, Dim> new_C = Matf<Dim, Dim>::Identity();
|
||||
new_C(0, 0) = axes(0);
|
||||
new_C(1, 1) = axes(1);
|
||||
new_C(2, 2) = axes(2);
|
||||
E.C_ = Rf * new_C * Rf.transpose();
|
||||
|
||||
vec_Vecf<Dim> obs_new;
|
||||
for(const auto &it: obs_inside) {
|
||||
if(1 - E.dist(it) > epsilon_)
|
||||
obs_new.push_back(it);
|
||||
}
|
||||
obs_inside = obs_new;
|
||||
}
|
||||
|
||||
this->ellipsoid_ = E;
|
||||
}
|
||||
|
||||
/// One end of line segment, input
|
||||
Vecf<Dim> p1_;
|
||||
/// The other end of line segment, input
|
||||
Vecf<Dim> p2_;
|
||||
};
|
||||
|
||||
typedef LineSegment<2> LineSegment2D;
|
||||
|
||||
typedef LineSegment<3> LineSegment3D;
|
||||
#endif
|
||||
@@ -0,0 +1,80 @@
|
||||
/**
|
||||
* @file seed_decomp.h
|
||||
* @brief SeedDecomp Class
|
||||
*/
|
||||
#ifndef SEED_DECOMP_H
|
||||
#define SEED_DECOMP_H
|
||||
|
||||
#include <decomp_util/decomp_base.h>
|
||||
|
||||
/**
|
||||
* @brief Seed Decomp Class
|
||||
*
|
||||
* Dilate around the given point
|
||||
*/
|
||||
template <int Dim>
|
||||
class SeedDecomp : public DecompBase<Dim> {
|
||||
public:
|
||||
///Simple constructor
|
||||
SeedDecomp() {};
|
||||
/**
|
||||
* @brief Basic constructor
|
||||
* @param p1 One end of the line seg
|
||||
* @param p2 The other end of the line seg
|
||||
*/
|
||||
SeedDecomp(const Vecf<Dim> &p) : p_(p) {}
|
||||
/**
|
||||
* @brief Inflate the seed with a sphere
|
||||
* @param radius Robot radius
|
||||
*/
|
||||
void dilate(decimal_t radius) {
|
||||
this->ellipsoid_ = Ellipsoid<Dim>(radius * Matf<Dim, Dim>::Identity(), p_);
|
||||
this->find_polyhedron();
|
||||
add_local_bbox(this->polyhedron_);
|
||||
}
|
||||
|
||||
/// Get the center
|
||||
Vecf<Dim> get_seed() const {
|
||||
return p_;
|
||||
}
|
||||
|
||||
protected:
|
||||
///Add the bounding box
|
||||
void add_local_bbox(Polyhedron<Dim> &Vs) {
|
||||
if(this->local_bbox_.norm() == 0)
|
||||
return;
|
||||
|
||||
//**** virtual walls x-y-z
|
||||
Vecf<Dim> dir = Vecf<Dim>::UnitX();
|
||||
Vecf<Dim> dir_h = Vecf<Dim>::UnitY();
|
||||
|
||||
Vecf<Dim> pp1 = p_ + dir_h * this->local_bbox_(1);
|
||||
Vecf<Dim> pp2 = p_ - dir_h * this->local_bbox_(1);
|
||||
Vs.add(Hyperplane<Dim>(pp1, dir_h));
|
||||
Vs.add(Hyperplane<Dim>(pp2, -dir_h));
|
||||
|
||||
// along y
|
||||
Vecf<Dim> pp3 = p_ + dir * this->local_bbox_(0);
|
||||
Vecf<Dim> pp4 = p_ - dir * this->local_bbox_(0);
|
||||
Vs.add(Hyperplane<Dim>(pp3, dir));
|
||||
Vs.add(Hyperplane<Dim>(pp4, -dir));
|
||||
|
||||
// along z
|
||||
if(Dim > 2) {
|
||||
Vecf<Dim> dir_v = Vecf<Dim>::UnitZ();
|
||||
Vecf<Dim> pp5 = p_ + dir_v * this->local_bbox_(2);
|
||||
Vecf<Dim> pp6 = p_ - dir_v * this->local_bbox_(2);
|
||||
Vs.add(Hyperplane<Dim>(pp5, dir_v));
|
||||
Vs.add(Hyperplane<Dim>(pp6, -dir_v));
|
||||
}
|
||||
}
|
||||
|
||||
///Seed location
|
||||
Vecf<Dim> p_;
|
||||
};
|
||||
|
||||
typedef SeedDecomp<2> SeedDecomp2D;
|
||||
|
||||
typedef SeedDecomp<3> SeedDecomp3D;
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>decomp_ros_utils</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The decomp_ros_utils package</description>
|
||||
|
||||
<maintainer email="sikang@todo.todo">sikang</maintainer>
|
||||
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>catkin_simple</build_depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>rviz</depend>
|
||||
<depend>decomp_ros_msgs</depend>
|
||||
|
||||
<export>
|
||||
<rviz plugin="${prefix}/plugin_description.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,18 @@
|
||||
<library path="lib/libdecomp_rviz_plugins">
|
||||
<class name="decomp_rviz_plugins/EllipsoidArray"
|
||||
type="decomp_rviz_plugins::EllipsoidArrayDisplay"
|
||||
base_class_type="rviz::Display">
|
||||
<description>
|
||||
visualize decomp_ros_msgs/EllipsoidArray msg.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
<class name="decomp_rviz_plugins/PolyhedronArray"
|
||||
type="decomp_rviz_plugins::PolyhedronArrayDisplay"
|
||||
base_class_type="rviz::Display">
|
||||
<description>
|
||||
visualize decomp_ros_msgs/PolyhedronArray msg.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
</library>
|
||||
@@ -0,0 +1,56 @@
|
||||
#include "bound_visual.h"
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
BoundVisual::BoundVisual(Ogre::SceneManager *scene_manager,
|
||||
Ogre::SceneNode *parent_node) {
|
||||
scene_manager_ = scene_manager;
|
||||
frame_node_ = parent_node->createChildSceneNode();
|
||||
}
|
||||
|
||||
BoundVisual::~BoundVisual() { scene_manager_->destroySceneNode(frame_node_); }
|
||||
|
||||
void BoundVisual::setMessage(const vec_E<vec_Vec3f>& bds) {
|
||||
objs_.clear();
|
||||
|
||||
if (bds.empty())
|
||||
return;
|
||||
|
||||
size_t num_faces = bds.size();
|
||||
objs_.resize(num_faces);
|
||||
for (auto &it : objs_)
|
||||
it.reset(new rviz::BillboardLine(scene_manager_, frame_node_));
|
||||
|
||||
int cnt = 0;
|
||||
for (const auto &vs : bds) {
|
||||
for (unsigned int i = 0; i <= vs.size(); i++) {
|
||||
if (i < vs.size()) {
|
||||
if(!std::isnan(vs[i](0)))
|
||||
objs_[cnt]->addPoint(Ogre::Vector3(vs[i](0), vs[i](1), vs[i](2)));
|
||||
}
|
||||
else {
|
||||
if(!std::isnan(vs[0](0)))
|
||||
objs_[cnt]->addPoint(Ogre::Vector3(vs[0](0), vs[0](1), vs[0](2)));
|
||||
}
|
||||
}
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
|
||||
void BoundVisual::setFramePosition(const Ogre::Vector3 &position) {
|
||||
frame_node_->setPosition(position);
|
||||
}
|
||||
|
||||
void BoundVisual::setFrameOrientation(const Ogre::Quaternion &orientation) {
|
||||
frame_node_->setOrientation(orientation);
|
||||
}
|
||||
|
||||
void BoundVisual::setColor(float r, float g, float b, float a) {
|
||||
for (auto &it : objs_)
|
||||
it->setColor(r, g, b, a);
|
||||
}
|
||||
|
||||
void BoundVisual::setScale(float s) {
|
||||
for (auto &it : objs_)
|
||||
it->setLineWidth(s);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
#ifndef BOUND_VISUAL_H
|
||||
#define BOUND_VISUAL_H
|
||||
|
||||
#include <decomp_basis/data_type.h>
|
||||
|
||||
#include <OGRE/OgreVector3.h>
|
||||
#include <OGRE/OgreSceneNode.h>
|
||||
#include <OGRE/OgreSceneManager.h>
|
||||
|
||||
#include <rviz/ogre_helpers/billboard_line.h>
|
||||
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
class BoundVisual {
|
||||
public:
|
||||
BoundVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node);
|
||||
~BoundVisual();
|
||||
|
||||
void setMessage(const vec_E<vec_Vec3f> &bds);
|
||||
void setFramePosition(const Ogre::Vector3 &position);
|
||||
void setFrameOrientation(const Ogre::Quaternion &orientation);
|
||||
|
||||
void setColor(float r, float g, float b, float a);
|
||||
void setScale(float s);
|
||||
|
||||
private:
|
||||
std::vector<std::unique_ptr<rviz::BillboardLine>> objs_;
|
||||
|
||||
Ogre::SceneNode *frame_node_;
|
||||
|
||||
Ogre::SceneManager *scene_manager_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,62 @@
|
||||
#include <tf/transform_listener.h>
|
||||
#include "ellipsoid_array_display.h"
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
|
||||
EllipsoidArrayDisplay::EllipsoidArrayDisplay() {
|
||||
color_property_ = new rviz::ColorProperty("Color", QColor(204, 51, 204),
|
||||
"Color of ellipsoids.",
|
||||
this, SLOT(updateColorAndAlpha()));
|
||||
alpha_property_ = new rviz::FloatProperty(
|
||||
"Alpha", 0.5, "0 is fully transparent, 1.0 is fully opaque.", this,
|
||||
SLOT(updateColorAndAlpha()));
|
||||
|
||||
}
|
||||
|
||||
void EllipsoidArrayDisplay::onInitialize() {
|
||||
MFDClass::onInitialize();
|
||||
}
|
||||
|
||||
EllipsoidArrayDisplay::~EllipsoidArrayDisplay() {}
|
||||
|
||||
void EllipsoidArrayDisplay::reset() {
|
||||
MFDClass::reset();
|
||||
visual_ = nullptr;
|
||||
}
|
||||
|
||||
void EllipsoidArrayDisplay::updateColorAndAlpha() {
|
||||
float alpha = alpha_property_->getFloat();
|
||||
Ogre::ColourValue color = color_property_->getOgreColor();
|
||||
|
||||
if (visual_)
|
||||
visual_->setColor(color.r, color.g, color.b, alpha);
|
||||
}
|
||||
|
||||
void EllipsoidArrayDisplay::processMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg) {
|
||||
Ogre::Quaternion orientation;
|
||||
Ogre::Vector3 position;
|
||||
if (!context_->getFrameManager()->getTransform(
|
||||
msg->header.frame_id, msg->header.stamp, position, orientation)) {
|
||||
ROS_DEBUG("Error transforming from frame '%s' to frame '%s'",
|
||||
msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
|
||||
return;
|
||||
}
|
||||
|
||||
std::shared_ptr<EllipsoidArrayVisual> visual;
|
||||
visual.reset(new EllipsoidArrayVisual(context_->getSceneManager(), scene_node_));
|
||||
|
||||
visual->setMessage(msg);
|
||||
visual->setFramePosition(position);
|
||||
visual->setFrameOrientation(orientation);
|
||||
|
||||
float alpha = alpha_property_->getFloat();
|
||||
Ogre::ColourValue color = color_property_->getOgreColor();
|
||||
visual->setColor(color.r, color.g, color.b, alpha);
|
||||
|
||||
visual_ = visual;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
PLUGINLIB_EXPORT_CLASS(decomp_rviz_plugins::EllipsoidArrayDisplay, rviz::Display)
|
||||
@@ -0,0 +1,41 @@
|
||||
#include <OGRE/OgreSceneNode.h>
|
||||
#include <OGRE/OgreSceneManager.h>
|
||||
#include <rviz/visualization_manager.h>
|
||||
#include <rviz/properties/color_property.h>
|
||||
#include <rviz/properties/float_property.h>
|
||||
#include <rviz/frame_manager.h>
|
||||
|
||||
#include <rviz/load_resource.h>
|
||||
|
||||
#include <decomp_ros_msgs/EllipsoidArray.h>
|
||||
#include <rviz/message_filter_display.h>
|
||||
#include "ellipsoid_array_visual.h"
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
|
||||
class EllipsoidArrayVisual;
|
||||
|
||||
class EllipsoidArrayDisplay
|
||||
: public rviz::MessageFilterDisplay<decomp_ros_msgs::EllipsoidArray> {
|
||||
Q_OBJECT
|
||||
public:
|
||||
EllipsoidArrayDisplay();
|
||||
~EllipsoidArrayDisplay();
|
||||
|
||||
protected:
|
||||
void onInitialize();
|
||||
|
||||
void reset();
|
||||
|
||||
private Q_SLOTS:
|
||||
void updateColorAndAlpha();
|
||||
|
||||
private:
|
||||
void processMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg);
|
||||
|
||||
std::shared_ptr<EllipsoidArrayVisual> visual_;
|
||||
|
||||
rviz::ColorProperty *color_property_;
|
||||
rviz::FloatProperty *alpha_property_;
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
#include "ellipsoid_array_visual.h"
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
EllipsoidArrayVisual::EllipsoidArrayVisual(Ogre::SceneManager *scene_manager,
|
||||
Ogre::SceneNode *parent_node) {
|
||||
scene_manager_ = scene_manager;
|
||||
frame_node_ = parent_node->createChildSceneNode();
|
||||
}
|
||||
|
||||
EllipsoidArrayVisual::~EllipsoidArrayVisual() {
|
||||
scene_manager_->destroySceneNode(frame_node_);
|
||||
}
|
||||
|
||||
void EllipsoidArrayVisual::setMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg) {
|
||||
objs_.clear();
|
||||
|
||||
if (msg->ellipsoids.empty())
|
||||
return;
|
||||
|
||||
for (const auto& it: msg->ellipsoids) {
|
||||
if(std::isnan(it.d[0]) ||
|
||||
std::isnan(it.d[1]) ||
|
||||
std::isnan(it.d[2]))
|
||||
return;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
if(std::isnan(it.E[3 * i + j]))
|
||||
return;
|
||||
}
|
||||
|
||||
objs_.resize(msg->ellipsoids.size());
|
||||
|
||||
for (auto &it : objs_)
|
||||
it.reset(new rviz::Shape(rviz::Shape::Type::Sphere, scene_manager_,
|
||||
frame_node_));
|
||||
|
||||
int cnt = 0;
|
||||
for (const auto &it : msg->ellipsoids) {
|
||||
Mat3f E;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
E(i, j) = it.E[3 * i + j];
|
||||
Eigen::SelfAdjointEigenSolver<Mat3f> es(E);
|
||||
|
||||
Ogre::Vector3 scale(2 * es.eigenvalues()[0], 2 * es.eigenvalues()[1],
|
||||
2 * es.eigenvalues()[2]);
|
||||
objs_[cnt]->setScale(scale);
|
||||
|
||||
Ogre::Vector3 d(it.d[0], it.d[1], it.d[2]);
|
||||
objs_[cnt]->setPosition(d);
|
||||
|
||||
Quatf q(es.eigenvectors().determinant() * es.eigenvectors());
|
||||
Ogre::Quaternion o(q.w(), q.x(), q.y(), q.z());
|
||||
objs_[cnt]->setOrientation(o);
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
|
||||
void EllipsoidArrayVisual::setFramePosition(const Ogre::Vector3 &position) {
|
||||
frame_node_->setPosition(position);
|
||||
}
|
||||
|
||||
void EllipsoidArrayVisual::setFrameOrientation(
|
||||
const Ogre::Quaternion &orientation) {
|
||||
frame_node_->setOrientation(orientation);
|
||||
}
|
||||
|
||||
void EllipsoidArrayVisual::setColor(float r, float g, float b, float a) {
|
||||
for (auto &it : objs_)
|
||||
it->setColor(r, g, b, a);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
#ifndef ELLIPSOIDS_VISUAL_H
|
||||
#define ELLIPSOIDS_VISUAL_H
|
||||
|
||||
#include <decomp_ros_msgs/EllipsoidArray.h>
|
||||
#include <decomp_geometry/ellipsoid.h>
|
||||
#include <Eigen/Eigenvalues>
|
||||
|
||||
#include <OGRE/OgreVector3.h>
|
||||
#include <OGRE/OgreSceneNode.h>
|
||||
#include <OGRE/OgreSceneManager.h>
|
||||
|
||||
#include <rviz/ogre_helpers/shape.h>
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
class EllipsoidArrayVisual {
|
||||
public:
|
||||
EllipsoidArrayVisual(Ogre::SceneManager *scene_manager,
|
||||
Ogre::SceneNode *parent_node);
|
||||
|
||||
virtual ~EllipsoidArrayVisual();
|
||||
|
||||
void setMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg);
|
||||
|
||||
void setFramePosition(const Ogre::Vector3 &position);
|
||||
void setFrameOrientation(const Ogre::Quaternion &orientation);
|
||||
|
||||
void setColor(float r, float g, float b, float a);
|
||||
|
||||
private:
|
||||
std::vector<std::unique_ptr<rviz::Shape>> objs_;
|
||||
|
||||
Ogre::SceneNode *frame_node_;
|
||||
|
||||
Ogre::SceneManager *scene_manager_;
|
||||
};
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,56 @@
|
||||
#include "mesh_visual.h"
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
MeshVisual::MeshVisual(Ogre::SceneManager *scene_manager,
|
||||
Ogre::SceneNode *parent_node) {
|
||||
scene_manager_ = scene_manager;
|
||||
frame_node_ = parent_node->createChildSceneNode();
|
||||
obj_.reset(new rviz::MeshShape(scene_manager_, frame_node_));
|
||||
}
|
||||
|
||||
MeshVisual::~MeshVisual() { scene_manager_->destroySceneNode(frame_node_); }
|
||||
|
||||
void MeshVisual::setMessage(const vec_E<vec_Vec3f> &bds) {
|
||||
obj_->clear();
|
||||
|
||||
if (bds.empty())
|
||||
return;
|
||||
|
||||
obj_->beginTriangles();
|
||||
int free_cnt = 0;
|
||||
for (const auto &vs: bds) {
|
||||
if (vs.size() > 2) {
|
||||
Vec3f p0 = vs[0];
|
||||
Vec3f p1 = vs[1];
|
||||
Vec3f p2 = vs[2];
|
||||
Vec3f n = (p2-p0).cross(p1-p0);
|
||||
n = n.normalized();
|
||||
if(std::isnan(n(0)))
|
||||
n = Vec3f(0, 0, -1);
|
||||
|
||||
int ref_cnt = free_cnt;
|
||||
Ogre::Vector3 normal(n(0), n(1), n(2));
|
||||
for (unsigned int i = 0; i < vs.size(); i++) {
|
||||
obj_->addVertex(Ogre::Vector3(vs[i](0), vs[i](1), vs[i](2)), normal);
|
||||
if (i > 1 && i < vs.size())
|
||||
obj_->addTriangle(ref_cnt, free_cnt - 1, free_cnt);
|
||||
free_cnt++;
|
||||
}
|
||||
}
|
||||
}
|
||||
obj_->endTriangles();
|
||||
}
|
||||
|
||||
// Position and orientation are passed through to the SceneNode.
|
||||
void MeshVisual::setFramePosition(const Ogre::Vector3 &position) {
|
||||
frame_node_->setPosition(position);
|
||||
}
|
||||
|
||||
void MeshVisual::setFrameOrientation(const Ogre::Quaternion &orientation) {
|
||||
frame_node_->setOrientation(orientation);
|
||||
}
|
||||
|
||||
void MeshVisual::setColor(float r, float g, float b, float a) {
|
||||
obj_->setColor(r, g, b, a);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
#ifndef MESH_VISUAL_H
|
||||
#define MESH_VISUAL_H
|
||||
|
||||
#include <decomp_geometry/polyhedron.h>
|
||||
#include <Eigen/Eigenvalues>
|
||||
|
||||
#include <OGRE/OgreVector3.h>
|
||||
#include <OGRE/OgreSceneNode.h>
|
||||
#include <OGRE/OgreSceneManager.h>
|
||||
|
||||
#include <rviz/ogre_helpers/mesh_shape.h>
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
class MeshVisual {
|
||||
public:
|
||||
MeshVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node);
|
||||
|
||||
virtual ~MeshVisual();
|
||||
|
||||
void setMessage(const vec_E<vec_Vec3f> &bds);
|
||||
void setFramePosition(const Ogre::Vector3 &position);
|
||||
void setFrameOrientation(const Ogre::Quaternion &orientation);
|
||||
|
||||
void setColor(float r, float g, float b, float a);
|
||||
|
||||
private:
|
||||
std::unique_ptr<rviz::MeshShape> obj_;
|
||||
|
||||
Ogre::SceneNode *frame_node_;
|
||||
|
||||
Ogre::SceneManager *scene_manager_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,186 @@
|
||||
#include "polyhedron_array_display.h"
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
|
||||
PolyhedronArrayDisplay::PolyhedronArrayDisplay() {
|
||||
mesh_color_property_ =
|
||||
new rviz::ColorProperty("MeshColor", QColor(0, 170, 255), "Mesh color.",
|
||||
this, SLOT(updateMeshColorAndAlpha()));
|
||||
bound_color_property_ =
|
||||
new rviz::ColorProperty("BoundColor", QColor(255, 0, 0), "Bound color.",
|
||||
this, SLOT(updateBoundColorAndAlpha()));
|
||||
|
||||
alpha_property_ = new rviz::FloatProperty(
|
||||
"Alpha", 0.2,
|
||||
"0 is fully transparent, 1.0 is fully opaque, only affect mesh", this,
|
||||
SLOT(updateMeshColorAndAlpha()));
|
||||
|
||||
scale_property_ = new rviz::FloatProperty("Scale", 0.1, "bound scale.", this,
|
||||
SLOT(updateScale()));
|
||||
|
||||
vs_scale_property_ = new rviz::FloatProperty("VsScale", 1.0, "Vs scale.", this,
|
||||
SLOT(updateVsScale()));
|
||||
|
||||
vs_color_property_ =
|
||||
new rviz::ColorProperty("VsColor", QColor(0, 255, 0), "Vs color.",
|
||||
this, SLOT(updateVsColorAndAlpha()));
|
||||
|
||||
|
||||
|
||||
state_property_ = new rviz::EnumProperty(
|
||||
"State", "Mesh", "A Polygon can be represented as two states: Mesh and "
|
||||
"Bound, this option allows selecting visualizing Polygon"
|
||||
"in corresponding state",
|
||||
this, SLOT(updateState()));
|
||||
state_property_->addOption("Mesh", 0);
|
||||
state_property_->addOption("Bound", 1);
|
||||
state_property_->addOption("Both", 2);
|
||||
state_property_->addOption("Vs", 3);
|
||||
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::onInitialize() { MFDClass::onInitialize(); }
|
||||
|
||||
PolyhedronArrayDisplay::~PolyhedronArrayDisplay() {}
|
||||
|
||||
void PolyhedronArrayDisplay::reset() {
|
||||
MFDClass::reset();
|
||||
visual_mesh_ = nullptr;
|
||||
visual_bound_ = nullptr;
|
||||
visual_vector_ = nullptr;
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::processMessage(const decomp_ros_msgs::PolyhedronArray::ConstPtr &msg) {
|
||||
if (!context_->getFrameManager()->getTransform(
|
||||
msg->header.frame_id, msg->header.stamp, position_, orientation_)) {
|
||||
ROS_DEBUG("Error transforming from frame '%s' to frame '%s'",
|
||||
msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
|
||||
return;
|
||||
}
|
||||
|
||||
vertices_.clear();
|
||||
vs_.clear();
|
||||
|
||||
const auto polys = DecompROS::ros_to_polyhedron_array(*msg);
|
||||
|
||||
for(const auto& polyhedron: polys){
|
||||
vec_E<vec_Vec3f> bds = cal_vertices(polyhedron);
|
||||
vertices_.insert(vertices_.end(), bds.begin(), bds.end());
|
||||
const auto vs = polyhedron.cal_normals();
|
||||
vs_.insert(vs_.end(), vs.begin(), vs.end());
|
||||
}
|
||||
|
||||
int state = state_property_->getOptionInt();
|
||||
visualizeMessage(state);
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::visualizeMesh() {
|
||||
std::shared_ptr<MeshVisual> visual_mesh;
|
||||
visual_mesh.reset(new MeshVisual(context_->getSceneManager(), scene_node_));
|
||||
|
||||
visual_mesh->setMessage(vertices_);
|
||||
visual_mesh->setFramePosition(position_);
|
||||
visual_mesh->setFrameOrientation(orientation_);
|
||||
|
||||
float alpha = alpha_property_->getFloat();
|
||||
Ogre::ColourValue color = mesh_color_property_->getOgreColor();
|
||||
visual_mesh->setColor(color.r, color.g, color.b, alpha);
|
||||
visual_mesh_ = visual_mesh;
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::visualizeBound() {
|
||||
std::shared_ptr<BoundVisual> visual_bound;
|
||||
visual_bound.reset(new BoundVisual(context_->getSceneManager(), scene_node_));
|
||||
|
||||
visual_bound->setMessage(vertices_);
|
||||
visual_bound->setFramePosition(position_);
|
||||
visual_bound->setFrameOrientation(orientation_);
|
||||
|
||||
Ogre::ColourValue color = bound_color_property_->getOgreColor();
|
||||
visual_bound->setColor(color.r, color.g, color.b, 1.0);
|
||||
float scale = scale_property_->getFloat();
|
||||
visual_bound->setScale(scale);
|
||||
|
||||
visual_bound_ = visual_bound;
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::visualizeVs() {
|
||||
std::shared_ptr<VectorVisual> visual_vector;
|
||||
visual_vector.reset(new VectorVisual(context_->getSceneManager(), scene_node_));
|
||||
|
||||
visual_vector->setMessage(vs_);
|
||||
visual_vector->setFramePosition(position_);
|
||||
visual_vector->setFrameOrientation(orientation_);
|
||||
|
||||
Ogre::ColourValue color = vs_color_property_->getOgreColor();
|
||||
visual_vector->setColor(color.r, color.g, color.b, 1.0);
|
||||
|
||||
visual_vector_ = visual_vector;
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::visualizeMessage(int state) {
|
||||
switch (state) {
|
||||
case 0:
|
||||
visual_bound_ = nullptr;
|
||||
visual_vector_ = nullptr;
|
||||
visualizeMesh();
|
||||
break;
|
||||
case 1:
|
||||
visual_mesh_ = nullptr;
|
||||
visual_vector_ = nullptr;
|
||||
visualizeBound();
|
||||
break;
|
||||
case 2:
|
||||
visual_vector_ = nullptr;
|
||||
visualizeMesh();
|
||||
visualizeBound();
|
||||
break;
|
||||
case 3:
|
||||
visualizeVs();
|
||||
break;
|
||||
default:
|
||||
std::cout << "Invalid State: " << state << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::updateMeshColorAndAlpha() {
|
||||
float alpha = alpha_property_->getFloat();
|
||||
Ogre::ColourValue color = mesh_color_property_->getOgreColor();
|
||||
|
||||
if(visual_mesh_)
|
||||
visual_mesh_->setColor(color.r, color.g, color.b, alpha);
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::updateBoundColorAndAlpha() {
|
||||
Ogre::ColourValue color = bound_color_property_->getOgreColor();
|
||||
if(visual_bound_)
|
||||
visual_bound_->setColor(color.r, color.g, color.b, 1.0);
|
||||
}
|
||||
|
||||
|
||||
void PolyhedronArrayDisplay::updateState() {
|
||||
int state = state_property_->getOptionInt();
|
||||
visualizeMessage(state);
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::updateScale() {
|
||||
float s = scale_property_->getFloat();
|
||||
if(visual_bound_)
|
||||
visual_bound_->setScale(s);
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::updateVsScale() {
|
||||
float s = vs_scale_property_->getFloat();
|
||||
if(visual_vector_)
|
||||
visual_vector_->setScale(s);
|
||||
}
|
||||
|
||||
void PolyhedronArrayDisplay::updateVsColorAndAlpha() {
|
||||
Ogre::ColourValue color = vs_color_property_->getOgreColor();
|
||||
if(visual_vector_)
|
||||
visual_vector_->setColor(color.r, color.g, color.b, 1);
|
||||
}
|
||||
}
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
PLUGINLIB_EXPORT_CLASS(decomp_rviz_plugins::PolyhedronArrayDisplay, rviz::Display)
|
||||
@@ -0,0 +1,68 @@
|
||||
#include <decomp_ros_msgs/PolyhedronArray.h>
|
||||
#include <rviz/message_filter_display.h>
|
||||
|
||||
#include <rviz/properties/color_property.h>
|
||||
#include <rviz/properties/float_property.h>
|
||||
#include <rviz/properties/int_property.h>
|
||||
#include <rviz/properties/enum_property.h>
|
||||
#include <rviz/visualization_manager.h>
|
||||
#include <rviz/frame_manager.h>
|
||||
#include <OGRE/OgreSceneNode.h>
|
||||
#include <OGRE/OgreSceneManager.h>
|
||||
|
||||
#include <rviz/load_resource.h>
|
||||
|
||||
#include "mesh_visual.h"
|
||||
#include "bound_visual.h"
|
||||
#include "vector_visual.h"
|
||||
#include <decomp_ros_utils/data_ros_utils.h>
|
||||
#include <decomp_geometry/geometric_utils.h>
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
class PolyhedronArrayDisplay
|
||||
: public rviz::MessageFilterDisplay<decomp_ros_msgs::PolyhedronArray> {
|
||||
Q_OBJECT
|
||||
public:
|
||||
PolyhedronArrayDisplay();
|
||||
virtual ~PolyhedronArrayDisplay();
|
||||
|
||||
protected:
|
||||
virtual void onInitialize();
|
||||
|
||||
virtual void reset();
|
||||
|
||||
private Q_SLOTS:
|
||||
void updateMeshColorAndAlpha();
|
||||
void updateBoundColorAndAlpha();
|
||||
void updateVsColorAndAlpha();
|
||||
void updateState();
|
||||
void updateScale();
|
||||
void updateVsScale();
|
||||
|
||||
private:
|
||||
void processMessage(const decomp_ros_msgs::PolyhedronArray::ConstPtr &msg);
|
||||
void visualizeMessage(int state);
|
||||
void visualizeMesh();
|
||||
void visualizeBound();
|
||||
void visualizeVs();
|
||||
|
||||
std::shared_ptr<MeshVisual> visual_mesh_;
|
||||
std::shared_ptr<BoundVisual> visual_bound_;
|
||||
std::shared_ptr<VectorVisual> visual_vector_;
|
||||
|
||||
rviz::ColorProperty *mesh_color_property_;
|
||||
rviz::ColorProperty *bound_color_property_;
|
||||
rviz::ColorProperty *vs_color_property_;
|
||||
rviz::FloatProperty *alpha_property_;
|
||||
rviz::FloatProperty *scale_property_;
|
||||
rviz::FloatProperty *vs_scale_property_;
|
||||
rviz::EnumProperty *state_property_;
|
||||
|
||||
Ogre::Vector3 position_;
|
||||
Ogre::Quaternion orientation_;
|
||||
|
||||
vec_E<vec_Vec3f> vertices_;
|
||||
vec_E<std::pair<Vec3f, Vec3f>> vs_;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
#include "vector_visual.h"
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
VectorVisual::VectorVisual(Ogre::SceneManager *scene_manager,
|
||||
Ogre::SceneNode *parent_node) {
|
||||
scene_manager_ = scene_manager;
|
||||
frame_node_ = parent_node->createChildSceneNode();
|
||||
}
|
||||
|
||||
VectorVisual::~VectorVisual() { scene_manager_->destroySceneNode(frame_node_); }
|
||||
|
||||
void VectorVisual::setMessage(const vec_E<std::pair<Vec3f, Vec3f>> &vs) {
|
||||
objs_.clear();
|
||||
vs_ = vs;
|
||||
if (vs.empty())
|
||||
return;
|
||||
objs_.resize(vs.size());
|
||||
for (auto &it : objs_)
|
||||
it.reset(new rviz::Arrow(scene_manager_, frame_node_));
|
||||
|
||||
int cnt = 0;
|
||||
for (const auto &v : vs) {
|
||||
Vec3f n = v.second.normalized();
|
||||
objs_[cnt]->setDirection(Ogre::Vector3(n(0), n(1), n(2)));
|
||||
objs_[cnt]->setPosition(Ogre::Vector3(v.first(0), v.first(1), v.first(2)));
|
||||
objs_[cnt]->setScale(s_ * Ogre::Vector3(1.0, 1.0, 1.0));
|
||||
|
||||
float d = s_ * v.second.norm();
|
||||
float shaft_length = 0.7 * d;
|
||||
float head_length = 0.3 * d;
|
||||
objs_[cnt]->set(shaft_length, d / 8, head_length, d / 5);
|
||||
cnt ++;
|
||||
}
|
||||
}
|
||||
|
||||
void VectorVisual::setFramePosition(const Ogre::Vector3 &position) {
|
||||
frame_node_->setPosition(position);
|
||||
}
|
||||
|
||||
void VectorVisual::setFrameOrientation(const Ogre::Quaternion &orientation) {
|
||||
frame_node_->setOrientation(orientation);
|
||||
}
|
||||
|
||||
void VectorVisual::setColor(float r, float g, float b, float a) {
|
||||
for (auto &it : objs_)
|
||||
it->setColor(r, g, b, a);
|
||||
}
|
||||
|
||||
void VectorVisual::setScale(float s) {
|
||||
s_ = s;
|
||||
for (unsigned int i = 0; i < objs_.size(); i++){
|
||||
float d = s_ * vs_[i].second.norm();
|
||||
float shaft_length = 0.7 * d;
|
||||
float head_length = 0.3 * d;
|
||||
objs_[i]->set(shaft_length, d / 8, head_length, d / 5);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
#ifndef VECTOR_VISUAL_H
|
||||
#define VECTOR_VISUAL_H
|
||||
|
||||
#include <decomp_basis/data_type.h>
|
||||
#include <OGRE/OgreVector3.h>
|
||||
#include <OGRE/OgreSceneNode.h>
|
||||
#include <OGRE/OgreSceneManager.h>
|
||||
#include <rviz/ogre_helpers/arrow.h>
|
||||
|
||||
namespace decomp_rviz_plugins {
|
||||
class VectorVisual {
|
||||
public:
|
||||
VectorVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node);
|
||||
~VectorVisual();
|
||||
|
||||
void setMessage(const vec_E<std::pair<Vec3f, Vec3f>> &vs);
|
||||
void setFramePosition(const Ogre::Vector3 &position);
|
||||
void setFrameOrientation(const Ogre::Quaternion &orientation);
|
||||
|
||||
void setColor(float r, float g, float b, float a);
|
||||
void setScale(float s);
|
||||
|
||||
private:
|
||||
std::vector<std::unique_ptr<rviz::Arrow>> objs_;
|
||||
|
||||
Ogre::SceneNode *frame_node_;
|
||||
|
||||
Ogre::SceneManager *scene_manager_;
|
||||
|
||||
float s_ = 1.0;
|
||||
vec_E<std::pair<Vec3f, Vec3f>> vs_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
9
trajectoryOpt/src/utils/catkin_simple/CMakeLists.txt
Normal file
9
trajectoryOpt/src/utils/catkin_simple/CMakeLists.txt
Normal file
@@ -0,0 +1,9 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(catkin_simple)
|
||||
|
||||
find_package(catkin REQUIRED)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS catkin
|
||||
CFG_EXTRAS catkin_simple-extras.cmake
|
||||
)
|
||||
@@ -0,0 +1,226 @@
|
||||
# Generated from: catkin_simple/cmake/catkin_simple-extras.cmake.em
|
||||
|
||||
if(_CATKIN_SIMPLE_EXTRAS_INCLUDED_)
|
||||
return()
|
||||
endif()
|
||||
set(_CATKIN_SIMPLE_EXTRAS_INCLUDED_ TRUE)
|
||||
|
||||
include(CMakeParseArguments)
|
||||
|
||||
@[if DEVELSPACE]@
|
||||
# cmake dir in develspace
|
||||
set(catkin_simple_CMAKE_DIR "@(CMAKE_CURRENT_SOURCE_DIR)/cmake")
|
||||
@[else]@
|
||||
# cmake dir in installspace
|
||||
set(catkin_simple_CMAKE_DIR "@(PKG_CMAKE_DIR)")
|
||||
@[end if]@
|
||||
|
||||
macro(catkin_simple)
|
||||
# Arguments
|
||||
# ALL_DEPS_REQUIRED -- Add the "REQUIRED" flag when calling
|
||||
# FIND_PACKAGE() for each dependency
|
||||
cmake_parse_arguments(cs_args "ALL_DEPS_REQUIRED" "" "" ${ARGN})
|
||||
|
||||
if(TARGET ${PROJECT_NAME}_package)
|
||||
message(WARNING "Could not create target '${${PROJECT_NAME}_package}' for project ${PROJECT_NAME}, as it already exists.")
|
||||
endif()
|
||||
add_custom_target(${PROJECT_NAME}_package)
|
||||
set(${PROJECT_NAME}_TARGETS)
|
||||
set(${PROJECT_NAME}_LIBRARIES)
|
||||
|
||||
find_package(catkin REQUIRED)
|
||||
# call catkin_package_xml() if it has not been called before
|
||||
if(NOT _CATKIN_CURRENT_PACKAGE)
|
||||
catkin_package_xml()
|
||||
endif()
|
||||
|
||||
set(${PROJECT_NAME}_CATKIN_BUILD_DEPENDS)
|
||||
set(${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS)
|
||||
foreach(dep ${${PROJECT_NAME}_BUILD_DEPENDS})
|
||||
# If this flag is defined, add the "REQUIRED" flag
|
||||
# to all FIND_PACKAGE calls
|
||||
if(cs_args_ALL_DEPS_REQUIRED)
|
||||
find_package(${dep} REQUIRED)
|
||||
else()
|
||||
find_package(${dep} QUIET)
|
||||
endif()
|
||||
|
||||
if(${dep}_FOUND_CATKIN_PROJECT)
|
||||
list(APPEND ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS ${dep})
|
||||
list(APPEND ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS ${${dep}_EXPORTED_TARGETS})
|
||||
endif()
|
||||
endforeach()
|
||||
|
||||
# Let find_package(catkin ...) do the heavy lifting
|
||||
find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS})
|
||||
|
||||
# add include directory if available
|
||||
set(${PROJECT_NAME}_LOCAL_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR})
|
||||
set(${PROJECT_NAME}_LOCAL_INCLUDE_DIR)
|
||||
endif()
|
||||
include_directories(${${PROJECT_NAME}_LOCAL_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
|
||||
|
||||
# perform action/msg/srv generation if necessary
|
||||
if(message_generation_FOUND_CATKIN_PROJECT)
|
||||
set(${PROJECT_NAME}_DO_MESSAGE_GENERATION FALSE)
|
||||
# add action files if available
|
||||
set(${PROJECT_NAME}_LOCAL_ACTION_DIR ${CMAKE_CURRENT_SOURCE_DIR}/action)
|
||||
if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_ACTION_DIR})
|
||||
set(${PROJECT_NAME}_LOCAL_ACTION_DIR)
|
||||
endif()
|
||||
if(${PROJECT_NAME}_LOCAL_ACTION_DIR)
|
||||
add_action_files(DIRECTORY action)
|
||||
set(${PROJECT_NAME}_DO_MESSAGE_GENERATION TRUE)
|
||||
endif()
|
||||
|
||||
# add message files if available
|
||||
set(${PROJECT_NAME}_LOCAL_MSG_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg)
|
||||
if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_MSG_DIR})
|
||||
set(${PROJECT_NAME}_LOCAL_MSG_DIR)
|
||||
endif()
|
||||
if(${PROJECT_NAME}_LOCAL_MSG_DIR)
|
||||
add_message_files(DIRECTORY msg)
|
||||
set(${PROJECT_NAME}_DO_MESSAGE_GENERATION TRUE)
|
||||
endif()
|
||||
|
||||
# add service files if available
|
||||
set(${PROJECT_NAME}_LOCAL_SRV_DIR ${CMAKE_CURRENT_SOURCE_DIR}/srv)
|
||||
if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_SRV_DIR})
|
||||
set(${PROJECT_NAME}_LOCAL_SRV_DIR)
|
||||
endif()
|
||||
if(${PROJECT_NAME}_LOCAL_SRV_DIR)
|
||||
add_service_files(DIRECTORY srv)
|
||||
set(${PROJECT_NAME}_DO_MESSAGE_GENERATION TRUE)
|
||||
endif()
|
||||
|
||||
# generate messages if necessary
|
||||
if(${PROJECT_NAME}_DO_MESSAGE_GENERATION)
|
||||
# identify all build dependencies which contain messages
|
||||
set(${PROJECT_NAME}_MSG_PACKAGES)
|
||||
foreach(dep ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS})
|
||||
set(${PROJECT_NAME}_MSG_PACKAGE_FILE ${${dep}_DIR}/${dep}-msg-paths.cmake)
|
||||
if(EXISTS ${${PROJECT_NAME}_MSG_PACKAGE_FILE})
|
||||
list(APPEND ${PROJECT_NAME}_MSG_PACKAGES ${dep})
|
||||
endif()
|
||||
endforeach()
|
||||
generate_messages(DEPENDENCIES ${${PROJECT_NAME}_MSG_PACKAGES})
|
||||
# add additional exported targets coming from generate_messages()
|
||||
list(INSERT ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS 0 ${${PROJECT_NAME}_EXPORTED_TARGETS})
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# generate dynamic reconfigure files
|
||||
if(dynamic_reconfigure_FOUND_CATKIN_PROJECT)
|
||||
set(${PROJECT_NAME}_LOCAL_CFG_DIR ${CMAKE_CURRENT_SOURCE_DIR}/cfg)
|
||||
if(IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_CFG_DIR})
|
||||
# create a list containing all the cfg files
|
||||
file(GLOB ${PROJECT_NAME}_LOCAL_CFG_FILES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${${PROJECT_NAME}_LOCAL_CFG_DIR}/*.cfg")
|
||||
if(${PROJECT_NAME}_LOCAL_CFG_FILES)
|
||||
generate_dynamic_reconfigure_options(${${PROJECT_NAME}_LOCAL_CFG_FILES})
|
||||
# add build dep on gencfg
|
||||
list(APPEND ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS ${PROJECT_NAME}_gencfg)
|
||||
endif()
|
||||
endif()
|
||||
endif()
|
||||
endmacro()
|
||||
|
||||
macro(cs_add_targets_to_package)
|
||||
add_dependencies(${PROJECT_NAME}_package ${ARGN})
|
||||
list(APPEND ${PROJECT_NAME}_TARGETS ${ARGN})
|
||||
endmacro()
|
||||
|
||||
macro(cs_add_executable _target)
|
||||
if(${_target} STREQUAL ${PROJECT_NAME}_package)
|
||||
message(WARNING "Could not create executable with name '${_target}' as '${PROJECT_NAME}_package' is reserved for the top level target name for this project.")
|
||||
endif()
|
||||
cmake_parse_arguments(cs_add_executable_args "NO_AUTO_LINK;NO_AUTO_DEP" "" "" ${ARGN})
|
||||
add_executable(${_target} ${cs_add_executable_args_UNPARSED_ARGUMENTS})
|
||||
if(NOT cs_add_executable_args_NO_AUTO_LINK)
|
||||
target_link_libraries(${_target} ${catkin_LIBRARIES})
|
||||
endif()
|
||||
if(NOT cs_add_executable_args_NO_AUTO_DEP)
|
||||
if(NOT "${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS}" STREQUAL "")
|
||||
add_dependencies(${_target} ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS})
|
||||
endif()
|
||||
endif()
|
||||
cs_add_targets_to_package(${_target})
|
||||
endmacro()
|
||||
|
||||
macro(cs_add_library _target)
|
||||
if(${_target} STREQUAL ${PROJECT_NAME}_package)
|
||||
message(WARNING "Could not create library with name '${_target}' as '${PROJECT_NAME}_package' is reserved for the top level target name for this project.")
|
||||
endif()
|
||||
cmake_parse_arguments(cs_add_library "NO_AUTO_LINK;NO_AUTO_DEP;NO_AUTO_EXPORT" "" "" ${ARGN})
|
||||
add_library(${_target} ${cs_add_library_UNPARSED_ARGUMENTS})
|
||||
if(NOT cs_add_library_NO_AUTO_LINK)
|
||||
target_link_libraries(${_target} ${catkin_LIBRARIES})
|
||||
endif()
|
||||
if(NOT cs_add_library_NO_AUTO_DEP)
|
||||
if(NOT "${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS}" STREQUAL "")
|
||||
add_dependencies(${_target} ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS})
|
||||
endif()
|
||||
endif()
|
||||
if(NOT cs_add_library_NO_AUTO_EXPORT)
|
||||
list(APPEND ${PROJECT_NAME}_LIBRARIES ${_target})
|
||||
endif()
|
||||
cs_add_targets_to_package(${_target})
|
||||
endmacro()
|
||||
|
||||
macro(cs_install)
|
||||
# Install targets (exec's and lib's)
|
||||
foreach(_target ${${PROJECT_NAME}_TARGETS})
|
||||
get_target_property(${_target}_type ${_target} TYPE)
|
||||
message(STATUS "Marking ${${_target}_type} \"${_target}\" of package \"${PROJECT_NAME}\" for installation")
|
||||
endforeach()
|
||||
install(TARGETS ${${PROJECT_NAME}_TARGETS} ${ARGN}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
if(EXISTS ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR})
|
||||
# Install include directory
|
||||
message(STATUS "Marking HEADER FILES in \"include\" folder of package \"${PROJECT_NAME}\" for installation")
|
||||
install(DIRECTORY ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR}/
|
||||
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
|
||||
PATTERN ".svn" EXCLUDE
|
||||
)
|
||||
endif()
|
||||
# Install shared content located in commonly used folders
|
||||
set(_shared_content_folders launch rviz urdf meshes maps worlds Media param)
|
||||
foreach(_folder ${_shared_content_folders})
|
||||
if(IS_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${_folder})
|
||||
message(STATUS "Marking SHARED CONTENT FOLDER \"${_folder}\" of package \"${PROJECT_NAME}\" for installation")
|
||||
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${_folder}/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${_folder}
|
||||
)
|
||||
endif()
|
||||
endforeach()
|
||||
endmacro()
|
||||
|
||||
macro(cs_install_scripts)
|
||||
install(PROGRAMS ${ARGN} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
|
||||
endmacro()
|
||||
|
||||
macro(cs_export)
|
||||
cmake_parse_arguments(CS_PROJECT
|
||||
"" "" "INCLUDE_DIRS;LIBRARIES;CATKIN_DEPENDS;DEPENDS;CFG_EXTRAS"
|
||||
${ARGN})
|
||||
|
||||
set(${PROJECT_NAME}_CATKIN_RUN_DEPENDS)
|
||||
foreach(dep ${${PROJECT_NAME}_RUN_DEPENDS})
|
||||
find_package(${dep} QUIET)
|
||||
if(${dep}_FOUND_CATKIN_PROJECT)
|
||||
list(APPEND ${PROJECT_NAME}_CATKIN_RUN_DEPENDS ${dep})
|
||||
endif()
|
||||
endforeach()
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR} ${CS_PROJECT_INCLUDE_DIRS}
|
||||
LIBRARIES ${${PROJECT_NAME}_LIBRARIES} ${CS_PROJECT_LIBRARIES}
|
||||
CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_RUN_DEPENDS} ${CS_PROJECT_CATKIN_DEPENDS}
|
||||
DEPENDS ${CS_PROJECT_DEPENDS}
|
||||
CFG_EXTRAS ${CS_PROJECT_CFG_EXTRAS}
|
||||
)
|
||||
endmacro()
|
||||
17
trajectoryOpt/src/utils/catkin_simple/package.xml
Normal file
17
trajectoryOpt/src/utils/catkin_simple/package.xml
Normal file
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>catkin_simple</name>
|
||||
<version>0.1.1</version>
|
||||
<description>catkin, simpler</description>
|
||||
|
||||
<maintainer email="william@osrfoundation.org">William Woodall</maintainer>
|
||||
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
|
||||
<license>BSD</license>
|
||||
|
||||
<author email="william@osrfoundation.org">William Woodall</author>
|
||||
<author email="dthomas@osrfoundation.org">Dirk Thomas</author>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<run_depend>catkin</run_depend>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1 @@
|
||||
void hello();
|
||||
@@ -0,0 +1,2 @@
|
||||
Header header
|
||||
string data
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>bar</name>
|
||||
<version>0.1.0</version>
|
||||
<description>The bar package</description>
|
||||
<maintainer email="william@osrfoundation.org">William Woodall</maintainer>
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<buildtool_depend>catkin_simple</buildtool_depend>
|
||||
<run_depend>boost</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<build_depend>boost-dev</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,16 @@
|
||||
#include <bar/hello.h>
|
||||
#include <bar/HeaderString.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
|
||||
inline void print_hello() {
|
||||
std::cout << "Hello ";
|
||||
}
|
||||
|
||||
void hello() {
|
||||
boost::thread t(print_hello);
|
||||
t.join();
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
#include <iostream>
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
void print_world() {
|
||||
std::cout << "world!" << std::endl;
|
||||
}
|
||||
|
||||
void world() {
|
||||
boost::thread t(print_world);
|
||||
t.join();
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>baz</name>
|
||||
<version>0.1.0</version>
|
||||
<description>The baz package</description>
|
||||
<maintainer email="william@osrfoundation.org">William Woodall</maintainer>
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<run_depend>boost</run_depend>
|
||||
<build_depend>boost-dev</build_depend>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>foo</name>
|
||||
<version>0.1.0</version>
|
||||
<description>The foo package</description>
|
||||
<maintainer email="william@osrfoundation.org">William Woodall</maintainer>
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<buildtool_depend>catkin_simple</buildtool_depend>
|
||||
<run_depend>bar</run_depend>
|
||||
<build_depend>bar</build_depend>
|
||||
<run_depend>baz</run_depend>
|
||||
<build_depend>baz</build_depend>
|
||||
<run_depend>boost</run_depend>
|
||||
<build_depend>boost-dev</build_depend>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,9 @@
|
||||
#include <bar/hello.h>
|
||||
#include <baz/world.h>
|
||||
|
||||
int main(void) {
|
||||
hello();
|
||||
world();
|
||||
|
||||
return 0;
|
||||
}
|
||||
40
trajectoryOpt/src/utils/tools/CMakeLists.txt
Normal file
40
trajectoryOpt/src/utils/tools/CMakeLists.txt
Normal file
@@ -0,0 +1,40 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(tools)
|
||||
|
||||
set(CMAKE_VERBOSE_MAKEFILE "true")
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
set(CMAKE_CXX_FLAGS "-std=c++14 -g")
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -Wall")
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
|
||||
|
||||
|
||||
find_package(PCL REQUIRED)
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
visualization_msgs
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
cv_bridge
|
||||
decomp_ros_utils
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${DECOMP_UTIL_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
|
||||
|
||||
#if this catkin packge's header is used by other packages, use catkin_package to
|
||||
#declare the include directories of this package.
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
CATKIN_DEPENDS decomp_ros_utils
|
||||
)
|
||||
@@ -0,0 +1,330 @@
|
||||
#include <math.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <random>
|
||||
#include <decomp_basis/data_type.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include "gridmap.hpp"
|
||||
|
||||
namespace plan_utils
|
||||
{
|
||||
|
||||
void corridorBuilder2d(Eigen::Vector2d origin, float radius, float max_x, float max_y,
|
||||
vec_Vec2f &data, std::vector<Eigen::Vector2d> &add_data,
|
||||
Eigen::MatrixXd &hPoly) {
|
||||
|
||||
//max_x max_y may be the local map?
|
||||
//why add_data?
|
||||
//radius = inf?
|
||||
//origin is the root point of the poly
|
||||
float interior_x = 0.0;
|
||||
float interior_y = 0.0;
|
||||
float safe_radius = radius;
|
||||
std::vector<cv::Point2f> flipData;
|
||||
|
||||
cv::Point2f point;
|
||||
vec_Vec2f new_data;
|
||||
|
||||
|
||||
for (size_t i=0; i<data.size(); i++) {
|
||||
float dx = data[i](0) - origin(0);
|
||||
float dy = data[i](1) - origin(1);
|
||||
float norm2 = std::sqrt(dx*dx + dy*dy);
|
||||
if ( abs(dx) > max_x || abs(dy) > max_y) {
|
||||
continue;
|
||||
}
|
||||
if (norm2 < safe_radius) safe_radius = norm2; //safe radius is the nearest distance between the root and obs
|
||||
if (norm2 == 0) continue;
|
||||
|
||||
point.x = dx + 2*(radius-norm2)*dx/norm2;
|
||||
point.y = dy + 2*(radius-norm2)*dy/norm2; //radius is an enough large value
|
||||
new_data.push_back(data[i]);
|
||||
flipData.push_back(point);
|
||||
}
|
||||
|
||||
for (size_t i=0; i<add_data.size(); i++) {
|
||||
float dx = add_data[i](0) - origin(0);
|
||||
float dy = add_data[i](1) - origin(1);
|
||||
float norm2 = std::sqrt(dx*dx + dy*dy);
|
||||
if (norm2 < safe_radius) safe_radius = norm2;
|
||||
if (norm2 == 0) continue;
|
||||
point.x = dx + 2*(radius-norm2)*dx/norm2;
|
||||
point.y = dy + 2*(radius-norm2)*dy/norm2;
|
||||
new_data.push_back(add_data[i]);
|
||||
flipData.push_back(point);
|
||||
}
|
||||
|
||||
std::vector<int> vertexIndice;
|
||||
cv::convexHull(flipData,vertexIndice,false,false);
|
||||
//obtain the poly containing flipData
|
||||
|
||||
bool isOriginAVertex = false;
|
||||
int OriginIndex = -1;
|
||||
std::vector<cv::Point2f> vertexData;
|
||||
for (size_t i=0; i<vertexIndice.size(); i++) {
|
||||
unsigned int v = vertexIndice[i];
|
||||
if (v == new_data.size()) {
|
||||
isOriginAVertex = true;
|
||||
OriginIndex = i;
|
||||
vertexData.push_back(cv::Point2f(origin(0), origin(1)));
|
||||
}else {
|
||||
vertexData.push_back(cv::Point2f(new_data[v](0), new_data[v](1)));
|
||||
}
|
||||
}
|
||||
|
||||
if (isOriginAVertex) {
|
||||
int last_index = (OriginIndex - 1)%vertexIndice.size();
|
||||
int next_index = (OriginIndex + 1)%vertexIndice.size();
|
||||
float dx = (new_data[vertexIndice[last_index]](0) + origin(0) + new_data[vertexIndice[next_index]](0))/3 - origin(0);
|
||||
float dy = (new_data[vertexIndice[last_index]](1) + origin(1) + new_data[vertexIndice[next_index]](1))/3 - origin(1);
|
||||
float d = std::sqrt(dx*dx + dy*dy);
|
||||
interior_x = 0.99*safe_radius*dx/d + origin(0);
|
||||
interior_y = 0.99*safe_radius*dy/d + origin(1);
|
||||
}else {
|
||||
interior_x = origin(0);
|
||||
interior_y = origin(1);
|
||||
}
|
||||
|
||||
std::vector<int> vIndex2;
|
||||
cv::convexHull(vertexData,vIndex2,false,false); // counterclockwise right-hand
|
||||
|
||||
|
||||
|
||||
std::vector<Eigen::Vector3f> constraints; // (a,b,c) a x + b y <= c
|
||||
for (size_t j=0; j<vIndex2.size(); j++) {
|
||||
int jplus1 = (j+1)%vIndex2.size();
|
||||
cv::Point2f rayV = vertexData[vIndex2[jplus1]] - vertexData[vIndex2[j]];
|
||||
Eigen::Vector2f normalJ(rayV.y, -rayV.x); // point to outside
|
||||
normalJ.normalize();
|
||||
int indexJ = vIndex2[j];
|
||||
while (indexJ != vIndex2[jplus1]) {
|
||||
float c = (vertexData[indexJ].x-interior_x) * normalJ(0) + (vertexData[indexJ].y-interior_y) * normalJ(1);
|
||||
constraints.push_back(Eigen::Vector3f(normalJ(0), normalJ(1), c));
|
||||
indexJ = (indexJ+1)%vertexData.size();
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<cv::Point2f> dualPoints(constraints.size(), cv::Point2f(0,0));
|
||||
for (size_t i=0; i<constraints.size(); i++) {
|
||||
dualPoints[i].x = constraints[i](0)/constraints[i](2);
|
||||
dualPoints[i].y = constraints[i](1)/constraints[i](2);
|
||||
}
|
||||
|
||||
std::vector<cv::Point2f> dualVertex, finalVertex;
|
||||
cv::convexHull(dualPoints,dualVertex,true,false);
|
||||
|
||||
for (size_t i=0; i<dualVertex.size(); i++) {
|
||||
int iplus1 = (i+1)%dualVertex.size();
|
||||
cv::Point2f rayi = dualVertex[iplus1] - dualVertex[i];
|
||||
float c = rayi.y*dualVertex[i].x - rayi.x*dualVertex[i].y;
|
||||
finalVertex.push_back(cv::Point2f(interior_x+rayi.y/c, interior_y-rayi.x/c));
|
||||
}
|
||||
|
||||
unsigned int size = finalVertex.size();
|
||||
hPoly.resize(4, size);
|
||||
for (unsigned int i = 0; i < size; i++){
|
||||
int iplus1 = (i+1)%size;
|
||||
cv::Point2f rayi = finalVertex[iplus1] - finalVertex[i];
|
||||
hPoly.col(i).tail<2>() = Eigen::Vector2d(finalVertex[i].x, finalVertex[i].y); // the points on the plane
|
||||
hPoly.col(i).head<2>() = Eigen::Vector2d(-rayi.y, rayi.x); // outside
|
||||
}
|
||||
|
||||
}
|
||||
Eigen::MatrixXd getDilateRec(Eigen::Vector3d state, map_util::OccMapUtil gridmap, int maxIndexL = 5, int maxIndexW = 8){
|
||||
|
||||
double resolution = gridmap.getRes();
|
||||
double step = resolution * 1.0;
|
||||
double limitBoundHalfL = maxIndexL * resolution;
|
||||
double limitBoundHalfW = maxIndexW * resolution;
|
||||
//generate a rectangle for this state px py yaw
|
||||
//generate a hPoly
|
||||
Eigen::Matrix<int,4,1> NotFinishTable = Eigen::Matrix<int,4,1>(1,1,1,1);
|
||||
Eigen::Vector2d sourcePt = state.head(2);
|
||||
double yaw = state[2];
|
||||
Eigen::Vector4d expandLength;
|
||||
expandLength << 0.0, 0.0, 0.0, 0.0;
|
||||
Eigen::Matrix2d egoR;
|
||||
egoR << cos(yaw), -sin(yaw),
|
||||
sin(yaw), cos(yaw);
|
||||
//dcr width length
|
||||
while(NotFinishTable.norm()>0){
|
||||
//+dx -dy -dx +dy
|
||||
for(int i = 0; i<4; i++){
|
||||
if(!NotFinishTable[i]) continue;
|
||||
//get the new source and vp
|
||||
Eigen::Vector4d tmp_expandLength = expandLength;
|
||||
Eigen::Vector2d cor1, cor2, cor3, cor4, cor5;
|
||||
bool isocc = false;
|
||||
switch (i)
|
||||
{
|
||||
//+dx
|
||||
case 0:
|
||||
tmp_expandLength[0] += step;
|
||||
cor1 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],tmp_expandLength[3]);
|
||||
cor2 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],-tmp_expandLength[1]);
|
||||
cor3 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],-tmp_expandLength[1]);
|
||||
cor4 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],tmp_expandLength[3]);
|
||||
cor5 = cor1;
|
||||
//1 new1 new1 new2 new2 2
|
||||
isocc = gridmap.isBlocked(cor1, cor2);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor2, cor3);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor3, cor4);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor4, cor5);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
if(tmp_expandLength[0] > limitBoundHalfL){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
expandLength = tmp_expandLength;
|
||||
break;
|
||||
//-dy
|
||||
case 1:
|
||||
tmp_expandLength[1] += step;
|
||||
cor1 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],tmp_expandLength[3]);
|
||||
cor2 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],-tmp_expandLength[1]);
|
||||
cor3 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],-tmp_expandLength[1]);
|
||||
cor4 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],tmp_expandLength[3]);
|
||||
cor5 = cor1;
|
||||
//1 new1 new1 new2 new2 2
|
||||
isocc = gridmap.isBlocked(cor1, cor2);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor2, cor3);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor3, cor4);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor4, cor5);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
if(tmp_expandLength[1] > limitBoundHalfW){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
expandLength = tmp_expandLength;
|
||||
break;
|
||||
//-dx
|
||||
case 2:
|
||||
tmp_expandLength[2] += step;
|
||||
cor1 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],tmp_expandLength[3]);
|
||||
cor2 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],-tmp_expandLength[1]);
|
||||
cor3 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],-tmp_expandLength[1]);
|
||||
cor4 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],tmp_expandLength[3]);
|
||||
cor5 = cor1;
|
||||
//1 new1 new1 new2 new2 2
|
||||
isocc = gridmap.isBlocked(cor1, cor2);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor2, cor3);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor3, cor4);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor4, cor5);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
if(tmp_expandLength[2] > limitBoundHalfL){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
expandLength = tmp_expandLength;
|
||||
break;
|
||||
//dy
|
||||
case 3:
|
||||
tmp_expandLength[3] += step;
|
||||
cor1 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],tmp_expandLength[3]);
|
||||
cor2 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],-tmp_expandLength[1]);
|
||||
cor3 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],-tmp_expandLength[1]);
|
||||
cor4 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],tmp_expandLength[3]);
|
||||
cor5 = cor1;
|
||||
//1 new1 new1 new2 new2 2
|
||||
isocc = gridmap.isBlocked(cor1, cor2);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor2, cor3);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor3, cor4);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
isocc = gridmap.isBlocked(cor4, cor5);
|
||||
if(isocc){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
if(tmp_expandLength[3] > limitBoundHalfW){
|
||||
NotFinishTable[i] = 0.0;
|
||||
break;
|
||||
}
|
||||
expandLength = tmp_expandLength;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
Eigen::MatrixXd poly;
|
||||
poly.resize(4,4);
|
||||
std::vector<Eigen::Vector2d> cors;
|
||||
cors.resize(4);
|
||||
cors[0] = sourcePt + egoR * Eigen::Vector2d(expandLength[0],expandLength[3]);
|
||||
cors[1] = sourcePt + egoR * Eigen::Vector2d(expandLength[0],-expandLength[1]);
|
||||
cors[2] = sourcePt + egoR * Eigen::Vector2d(-expandLength[2],-expandLength[1]);
|
||||
cors[3] = sourcePt + egoR * Eigen::Vector2d(-expandLength[2],expandLength[3]);
|
||||
poly.col(0) << cors[0], cos(yaw), sin(yaw);
|
||||
poly.col(1) << cors[1], sin(yaw), -cos(yaw);
|
||||
poly.col(2) << cors[2], -cos(yaw), -sin(yaw);
|
||||
poly.col(3) << cors[3],-sin(yaw), cos(yaw);
|
||||
|
||||
return poly;
|
||||
|
||||
}
|
||||
bool ifInside(Eigen::Vector2d pt, Eigen::MatrixXd poly){
|
||||
for(int i = 0; i < poly.cols(); i++){
|
||||
Eigen::Vector2d p = poly.col(i).head(2);
|
||||
Eigen::Vector2d n = poly.col(i).tail(2);
|
||||
if((pt-p).dot(n)>0){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
|
||||
}
|
||||
} // namespace plan_utils
|
||||
144
trajectoryOpt/src/utils/tools/include/tools/config.hpp
Normal file
144
trajectoryOpt/src/utils/tools/include/tools/config.hpp
Normal file
@@ -0,0 +1,144 @@
|
||||
#ifndef CONFIG.HPP
|
||||
#define CONFIG.HPP
|
||||
#include <ros/ros.h>
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <cfloat>
|
||||
#include <vector>
|
||||
#include <Eigen/Eigen>
|
||||
#include <XmlRpcValue.h>
|
||||
struct Config
|
||||
{
|
||||
double mini_T = 0.0;
|
||||
double wei_time_ = 500.0;
|
||||
double kmax = 0.45;
|
||||
double vmax = 3.0;
|
||||
double latAccmax = 3.0;
|
||||
double lonAccmax = 3.0;
|
||||
double accRatemax = 8.0;
|
||||
double kdotmax = 5.0;
|
||||
double yawdotmax = 3.14;
|
||||
int traj_res = 16;
|
||||
double scaling_wf_min_ = 0.01, scaling_wc_min_ = 0.01;
|
||||
double rho = 1.0, rho_max =1000.0;
|
||||
double gamma = 1;
|
||||
double cons_eps_ = 0.20;
|
||||
int mem_size = 256;
|
||||
int past = 3; //3
|
||||
double g_epsilon = 1.0e-3;
|
||||
double min_step = 1.0e-32;
|
||||
double delta = 1.0e-4;
|
||||
int max_iterations = 12000;
|
||||
int amlMaxIter = 15;
|
||||
double pieceTime = 0.7;
|
||||
XmlRpc::XmlRpcValue xmlconpts;
|
||||
std::vector<Eigen::Vector2d> conpts;
|
||||
/*map parameters*/
|
||||
double mapRes = 0.1;
|
||||
double mapX = 50.0, mapY = 50.0, mapZ = 10.0;
|
||||
int expandSize = 1;
|
||||
/*front end*/
|
||||
double wheel_base = 0.6;
|
||||
double horizon_ = 50.0;
|
||||
double yaw_resolution_ = 0.3;
|
||||
double lambda_heu_ = 5.0;
|
||||
int allocate_num_ = 100000;
|
||||
int check_num_ = 5;
|
||||
double max_seach_time = 1.0;
|
||||
double step_arc = 0.9;
|
||||
double checkl = 0.2;
|
||||
double non_siguav = 0.0;
|
||||
double backW = 2.0;
|
||||
double penaWei = 500.0;
|
||||
double esdfWei = 1000.0;
|
||||
double miniS =0.9;
|
||||
bool isdebug = true;
|
||||
bool isfixGear = false;
|
||||
bool isVis = false;
|
||||
bool enable_shot = false;
|
||||
double safeMargin = 0.1;
|
||||
bool useDP = true;
|
||||
double phimax = 0.785;
|
||||
double omegamax = 0.3;
|
||||
double steer_res = 0.5;
|
||||
std::string model_path = "/home/han/2023codes/NeuralTraj/model/big_cock.pt";
|
||||
|
||||
|
||||
|
||||
Config(const ros::NodeHandle &nh_priv)
|
||||
{
|
||||
nh_priv.param("mini_T", mini_T, 0.0);
|
||||
nh_priv.param("wei_time_", wei_time_, 500.0);
|
||||
nh_priv.param("kmax", kmax, 0.45);
|
||||
nh_priv.param("vmax", vmax, 3.0);
|
||||
nh_priv.param("latAccmax", latAccmax, 3.0);
|
||||
nh_priv.param("lonAccmax", lonAccmax, 3.0);
|
||||
nh_priv.param("accRatemax", accRatemax, 8.0);
|
||||
nh_priv.param("kdotmax", kdotmax, 1.0);
|
||||
nh_priv.param("yawdotmax", yawdotmax, 3.14);
|
||||
nh_priv.param("traj_res", traj_res, 16);
|
||||
nh_priv.param("rho", rho, 1.0);
|
||||
nh_priv.param("rho_max", rho_max, 1000.0);
|
||||
nh_priv.param("gamma", gamma, 1.0);
|
||||
nh_priv.param("cons_eps_", cons_eps_, 0.2);
|
||||
nh_priv.param("mem_size", mem_size, 256);
|
||||
nh_priv.param("past", past, 3);
|
||||
nh_priv.param("g_epsilon", g_epsilon, 1.0e-3);
|
||||
nh_priv.param("min_step", min_step, 1.0e-32);
|
||||
nh_priv.param("delta", delta, 1.0e-4);
|
||||
nh_priv.param("max_iterations", max_iterations, 10000);
|
||||
nh_priv.param("amlMaxIter", amlMaxIter, 15);
|
||||
nh_priv.param("pieceTime", pieceTime, 0.7);
|
||||
nh_priv.getParam("conpts", xmlconpts);
|
||||
nh_priv.getParam("penaWei", penaWei);
|
||||
nh_priv.getParam("esdfWei", esdfWei);
|
||||
nh_priv.getParam("miniS", miniS);
|
||||
nh_priv.getParam("safeMargin", safeMargin);
|
||||
nh_priv.getParam("phimax", phimax);
|
||||
nh_priv.getParam("omegamax", omegamax);
|
||||
nh_priv.getParam("useDP", useDP);
|
||||
nh_priv.getParam("steer_res", steer_res);
|
||||
for(int i = 0; i < xmlconpts.size(); ++i)
|
||||
{
|
||||
Eigen::Vector2d pt;
|
||||
for(int j = 0; j < 2; ++j)
|
||||
{
|
||||
pt[j] = xmlconpts[i][j];
|
||||
}
|
||||
conpts.push_back(pt);
|
||||
}
|
||||
nh_priv.param("mapRes", mapRes, 0.1);
|
||||
nh_priv.param("mapX", mapX, 50.0);
|
||||
nh_priv.param("mapY", mapY, 50.0);
|
||||
nh_priv.param("expandSize", expandSize, 2);
|
||||
nh_priv.param("wheel_base", wheel_base, 0.6);
|
||||
nh_priv.param("horizon_", horizon_, 50.0);
|
||||
nh_priv.param("yaw_resolution_", yaw_resolution_, 0.3);
|
||||
nh_priv.param("lambda_heu_", lambda_heu_, 5.0);
|
||||
nh_priv.param("allocate_num_", allocate_num_, 100000);
|
||||
nh_priv.param("check_num_", check_num_, 5);
|
||||
nh_priv.param("max_seach_time", max_seach_time, 1.0);
|
||||
nh_priv.param("step_arc", step_arc, 0.9);
|
||||
nh_priv.param("checkl", checkl, 0.2);
|
||||
nh_priv.param("non_siguav", non_siguav, 0.0);
|
||||
nh_priv.param("isdebug", isdebug, false);
|
||||
nh_priv.param("isfixGear", isfixGear, false);
|
||||
nh_priv.param("isVis", isVis, false);
|
||||
nh_priv.param("enable_shot", enable_shot, false);
|
||||
nh_priv.param("backW", backW, 2.0);
|
||||
|
||||
}
|
||||
};
|
||||
double normalize_angle(const double& theta) {
|
||||
double theta_tmp = theta;
|
||||
while(theta_tmp >= acos(-1.0)){
|
||||
theta_tmp -= 2 * acos(-1.0);
|
||||
}
|
||||
while(theta_tmp < -acos(-1.0)){
|
||||
theta_tmp += 2 * acos(-1.0);
|
||||
}
|
||||
return theta_tmp;
|
||||
}
|
||||
typedef std::shared_ptr<Config> ConfigPtr;
|
||||
|
||||
#endif
|
||||
792
trajectoryOpt/src/utils/tools/include/tools/gridmap.hpp
Normal file
792
trajectoryOpt/src/utils/tools/include/tools/gridmap.hpp
Normal file
@@ -0,0 +1,792 @@
|
||||
#ifndef GRIDMAP_H
|
||||
#define GRIDMAP_H
|
||||
|
||||
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <ros/console.h>
|
||||
#include <map>
|
||||
#include <tools/config.hpp>
|
||||
#include <decomp_basis/data_type.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
namespace map_util{
|
||||
using Tmap = std::vector<signed char>;
|
||||
template <int Dim> class MapUtil {
|
||||
public:
|
||||
///Simple constructor
|
||||
MapUtil() {}
|
||||
///Get map data
|
||||
Tmap getMap() { return map_; }
|
||||
//
|
||||
bool has_map_(){return has_map;}
|
||||
///Get resolution
|
||||
double getRes() { return res_; }
|
||||
///Get dimensions
|
||||
Veci<Dim> getDim() { return dim_; }
|
||||
///Get origin
|
||||
Vecf<Dim> getOrigin() { return origin_d_; }
|
||||
Vecf<Dim> getMapSize() { return map_size; }
|
||||
void setParam(ConfigPtr config_, ros::NodeHandle& nh){
|
||||
res_ = config_->mapRes;
|
||||
int buffer_size;
|
||||
expand_size = config_->expandSize;
|
||||
priv_config_ = config_;
|
||||
if(Dim == 3){
|
||||
map_size(0) = config_->mapX;
|
||||
map_size(1) = config_->mapY;
|
||||
map_size(2) = config_->mapZ;
|
||||
origin_d_[0] = -map_size(0)/2;
|
||||
origin_d_[1] = -map_size(1)/2;
|
||||
origin_d_[2] = 0;
|
||||
dim_(0) = map_size(0)/res_;
|
||||
dim_(1) = map_size(1)/res_;
|
||||
dim_(2) = map_size(2)/res_;
|
||||
buffer_size = dim_(0)*dim_(1)*dim_(2);
|
||||
}
|
||||
else if(Dim==2){
|
||||
map_size(0) = config_->mapX + 1.0e-4;
|
||||
map_size(1) = config_->mapY + 1.0e-4;
|
||||
origin_d_[0] = -map_size(0)/2;
|
||||
origin_d_[1] = -map_size(1)/2;
|
||||
dim_(0) = map_size(0)/res_;
|
||||
dim_(1) = map_size(1)/res_;
|
||||
buffer_size = dim_(0)*dim_(1);
|
||||
|
||||
/*ESDF*/
|
||||
distance_buffer_ = std::vector<double>(buffer_size, 10000);
|
||||
distance_buffer_neg_ = std::vector<double>(buffer_size, 10000);
|
||||
distance_buffer_all_ = std::vector<double>(buffer_size, 10000);
|
||||
tmp_buffer1_ = std::vector<double>(buffer_size, 0);
|
||||
occupancy_buffer_neg = std::vector<char>(buffer_size, 0);
|
||||
esdf_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/global_esdf", 10);
|
||||
pcl_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/global_pcl", 10);
|
||||
|
||||
}
|
||||
else{
|
||||
ROS_ERROR("grid map dimensions must be 2 or 3!");
|
||||
}
|
||||
map_.resize(buffer_size,val_free);
|
||||
point_cloud_sub_ = nh.subscribe("/global_map", 10, &MapUtil::MapBuild, this);
|
||||
}
|
||||
void setEnv(const sensor_msgs::PointCloud2 & pointcloud_map){
|
||||
//only used in 2D environment
|
||||
int buffer_size;
|
||||
buffer_size = dim_(0)*dim_(1);
|
||||
distance_buffer_ = std::vector<double>(buffer_size, 10000);
|
||||
distance_buffer_neg_ = std::vector<double>(buffer_size, 10000);
|
||||
distance_buffer_all_ = std::vector<double>(buffer_size, 10000);
|
||||
tmp_buffer1_ = std::vector<double>(buffer_size, 0);
|
||||
occupancy_buffer_neg = std::vector<char>(buffer_size, 0);
|
||||
std::fill(map_.begin(), map_.end(), val_free);
|
||||
|
||||
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud;
|
||||
pcl::fromROSMsg(pointcloud_map, cloud);
|
||||
if( (int)cloud.points.size() == 0 ) return;
|
||||
pcl::PointXYZ pt;
|
||||
for (int idx = 0; idx < (int)cloud.points.size(); idx++)
|
||||
{
|
||||
pt = cloud.points[idx];
|
||||
setObs(pt);
|
||||
}
|
||||
has_map = true;
|
||||
// std::cout << "!map build\n";
|
||||
updateESDF2d();
|
||||
publishESDF();
|
||||
publishPCL();
|
||||
}
|
||||
void MapBuild(const sensor_msgs::PointCloud2 & pointcloud_map){
|
||||
if(has_map) {
|
||||
publishESDF();
|
||||
return;}
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud;
|
||||
pcl::fromROSMsg(pointcloud_map, cloud);
|
||||
if( (int)cloud.points.size() == 0 ) return;
|
||||
pcl::PointXYZ pt;
|
||||
// cloud.points.clear();//data generaion
|
||||
|
||||
for (int idx = 0; idx < (int)cloud.points.size(); idx++)
|
||||
{
|
||||
pt = cloud.points[idx];
|
||||
setObs(pt);
|
||||
}
|
||||
has_map = true;
|
||||
updateESDF2d();
|
||||
publishESDF();
|
||||
}
|
||||
void setObs(pcl::PointXYZ pt){
|
||||
if(Dim==3){
|
||||
double coord_x = pt.x;
|
||||
double coord_y = pt.y;
|
||||
double coord_z = pt.z;
|
||||
Vec3f pt = Vec3f(coord_x,coord_y,coord_z);
|
||||
Veci<Dim> index3i;
|
||||
for(int i = 0; i < Dim; i++)
|
||||
index3i(i) = std::round((pt(i) - origin_d_(i)) / res_ - 0.5);
|
||||
if(isOutside(index3i))
|
||||
return;
|
||||
for (int i=-expand_size;i<=expand_size;i++)
|
||||
for (int j=-expand_size;j<=expand_size;j++)
|
||||
for (int k=-expand_size;k<=expand_size;k++)
|
||||
{
|
||||
Veci<Dim> temp_index;
|
||||
temp_index(0) = index3i(0)+i;
|
||||
temp_index(1) = index3i(1)+j;
|
||||
temp_index(2) = index3i(2)+k;
|
||||
if(isOutside(temp_index)) continue;
|
||||
map_[getIndex(temp_index)] = val_occ;
|
||||
}
|
||||
}
|
||||
else{
|
||||
double coord_x = pt.x;
|
||||
double coord_y = pt.y;
|
||||
Vec2f pt = Vec2f(coord_x,coord_y);
|
||||
Veci<Dim> index2i;
|
||||
for(int i = 0; i < Dim; i++)
|
||||
index2i(i) = std::round((pt(i) - origin_d_(i)) / res_ - 0.5);
|
||||
if(isOutside(index2i))
|
||||
return;
|
||||
for (int i=-expand_size;i<=expand_size;i++)
|
||||
for (int j=-expand_size;j<=expand_size;j++)
|
||||
{
|
||||
Veci<Dim> temp_index;
|
||||
temp_index(0) = index2i(0)+i;
|
||||
temp_index(1) = index2i(1)+j;
|
||||
if(isOutside(temp_index)) continue;
|
||||
map_[getIndex(temp_index)] = val_occ;
|
||||
}
|
||||
|
||||
}
|
||||
return;
|
||||
|
||||
}
|
||||
///Get index of a cell
|
||||
int getIndex(const Veci<Dim>& pn) {
|
||||
return Dim == 2 ? pn(0) + dim_(0) * pn(1) :
|
||||
pn(0) + dim_(0) * pn(1) + dim_(0) * dim_(1) * pn(2);
|
||||
}
|
||||
///Check if the given cell is outside of the map in i-the dimension
|
||||
bool isOutsideXYZ(const Veci<Dim> &n, int i) { return n(i) < 0 || n(i) >= dim_(i); }
|
||||
///Check if the cell is free by index
|
||||
bool isFree(int idx) { return map_[idx] == val_free; }
|
||||
///Check if the cell is unknown by index
|
||||
bool isUnknown(int idx) { return map_[idx] == val_unknown; }
|
||||
///Check if the cell is occupied by index
|
||||
bool isOccupied(int idx) { return map_[idx] > val_free; }
|
||||
//free 0 occ 100 unknow -1
|
||||
///Check if the cell is outside by coordinate
|
||||
bool isOutside(const Veci<Dim> &pn) {
|
||||
for(int i = 0; i < Dim; i++)
|
||||
if (pn(i) < 0 || pn(i) >= dim_(i))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
///Check if the given cell is free by coordinate
|
||||
bool isFree(const Veci<Dim> &pn) {
|
||||
if (isOutside(pn))
|
||||
return false;
|
||||
else
|
||||
return isFree(getIndex(pn));
|
||||
}
|
||||
///Check if the given cell is occupied by coordinate
|
||||
bool isOccupied(const Veci<Dim> &pn) {
|
||||
if (isOutside(pn))
|
||||
return true;
|
||||
else
|
||||
return isOccupied(getIndex(pn));
|
||||
}
|
||||
bool isOccupied(const Vecf<Dim> &pt) {
|
||||
Veci<Dim> pn = floatToInt(pt);
|
||||
if (isOutside(pn))
|
||||
return true;
|
||||
else
|
||||
return isOccupied(getIndex(pn));
|
||||
}
|
||||
|
||||
|
||||
///Check if the given cell is unknown by coordinate
|
||||
bool isUnknown(const Veci<Dim> &pn) {
|
||||
if (isOutside(pn))
|
||||
return false;
|
||||
return map_[getIndex(pn)] == val_unknown;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Set map
|
||||
*
|
||||
* @param ori origin position
|
||||
* @param dim number of cells in each dimension
|
||||
* @param map array of cell values
|
||||
* @param res map resolution
|
||||
*/
|
||||
void setMap(const Vecf<Dim>& ori, const Veci<Dim>& dim, const Tmap &map, double res) {
|
||||
map_ = map;
|
||||
dim_ = dim;
|
||||
origin_d_ = ori;
|
||||
res_ = res;
|
||||
}
|
||||
|
||||
///Print basic information about the util
|
||||
void info() {
|
||||
Vecf<Dim> range = dim_.template cast<double>() * res_;
|
||||
std::cout << "MapUtil Info ========================== " << std::endl;
|
||||
std::cout << " res: [" << res_ << "]" << std::endl;
|
||||
std::cout << " origin: [" << origin_d_.transpose() << "]" << std::endl;
|
||||
std::cout << " range: [" << range.transpose() << "]" << std::endl;
|
||||
std::cout << " dim: [" << dim_.transpose() << "]" << std::endl;
|
||||
};
|
||||
|
||||
///Float position to discrete cell coordinate
|
||||
Veci<Dim> floatToInt(const Vecf<Dim> &pt) {
|
||||
Veci<Dim> pn;
|
||||
for(int i = 0; i < Dim; i++)
|
||||
pn(i) = floor((pt(i) - origin_d_(i)) / res_);
|
||||
return pn;
|
||||
}
|
||||
///Discrete cell coordinate to float position
|
||||
Vecf<Dim> intToFloat(const Veci<Dim> &pn) {
|
||||
//return pn.template cast<double>() * res_ + origin_d_;
|
||||
return (pn.template cast<double>() + Vecf<Dim>::Constant(0.5)) * res_ + origin_d_;
|
||||
}
|
||||
|
||||
///Raytrace from float point pt1 to pt2
|
||||
vec_Veci<Dim> rayTrace(const Vecf<Dim> &pt1, const Vecf<Dim> &pt2) {
|
||||
Vecf<Dim> diff = pt2 - pt1;
|
||||
double k = 0.8;
|
||||
int max_diff = (diff / res_).template lpNorm<Eigen::Infinity>() / k;
|
||||
double s = 1.0 / max_diff;
|
||||
Vecf<Dim> step = diff * s;
|
||||
|
||||
vec_Veci<Dim> pns;
|
||||
Veci<Dim> prev_pn = Veci<Dim>::Constant(-1);
|
||||
for (int n = 1; n < max_diff; n++) {
|
||||
Vecf<Dim> pt = pt1 + step * n;
|
||||
Veci<Dim> new_pn = floatToInt(pt);
|
||||
if (isOutside(new_pn))
|
||||
break;
|
||||
if (new_pn != prev_pn)
|
||||
pns.push_back(new_pn);
|
||||
prev_pn = new_pn;
|
||||
}
|
||||
return pns;
|
||||
}
|
||||
|
||||
///Check if the ray from p1 to p2 is occluded
|
||||
bool isBlocked(const Vecf<Dim>& p1, const Vecf<Dim>& p2, int8_t val = 100) {
|
||||
vec_Veci<Dim> pns = rayTrace(p1, p2);
|
||||
for (const auto &pn : pns) {
|
||||
if (map_[getIndex(pn)] >= val)
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
///Get occupied voxels
|
||||
vec_Vecf<Dim> getCloud() {
|
||||
vec_Vecf<Dim> cloud;
|
||||
Veci<Dim> n;
|
||||
if(Dim == 3) {
|
||||
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
|
||||
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
|
||||
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
|
||||
if (isOccupied(getIndex(n)))
|
||||
cloud.push_back(intToFloat(n));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (Dim == 2) {
|
||||
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
|
||||
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
|
||||
if (isOccupied(getIndex(n)))
|
||||
cloud.push_back(intToFloat(n));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return cloud;
|
||||
}
|
||||
void updateESDF2d() {
|
||||
if(Dim != 2){
|
||||
ROS_ERROR("Dimension mismatch!");
|
||||
}
|
||||
Eigen::Vector2i min_esdf = Eigen::Vector2i(0,0);
|
||||
Eigen::Vector2i max_esdf = Eigen::Vector2i(dim_(0) - 1, dim_(1) - 1);
|
||||
|
||||
/* ========== compute positive DT ========== */
|
||||
|
||||
for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
|
||||
fillESDF(
|
||||
[&](int y) {
|
||||
return isOccupied(Vec2i(x, y))?
|
||||
0 :
|
||||
std::numeric_limits<double>::max();
|
||||
},
|
||||
[&](int y, double val) {tmp_buffer1_[getIndex(Vec2i(x, y))] = val; }, min_esdf[1],
|
||||
max_esdf[1], 1);
|
||||
}
|
||||
|
||||
for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
|
||||
fillESDF([&](int x) { return tmp_buffer1_[getIndex(Vec2i(x, y))]; },
|
||||
[&](int x, double val) {
|
||||
distance_buffer_[getIndex(Vec2i(x, y))] = res_ * std::sqrt(val);
|
||||
// min(mp_.resolution_ * std::sqrt(val),
|
||||
// md_.distance_buffer_[toAddress(x, y, z)]);
|
||||
},
|
||||
min_esdf[0], max_esdf[0], 0);
|
||||
}
|
||||
|
||||
/* ========== compute negative distance ========== */
|
||||
for (int x = min_esdf(0); x <= max_esdf(0); ++x)
|
||||
for (int y = min_esdf(1); y <= max_esdf(1); ++y)
|
||||
{
|
||||
int idx = getIndex(Vec2i(x, y));
|
||||
if (isFree(idx)) {
|
||||
occupancy_buffer_neg[idx] = 1;
|
||||
|
||||
} else if (isOccupied(idx)) {
|
||||
occupancy_buffer_neg[idx] = 0;
|
||||
} else {
|
||||
ROS_ERROR("what?");
|
||||
}
|
||||
}
|
||||
|
||||
ros::Time t1, t2;
|
||||
|
||||
for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
|
||||
fillESDF(
|
||||
[&](int y) {
|
||||
return occupancy_buffer_neg[getIndex(Vec2i(x, y))] == 1 ?
|
||||
0 :
|
||||
std::numeric_limits<double>::max();
|
||||
},
|
||||
[&](int y, double val) { tmp_buffer1_[getIndex(Vec2i(x, y))] = val; }, min_esdf[1],
|
||||
max_esdf[1], 1);
|
||||
}
|
||||
|
||||
for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
|
||||
fillESDF([&](int x) { return tmp_buffer1_[getIndex(Vec2i(x, y))]; },
|
||||
[&](int x, double val) {
|
||||
distance_buffer_neg_[getIndex(Vec2i(x, y))] = res_ * std::sqrt(val);
|
||||
},
|
||||
min_esdf[0], max_esdf[0], 0);
|
||||
}
|
||||
|
||||
/* ========== combine pos and neg DT ========== */
|
||||
for (int x = min_esdf(0); x <= max_esdf(0); ++x)
|
||||
for (int y = min_esdf(1); y <= max_esdf(1); ++y)
|
||||
{
|
||||
int idx = getIndex(Vec2i(x, y));
|
||||
distance_buffer_all_[idx] = distance_buffer_[idx];
|
||||
|
||||
if (distance_buffer_neg_[idx] > 0.0){
|
||||
distance_buffer_all_[idx] += (-distance_buffer_neg_[idx] + res_);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
template <typename F_get_val, typename F_set_val>
|
||||
void fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int d) {
|
||||
int v[dim_(d)];
|
||||
double z[dim_(d) + 1];
|
||||
|
||||
int k = start;
|
||||
v[start] = start;
|
||||
z[start] = -std::numeric_limits<double>::max();
|
||||
z[start + 1] = std::numeric_limits<double>::max();
|
||||
|
||||
for (int q = start + 1; q <= end; q++) {
|
||||
k++;
|
||||
double s;
|
||||
|
||||
do {
|
||||
k--;
|
||||
s = ((f_get_val(q) + q * q) - (f_get_val(v[k]) + v[k] * v[k])) / (2 * q - 2 * v[k]);
|
||||
} while (s <= z[k]);
|
||||
|
||||
k++;
|
||||
|
||||
v[k] = q;
|
||||
z[k] = s;
|
||||
z[k + 1] = std::numeric_limits<double>::max();
|
||||
}
|
||||
|
||||
k = start;
|
||||
|
||||
for (int q = start; q <= end; q++) {
|
||||
while (z[k + 1] < q) k++;
|
||||
double val = (q - v[k]) * (q - v[k]) + f_get_val(v[k]);
|
||||
f_set_val(q, val);
|
||||
}
|
||||
}
|
||||
inline double getDistance(const Eigen::Vector2d& pos) {
|
||||
Eigen::Vector2i id;
|
||||
id = floatToInt(pos);
|
||||
boundIndex(id);
|
||||
return distance_buffer_all_[getIndex(id)];
|
||||
}
|
||||
inline double getDistance(const Eigen::Vector2i& id) {
|
||||
Eigen::Vector2i id1 = id;
|
||||
boundIndex(id1);
|
||||
return distance_buffer_all_[getIndex(id1)];
|
||||
}
|
||||
inline void boundIndex(Eigen::Vector2i& id) {
|
||||
Eigen::Vector2i id1;
|
||||
id1(0) = std::max(std::min(id(0), dim_(0) - 1), 0);
|
||||
id1(1) = std::max(std::min(id(1), dim_(1) - 1), 0);
|
||||
id = id1;
|
||||
}
|
||||
void publishPCL(){
|
||||
vec_Vecf<Dim> pcs = getCloud();
|
||||
pcl::PointCloud<pcl::PointXYZI> cloud;
|
||||
pcl::PointXYZI pt;
|
||||
for(const auto& pc : pcs){
|
||||
pt.x = pc[0];
|
||||
pt.y = pc[1];
|
||||
pt.z = 0.0;
|
||||
cloud.push_back(pt);
|
||||
}
|
||||
cloud.width = cloud.points.size();
|
||||
cloud.height = 1;
|
||||
cloud.is_dense = true;
|
||||
cloud.header.frame_id = "world";
|
||||
sensor_msgs::PointCloud2 cloud_msg;
|
||||
pcl::toROSMsg(cloud, cloud_msg);
|
||||
pcl_pub_.publish(cloud_msg);
|
||||
|
||||
}
|
||||
void publishESDF() {
|
||||
double dist;
|
||||
pcl::PointCloud<pcl::PointXYZI> cloud;
|
||||
pcl::PointXYZI pt;
|
||||
const double min_dist = -4.0;
|
||||
const double max_dist = 4.0;
|
||||
Eigen::Vector2i min_cut = Eigen::Vector2i(0,0);
|
||||
Eigen::Vector2i max_cut = Eigen::Vector2i(dim_(0)-1,dim_(1)-1);
|
||||
|
||||
for (int x = min_cut(0); x <= max_cut(0); ++x)
|
||||
for (int y = min_cut(1); y <= max_cut(1); ++y) {
|
||||
Vec2f pos;
|
||||
pos = intToFloat(Vec2i(x, y));
|
||||
dist = getDistance(pos);
|
||||
// if(dist<min_dist) dist = min_dist;
|
||||
// if(dist>max_dist) dist = max_dist;
|
||||
pt.x = pos(0);
|
||||
pt.y = pos(1);
|
||||
pt.z = dist;
|
||||
pt.intensity = (dist - min_dist) / (max_dist - min_dist);
|
||||
cloud.push_back(pt);
|
||||
// }
|
||||
}
|
||||
cloud.width = cloud.points.size();
|
||||
cloud.height = 1;
|
||||
cloud.is_dense = true;
|
||||
cloud.header.frame_id = "world";
|
||||
sensor_msgs::PointCloud2 cloud_msg;
|
||||
pcl::toROSMsg(cloud, cloud_msg);
|
||||
esdf_pub_.publish(cloud_msg);
|
||||
}
|
||||
inline double getDistGrad(Eigen::Vector2d po, Eigen::Vector2d& grad) {
|
||||
Eigen::Vector2d pos(po(0),po(1));
|
||||
if (isOutside(floatToInt(pos))) {
|
||||
grad.setZero();
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* use bilinear interpolation */
|
||||
Eigen::Vector2d pos_m = pos - 0.5 * res_ * Eigen::Vector2d::Ones();
|
||||
|
||||
Eigen::Vector2i idx;
|
||||
idx = floatToInt(pos_m);
|
||||
Eigen::Vector2d idx_pos, diff;
|
||||
idx_pos = intToFloat(idx);
|
||||
|
||||
diff = (pos - idx_pos) / res_;
|
||||
|
||||
double values[2][2];
|
||||
for (int x = 0; x < 2; x++) {
|
||||
for (int y = 0; y < 2; y++) {
|
||||
Eigen::Vector2i current_idx = idx + Eigen::Vector2i(x, y);
|
||||
values[x][y] = getDistance(current_idx);
|
||||
}
|
||||
}
|
||||
double v00 = (1-diff(0)) * values[0][0] + diff(0) * values[1][0];
|
||||
double v10 = (1-diff(0)) * values[0][1] + diff(0) * values[1][1];
|
||||
double v0 = (1-diff(1)) * v00 + diff(1) * v10;
|
||||
// double dist = v0;
|
||||
grad[1] = (v10 - v00) / res_;
|
||||
grad[0] = ((1-diff(1)) * (values[1][0]-values[0][0]) + diff(1) * (values[1][1]-values[0][1])) / res_;
|
||||
|
||||
return v0;
|
||||
}
|
||||
|
||||
///Get free voxels
|
||||
vec_Vecf<Dim> getFreeCloud() {
|
||||
vec_Vecf<Dim> cloud;
|
||||
Veci<Dim> n;
|
||||
if(Dim == 3) {
|
||||
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
|
||||
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
|
||||
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
|
||||
if (isFree(getIndex(n)))
|
||||
cloud.push_back(intToFloat(n));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (Dim == 2) {
|
||||
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
|
||||
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
|
||||
if (isFree(getIndex(n)))
|
||||
cloud.push_back(intToFloat(n));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return cloud;
|
||||
}
|
||||
|
||||
///Get unknown voxels
|
||||
vec_Vecf<Dim> getUnknownCloud() {
|
||||
vec_Vecf<Dim> cloud;
|
||||
Veci<Dim> n;
|
||||
if(Dim == 3) {
|
||||
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
|
||||
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
|
||||
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
|
||||
if (isUnknown(getIndex(n)))
|
||||
cloud.push_back(intToFloat(n));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (Dim == 2) {
|
||||
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
|
||||
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
|
||||
if (isUnknown(getIndex(n)))
|
||||
cloud.push_back(intToFloat(n));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return cloud;
|
||||
}
|
||||
|
||||
///Dilate occupied cells
|
||||
void dilate(const vec_Veci<Dim>& dilate_neighbor) {
|
||||
Tmap map = map_;
|
||||
Veci<Dim> n = Veci<Dim>::Zero();
|
||||
if(Dim == 3) {
|
||||
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
|
||||
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
|
||||
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
|
||||
if (isOccupied(getIndex(n))) {
|
||||
for (const auto &it : dilate_neighbor) {
|
||||
if (!isOutside(n + it))
|
||||
map[getIndex(n + it)] = val_occ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(Dim == 2) {
|
||||
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
|
||||
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
|
||||
if (isOccupied(getIndex(n))) {
|
||||
for (const auto &it : dilate_neighbor) {
|
||||
if (!isOutside(n + it))
|
||||
map[getIndex(n + it)] = val_occ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
map_ = map;
|
||||
}
|
||||
|
||||
///Free unknown voxels
|
||||
void freeUnknown() {
|
||||
Veci<Dim> n;
|
||||
if(Dim == 3) {
|
||||
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
|
||||
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
|
||||
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
|
||||
if (isUnknown(getIndex(n)))
|
||||
map_[getIndex(n)] = val_free;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (Dim == 2) {
|
||||
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
|
||||
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
|
||||
if (isUnknown(getIndex(n)))
|
||||
map_[getIndex(n)] = val_free;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
void CheckIfCollisionUsingPosAndYaw(const Eigen::Vector3d &state, bool *res, double flat = 0.0){
|
||||
if(Dim==3){
|
||||
ROS_ERROR("CheckIfCollisionUsingPosAndYaw Only works with Dimension = 2!");
|
||||
return;
|
||||
}
|
||||
else{
|
||||
Eigen::Matrix2d rotM;
|
||||
double yaw = state[2];
|
||||
rotM << cos(yaw), -sin(yaw),
|
||||
sin(yaw), cos(yaw);
|
||||
Eigen::Vector2d p = state.head(2);
|
||||
for(int i = 0; i < priv_config_->conpts.size(); i++){
|
||||
// Eigen::Vector2d rep1 = priv_config_->conpts[i];
|
||||
// Eigen::Vector2d rep2 = priv_config_->conpts[(i+1)%priv_config_->conpts.size()];
|
||||
// if(rep1[0] > 0){
|
||||
// rep1[0] += res_;
|
||||
// }
|
||||
// else{
|
||||
// rep1[0] -= res_;
|
||||
// }
|
||||
// if(rep1[1] > 0){
|
||||
// rep1[1] += res_;
|
||||
// }
|
||||
// else{
|
||||
// rep1[1] -= res_;
|
||||
// }
|
||||
// if(rep2[0] > 0){
|
||||
// rep2[0] += res_;
|
||||
// }
|
||||
// else{
|
||||
// rep2[0] -= res_;
|
||||
// }
|
||||
// if(rep2[1] > 0){
|
||||
// rep2[1] += res_;
|
||||
// }
|
||||
// else{
|
||||
// rep2[1] -= res_;
|
||||
// }
|
||||
// Eigen::Vector2d pt1 = p + rotM * rep1;
|
||||
// Eigen::Vector2d pt2 = p + rotM * rep2;
|
||||
// if(isBlocked(pt1, pt2)){
|
||||
// *res = true;
|
||||
// return;
|
||||
// }
|
||||
|
||||
Eigen::Vector2d pt = p + rotM * priv_config_->conpts[i];
|
||||
Eigen::Vector2d grad;
|
||||
// if(flat > 0){
|
||||
// std::cout << "dis: "<<getDistGrad(pt, grad)<<"\n";
|
||||
// }
|
||||
if(getDistGrad(pt, grad)<priv_config_->safeMargin + flat){
|
||||
*res = true;
|
||||
return;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
*res = false;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
void CheckIfCollisionForBackEnd(const Eigen::Vector3d &state, bool *res){
|
||||
if(Dim==3){
|
||||
ROS_ERROR("CheckIfCollisionUsingPosAndYaw Only works with Dimension = 2!");
|
||||
return;
|
||||
}
|
||||
else{
|
||||
Eigen::Matrix2d rotM;
|
||||
double yaw = state[2];
|
||||
rotM << cos(yaw), -sin(yaw),
|
||||
sin(yaw), cos(yaw);
|
||||
Eigen::Vector2d p = state.head(2);
|
||||
for(int i = 0; i < priv_config_->conpts.size(); i++){
|
||||
// Eigen::Vector2d rep1 = priv_config_->conpts[i];
|
||||
// Eigen::Vector2d rep2 = priv_config_->conpts[(i+1)%priv_config_->conpts.size()];
|
||||
// if(rep1[0] > 0){
|
||||
// rep1[0] += res_;
|
||||
// }
|
||||
// else{
|
||||
// rep1[0] -= res_;
|
||||
// }
|
||||
// if(rep1[1] > 0){
|
||||
// rep1[1] += res_;
|
||||
// }
|
||||
// else{
|
||||
// rep1[1] -= res_;
|
||||
// }
|
||||
// if(rep2[0] > 0){
|
||||
// rep2[0] += res_;
|
||||
// }
|
||||
// else{
|
||||
// rep2[0] -= res_;
|
||||
// }
|
||||
// if(rep2[1] > 0){
|
||||
// rep2[1] += res_;
|
||||
// }
|
||||
// else{
|
||||
// rep2[1] -= res_;
|
||||
// }
|
||||
// Eigen::Vector2d pt1 = p + rotM * rep1;
|
||||
// Eigen::Vector2d pt2 = p + rotM * rep2;
|
||||
// if(isBlocked(pt1, pt2)){
|
||||
// *res = true;
|
||||
// return;
|
||||
// }
|
||||
|
||||
Eigen::Vector2d pt = p + rotM * priv_config_->conpts[i];
|
||||
Eigen::Vector2d grad;
|
||||
if(getDistGrad(pt, grad)<priv_config_->safeMargin){
|
||||
*res = true;
|
||||
return;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
*res = false;
|
||||
return;
|
||||
}
|
||||
///Map entity
|
||||
Tmap map_;
|
||||
std::vector<double> distance_buffer_;
|
||||
std::vector<double> distance_buffer_neg_;
|
||||
std::vector<char> occupancy_buffer_neg;
|
||||
std::vector<double> distance_buffer_all_;
|
||||
std::vector<double> tmp_buffer1_;
|
||||
std::string world_frame_id;
|
||||
|
||||
protected:
|
||||
///Resolution
|
||||
double res_;
|
||||
///Origin, float type
|
||||
Vecf<Dim> origin_d_;
|
||||
///Dimension, int type
|
||||
Veci<Dim> dim_;
|
||||
Vecf<Dim> map_size;
|
||||
///Assume occupied cell has value 100
|
||||
int8_t val_occ = 100;
|
||||
///Assume free cell has value 0
|
||||
int8_t val_free = 0;
|
||||
///Assume unknown cell has value -1
|
||||
int8_t val_unknown = -1;
|
||||
///inflate size
|
||||
int expand_size = 1;
|
||||
bool has_map = false;
|
||||
ros::Subscriber point_cloud_sub_;
|
||||
ConfigPtr priv_config_;
|
||||
ros::Publisher esdf_pub_;
|
||||
ros::Publisher pcl_pub_;
|
||||
};
|
||||
typedef MapUtil<2> OccMapUtil;
|
||||
typedef MapUtil<3> VoxelMapUtil;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
53
trajectoryOpt/src/utils/tools/include/tools/tic_toc.hpp
Normal file
53
trajectoryOpt/src/utils/tools/include/tools/tic_toc.hpp
Normal file
@@ -0,0 +1,53 @@
|
||||
/**
|
||||
* @file tic_toc.h
|
||||
* @author HKUST Aerial Robotics Group
|
||||
* @brief
|
||||
* @version 0.1
|
||||
* @date 2019-03-17
|
||||
*
|
||||
* @copyright Copyright (c) 2019
|
||||
*/
|
||||
#ifndef _COMMON_INC_COMMON_BASICS_TIC_TOC_H_
|
||||
#define _COMMON_INC_COMMON_BASICS_TIC_TOC_H_
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdlib>
|
||||
#include <ctime>
|
||||
|
||||
class TicToc {
|
||||
public:
|
||||
TicToc() { tic(); }
|
||||
|
||||
/**
|
||||
* @brief start timing
|
||||
* @note the unit of time is in millisecond (ms)
|
||||
*/
|
||||
void tic() { start = std::chrono::system_clock::now(); }
|
||||
|
||||
/**
|
||||
* @brief get time elapsed
|
||||
* @note the unit of time is in millisecond (ms)
|
||||
*/
|
||||
double toc() {
|
||||
end = std::chrono::system_clock::now();
|
||||
std::chrono::duration<double> elapsed_seconds = end - start;
|
||||
return elapsed_seconds.count() * 1000;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert chrono::system_clock::time_point to double
|
||||
*
|
||||
* @param t
|
||||
* @return double
|
||||
*/
|
||||
static double TimePointToDouble(
|
||||
const std::chrono::system_clock::time_point& t) {
|
||||
auto tt = std::chrono::duration<double>(t.time_since_epoch());
|
||||
return tt.count();
|
||||
}
|
||||
|
||||
private:
|
||||
std::chrono::time_point<std::chrono::system_clock> start, end;
|
||||
};
|
||||
|
||||
#endif
|
||||
619
trajectoryOpt/src/utils/tools/include/tools/visualization.hpp
Executable file
619
trajectoryOpt/src/utils/tools/include/tools/visualization.hpp
Executable file
@@ -0,0 +1,619 @@
|
||||
/*
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2020 Jialin Ji ()
|
||||
2021 Hongkai Ye (kyle_yeh@163.com)
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
*/
|
||||
#pragma once
|
||||
#include <nav_msgs/Path.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <decomp_util/ellipsoid_decomp.h>
|
||||
#include <decomp_ros_utils/data_ros_utils.h>
|
||||
#include <Eigen/Geometry>
|
||||
#include <iostream>
|
||||
#include <unordered_map>
|
||||
|
||||
namespace visualization {
|
||||
struct BALL {
|
||||
Eigen::Vector3d center;
|
||||
double radius;
|
||||
BALL(const Eigen::Vector3d& c, double r) : center(c), radius(r){};
|
||||
BALL(){};
|
||||
};
|
||||
|
||||
using PublisherMap = std::unordered_map<std::string, ros::Publisher>;
|
||||
enum Color { white,
|
||||
red,
|
||||
green,
|
||||
blue,
|
||||
yellow,
|
||||
chartreuse,
|
||||
black,
|
||||
gray,
|
||||
orange,
|
||||
purple,
|
||||
pink,
|
||||
steelblue };
|
||||
|
||||
class Visualization {
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
PublisherMap publisher_map_;
|
||||
|
||||
void setMarkerColor(visualization_msgs::Marker& marker,
|
||||
Color color = blue,
|
||||
double a = 1) {
|
||||
marker.color.a = a;
|
||||
switch (color) {
|
||||
case white:
|
||||
marker.color.r = 1;
|
||||
marker.color.g = 1;
|
||||
marker.color.b = 1;
|
||||
break;
|
||||
case red:
|
||||
marker.color.r = 1;
|
||||
marker.color.g = 0;
|
||||
marker.color.b = 0;
|
||||
break;
|
||||
case green:
|
||||
marker.color.r = 0;
|
||||
marker.color.g = 1;
|
||||
marker.color.b = 0;
|
||||
break;
|
||||
case blue:
|
||||
marker.color.r = 0;
|
||||
marker.color.g = 0;
|
||||
marker.color.b = 1;
|
||||
break;
|
||||
case yellow:
|
||||
marker.color.r = 1;
|
||||
marker.color.g = 1;
|
||||
marker.color.b = 0;
|
||||
break;
|
||||
case chartreuse:
|
||||
marker.color.r = 0.5;
|
||||
marker.color.g = 1;
|
||||
marker.color.b = 0;
|
||||
break;
|
||||
case black:
|
||||
marker.color.r = 0;
|
||||
marker.color.g = 0;
|
||||
marker.color.b = 0;
|
||||
break;
|
||||
case gray:
|
||||
marker.color.r = 0.5;
|
||||
marker.color.g = 0.5;
|
||||
marker.color.b = 0.5;
|
||||
break;
|
||||
case orange:
|
||||
marker.color.r = 1;
|
||||
marker.color.g = 0.5;
|
||||
marker.color.b = 0;
|
||||
break;
|
||||
case purple:
|
||||
marker.color.r = 0.5;
|
||||
marker.color.g = 0;
|
||||
marker.color.b = 1;
|
||||
break;
|
||||
case pink:
|
||||
marker.color.r = 1;
|
||||
marker.color.g = 0;
|
||||
marker.color.b = 0.6;
|
||||
break;
|
||||
case steelblue:
|
||||
marker.color.r = 0.4;
|
||||
marker.color.g = 0.7;
|
||||
marker.color.b = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void setMarkerColor(visualization_msgs::Marker& marker,
|
||||
double a,
|
||||
double r,
|
||||
double g,
|
||||
double b) {
|
||||
marker.color.a = a;
|
||||
marker.color.r = r;
|
||||
marker.color.g = g;
|
||||
marker.color.b = b;
|
||||
}
|
||||
|
||||
void setMarkerScale(visualization_msgs::Marker& marker,
|
||||
const double& x,
|
||||
const double& y,
|
||||
const double& z) {
|
||||
marker.scale.x = x;
|
||||
marker.scale.y = y;
|
||||
marker.scale.z = z;
|
||||
}
|
||||
|
||||
void setMarkerPose(visualization_msgs::Marker& marker,
|
||||
const double& x,
|
||||
const double& y,
|
||||
const double& z) {
|
||||
marker.pose.position.x = x;
|
||||
marker.pose.position.y = y;
|
||||
marker.pose.position.z = z;
|
||||
marker.pose.orientation.w = 1;
|
||||
marker.pose.orientation.x = 0;
|
||||
marker.pose.orientation.y = 0;
|
||||
marker.pose.orientation.z = 0;
|
||||
}
|
||||
template <class ROTATION>
|
||||
void setMarkerPose(visualization_msgs::Marker& marker,
|
||||
const double& x,
|
||||
const double& y,
|
||||
const double& z,
|
||||
const ROTATION& R) {
|
||||
marker.pose.position.x = x;
|
||||
marker.pose.position.y = y;
|
||||
marker.pose.position.z = z;
|
||||
Eigen::Quaterniond r(R);
|
||||
marker.pose.orientation.w = r.w();
|
||||
marker.pose.orientation.x = r.x();
|
||||
marker.pose.orientation.y = r.y();
|
||||
marker.pose.orientation.z = r.z();
|
||||
}
|
||||
|
||||
public:
|
||||
Visualization(ros::NodeHandle& nh) : nh_(nh) {}
|
||||
|
||||
template <class CENTER, class TOPIC>
|
||||
void visualize_a_ball(const CENTER& c,
|
||||
const double& r,
|
||||
const TOPIC& topic,
|
||||
const Color color = blue,
|
||||
const double a = 1) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub = nh_.advertise<visualization_msgs::Marker>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
visualization_msgs::Marker marker;
|
||||
marker.header.frame_id = "world";
|
||||
marker.type = visualization_msgs::Marker::SPHERE;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
setMarkerColor(marker, color, a);
|
||||
setMarkerScale(marker, 2 * r, 2 * r, 2 * r);
|
||||
setMarkerPose(marker, c[0], c[1], c[2]);
|
||||
marker.header.stamp = ros::Time::now();
|
||||
publisher_map_[topic].publish(marker);
|
||||
}
|
||||
|
||||
template <class PC, class TOPIC>
|
||||
void visualize_pointcloud(const PC& pc, const TOPIC& topic) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub = nh_.advertise<sensor_msgs::PointCloud2>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
pcl::PointCloud<pcl::PointXYZ> point_cloud;
|
||||
sensor_msgs::PointCloud2 point_cloud_msg;
|
||||
point_cloud.reserve(pc.size());
|
||||
for (const auto& pt : pc) {
|
||||
if(pt.size()==3){
|
||||
point_cloud.points.emplace_back(pt[0], pt[1], pt[2]);
|
||||
}
|
||||
else{
|
||||
point_cloud.points.emplace_back(pt[0], pt[1], 0.0);
|
||||
}
|
||||
}
|
||||
pcl::toROSMsg(point_cloud, point_cloud_msg);
|
||||
point_cloud_msg.header.frame_id = "world";
|
||||
point_cloud_msg.header.stamp = ros::Time::now();
|
||||
publisher_map_[topic].publish(point_cloud_msg);
|
||||
}
|
||||
|
||||
template <class PATH, class TOPIC>
|
||||
void visualize_path(const PATH& path, const TOPIC& topic) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub = nh_.advertise<nav_msgs::Path>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
nav_msgs::Path path_msg;
|
||||
geometry_msgs::PoseStamped tmpPose;
|
||||
tmpPose.header.frame_id = "world";
|
||||
for (const auto& pt : path) {
|
||||
if(pt.size() == 3){
|
||||
tmpPose.pose.position.x = pt[0];
|
||||
tmpPose.pose.position.y = pt[1];
|
||||
tmpPose.pose.position.z = pt[2];
|
||||
}
|
||||
else if(pt.size()==2){
|
||||
tmpPose.pose.position.x = pt[0];
|
||||
tmpPose.pose.position.y = pt[1];
|
||||
tmpPose.pose.position.z = 0.0;
|
||||
}
|
||||
else{
|
||||
ROS_ERROR("Dim must be 2 or 3 ");
|
||||
}
|
||||
path_msg.poses.push_back(tmpPose);
|
||||
}
|
||||
path_msg.header.frame_id = "world";
|
||||
path_msg.header.stamp = ros::Time::now();
|
||||
publisher_map_[topic].publish(path_msg);
|
||||
}
|
||||
|
||||
template <class BALLS, class TOPIC>
|
||||
void visualize_balls(const BALLS& balls,
|
||||
const TOPIC& topic,
|
||||
const Color color = blue,
|
||||
const double a = 0.2) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub =
|
||||
nh_.advertise<visualization_msgs::MarkerArray>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
visualization_msgs::Marker marker;
|
||||
marker.header.frame_id = "world";
|
||||
marker.type = visualization_msgs::Marker::SPHERE;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
marker.id = 0;
|
||||
setMarkerColor(marker, color, a);
|
||||
visualization_msgs::MarkerArray marker_array;
|
||||
marker_array.markers.reserve(balls.size() + 1);
|
||||
marker.action = visualization_msgs::Marker::DELETEALL;
|
||||
marker_array.markers.push_back(marker);
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
for (const auto& ball : balls) {
|
||||
setMarkerPose(marker, ball.center[0], ball.center[1], ball.center[2]);
|
||||
auto d = 2 * ball.radius;
|
||||
setMarkerScale(marker, d, d, d);
|
||||
marker_array.markers.push_back(marker);
|
||||
marker.id++;
|
||||
}
|
||||
publisher_map_[topic].publish(marker_array);
|
||||
}
|
||||
|
||||
template <class ELLIPSOID, class TOPIC>
|
||||
void visualize_ellipsoids(const ELLIPSOID& ellipsoids,
|
||||
const TOPIC& topic,
|
||||
const Color color = blue,
|
||||
const double a = 0.2) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub =
|
||||
nh_.advertise<visualization_msgs::MarkerArray>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
visualization_msgs::Marker marker;
|
||||
marker.header.frame_id = "world";
|
||||
marker.type = visualization_msgs::Marker::SPHERE;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
marker.id = 0;
|
||||
setMarkerColor(marker, color, a);
|
||||
visualization_msgs::MarkerArray marker_array;
|
||||
marker_array.markers.reserve(ellipsoids.size() + 1);
|
||||
marker.action = visualization_msgs::Marker::DELETEALL;
|
||||
marker_array.markers.push_back(marker);
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
for (const auto& e : ellipsoids) {
|
||||
setMarkerPose(marker, e.c[0], e.c[1], e.c[2], e.R);
|
||||
setMarkerScale(marker, 2 * e.rx, 2 * e.ry, 2 * e.rz);
|
||||
marker_array.markers.push_back(marker);
|
||||
marker.id++;
|
||||
}
|
||||
publisher_map_[topic].publish(marker_array);
|
||||
}
|
||||
|
||||
template <class PAIRLINE, class TOPIC>
|
||||
// eg for PAIRLINE: std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>
|
||||
void visualize_pairline(const PAIRLINE& pairline, const TOPIC& topic, const Color& color = green, double scale = 0.1) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub = nh_.advertise<visualization_msgs::Marker>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
visualization_msgs::Marker marker;
|
||||
marker.header.frame_id = "world";
|
||||
marker.type = visualization_msgs::Marker::LINE_LIST;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
setMarkerPose(marker, 0, 0, 0);
|
||||
setMarkerColor(marker, color, 1);
|
||||
setMarkerScale(marker, scale, scale, scale);
|
||||
marker.points.resize(2 * pairline.size());
|
||||
for (size_t i = 0; i < pairline.size(); ++i) {
|
||||
marker.points[2 * i + 0].x = pairline[i].first[0];
|
||||
marker.points[2 * i + 0].y = pairline[i].first[1];
|
||||
marker.points[2 * i + 0].z = pairline[i].first[2];
|
||||
marker.points[2 * i + 1].x = pairline[i].second[0];
|
||||
marker.points[2 * i + 1].y = pairline[i].second[1];
|
||||
marker.points[2 * i + 1].z = pairline[i].second[2];
|
||||
}
|
||||
publisher_map_[topic].publish(marker);
|
||||
}
|
||||
|
||||
template <class ARROWS, class TOPIC>
|
||||
// ARROWS: pair<Vector3d, Vector3d>
|
||||
void visualize_arrows(const ARROWS& arrows, const TOPIC& topic, const Color& color) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub =
|
||||
nh_.advertise<visualization_msgs::MarkerArray>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
visualization_msgs::Marker clear_previous_msg;
|
||||
clear_previous_msg.action = visualization_msgs::Marker::DELETEALL;
|
||||
visualization_msgs::Marker arrow_msg;
|
||||
arrow_msg.type = visualization_msgs::Marker::ARROW;
|
||||
arrow_msg.action = visualization_msgs::Marker::ADD;
|
||||
arrow_msg.header.frame_id = "world";
|
||||
arrow_msg.id = 0;
|
||||
arrow_msg.points.resize(2);
|
||||
setMarkerPose(arrow_msg, 0, 0, 0);
|
||||
setMarkerScale(arrow_msg, 0.02, 0.12, 0);
|
||||
setMarkerColor(arrow_msg, color, 0.7);
|
||||
visualization_msgs::MarkerArray arrow_list_msg;
|
||||
arrow_list_msg.markers.reserve(1 + arrows.size());
|
||||
arrow_list_msg.markers.push_back(clear_previous_msg);
|
||||
for (const auto& arrow : arrows) {
|
||||
if(arrow.first.size()==3){
|
||||
arrow_msg.points[0].x = arrow.first[0];
|
||||
arrow_msg.points[0].y = arrow.first[1];
|
||||
arrow_msg.points[0].z = arrow.first[2];
|
||||
arrow_msg.points[1].x = arrow.second[0];
|
||||
arrow_msg.points[1].y = arrow.second[1];
|
||||
arrow_msg.points[1].z = arrow.second[2];
|
||||
}
|
||||
else{
|
||||
arrow_msg.points[0].x = arrow.first[0];
|
||||
arrow_msg.points[0].y = arrow.first[1];
|
||||
arrow_msg.points[0].z = 4.0;
|
||||
arrow_msg.points[1].x = arrow.second[0];
|
||||
arrow_msg.points[1].y = arrow.second[1];
|
||||
arrow_msg.points[1].z = 4.0;
|
||||
}
|
||||
arrow_list_msg.markers.push_back(arrow_msg);
|
||||
arrow_msg.id += 1;
|
||||
}
|
||||
publisher_map_[topic].publish(arrow_list_msg);
|
||||
}
|
||||
|
||||
template <class TRAJ, class TOPIC>
|
||||
// TRAJ:
|
||||
void visualize_traj(const TRAJ& traj, const TOPIC& topic) {
|
||||
std::vector<Eigen::Vector3d> path;
|
||||
auto duration = traj.getTotalDuration();
|
||||
for (double t = 0; t < duration; t += 0.01) {
|
||||
Eigen::VectorXd pos = traj.getPos(t);
|
||||
if(pos.size()==3){
|
||||
Eigen::Vector3d p3;
|
||||
p3 << pos;
|
||||
path.push_back(p3);
|
||||
}
|
||||
else if (pos.size()==2){
|
||||
Eigen::Vector3d p3;
|
||||
p3 << pos, 0.0;
|
||||
path.push_back(p3);
|
||||
}
|
||||
else{
|
||||
ROS_ERROR("Dim must be 3 or 2!");
|
||||
}
|
||||
}
|
||||
visualize_path(path, topic);
|
||||
// std::vector<Eigen::Vector3d> wayPts;
|
||||
// for (const auto& piece : traj) {
|
||||
// wayPts.push_back(piece.getPos(0));
|
||||
// }
|
||||
// visualize_pointcloud(wayPts, std::string(topic) + "_wayPts");
|
||||
}
|
||||
|
||||
template <class TRAJ, class TOPIC>
|
||||
// TRAJ:
|
||||
void visualize_se2traj(const TRAJ& traj, const TOPIC& topic) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub = nh_.advertise<visualization_msgs::Marker>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
visualization_msgs::Marker carMarkers;
|
||||
auto duration = traj.getTotalDuration();
|
||||
for (double t = 0; t < duration; t += 0.3) {
|
||||
Eigen::Vector2d pos = traj.getPos(t);
|
||||
double yaw = traj.getYaw(t);
|
||||
carMarkers.action = visualization_msgs::Marker::ADD;
|
||||
carMarkers.id = 0;
|
||||
carMarkers.type = visualization_msgs::Marker::LINE_LIST;
|
||||
carMarkers.pose.orientation.w = 1.00;
|
||||
carMarkers.ns = "se2traj";
|
||||
carMarkers.color.r = 0.00;
|
||||
carMarkers.color.g = 0.00;
|
||||
carMarkers.color.b = 0.5;
|
||||
carMarkers.color.a = 1.00;
|
||||
carMarkers.scale.x = 0.05;
|
||||
carMarkers.header.frame_id = "world";
|
||||
geometry_msgs::Point point1;
|
||||
geometry_msgs::Point point2;
|
||||
geometry_msgs::Point point3;
|
||||
geometry_msgs::Point point4;
|
||||
Eigen::Matrix2d R;
|
||||
R << cos(yaw),-sin(yaw),
|
||||
sin(yaw),cos(yaw);
|
||||
Eigen::Vector2d offset1, tmp1;
|
||||
offset1 = R*Eigen::Vector2d(0.68,0.23);
|
||||
tmp1 = pos+offset1;
|
||||
point1.x = tmp1[0];
|
||||
point1.y = tmp1[1];
|
||||
point1.z = 0;
|
||||
Eigen::Vector2d offset2, tmp2;
|
||||
offset2 = R*Eigen::Vector2d(0.68, -0.23);
|
||||
tmp2 = pos+offset2;
|
||||
point2.x = tmp2[0];
|
||||
point2.y = tmp2[1];
|
||||
point2.z = 0;
|
||||
Eigen::Vector2d offset3, tmp3;
|
||||
offset3 = R*Eigen::Vector2d(-0.08, -0.23);
|
||||
tmp3 = pos+offset3;
|
||||
point3.x = tmp3[0];
|
||||
point3.y = tmp3[1];
|
||||
point3.z = 0;
|
||||
Eigen::Vector2d offset4, tmp4;
|
||||
offset4 = R*Eigen::Vector2d(-0.08, 0.23);
|
||||
tmp4 = pos+offset4;
|
||||
point4.x = tmp4[0];
|
||||
point4.y = tmp4[1];
|
||||
point4.z = 0;
|
||||
carMarkers.points.push_back(point1);
|
||||
carMarkers.points.push_back(point2);
|
||||
carMarkers.points.push_back(point2);
|
||||
carMarkers.points.push_back(point3);
|
||||
carMarkers.points.push_back(point3);
|
||||
carMarkers.points.push_back(point4);
|
||||
carMarkers.points.push_back(point4);
|
||||
carMarkers.points.push_back(point1);
|
||||
|
||||
}
|
||||
carMarkers.header.stamp = ros::Time::now();
|
||||
publisher_map_[topic].publish(carMarkers);
|
||||
// std::vector<Eigen::Vector3d> wayPts;
|
||||
// for (const auto& piece : traj) {
|
||||
// wayPts.push_back(piece.getPos(0));
|
||||
// }
|
||||
// visualize_pointcloud(wayPts, std::string(topic) + "_wayPts");
|
||||
}
|
||||
|
||||
template <class TRAJLIST, class TOPIC>
|
||||
// TRAJLIST: std::vector<TRAJ>
|
||||
void visualize_traj_list(const TRAJLIST& traj_list, const TOPIC& topic,
|
||||
const Color color = blue, double scale = 0.1) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub =
|
||||
nh_.advertise<visualization_msgs::MarkerArray>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
visualization_msgs::Marker clear_previous_msg;
|
||||
clear_previous_msg.action = visualization_msgs::Marker::DELETEALL;
|
||||
visualization_msgs::Marker path_msg;
|
||||
path_msg.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
path_msg.action = visualization_msgs::Marker::ADD;
|
||||
path_msg.header.frame_id = "world";
|
||||
path_msg.id = 0;
|
||||
setMarkerPose(path_msg, 0, 0, 0);
|
||||
setMarkerScale(path_msg, scale, scale, scale);
|
||||
visualization_msgs::MarkerArray path_list_msg;
|
||||
path_list_msg.markers.reserve(1 + traj_list.size());
|
||||
path_list_msg.markers.push_back(clear_previous_msg);
|
||||
// double a_step = 0.8 / traj_list.size();
|
||||
double a = 1.0;
|
||||
geometry_msgs::Point p_msg;
|
||||
for (const auto& traj : traj_list) {
|
||||
setMarkerColor(path_msg, color, a);
|
||||
// a = a + a_step;
|
||||
path_msg.points.clear();
|
||||
for (double t = 0; t < traj.getDuration(); t += 0.01) {
|
||||
auto p = traj.getSigma(t);
|
||||
if(p.size()==3){
|
||||
p_msg.x = p[0];
|
||||
p_msg.y = p[1];
|
||||
p_msg.z = p[2];
|
||||
}
|
||||
else{
|
||||
p_msg.x = p[0];
|
||||
p_msg.y = p[1];
|
||||
p_msg.z = 0.0;
|
||||
|
||||
}
|
||||
path_msg.points.push_back(p_msg);
|
||||
}
|
||||
path_list_msg.markers.push_back(path_msg);
|
||||
path_msg.id += 1;
|
||||
}
|
||||
publisher_map_[topic].publish(path_list_msg);
|
||||
}
|
||||
|
||||
template <class PATHLIST, class TOPIC>
|
||||
// PATHLIST: std::vector<PATH>
|
||||
void visualize_path_list(const PATHLIST& path_list, const TOPIC& topic,
|
||||
const Color color = steelblue, double scale = 0.1) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub =
|
||||
nh_.advertise<visualization_msgs::MarkerArray>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
visualization_msgs::Marker clear_previous_msg;
|
||||
clear_previous_msg.action = visualization_msgs::Marker::DELETEALL;
|
||||
visualization_msgs::Marker path_msg;
|
||||
path_msg.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
path_msg.action = visualization_msgs::Marker::ADD;
|
||||
path_msg.header.frame_id = "world";
|
||||
path_msg.id = 0;
|
||||
setMarkerPose(path_msg, 0, 0, 0);
|
||||
setMarkerScale(path_msg, scale, scale, scale);
|
||||
visualization_msgs::MarkerArray path_list_msg;
|
||||
path_list_msg.markers.reserve(1 + path_list.size());
|
||||
path_list_msg.markers.push_back(clear_previous_msg);
|
||||
setMarkerColor(path_msg, color);
|
||||
for (const auto& path : path_list) {
|
||||
path_msg.points.resize(path.size());
|
||||
for (size_t i = 0; i < path.size(); ++i) {
|
||||
path_msg.points[i].x = path[i].x();
|
||||
path_msg.points[i].y = path[i].y();
|
||||
path_msg.points[i].z = path[i].z();
|
||||
}
|
||||
path_list_msg.markers.push_back(path_msg);
|
||||
path_msg.id += 1;
|
||||
}
|
||||
publisher_map_[topic].publish(path_list_msg);
|
||||
}
|
||||
template <class SFCS, class TOPIC>
|
||||
// SFCS: std::vector<Eigen::MatrixXd>
|
||||
void visualize_sfc(const SFCS& hPolys, const TOPIC& topic) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub =
|
||||
nh_.advertise<decomp_ros_msgs::PolyhedronArray>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
vec_E<Polyhedron2D> polyhedra;
|
||||
polyhedra.reserve(hPolys.size());
|
||||
for (const Eigen::MatrixXd &ele : hPolys){
|
||||
Polyhedron2D hPoly;
|
||||
for (int i = 0; i < ele.cols(); i++){
|
||||
hPoly.add(Hyperplane2D(ele.col(i).tail<2>(), ele.col(i).head<2>()));
|
||||
}
|
||||
polyhedra.push_back(hPoly);
|
||||
}
|
||||
|
||||
decomp_ros_msgs::PolyhedronArray poly_msg = DecompROS::polyhedron_array_to_ros(polyhedra);
|
||||
poly_msg.header.frame_id = "world";
|
||||
poly_msg.header.stamp = ros::Time::now();
|
||||
publisher_map_[topic].publish(poly_msg);
|
||||
}
|
||||
|
||||
template <class TOPIC_TYPE, class TOPIC>
|
||||
void registe(const TOPIC& topic) {
|
||||
auto got = publisher_map_.find(topic);
|
||||
if (got == publisher_map_.end()) {
|
||||
ros::Publisher pub = nh_.advertise<TOPIC_TYPE>(topic, 10);
|
||||
publisher_map_[topic] = pub;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace visualization
|
||||
34
trajectoryOpt/src/utils/tools/package.xml
Normal file
34
trajectoryOpt/src/utils/tools/package.xml
Normal file
@@ -0,0 +1,34 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>tools</name>
|
||||
|
||||
<maintainer email="zhichaohan@zju.edu.cn">zhichaohan</maintainer>
|
||||
|
||||
<version>0.1.0</version>
|
||||
<description> useful tools</description>
|
||||
<license>GPLV3</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>visualization_msgs</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
<build_depend>cv_bridge</build_depend>
|
||||
<build_depend>decomp_ros_utils</build_depend>
|
||||
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>visualization_msgs</run_depend>
|
||||
<run_depend>tf</run_depend>
|
||||
<run_depend>cv_bridge</run_depend>
|
||||
<run_depend>decomp_ros_utils</run_depend>
|
||||
|
||||
|
||||
|
||||
</package>
|
||||
|
||||
Reference in New Issue
Block a user