将多模态算法集成到无人机内部的 Prometheus 中 #13
Merged
p5vjqsb6e
merged 4 commits from luogang_branch into master 1 week ago
@ -1,204 +0,0 @@
|
||||
{
|
||||
"name": "智途投送软件系统",
|
||||
"lockfileVersion": 3,
|
||||
"requires": true,
|
||||
"packages": {
|
||||
"": {
|
||||
"dependencies": {
|
||||
"docx": "^9.6.1"
|
||||
}
|
||||
},
|
||||
"node_modules/@types/node": {
|
||||
"version": "25.6.0",
|
||||
"resolved": "https://registry.npmjs.org/@types/node/-/node-25.6.0.tgz",
|
||||
"integrity": "sha512-+qIYRKdNYJwY3vRCZMdJbPLJAtGjQBudzZzdzwQYkEPQd+PJGixUL5QfvCLDaULoLv+RhT3LDkwEfKaAkgSmNQ==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"undici-types": "~7.19.0"
|
||||
}
|
||||
},
|
||||
"node_modules/core-util-is": {
|
||||
"version": "1.0.3",
|
||||
"resolved": "https://registry.npmjs.org/core-util-is/-/core-util-is-1.0.3.tgz",
|
||||
"integrity": "sha512-ZQBvi1DcpJ4GDqanjucZ2Hj3wEO5pZDS89BWbkcrvdxksJorwUDDZamX9ldFkp9aw2lmBDLgkObEA4DWNJ9FYQ==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/docx": {
|
||||
"version": "9.6.1",
|
||||
"resolved": "https://registry.npmjs.org/docx/-/docx-9.6.1.tgz",
|
||||
"integrity": "sha512-ZJja9/KBUuFC109sCMzovoq2GR2wCG/AuxivjA+OHj/q0TEgJIm3S7yrlUxIy3B+bV8YDj/BiHfWyrRFmyWpDQ==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"@types/node": "^25.2.3",
|
||||
"hash.js": "^1.1.7",
|
||||
"jszip": "^3.10.1",
|
||||
"nanoid": "^5.1.3",
|
||||
"xml": "^1.0.1",
|
||||
"xml-js": "^1.6.8"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">=10"
|
||||
}
|
||||
},
|
||||
"node_modules/hash.js": {
|
||||
"version": "1.1.7",
|
||||
"resolved": "https://registry.npmjs.org/hash.js/-/hash.js-1.1.7.tgz",
|
||||
"integrity": "sha512-taOaskGt4z4SOANNseOviYDvjEJinIkRgmp7LbKP2YTTmVxWBl87s/uzK9r+44BclBSp2X7K1hqeNfz9JbBeXA==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"inherits": "^2.0.3",
|
||||
"minimalistic-assert": "^1.0.1"
|
||||
}
|
||||
},
|
||||
"node_modules/immediate": {
|
||||
"version": "3.0.6",
|
||||
"resolved": "https://registry.npmjs.org/immediate/-/immediate-3.0.6.tgz",
|
||||
"integrity": "sha512-XXOFtyqDjNDAQxVfYxuF7g9Il/IbWmmlQg2MYKOH8ExIT1qg6xc4zyS3HaEEATgs1btfzxq15ciUiY7gjSXRGQ==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/inherits": {
|
||||
"version": "2.0.4",
|
||||
"resolved": "https://registry.npmjs.org/inherits/-/inherits-2.0.4.tgz",
|
||||
"integrity": "sha512-k/vGaX4/Yla3WzyMCvTQOXYeIHvqOKtnqBduzTHpzpQZzAskKMhZ2K+EnBiSM9zGSoIFeMpXKxa4dYeZIQqewQ==",
|
||||
"license": "ISC"
|
||||
},
|
||||
"node_modules/isarray": {
|
||||
"version": "1.0.0",
|
||||
"resolved": "https://registry.npmjs.org/isarray/-/isarray-1.0.0.tgz",
|
||||
"integrity": "sha512-VLghIWNM6ELQzo7zwmcg0NmTVyWKYjvIeM83yjp0wRDTmUnrM678fQbcKBo6n2CJEF0szoG//ytg+TKla89ALQ==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/jszip": {
|
||||
"version": "3.10.1",
|
||||
"resolved": "https://registry.npmjs.org/jszip/-/jszip-3.10.1.tgz",
|
||||
"integrity": "sha512-xXDvecyTpGLrqFrvkrUSoxxfJI5AH7U8zxxtVclpsUtMCq4JQ290LY8AW5c7Ggnr/Y/oK+bQMbqK2qmtk3pN4g==",
|
||||
"license": "(MIT OR GPL-3.0-or-later)",
|
||||
"dependencies": {
|
||||
"lie": "~3.3.0",
|
||||
"pako": "~1.0.2",
|
||||
"readable-stream": "~2.3.6",
|
||||
"setimmediate": "^1.0.5"
|
||||
}
|
||||
},
|
||||
"node_modules/lie": {
|
||||
"version": "3.3.0",
|
||||
"resolved": "https://registry.npmjs.org/lie/-/lie-3.3.0.tgz",
|
||||
"integrity": "sha512-UaiMJzeWRlEujzAuw5LokY1L5ecNQYZKfmyZ9L7wDHb/p5etKaxXhohBcrw0EYby+G/NA52vRSN4N39dxHAIwQ==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"immediate": "~3.0.5"
|
||||
}
|
||||
},
|
||||
"node_modules/minimalistic-assert": {
|
||||
"version": "1.0.1",
|
||||
"resolved": "https://registry.npmjs.org/minimalistic-assert/-/minimalistic-assert-1.0.1.tgz",
|
||||
"integrity": "sha512-UtJcAD4yEaGtjPezWuO9wC4nwUnVH/8/Im3yEHQP4b67cXlD/Qr9hdITCU1xDbSEXg2XKNaP8jsReV7vQd00/A==",
|
||||
"license": "ISC"
|
||||
},
|
||||
"node_modules/nanoid": {
|
||||
"version": "5.1.9",
|
||||
"resolved": "https://registry.npmjs.org/nanoid/-/nanoid-5.1.9.tgz",
|
||||
"integrity": "sha512-ZUvP7KeBLe3OZ1ypw6dI/TzYJuvHP77IM4Ry73waSQTLn8/g8rpdjfyVAh7t1/+FjBtG4lCP42MEbDxOsRpBMw==",
|
||||
"funding": [
|
||||
{
|
||||
"type": "github",
|
||||
"url": "https://github.com/sponsors/ai"
|
||||
}
|
||||
],
|
||||
"license": "MIT",
|
||||
"bin": {
|
||||
"nanoid": "bin/nanoid.js"
|
||||
},
|
||||
"engines": {
|
||||
"node": "^18 || >=20"
|
||||
}
|
||||
},
|
||||
"node_modules/pako": {
|
||||
"version": "1.0.11",
|
||||
"resolved": "https://registry.npmjs.org/pako/-/pako-1.0.11.tgz",
|
||||
"integrity": "sha512-4hLB8Py4zZce5s4yd9XzopqwVv/yGNhV1Bl8NTmCq1763HeK2+EwVTv+leGeL13Dnh2wfbqowVPXCIO0z4taYw==",
|
||||
"license": "(MIT AND Zlib)"
|
||||
},
|
||||
"node_modules/process-nextick-args": {
|
||||
"version": "2.0.1",
|
||||
"resolved": "https://registry.npmjs.org/process-nextick-args/-/process-nextick-args-2.0.1.tgz",
|
||||
"integrity": "sha512-3ouUOpQhtgrbOa17J7+uxOTpITYWaGP7/AhoR3+A+/1e9skrzelGi/dXzEYyvbxubEF6Wn2ypscTKiKJFFn1ag==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/readable-stream": {
|
||||
"version": "2.3.8",
|
||||
"resolved": "https://registry.npmjs.org/readable-stream/-/readable-stream-2.3.8.tgz",
|
||||
"integrity": "sha512-8p0AUk4XODgIewSi0l8Epjs+EVnWiK7NoDIEGU0HhE7+ZyY8D1IMY7odu5lRrFXGg71L15KG8QrPmum45RTtdA==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"core-util-is": "~1.0.0",
|
||||
"inherits": "~2.0.3",
|
||||
"isarray": "~1.0.0",
|
||||
"process-nextick-args": "~2.0.0",
|
||||
"safe-buffer": "~5.1.1",
|
||||
"string_decoder": "~1.1.1",
|
||||
"util-deprecate": "~1.0.1"
|
||||
}
|
||||
},
|
||||
"node_modules/safe-buffer": {
|
||||
"version": "5.1.2",
|
||||
"resolved": "https://registry.npmjs.org/safe-buffer/-/safe-buffer-5.1.2.tgz",
|
||||
"integrity": "sha512-Gd2UZBJDkXlY7GbJxfsE8/nvKkUEU1G38c1siN6QP6a9PT9MmHB8GnpscSmMJSoF8LOIrt8ud/wPtojys4G6+g==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/sax": {
|
||||
"version": "1.6.0",
|
||||
"resolved": "https://registry.npmjs.org/sax/-/sax-1.6.0.tgz",
|
||||
"integrity": "sha512-6R3J5M4AcbtLUdZmRv2SygeVaM7IhrLXu9BmnOGmmACak8fiUtOsYNWUS4uK7upbmHIBbLBeFeI//477BKLBzA==",
|
||||
"license": "BlueOak-1.0.0",
|
||||
"engines": {
|
||||
"node": ">=11.0.0"
|
||||
}
|
||||
},
|
||||
"node_modules/setimmediate": {
|
||||
"version": "1.0.5",
|
||||
"resolved": "https://registry.npmjs.org/setimmediate/-/setimmediate-1.0.5.tgz",
|
||||
"integrity": "sha512-MATJdZp8sLqDl/68LfQmbP8zKPLQNV6BIZoIgrscFDQ+RsvK/BxeDQOgyxKKoh0y/8h3BqVFnCqQ/gd+reiIXA==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/string_decoder": {
|
||||
"version": "1.1.1",
|
||||
"resolved": "https://registry.npmjs.org/string_decoder/-/string_decoder-1.1.1.tgz",
|
||||
"integrity": "sha512-n/ShnvDi6FHbbVfviro+WojiFzv+s8MPMHBczVePfUpDJLwoLT0ht1l4YwBCbi8pJAveEEdnkHyPyTP/mzRfwg==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"safe-buffer": "~5.1.0"
|
||||
}
|
||||
},
|
||||
"node_modules/undici-types": {
|
||||
"version": "7.19.2",
|
||||
"resolved": "https://registry.npmjs.org/undici-types/-/undici-types-7.19.2.tgz",
|
||||
"integrity": "sha512-qYVnV5OEm2AW8cJMCpdV20CDyaN3g0AjDlOGf1OW4iaDEx8MwdtChUp4zu4H0VP3nDRF/8RKWH+IPp9uW0YGZg==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/util-deprecate": {
|
||||
"version": "1.0.2",
|
||||
"resolved": "https://registry.npmjs.org/util-deprecate/-/util-deprecate-1.0.2.tgz",
|
||||
"integrity": "sha512-EPD5q1uXyFxJpCrLnCc1nHnq3gOa6DZBocAIiI2TaSCA7VCJ1UJDMagCzIkXNsUYfD1daK//LTEQ8xiIbrHtcw==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/xml": {
|
||||
"version": "1.0.1",
|
||||
"resolved": "https://registry.npmjs.org/xml/-/xml-1.0.1.tgz",
|
||||
"integrity": "sha512-huCv9IH9Tcf95zuYCsQraZtWnJvBtLVE0QHMOs8bWyZAFZNDcYjsPq1nEx8jKA9y+Beo9v+7OBPRisQTjinQMw==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/xml-js": {
|
||||
"version": "1.6.11",
|
||||
"resolved": "https://registry.npmjs.org/xml-js/-/xml-js-1.6.11.tgz",
|
||||
"integrity": "sha512-7rVi2KMfwfWFl+GpPg6m80IVMWXLRjO+PxTq7V2CDhoGak0wzYzFgUY2m4XJ47OGdXd8eLE8EmwfAmdjw7lC1g==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"sax": "^1.2.4"
|
||||
},
|
||||
"bin": {
|
||||
"xml-js": "bin/cli.js"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,5 +0,0 @@
|
||||
{
|
||||
"dependencies": {
|
||||
"docx": "^9.6.1"
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,69 @@
|
||||
# toplevel CMakeLists.txt for a catkin workspace
|
||||
# catkin/cmake/toplevel.cmake
|
||||
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
|
||||
project(Project)
|
||||
|
||||
set(CATKIN_TOPLEVEL TRUE)
|
||||
|
||||
# search for catkin within the workspace
|
||||
set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
|
||||
execute_process(COMMAND ${_cmd}
|
||||
RESULT_VARIABLE _res
|
||||
OUTPUT_VARIABLE _out
|
||||
ERROR_VARIABLE _err
|
||||
OUTPUT_STRIP_TRAILING_WHITESPACE
|
||||
ERROR_STRIP_TRAILING_WHITESPACE
|
||||
)
|
||||
if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
|
||||
# searching for catkin resulted in an error
|
||||
string(REPLACE ";" " " _cmd_str "${_cmd}")
|
||||
message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
|
||||
endif()
|
||||
|
||||
# include catkin from workspace or via find_package()
|
||||
if(_res EQUAL 0)
|
||||
set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
|
||||
# include all.cmake without add_subdirectory to let it operate in same scope
|
||||
include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
|
||||
add_subdirectory("${_out}")
|
||||
|
||||
else()
|
||||
# use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
|
||||
# or CMAKE_PREFIX_PATH from the environment
|
||||
if(NOT DEFINED CMAKE_PREFIX_PATH)
|
||||
if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
|
||||
if(NOT WIN32)
|
||||
string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
|
||||
else()
|
||||
set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
|
||||
endif()
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# list of catkin workspaces
|
||||
set(catkin_search_path "")
|
||||
foreach(path ${CMAKE_PREFIX_PATH})
|
||||
if(EXISTS "${path}/.catkin")
|
||||
list(FIND catkin_search_path ${path} _index)
|
||||
if(_index EQUAL -1)
|
||||
list(APPEND catkin_search_path ${path})
|
||||
endif()
|
||||
endif()
|
||||
endforeach()
|
||||
|
||||
# search for catkin in all workspaces
|
||||
set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
|
||||
find_package(catkin QUIET
|
||||
NO_POLICY_SCOPE
|
||||
PATHS ${catkin_search_path}
|
||||
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
|
||||
unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
|
||||
|
||||
if(NOT catkin_FOUND)
|
||||
message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
catkin_workspace()
|
||||
@ -0,0 +1,58 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(prometheus_global_planner)
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL 1.7 REQUIRED)
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
laser_geometry
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
pcl_ros
|
||||
visualization_msgs
|
||||
gazebo_msgs
|
||||
prometheus_msgs
|
||||
)
|
||||
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
)
|
||||
|
||||
include_directories(
|
||||
SYSTEM
|
||||
include
|
||||
${PROJECT_SOURCE_DIR}/include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Eigen3_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${PROJECT_SOURCE_DIR}/../../common/include
|
||||
)
|
||||
link_directories(${PCL_LIBRARY_DIRS})
|
||||
|
||||
add_library(occupy_map src/occupy_map.cpp)
|
||||
target_link_libraries(occupy_map ${PCL_LIBRARIES})
|
||||
add_library(A_star src/A_star.cpp)
|
||||
target_link_libraries(A_star occupy_map)
|
||||
add_library(global_planner src/global_planner.cpp)
|
||||
target_link_libraries(global_planner A_star)
|
||||
|
||||
add_executable(global_planner_main src/global_planner_node.cpp)
|
||||
add_dependencies(global_planner_main prometheus_global_planner_gencpp)
|
||||
target_link_libraries(global_planner_main ${catkin_LIBRARIES})
|
||||
target_link_libraries(global_planner_main ${PCL_LIBRARIES})
|
||||
target_link_libraries(global_planner_main global_planner)
|
||||
|
||||
## Mark cpp header files for installation
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
|
||||
)
|
||||
@ -0,0 +1,181 @@
|
||||
#ifndef _ASTAR_H
|
||||
#define _ASTAR_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
#include <queue>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <sstream>
|
||||
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
|
||||
#include "occupy_map.h"
|
||||
#include "printf_utils.h"
|
||||
#include "global_planner_utils.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
|
||||
#define IN_CLOSE_SET 'a'
|
||||
#define IN_OPEN_SET 'b'
|
||||
#define NOT_EXPAND 'c'
|
||||
#define inf 1 >> 30
|
||||
|
||||
class Node
|
||||
{
|
||||
public:
|
||||
/* -------------------- */
|
||||
Eigen::Vector3i index;
|
||||
Eigen::Vector3d position;
|
||||
double g_score, f_score;
|
||||
Node *parent;
|
||||
char node_state;
|
||||
|
||||
double time; // dyn
|
||||
int time_idx;
|
||||
|
||||
Node()
|
||||
{
|
||||
parent = NULL;
|
||||
node_state = NOT_EXPAND;
|
||||
}
|
||||
~Node(){};
|
||||
};
|
||||
typedef Node *NodePtr;
|
||||
|
||||
// 为什么这么麻烦，不能直接比较吗
|
||||
class NodeComparator0
|
||||
{
|
||||
public:
|
||||
bool operator()(NodePtr node1, NodePtr node2)
|
||||
{
|
||||
return node1->f_score > node2->f_score;
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct matrix_hash0 : std::unary_function<T, size_t>
|
||||
{
|
||||
std::size_t operator()(T const &matrix) const
|
||||
{
|
||||
size_t seed = 0;
|
||||
for (size_t i = 0; i < matrix.size(); ++i)
|
||||
{
|
||||
auto elem = *(matrix.data() + i);
|
||||
seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
|
||||
}
|
||||
return seed;
|
||||
}
|
||||
};
|
||||
|
||||
class NodeHashTable0
|
||||
{
|
||||
private:
|
||||
/* data */
|
||||
std::unordered_map<Eigen::Vector3i, NodePtr, matrix_hash0<Eigen::Vector3i>> data_3d_;
|
||||
|
||||
public:
|
||||
NodeHashTable0(/* args */)
|
||||
{
|
||||
}
|
||||
~NodeHashTable0()
|
||||
{
|
||||
}
|
||||
void insert(Eigen::Vector3i idx, NodePtr node)
|
||||
{
|
||||
data_3d_.insert(std::make_pair(idx, node));
|
||||
}
|
||||
|
||||
NodePtr find(Eigen::Vector3i idx)
|
||||
{
|
||||
auto iter = data_3d_.find(idx);
|
||||
return iter == data_3d_.end() ? NULL : iter->second;
|
||||
}
|
||||
|
||||
void clear()
|
||||
{
|
||||
data_3d_.clear();
|
||||
}
|
||||
};
|
||||
|
||||
class Astar
|
||||
{
|
||||
private:
|
||||
// 备选路径点指针容器
|
||||
std::vector<NodePtr> path_node_pool_;
|
||||
// 使用节点计数器、迭代次数计数器
|
||||
int use_node_num_, iter_num_;
|
||||
// 扩展的节点
|
||||
NodeHashTable0 expanded_nodes_;
|
||||
// open set （根据规则已排序好）
|
||||
std::priority_queue<NodePtr, std::vector<NodePtr>, NodeComparator0> open_set_;
|
||||
// 最终路径点容器
|
||||
std::vector<NodePtr> path_nodes_;
|
||||
|
||||
// 参数
|
||||
// 启发式参数
|
||||
double lambda_heu_;
|
||||
double lambda_cost_;
|
||||
// 最大搜索次数
|
||||
int max_search_num;
|
||||
// tie breaker
|
||||
double tie_breaker_;
|
||||
bool is_2D;
|
||||
double fly_height;
|
||||
|
||||
// 目标点
|
||||
Eigen::Vector3d goal_pos;
|
||||
|
||||
// 地图相关
|
||||
std::vector<int> occupancy_buffer_;
|
||||
double resolution_, inv_resolution_;
|
||||
Eigen::Vector3d origin_, map_size_3d_;
|
||||
|
||||
// 辅助函数
|
||||
Eigen::Vector3i posToIndex(Eigen::Vector3d pt);
|
||||
void indexToPos(Eigen::Vector3i id, Eigen::Vector3d &pos);
|
||||
void retrievePath(NodePtr end_node);
|
||||
|
||||
// 搜索启发函数，三种形式，选用其中一种即可
|
||||
double getDiagHeu(Eigen::Vector3d x1, Eigen::Vector3d x2);
|
||||
double getManhHeu(Eigen::Vector3d x1, Eigen::Vector3d x2);
|
||||
double getEuclHeu(Eigen::Vector3d x1, Eigen::Vector3d x2);
|
||||
|
||||
public:
|
||||
Astar(){}
|
||||
~Astar();
|
||||
|
||||
enum
|
||||
{
|
||||
REACH_END = 1,
|
||||
NO_PATH = 2
|
||||
};
|
||||
|
||||
// 占据图类
|
||||
Occupy_map::Ptr Occupy_map_ptr;
|
||||
|
||||
// 重置
|
||||
void reset();
|
||||
// 初始化
|
||||
void init(ros::NodeHandle &nh);
|
||||
// 检查安全性
|
||||
bool check_safety(Eigen::Vector3d &cur_pos, double safe_distance);
|
||||
// 搜索
|
||||
int search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
|
||||
// 返回路径
|
||||
std::vector<Eigen::Vector3d> getPath();
|
||||
// 返回ros消息格式的路径
|
||||
nav_msgs::Path get_ros_path();
|
||||
// 返回访问过的节点
|
||||
std::vector<NodePtr> getVisitedNodes();
|
||||
|
||||
typedef std::shared_ptr<Astar> Ptr;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,121 @@
|
||||
#ifndef GLOBAL_PLANNER
|
||||
#define GLOBAL_PLANNER
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
#include <laser_geometry/laser_geometry.h>
|
||||
#include <prometheus_msgs/UAVState.h>
|
||||
#include <prometheus_msgs/UAVCommand.h>
|
||||
#include <prometheus_msgs/UAVControlState.h>
|
||||
|
||||
#include "A_star.h"
|
||||
#include "occupy_map.h"
|
||||
#include "printf_utils.h"
|
||||
#include "global_planner_utils.h"
|
||||
using namespace std;
|
||||
|
||||
|
||||
class GlobalPlanner
|
||||
{
|
||||
public:
|
||||
GlobalPlanner(ros::NodeHandle &nh);
|
||||
ros::NodeHandle global_planner_nh;
|
||||
|
||||
private:
|
||||
string uav_name; // 无人机名字
|
||||
int uav_id; // 无人机编号
|
||||
bool sim_mode;
|
||||
int map_input_source;
|
||||
double fly_height;
|
||||
double safe_distance;
|
||||
double time_per_path;
|
||||
double replan_time;
|
||||
bool consider_neighbour;
|
||||
string global_pcl_topic_name, local_pcl_topic_name, scan_topic_name;
|
||||
|
||||
// 订阅无人机状态、目标点、传感器数据（生成地图）
|
||||
ros::Subscriber goal_sub;
|
||||
ros::Subscriber uav_state_sub;
|
||||
// 支持2维激光雷达、3维激光雷达、D435i等实体传感器
|
||||
// 支持直接输入全局已知点云
|
||||
ros::Subscriber Gpointcloud_sub;
|
||||
ros::Subscriber Lpointcloud_sub;
|
||||
ros::Subscriber laserscan_sub;
|
||||
ros::Subscriber uav_control_state_sub;
|
||||
// 发布控制指令
|
||||
ros::Publisher uav_cmd_pub;
|
||||
ros::Publisher path_cmd_pub;
|
||||
ros::Timer mainloop_timer;
|
||||
ros::Timer track_path_timer;
|
||||
ros::Timer safety_timer;
|
||||
// A星规划器
|
||||
Astar::Ptr Astar_ptr;
|
||||
|
||||
// laserscan2pointcloud2 投影器
|
||||
laser_geometry::LaserProjection projector_;
|
||||
|
||||
prometheus_msgs::UAVState uav_state; // 无人机状态
|
||||
prometheus_msgs::UAVControlState uav_control_state;
|
||||
nav_msgs::Odometry uav_odom;
|
||||
Eigen::Vector3d uav_pos; // 无人机位置
|
||||
Eigen::Vector3d uav_vel; // 无人机速度
|
||||
Eigen::Quaterniond uav_quat; // 无人机四元数
|
||||
double uav_yaw;
|
||||
// 规划终端状态
|
||||
Eigen::Vector3d goal_pos, goal_vel;
|
||||
prometheus_msgs::UAVCommand uav_command;
|
||||
nav_msgs::Path path_cmd;
|
||||
double distance_to_goal;
|
||||
|
||||
// 规划器状态
|
||||
bool odom_ready;
|
||||
bool drone_ready;
|
||||
bool sensor_ready;
|
||||
bool goal_ready;
|
||||
bool is_safety;
|
||||
bool is_new_path;
|
||||
bool path_ok;
|
||||
int start_point_index;
|
||||
int Num_total_wp;
|
||||
int cur_id;
|
||||
float desired_yaw;
|
||||
|
||||
ros::Time last_replan_time;
|
||||
|
||||
// 五种状态机
|
||||
enum EXEC_STATE
|
||||
{
|
||||
WAIT_GOAL,
|
||||
PLANNING,
|
||||
TRACKING,
|
||||
LANDING,
|
||||
};
|
||||
EXEC_STATE exec_state;
|
||||
|
||||
// 回调函数
|
||||
void goal_cb(const geometry_msgs::PoseStampedConstPtr &msg);
|
||||
void uav_state_cb(const prometheus_msgs::UAVStateConstPtr &msg);
|
||||
void Gpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
|
||||
void Lpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
|
||||
void laser_cb(const sensor_msgs::LaserScanConstPtr &msg);
|
||||
void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg);
|
||||
void safety_cb(const ros::TimerEvent &e);
|
||||
void mainloop_cb(const ros::TimerEvent &e);
|
||||
void track_path_cb(const ros::TimerEvent &e);
|
||||
void debug_info();
|
||||
int get_start_point_id(void);
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,8 @@
|
||||
#ifndef GLOBAL_PLANNER_UTILS_H
|
||||
#define GLOBAL_PLANNER_UTILS_H
|
||||
|
||||
#define NODE_NAME "GlobalPlanner [Astar] "
|
||||
#define MIN_DIS 0.1
|
||||
|
||||
#endif
|
||||
|
||||
@ -0,0 +1,40 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_global_planner</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The prometheus_global_planner package</description>
|
||||
|
||||
<maintainer email="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
|
||||
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>prometheus_msgs</build_depend>
|
||||
<build_depend>pcl_ros</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>sensor_msgs</build_export_depend>
|
||||
<build_export_depend>geometry_msgs</build_export_depend>
|
||||
<build_export_depend>nav_msgs</build_export_depend>
|
||||
<build_export_depend>prometheus_msgs</build_export_depend>
|
||||
<build_export_depend>pcl_ros</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>prometheus_msgs</exec_depend>
|
||||
<exec_depend>pcl_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,28 @@
|
||||
#include <ros/ros.h>
|
||||
#include <signal.h>
|
||||
|
||||
#include "global_planner.h"
|
||||
|
||||
;
|
||||
|
||||
void mySigintHandler(int sig)
|
||||
{
|
||||
ROS_INFO("[global_planner_node] exit...");
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "global_planner");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
signal(SIGINT, mySigintHandler);
|
||||
ros::Duration(1.0).sleep();
|
||||
|
||||
GlobalPlanner global_planner(nh);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -0,0 +1,50 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(prometheus_local_planner)
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL 1.7 REQUIRED)
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
laser_geometry
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
pcl_ros
|
||||
visualization_msgs
|
||||
prometheus_msgs
|
||||
mavros_msgs
|
||||
)
|
||||
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES LocalPlannerNS
|
||||
)
|
||||
|
||||
include_directories(
|
||||
SYSTEM
|
||||
include
|
||||
${PROJECT_SOURCE_DIR}/include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Eigen3_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${PROJECT_SOURCE_DIR}/../../common/include
|
||||
)
|
||||
link_directories(${PCL_LIBRARY_DIRS})
|
||||
|
||||
|
||||
|
||||
add_library(vfh src/vfh.cpp)
|
||||
target_link_libraries(vfh ${PCL_LIBRARIES})
|
||||
add_library(apf src/apf.cpp)
|
||||
target_link_libraries(apf ${PCL_LIBRARIES})
|
||||
add_library(local_planner src/local_planner.cpp)
|
||||
target_link_libraries(local_planner vfh)
|
||||
target_link_libraries(local_planner apf)
|
||||
|
||||
add_executable(local_planner_main src/local_planner_node.cpp)
|
||||
add_dependencies(local_planner_main prometheus_local_planner_gencpp)
|
||||
target_link_libraries(local_planner_main ${catkin_LIBRARIES})
|
||||
target_link_libraries(local_planner_main local_planner)
|
||||
@ -0,0 +1,63 @@
|
||||
#ifndef APF_H
|
||||
#define APF_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Empty.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include "local_planner_alg.h"
|
||||
#include "local_planner_utils.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class APF : public local_planner_alg
|
||||
{
|
||||
private:
|
||||
// 参数
|
||||
double inflate_distance;
|
||||
double sensor_max_range;
|
||||
double max_att_dist;
|
||||
double k_repulsion;
|
||||
double k_attraction;
|
||||
double min_dist;
|
||||
double ground_height;
|
||||
double ground_safe_height;
|
||||
double safe_distance;
|
||||
|
||||
bool has_local_map_;
|
||||
bool has_odom_;
|
||||
|
||||
Eigen::Vector3d repulsive_force;
|
||||
Eigen::Vector3d attractive_force;
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ> latest_local_pcl_;
|
||||
sensor_msgs::PointCloud2ConstPtr local_map_ptr_;
|
||||
nav_msgs::Odometry current_odom;
|
||||
|
||||
public:
|
||||
virtual void set_odom(nav_msgs::Odometry cur_odom);
|
||||
virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr);
|
||||
virtual void set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr);
|
||||
virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel);
|
||||
virtual void init(ros::NodeHandle &nh);
|
||||
APF() {}
|
||||
~APF() {}
|
||||
|
||||
typedef shared_ptr<APF> Ptr;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,115 @@
|
||||
#ifndef LOCAL_PLANNER_H
|
||||
#define LOCAL_PLANNER_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Int8.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <prometheus_msgs/UAVState.h>
|
||||
#include <prometheus_msgs/UAVCommand.h>
|
||||
#include <prometheus_msgs/UAVControlState.h>
|
||||
|
||||
#include "apf.h"
|
||||
#include "vfh.h"
|
||||
#include "local_planner_utils.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class LocalPlanner
|
||||
{
|
||||
public:
|
||||
LocalPlanner(ros::NodeHandle &nh);
|
||||
ros::NodeHandle local_planner_nh;
|
||||
|
||||
private:
|
||||
// 参数
|
||||
int uav_id;
|
||||
int algorithm_mode;
|
||||
int map_input_source;
|
||||
double max_planning_vel;
|
||||
double fly_height;
|
||||
double safe_distance;
|
||||
bool sim_mode;
|
||||
bool map_groundtruth;
|
||||
string local_pcl_topic_name;
|
||||
|
||||
// 订阅无人机状态、目标点、传感器数据（生成地图）
|
||||
ros::Subscriber goal_sub;
|
||||
ros::Subscriber uav_state_sub;
|
||||
ros::Subscriber uav_control_state_sub;
|
||||
ros::Subscriber local_point_cloud_sub;
|
||||
|
||||
// 发布控制指令
|
||||
ros::Publisher uav_cmd_pub;
|
||||
ros::Publisher rviz_vel_pub;
|
||||
ros::Timer mainloop_timer;
|
||||
ros::Timer control_timer;
|
||||
|
||||
// 局部避障算法 算子
|
||||
local_planner_alg::Ptr local_alg_ptr;
|
||||
|
||||
prometheus_msgs::UAVState uav_state; // 无人机状态
|
||||
prometheus_msgs::UAVControlState uav_control_state;
|
||||
nav_msgs::Odometry uav_odom;
|
||||
prometheus_msgs::UAVCommand uav_command;
|
||||
|
||||
double distance_to_goal;
|
||||
// 规划器状态
|
||||
bool odom_ready;
|
||||
bool drone_ready;
|
||||
bool sensor_ready;
|
||||
bool goal_ready;
|
||||
bool is_safety;
|
||||
bool path_ok;
|
||||
|
||||
// 规划初始状态及终端状态
|
||||
Eigen::Vector3d uav_pos; // 无人机位置
|
||||
Eigen::Vector3d uav_vel; // 无人机速度
|
||||
Eigen::Quaterniond uav_quat; // 无人机四元数
|
||||
double uav_yaw;
|
||||
// 规划终端状态
|
||||
Eigen::Vector3d goal_pos, goal_vel;
|
||||
|
||||
int planner_state;
|
||||
Eigen::Vector3d desired_vel;
|
||||
float desired_yaw;
|
||||
|
||||
// 五种状态机
|
||||
enum EXEC_STATE
|
||||
{
|
||||
WAIT_GOAL,
|
||||
PLANNING,
|
||||
TRACKING,
|
||||
LANDING,
|
||||
};
|
||||
EXEC_STATE exec_state;
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr local_map_ptr_;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr;
|
||||
pcl::PointCloud<pcl::PointXYZ> latest_local_pcl_;
|
||||
|
||||
void goal_cb(const geometry_msgs::PoseStampedConstPtr &msg);
|
||||
void uav_control_state_cb(const prometheus_msgs::UAVControlState::ConstPtr &msg);
|
||||
void uav_state_cb(const prometheus_msgs::UAVStateConstPtr &msg);
|
||||
void pcl_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
|
||||
void laserscan_cb(const sensor_msgs::LaserScanConstPtr &msg);
|
||||
void mainloop_cb(const ros::TimerEvent &e);
|
||||
void control_cb(const ros::TimerEvent &e);
|
||||
void debug_info();
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,37 @@
|
||||
#ifndef LOCAL_PLANNING_ALG
|
||||
#define LOCAL_PLANNING_ALG
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Empty.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class local_planner_alg
|
||||
{
|
||||
public:
|
||||
local_planner_alg() {}
|
||||
~local_planner_alg() {}
|
||||
virtual void set_odom(nav_msgs::Odometry cur_odom) = 0;
|
||||
virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr) = 0;
|
||||
virtual void set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr) = 0;
|
||||
virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel) = 0;
|
||||
virtual void init(ros::NodeHandle &nh) = 0;
|
||||
|
||||
typedef shared_ptr<local_planner_alg> Ptr;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,10 @@
|
||||
#ifndef LOCAL_PLANNER_UTILS_H
|
||||
#define LOCAL_PLANNER_UTILS_H
|
||||
|
||||
#include "printf_utils.h"
|
||||
|
||||
#define NODE_NAME "LocalPlanner"
|
||||
#define MIN_DIS 0.1
|
||||
|
||||
#endif
|
||||
|
||||
@ -0,0 +1,73 @@
|
||||
#ifndef VFH_H
|
||||
#define VFH_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Empty.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include "math.h"
|
||||
#include "local_planner_alg.h"
|
||||
#include "local_planner_utils.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
class VFH : public local_planner_alg
|
||||
{
|
||||
private:
|
||||
// 参数
|
||||
double inflate_distance;
|
||||
double sensor_max_range;
|
||||
double safe_distance;
|
||||
bool has_local_map_;
|
||||
bool has_odom_;
|
||||
|
||||
double goalWeight, obstacle_weight;
|
||||
double inflate_and_safe_distance;
|
||||
double velocity;
|
||||
|
||||
double *Hdata;
|
||||
double Hres;
|
||||
int Hcnt; // 直方图个数
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ> latest_local_pcl_;
|
||||
sensor_msgs::PointCloud2ConstPtr local_map_ptr_;
|
||||
nav_msgs::Odometry current_odom;
|
||||
|
||||
bool isIgnored(float x, float y, float z, float ws);
|
||||
int find_Hcnt(double angle);
|
||||
double find_angle(int cnt);
|
||||
double angle_error(double angle1, double angle2);
|
||||
void generate_voxel_data(double angle_cen, double angle_range, double val);
|
||||
int find_optimization_path(void);
|
||||
|
||||
public:
|
||||
virtual void set_odom(nav_msgs::Odometry cur_odom);
|
||||
virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr);
|
||||
virtual void set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr);
|
||||
virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel);
|
||||
virtual void init(ros::NodeHandle &nh);
|
||||
|
||||
VFH() {}
|
||||
~VFH()
|
||||
{
|
||||
delete Hdata;
|
||||
}
|
||||
|
||||
typedef shared_ptr<VFH> Ptr;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,31 @@
|
||||
## local_planner
|
||||
|
||||
|
||||
|
||||
#### 情况讨论
|
||||
|
||||
- 局部点云情况
|
||||
- 对应真实情况:D435i等RGBD相机,三维激光雷达
|
||||
- map_generator生成全局点云,模拟得到局部点云信息
|
||||
- 无人机不需要搭载传感器
|
||||
- PX4动力学 & fake_odom模块
|
||||
|
||||
roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch
|
||||
roslaunch prometheus_local_planner sitl_apf_with_local_point.launch
|
||||
|
||||
- 2dlidar情况
|
||||
- 对应真实情况:二维激光雷达
|
||||
- projector_.projectLaser(*local_point, input_laser_scan)将scan转换为点云,即对应回到局部点云情况
|
||||
- map_generator生成全局点云,模拟得到局部点云信息
|
||||
- 无人机不需要搭载传感器
|
||||
- PX4动力学 & fake_odom模块
|
||||
|
||||
roslaunch prometheus_simulator_utils map_generator_with_fake_odom.launch
|
||||
roslaunch prometheus_local_planner sitl_global_planner_with_2dlidar.launch
|
||||
|
||||
- 多机情况
|
||||
- 建议使用 全局点云情况 + 多个无人机
|
||||
- fake_odom模块
|
||||
|
||||
## 运行
|
||||
|
||||
@ -0,0 +1,37 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_local_planner</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The prometheus_local_planner package</description>
|
||||
|
||||
<maintainer email="qiyh8@mail.sysu.edu.cn">Yuhua Qi</maintainer>
|
||||
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>prometheus_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>sensor_msgs</build_export_depend>
|
||||
<build_export_depend>geometry_msgs</build_export_depend>
|
||||
<build_export_depend>nav_msgs</build_export_depend>
|
||||
<build_export_depend>prometheus_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>prometheus_msgs</exec_depend>
|
||||
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,27 @@
|
||||
#include <ros/ros.h>
|
||||
#include <signal.h>
|
||||
|
||||
#include "local_planner.h"
|
||||
|
||||
|
||||
|
||||
void mySigintHandler(int sig)
|
||||
{
|
||||
ROS_INFO("[local_planner_node] exit...");
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "local_planner_node");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
signal(SIGINT, mySigintHandler);
|
||||
ros::Duration(1.0).sleep();
|
||||
|
||||
LocalPlanner local_planner(nh);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,286 @@
|
||||
#include "vfh.h"
|
||||
|
||||
|
||||
|
||||
void VFH::init(ros::NodeHandle &nh)
|
||||
{
|
||||
// 【参数】障碍物膨胀距离
|
||||
nh.param("vfh/inflate_distance", inflate_distance, 0.2);
|
||||
// 【参数】感知障碍物距离
|
||||
nh.param("vfh/sensor_max_range", sensor_max_range, 5.0);
|
||||
// 【参数】目标权重
|
||||
nh.param("vfh/goalWeight", goalWeight, 0.2);
|
||||
// 【参数】直方图 个数
|
||||
nh.param("vfh/h_res", Hcnt, 180);
|
||||
// 【参数】障碍物权重
|
||||
nh.param("vfh/obstacle_weight", obstacle_weight, 0.0);
|
||||
// 【参数】安全停止距离
|
||||
nh.param("vfh/safe_distance", safe_distance, 0.2);
|
||||
// 【参数】设定速度
|
||||
nh.param("vfh/velocity", velocity, 1.0);
|
||||
|
||||
inflate_and_safe_distance = safe_distance + inflate_distance;
|
||||
Hres = 2 * M_PI / Hcnt;
|
||||
Hdata = new double[Hcnt]();
|
||||
for (int i(0); i < Hcnt; i++)
|
||||
{
|
||||
Hdata[i] = 0.0;
|
||||
}
|
||||
|
||||
has_local_map_ = false;
|
||||
}
|
||||
|
||||
// get the map
|
||||
void VFH::set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr)
|
||||
{
|
||||
local_map_ptr_ = local_map_ptr;
|
||||
pcl::fromROSMsg(*local_map_ptr, latest_local_pcl_);
|
||||
has_local_map_ = true;
|
||||
}
|
||||
|
||||
void VFH::set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr)
|
||||
{
|
||||
latest_local_pcl_ = *pcl_ptr;
|
||||
has_local_map_ = true;
|
||||
}
|
||||
|
||||
void VFH::set_odom(nav_msgs::Odometry cur_odom)
|
||||
{
|
||||
current_odom = cur_odom;
|
||||
has_odom_ = true;
|
||||
}
|
||||
|
||||
int VFH::compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel)
|
||||
{
|
||||
// 0 for not init; 1for safe; 2 for dangerous
|
||||
int local_planner_state = 0;
|
||||
int safe_cnt = 0;
|
||||
|
||||
if (!has_local_map_ || !has_odom_)
|
||||
return 0;
|
||||
|
||||
if ((int)latest_local_pcl_.points.size() == 0)
|
||||
return 0;
|
||||
|
||||
if (isnan(goal(0)) || isnan(goal(1)) || isnan(goal(2)))
|
||||
return 0;
|
||||
|
||||
// clear the Hdata
|
||||
for (int i = 0; i < Hcnt; i++)
|
||||
{
|
||||
Hdata[i] = 0;
|
||||
}
|
||||
|
||||
ros::Time begin_collision = ros::Time::now();
|
||||
|
||||
// 计算障碍物相关cost
|
||||
Eigen::Vector3d p3d;
|
||||
vector<Eigen::Vector3d> obstacles;
|
||||
Eigen::Vector3d p3d_gloabl_rot;
|
||||
|
||||
// 排斥力
|
||||
Eigen::Quaterniond cur_rotation_local_to_global(current_odom.pose.pose.orientation.w, current_odom.pose.pose.orientation.x, current_odom.pose.pose.orientation.y, current_odom.pose.pose.orientation.z);
|
||||
|
||||
Eigen::Matrix<double, 3, 3> rotation_mat_local_to_global = cur_rotation_local_to_global.toRotationMatrix();
|
||||
Eigen::Vector3d eulerAngle_yrp = rotation_mat_local_to_global.eulerAngles(2, 1, 0);
|
||||
rotation_mat_local_to_global = Eigen::AngleAxisd(eulerAngle_yrp(0), Eigen::Vector3d::UnitZ()).toRotationMatrix();
|
||||
|
||||
// 遍历点云中的所有点
|
||||
for (size_t i = 0; i < latest_local_pcl_.points.size(); ++i)
|
||||
{
|
||||
// 提取障碍物点
|
||||
p3d(0) = latest_local_pcl_.points[i].x;
|
||||
p3d(1) = latest_local_pcl_.points[i].y;
|
||||
p3d(2) = latest_local_pcl_.points[i].z;
|
||||
|
||||
// 将本地点云转化为全局点云点(主要是yaw角)
|
||||
p3d_gloabl_rot = rotation_mat_local_to_global * p3d;
|
||||
|
||||
// sensor_max_range为感知距离,只考虑感知距离内的障碍
|
||||
if (isIgnored(p3d_gloabl_rot(0), p3d_gloabl_rot(1), p3d_gloabl_rot(2), sensor_max_range))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
double obs_dist = p3d_gloabl_rot.norm();
|
||||
double obs_angle = atan2(p3d_gloabl_rot(1), p3d_gloabl_rot(0));
|
||||
double angle_range;
|
||||
if (obs_dist > inflate_and_safe_distance)
|
||||
{
|
||||
angle_range = asin(inflate_and_safe_distance / obs_dist);
|
||||
}
|
||||
else if (obs_dist <= inflate_and_safe_distance)
|
||||
{
|
||||
safe_cnt++; // 非常危险
|
||||
continue;
|
||||
}
|
||||
|
||||
double obstacle_cost = obstacle_weight * (1 / obs_dist - 1 / sensor_max_range) * 1.0 / (obs_dist * obs_dist);
|
||||
generate_voxel_data(obs_angle, angle_range, obstacle_cost);
|
||||
|
||||
obstacles.push_back(p3d);
|
||||
}
|
||||
|
||||
// 与目标点相关cost计算
|
||||
// 当前位置
|
||||
Eigen::Vector3d current_pos;
|
||||
current_pos[0] = current_odom.pose.pose.position.x;
|
||||
current_pos[1] = current_odom.pose.pose.position.y;
|
||||
current_pos[2] = current_odom.pose.pose.position.z;
|
||||
Eigen::Vector3d uav2goal = goal - current_pos;
|
||||
// 不考虑高度影响
|
||||
uav2goal(2) = 0.0;
|
||||
double dist_att = uav2goal.norm();
|
||||
double goal_heading = atan2(uav2goal(1), uav2goal(0));
|
||||
|
||||
for (int i = 0; i < Hcnt; i++)
|
||||
{
|
||||
// Hdata;
|
||||
// angle_i 为当前角度
|
||||
double angle_i = find_angle(i);
|
||||
|
||||
double goal_cost = 0;
|
||||
double angle_er = angle_error(angle_i, goal_heading);
|
||||
float goal_gain;
|
||||
if (dist_att > 3.0)
|
||||
{
|
||||
goal_gain = 3.0;
|
||||
}
|
||||
else if (dist_att < 0.5)
|
||||
{
|
||||
goal_gain = 0.5;
|
||||
}
|
||||
else
|
||||
{
|
||||
goal_gain = dist_att;
|
||||
}
|
||||
// 当前角度与目标角度差的越多,则该代价越大
|
||||
goal_cost = goalWeight * angle_er * goal_gain;
|
||||
|
||||
Hdata[i] += goal_cost;
|
||||
}
|
||||
|
||||
// 寻找cost最小的路径
|
||||
int best_ind = find_optimization_path(); // direction
|
||||
// 提取最优路径的航向角
|
||||
double best_heading = find_angle(best_ind);
|
||||
|
||||
desired_vel(0) = cos(best_heading) * velocity;
|
||||
desired_vel(1) = sin(best_heading) * velocity;
|
||||
// 定高飞行
|
||||
desired_vel(2) = 0.0;
|
||||
|
||||
// 如果不安全的点超出指定数量
|
||||
if (safe_cnt > 5)
|
||||
{
|
||||
local_planner_state = 2; //成功规划，但是飞机不安全
|
||||
}
|
||||
else
|
||||
{
|
||||
local_planner_state = 1; //成功规划， 安全
|
||||
}
|
||||
|
||||
static int exec_num = 0;
|
||||
exec_num++;
|
||||
|
||||
// 此处改为根据循环时间计算的数值
|
||||
if (exec_num == 50)
|
||||
{
|
||||
cout << GREEN << NODE_NAME << "VFH calculate take: " << (ros::Time::now() - begin_collision).toSec() << " [s] " << TAIL << endl;
|
||||
exec_num = 0;
|
||||
}
|
||||
|
||||
return local_planner_state;
|
||||
}
|
||||
|
||||
// 寻找最小
|
||||
int VFH::find_optimization_path(void)
|
||||
{
|
||||
int bset_ind = 10000;
|
||||
double best_cost = 100000;
|
||||
for (int i = 0; i < Hcnt; i++)
|
||||
{
|
||||
if (Hdata[i] < best_cost)
|
||||
{
|
||||
best_cost = Hdata[i];
|
||||
bset_ind = i;
|
||||
}
|
||||
}
|
||||
return bset_ind;
|
||||
}
|
||||
|
||||
bool VFH::isIgnored(float x, float y, float z, float ws)
|
||||
{
|
||||
z = 0;
|
||||
if (isnan(x) || isnan(y) || isnan(z))
|
||||
return true;
|
||||
|
||||
if (x * x + y * y + z * z > ws)
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void VFH::generate_voxel_data(double angle_cen, double angle_range, double val) // set the map obstacle into the H data
|
||||
{
|
||||
double angle_max = angle_cen + angle_range;
|
||||
double angle_min = angle_cen - angle_range;
|
||||
int cnt_min = find_Hcnt(angle_min);
|
||||
int cnt_max = find_Hcnt(angle_max);
|
||||
if (cnt_min > cnt_max)
|
||||
{
|
||||
for (int i = cnt_min; i < Hcnt; i++)
|
||||
{
|
||||
Hdata[i] = +val;
|
||||
}
|
||||
for (int i = 0; i < cnt_max; i++)
|
||||
{
|
||||
Hdata[i] += val;
|
||||
}
|
||||
}
|
||||
else if (cnt_max >= cnt_min)
|
||||
{
|
||||
for (int i = cnt_min; i <= cnt_max; i++)
|
||||
{
|
||||
Hdata[i] += val;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// angle: deg
|
||||
int VFH::find_Hcnt(double angle)
|
||||
{
|
||||
if (angle < 0)
|
||||
{
|
||||
angle += 2 * M_PI;
|
||||
}
|
||||
if (angle > 2 * M_PI)
|
||||
{
|
||||
angle -= 2 * M_PI;
|
||||
}
|
||||
int cnt = floor(angle / Hres);
|
||||
return cnt;
|
||||
}
|
||||
|
||||
double VFH::find_angle(int cnt)
|
||||
{
|
||||
double angle = (cnt + 0.5) * Hres;
|
||||
return angle;
|
||||
}
|
||||
|
||||
double VFH::angle_error(double angle1, double angle2)
|
||||
{
|
||||
double angle_er = angle1 - angle2;
|
||||
while (angle_er > M_PI)
|
||||
{
|
||||
angle_er = angle_er - 2 * M_PI;
|
||||
}
|
||||
|
||||
while (angle_er < -M_PI)
|
||||
{
|
||||
angle_er = angle_er + 2 * M_PI;
|
||||
}
|
||||
angle_er = fabs(angle_er);
|
||||
return angle_er;
|
||||
}
|
||||
|
||||
@ -0,0 +1,231 @@
|
||||
# Git Commit Message
|
||||
|
||||
## 主提交 (Main Commit)
|
||||
|
||||
```
|
||||
feat: Add local_planner_px4_interface for integrating obstacle avoidance with PX4 autopilot
|
||||
|
||||
Implement a bridge module between Prometheus local_planner and PX4 autopilot,
|
||||
enabling local obstacle avoidance planning to control PX4-powered UAVs via MAVROS.
|
||||
|
||||
Features:
|
||||
- Convert Prometheus UAVCommand messages to MAVROS PositionTarget format
|
||||
- Automatic PX4 Offboard mode management with heartbeat mechanism (20Hz)
|
||||
- Support for multiple control modes (XY_VEL_Z_POS, XYZ_POS, XYZ_VEL)
|
||||
- Safety features: command timeout detection, auto-hover on planner failure
|
||||
- Goal arrival detection with configurable acceptance radius
|
||||
- Real-time status feedback and comprehensive logging
|
||||
|
||||
Technical Details:
|
||||
- C++14 implementation with ROS Melodic/Noetic support
|
||||
- Eigen3 for coordinate transformations
|
||||
- Publisher/Subscriber architecture with uORB-inspired message flow
|
||||
- Parameter server integration for runtime configuration
|
||||
- Modular design allowing easy extension for additional algorithms
|
||||
|
||||
Safety Considerations:
|
||||
- 500ms command timeout protection (auto-switch to hover)
|
||||
- Continuous Offboard mode monitoring
|
||||
- Configurable goal acceptance radius (default: 0.2m)
|
||||
- Optional auto-arm on startup (disabled by default for safety)
|
||||
|
||||
Testing:
|
||||
- Verified with PX4 SITL simulation (Gazebo + Iris/P450 models)
|
||||
- Tested with both APF and VFH algorithms
|
||||
- Confirmed 20Hz stable setpoint publishing
|
||||
- Validated timeout protection and hover recovery
|
||||
|
||||
Closes: #<issue_number>
|
||||
Related: Prometheus local_planner, ego_planner_swarm integration
|
||||
|
||||
Signed-off-by: Prometheus Team <prometheus@example.com>
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 拆分提交方案 (Split Commits)
|
||||
|
||||
如果需要分多次提交，建议按以下顺序：
|
||||
|
||||
### Commit 1: 基础框架
|
||||
|
||||
```
|
||||
feat(local_planner_px4_interface): Add package structure and build system
|
||||
|
||||
Add new ROS package for PX4 interface with:
|
||||
- CMakeLists.txt with all dependencies
|
||||
- package.xml with metadata
|
||||
- Directory structure (src, include, launch, config)
|
||||
- Parameter configuration file
|
||||
|
||||
Dependencies: roscpp, mavros_msgs, prometheus_msgs, eigen3
|
||||
```
|
||||
|
||||
### Commit 2: 核心库实现
|
||||
|
||||
```
|
||||
feat(local_planner_px4_interface): Implement PX4 interface library
|
||||
|
||||
Add LocalPlannerPX4Interface class with core functionality:
|
||||
- Message conversion (UAVCommand -> PositionTarget)
|
||||
- MAVROS connection handling
|
||||
- Offboard mode management
|
||||
- Parameter loading system
|
||||
|
||||
Header: include/local_planner_px4_interface.hpp
|
||||
Source: src/local_planner_px4_interface.cpp
|
||||
```
|
||||
|
||||
### Commit 3: 节点入口
|
||||
|
||||
```
|
||||
feat(local_planner_px4_interface): Add ROS node executable
|
||||
|
||||
Add main node implementation:
|
||||
- Node initialization and error handling
|
||||
- Signal handling for graceful shutdown
|
||||
- Version info and startup banner
|
||||
|
||||
File: src/local_planner_px4_interface_node.cpp
|
||||
```
|
||||
|
||||
### Commit 4: 启动配置
|
||||
|
||||
```
|
||||
feat(local_planner_px4_interface): Add launch files and scripts
|
||||
|
||||
Provide convenient launch configurations:
|
||||
- local_planner_px4_interface.launch: Single UAV launch
|
||||
- local_planner_px4_basic.sh: Full simulation stack
|
||||
- local_planner_px4_minimal.sh: Minimal test setup
|
||||
|
||||
All scripts support UAV_ID parameter for multi-UAV scenarios
|
||||
```
|
||||
|
||||
### Commit 5: 文档
|
||||
|
||||
```
|
||||
docs(local_planner_px4_interface): Add comprehensive documentation
|
||||
|
||||
Add user and developer documentation:
|
||||
- README.md with usage instructions and API reference
|
||||
- TESTING.md with validation procedures
|
||||
- Architecture diagrams and workflow descriptions
|
||||
|
||||
Includes quick-start guide and troubleshooting section
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 快速提交命令
|
||||
|
||||
```bash
|
||||
# 进入Prometheus仓库
|
||||
cd ~/Prometheus
|
||||
|
||||
# 添加新包
|
||||
git add Modules/motion_planning/local_planner_px4_interface/
|
||||
git add Scripts/simulation/local_planner_px4/
|
||||
|
||||
# 提交（使用主提交信息）
|
||||
git commit -F- <<'EOF'
|
||||
feat: Add local_planner_px4_interface for integrating obstacle avoidance with PX4 autopilot
|
||||
|
||||
Implement a bridge module between Prometheus local_planner and PX4 autopilot,
|
||||
enabling local obstacle avoidance planning to control PX4-powered UAVs via MAVROS.
|
||||
|
||||
[详细内容同上...]
|
||||
EOF
|
||||
|
||||
# 推送到远程
|
||||
git push origin main
|
||||
|
||||
# 或者创建Pull Request
|
||||
git push origin feature/local-planner-px4-interface
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Pull Request 描述模板
|
||||
|
||||
```markdown
|
||||
## Summary
|
||||
This PR introduces `prometheus_local_planner_px4_interface`, a bridge module that integrates Prometheus local obstacle avoidance planner with PX4 autopilot via MAVROS.
|
||||
|
||||
## Motivation
|
||||
Previously, Prometheus local_planner could only be used with the custom prometheus_px4 firmware or through complex ROS-to-MAVROS manual bridging. This package provides a standardized, safe, and easy-to-use interface.
|
||||
|
||||
## Changes
|
||||
- New ROS package: `prometheus_local_planner_px4_interface`
|
||||
- C++ library for message conversion and MAVROS communication
|
||||
- Support for multiple control modes (position, velocity, hybrid)
|
||||
- Safety features: timeout protection, mode monitoring
|
||||
- Launch files and helper scripts for quick deployment
|
||||
- Comprehensive documentation and testing guide
|
||||
|
||||
## Testing Performed
|
||||
- [x] SITL simulation with Gazebo + PX4
|
||||
- [x] Tested with APF algorithm
|
||||
- [x] Tested with VFH algorithm
|
||||
- [x] Verified 20Hz setpoint streaming
|
||||
- [x] Validated timeout protection
|
||||
- [x] Multi-UAV scenario (UAV_ID parameter)
|
||||
|
||||
## Usage Example
|
||||
```bash
|
||||
# Launch full simulation stack
|
||||
./Scripts/simulation/local_planner_px4/local_planner_px4_basic.sh 1
|
||||
|
||||
# In QGC: Arm and switch to OFFBOARD mode
|
||||
# In RVIZ: Publish goal to /uav1/prometheus/motion_planning/goal
|
||||
```
|
||||
|
||||
## Safety Considerations
|
||||
- Auto-hover on planner command timeout (500ms)
|
||||
- Configurable goal acceptance radius
|
||||
- Optional auto-arm (disabled by default)
|
||||
- Compatible with PX4 failsafe system
|
||||
|
||||
## Documentation
|
||||
- README.md with architecture and API docs
|
||||
- TESTING.md with validation procedures
|
||||
- Inline code comments
|
||||
|
||||
## Checklist
|
||||
- [x] Code compiles without warnings
|
||||
- [x] Tested in SITL simulation
|
||||
- [x] Documentation updated
|
||||
- [x] No breaking changes to existing modules
|
||||
- [x] Follows project coding standards
|
||||
|
||||
## Related Issues
|
||||
Closes: #<issue_number>
|
||||
Related to: ego_planner integration, PX4 support enhancement
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 标签建议
|
||||
|
||||
```
|
||||
Type: Feature
|
||||
Component: Motion Planning / PX4 Integration
|
||||
Priority: High
|
||||
Impact: New Capability
|
||||
Testing: SITL Validated
|
||||
Documentation: Complete
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 版本标签 (可选)
|
||||
|
||||
```bash
|
||||
# 如果这是重要功能发布，可以打标签
|
||||
git tag -a v1.1.0 -m "Add local_planner PX4 integration"
|
||||
git push origin v1.1.0
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
**提交完成后，建议通知团队成员进行代码审查和实机测试。**
|
||||
@ -0,0 +1,34 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- 参数配置 -->
|
||||
<arg name="uav_id" default="1"/>
|
||||
<arg name="heartbeat_rate" default="20.0"/>
|
||||
<arg name="goal_acceptance_radius" default="0.2"/>
|
||||
<arg name="enable_arm_on_start" default="false"/>
|
||||
|
||||
<!-- 打印启动信息 -->
|
||||
<node pkg="prometheus_local_planner_px4_interface"
|
||||
type="local_planner_px4_interface"
|
||||
name="local_planner_px4_interface_$(arg uav_id)"
|
||||
output="screen"
|
||||
respawn="true"
|
||||
respawn_delay="5">
|
||||
|
||||
<!-- 加载参数 -->
|
||||
<param name="uav_id" value="$(arg uav_id)"/>
|
||||
<param name="heartbeat_rate" value="$(arg heartbeat_rate)"/>
|
||||
<param name="goal_acceptance_radius" value="$(arg goal_acceptance_radius)"/>
|
||||
<param name="enable_arm_on_start" value="$(arg enable_arm_on_start)"/>
|
||||
|
||||
<!-- 话题重映射（可选） -->
|
||||
<!-- <remap from="/mavros/setpoint_raw/local" to="/uav$(arg uav_id)/mavros/setpoint_raw/local"/> -->
|
||||
<!-- <remap from="/mavros/state" to="/uav$(arg uav_id)/mavros/state"/> -->
|
||||
|
||||
</node>
|
||||
|
||||
<!-- 启动可视化节点（可选） -->
|
||||
<!-- <node pkg="prometheus_local_planner_px4_interface"
|
||||
type="interface_visualization_node"
|
||||
name="interface_visualization_$(arg uav_id)"
|
||||
output="screen"/> -->
|
||||
</launch>
|
||||
@ -0,0 +1,63 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>prometheus_local_planner_px4_interface</name>
|
||||
<version>1.0.0</version>
|
||||
<description>
|
||||
Prometheus Local Planner PX4 Interface
|
||||
|
||||
该包提供Prometheus local_planner与PX4飞控之间的接口。
|
||||
将局部避障规划器的控制指令通过MAVROS发送到PX4飞控的Offboard模式。
|
||||
|
||||
This package provides an interface between Prometheus local_planner and PX4 autopilot.
|
||||
Converts local obstacle avoidance planner commands to MAVROS messages for PX4 Offboard mode.
|
||||
</description>
|
||||
|
||||
<maintainer email="prometheus@example.com">Prometheus Team</maintainer>
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">https://github.com/amov-lab/Prometheus</url>
|
||||
<url type="bugtracker">https://github.com/amov-lab/Prometheus/issues</url>
|
||||
|
||||
<author>Prometheus Team</author>
|
||||
|
||||
<!-- 构建工具依赖 -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<!-- 构建依赖 -->
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>mavros_msgs</build_depend>
|
||||
<build_depend>prometheus_msgs</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
|
||||
<!-- 导出依赖 -->
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>geometry_msgs</build_export_depend>
|
||||
<build_export_depend>nav_msgs</build_export_depend>
|
||||
<build_export_depend>sensor_msgs</build_export_depend>
|
||||
<build_export_depend>mavros_msgs</build_export_depend>
|
||||
<build_export_depend>prometheus_msgs</build_export_depend>
|
||||
<build_export_depend>tf</build_export_depend>
|
||||
|
||||
<!-- 运行依赖 -->
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>mavros_msgs</exec_depend>
|
||||
<exec_depend>prometheus_msgs</exec_depend>
|
||||
<exec_depend>tf</exec_depend>
|
||||
<exec_depend>mavros</exec_depend>
|
||||
|
||||
<export>
|
||||
<!-- 其他工具可以请求在此处放置额外信息 -->
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,7 @@
|
||||
#roscore & sleep 5;
|
||||
roslaunch min_snap real.launch & sleep 5;
|
||||
roslaunch vicon_bridge vicon.launch & sleep 3;
|
||||
roslaunch mavros px4.launch & sleep 3;
|
||||
roslaunch ekf PX4_vicon.launch & sleep 3;
|
||||
roslaunch px4ctrl run_ctrl.launch
|
||||
#rosbag record /vicon_imu_ekf_odom /debugPx4ctrl
|
||||
@ -0,0 +1,221 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(min_snap)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
quadrotor_msgs
|
||||
mini_snap_traj_utils
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a exec_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
# generate_messages(
|
||||
# DEPENDENCIES
|
||||
# std_msgs
|
||||
# )
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
# LIBRARIES min_snap
|
||||
CATKIN_DEPENDS roscpp rospy std_msgs mini_snap_traj_utils
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/min_snap.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
# add_executable(${PROJECT_NAME}_node src/min_snap_node.cpp)
|
||||
|
||||
## Rename C++ executable without prefix
|
||||
## The above recommended prefix causes long target names, the following renames the
|
||||
## target back to the shorter version for ease of user use
|
||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(${PROJECT_NAME}_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# catkin_install_python(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
|
||||
# install(TARGETS ${PROJECT_NAME}_node
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark libraries for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
|
||||
# install(TARGETS ${PROJECT_NAME}
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
add_library( minsnapCloseform
|
||||
include/min_snap/min_snap_closeform.h
|
||||
src/min_snap_closeform.cpp
|
||||
)
|
||||
target_link_libraries(minsnapCloseform ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(min_snap_generator src/min_snap_generator.cpp)
|
||||
target_link_libraries(min_snap_generator minsnapCloseform ${catkin_LIBRARIES})
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_min_snap.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
@ -0,0 +1,42 @@
|
||||
#ifndef _MIN_SNAP_CLOSEFORM_H_
|
||||
#define _MIN_SNAP_CLOSEFORM_H_
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <iostream>
|
||||
|
||||
using std::vector;
|
||||
using namespace std;
|
||||
|
||||
namespace my_planner
|
||||
{
|
||||
class minsnapCloseform
|
||||
{
|
||||
private:
|
||||
vector<Eigen::Vector3d> wps;
|
||||
int n_order, n_seg, n_per_seg;
|
||||
double mean_vel;
|
||||
Eigen::VectorXd ts;
|
||||
Eigen::VectorXd poly_coef_x, poly_coef_y, poly_coef_z;
|
||||
Eigen::VectorXd dec_vel_x, dec_vel_y, dec_vel_z;
|
||||
Eigen::MatrixXd Q, M, Ct;
|
||||
|
||||
int fact(int n);
|
||||
void init_ts(int init_type);
|
||||
std::pair<Eigen::VectorXd, Eigen::VectorXd> MinSnapCloseFormServer(const Eigen::VectorXd &wp);
|
||||
Eigen::VectorXd calDecVel(const Eigen::VectorXd decvel);
|
||||
void calQ();
|
||||
void calM();
|
||||
void calCt();
|
||||
|
||||
public:
|
||||
minsnapCloseform(){};
|
||||
~minsnapCloseform(){};
|
||||
minsnapCloseform(const vector<Eigen::Vector3d> &waypoints, double meanvel = 1.0);
|
||||
void Init(const vector<Eigen::Vector3d> &waypoints, double meanvel = 1.0);
|
||||
void calMinsnap_polycoef();
|
||||
Eigen::MatrixXd getPolyCoef();
|
||||
Eigen::MatrixXd getDecVel();
|
||||
Eigen::VectorXd getTime();
|
||||
};
|
||||
}
|
||||
#endif
|
||||
@ -0,0 +1,312 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Planning1
|
||||
- /Planning1/jerk_dir1
|
||||
- /Planning1/vel_dir1
|
||||
- /Planning1/acc_dir1
|
||||
- /Planning1/Goal_point1
|
||||
- /Planning1/plan_traj1
|
||||
- /Mapping1/simulation_map1/Autocompute Value Bounds1
|
||||
- /Simulation1/robot1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 865
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /3D Nav Goal1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
- /ThirdPersonFollower1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Class: rviz/Axes
|
||||
Enabled: true
|
||||
Length: 1
|
||||
Name: Axes
|
||||
Radius: 0.10000000149011612
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 40
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /min_snap_visual/jerk_dir
|
||||
Name: jerk_dir
|
||||
Namespaces:
|
||||
"": true
|
||||
Queue Size: 1
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /min_snap_visual/vel_dir
|
||||
Name: vel_dir
|
||||
Namespaces:
|
||||
"": true
|
||||
Queue Size: 1
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /min_snap_visual/acc_dir
|
||||
Name: acc_dir
|
||||
Namespaces:
|
||||
"": true
|
||||
Queue Size: 1
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /min_snap_visual/goal_point
|
||||
Name: Goal_point
|
||||
Namespaces:
|
||||
"": true
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /min_snap_visual/poly_traj
|
||||
Name: plan_traj
|
||||
Namespaces:
|
||||
"": true
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Billboards
|
||||
Line Width: 0.029999999329447746
|
||||
Name: drone_path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Queue Size: 10
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /odom_visualization/path
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Name: Planning
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 0.20000000298023224
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 2
|
||||
Min Value: -1
|
||||
Value: false
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 85; 170; 255
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: simulation_map
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.10000000149011612
|
||||
Style: Boxes
|
||||
Topic: /map_generator/global_cloud
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 2.3399999141693115
|
||||
Min Value: 0.03999999910593033
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: map inflate
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.10000000149011612
|
||||
Style: Flat Squares
|
||||
Topic: /grid_map/occupancy_inflate
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 2.7592508792877197
|
||||
Min Value: -0.9228500127792358
|
||||
Value: false
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 187; 187; 187
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: real_map
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.10000000149011612
|
||||
Style: Boxes
|
||||
Topic: /grid_map/occupancy
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Enabled: false
|
||||
Name: Mapping
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /odom_visualization/robot
|
||||
Name: robot
|
||||
Namespaces:
|
||||
mesh: true
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
Enabled: true
|
||||
Name: Simulation
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 255; 253; 224
|
||||
Default Light: true
|
||||
Fixed Frame: world
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz_plugins/Goal3DTool
|
||||
Topic: /rviz/3d_nav_goal
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/ThirdPersonFollower
|
||||
Distance: 9.049065589904785
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: 2.164052724838257
|
||||
Y: -1.1675572395324707
|
||||
Z: 2.1457672119140625e-06
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.7747965455055237
|
||||
Target Frame: world
|
||||
Yaw: 4.195437908172607
|
||||
Saved:
|
||||
- Class: rviz/ThirdPersonFollower
|
||||
Distance: 17.48538589477539
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: -16.308002471923828
|
||||
Y: 0.4492051601409912
|
||||
Z: 8.589673598180525e-06
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: ThirdPersonFollower
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.0347968339920044
|
||||
Target Frame: <Fixed Frame>
|
||||
Yaw: 3.150407314300537
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000000a005600690065007700730000000114000001c9000000a400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001d1000000b50000000000000000fb0000000a0049006d00610067006502000001a2000001e1000000f8000000b5fb0000000a0064006500700074006800000002da000001010000000000000000fb0000000a0049006d0061006700650100000415000000f80000000000000000fb0000000a0049006d00610067006501000003f4000001190000000000000000fb0000000a006400650070007400680100000459000000f50000000000000000000000010000010f00000385fc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006100000003bfc0100000002fb0000000800540069006d0065000000000000000610000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1848
|
||||
X: 72
|
||||
Y: 27
|
||||
@ -0,0 +1,50 @@
|
||||
<launch>
|
||||
<include file="$(find min_snap)/launch/rviz.launch" />
|
||||
|
||||
<!-- size of map, change the size inflate x, y, z according to your application -->
|
||||
<arg name="map_size_x" value="10.0"/>
|
||||
<arg name="map_size_y" value="10.0"/>
|
||||
<arg name="map_size_z" value=" 3.0"/>
|
||||
<arg name="mean_vel" value="5.0" />
|
||||
|
||||
<!-- topic of your odometry such as VIO or LIO -->
|
||||
<arg name="odom_topic" value="/visual_slam/odom" />
|
||||
|
||||
<!-- use simulator -->
|
||||
<include file="$(find traj_server)/launch/my_sim.xml">
|
||||
<arg name="map_size_x_" value="$(arg map_size_x)"/>
|
||||
<arg name="map_size_y_" value="$(arg map_size_y)"/>
|
||||
<arg name="map_size_z_" value="$(arg map_size_z)"/>
|
||||
<arg name="c_num" value="200"/>
|
||||
<arg name="p_num" value="200"/>
|
||||
<arg name="min_dist" value="1.2"/>
|
||||
|
||||
<arg name="odometry_topic" value="$(arg odom_topic)" />
|
||||
</include>
|
||||
|
||||
<!-- minimum snap close_form, pub poly coefs -->
|
||||
<node pkg="min_snap" name="min_snap_generator" type="min_snap_generator" output="screen">
|
||||
<remap from="/rviz_goal" to="/rviz/3d_nav_goal" />
|
||||
<remap from="/goal_list" to="/planning/goal_list" />
|
||||
<remap from="/poly_coefs" to="/planning/poly_coefs" />
|
||||
<remap from="/odom_topic" to="$(arg odom_topic)" />
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<param name="meanvel" value="$(arg mean_vel)" />
|
||||
</node>
|
||||
|
||||
<!-- visualization, display goals, vel, acc, jerk, planning path -->
|
||||
<node pkg="prometheus_simulator_utils" name="min_snap_visual" type="min_snap_visual" output="screen">
|
||||
<remap from="/goal_list" to="/planning/goal_list" />
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<remap from="/traj_pts" to="/planning/traj_pts" />
|
||||
<remap from="/odometry" to="$(arg odom_topic)" />
|
||||
</node>
|
||||
|
||||
<!-- publish position_cmd -->
|
||||
<node pkg="traj_server" name="my_traj_server" type="my_traj_server" output="screen">
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<remap from="/poly_coefs" to="/planning/poly_coefs" />
|
||||
<remap from="/traj_pts" to="/planning/traj_pts" />
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -0,0 +1,50 @@
|
||||
<launch>
|
||||
<include file="$(find min_snap)/launch/rviz.launch" />
|
||||
|
||||
<!-- size of map, change the size inflate x, y, z according to your application -->
|
||||
<arg name="map_size_x" value="10.0"/>
|
||||
<arg name="map_size_y" value="10.0"/>
|
||||
<arg name="map_size_z" value=" 3.0"/>
|
||||
<arg name="mean_vel" value="0.5" />
|
||||
|
||||
<!-- topic of your odometry such as VIO or LIO -->
|
||||
<arg name="odom_topic" value="/vicon_imu_ekf_odom" />
|
||||
|
||||
<!-- use simulator -->
|
||||
<include file="$(find traj_server)/launch/real_traj_server.xml">
|
||||
<arg name="map_size_x_" value="$(arg map_size_x)"/>
|
||||
<arg name="map_size_y_" value="$(arg map_size_y)"/>
|
||||
<arg name="map_size_z_" value="$(arg map_size_z)"/>
|
||||
<arg name="c_num" value="200"/>
|
||||
<arg name="p_num" value="200"/>
|
||||
<arg name="min_dist" value="1.2"/>
|
||||
|
||||
<arg name="odometry_topic" value="$(arg odom_topic)" />
|
||||
</include>
|
||||
|
||||
<!-- minimum snap close_form, pub poly coefs -->
|
||||
<node pkg="min_snap" name="min_snap_generator" type="min_snap_generator" output="screen">
|
||||
<remap from="/rviz_goal" to="/rviz/3d_nav_goal" />
|
||||
<remap from="/goal_list" to="/planning/goal_list" />
|
||||
<remap from="/poly_coefs" to="/planning/poly_coefs" />
|
||||
<remap from="/odom_topic" to="$(arg odom_topic)" />
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<param name="meanvel" value="$(arg mean_vel)" />
|
||||
</node>
|
||||
|
||||
<!-- visualization, display goals, vel, acc, jerk, planning path -->
|
||||
<node pkg="my_visualization" name="min_snap_visual" type="min_snap_visual" output="screen">
|
||||
<remap from="/goal_list" to="/planning/goal_list" />
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<remap from="/traj_pts" to="/planning/traj_pts" />
|
||||
<remap from="/odometry" to="$(arg odom_topic)" />
|
||||
</node>
|
||||
|
||||
<!-- publish position_cmd -->
|
||||
<node pkg="traj_server" name="my_traj_server" type="my_traj_server" output="screen">
|
||||
<remap from="/position_cmd" to="/planning/pos_cmd" />
|
||||
<remap from="/poly_coefs" to="/planning/poly_coefs" />
|
||||
<remap from="/traj_pts" to="/planning/traj_pts" />
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -0,0 +1,3 @@
|
||||
<launch>
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find min_snap)/launch/default.rviz" required="true" />
|
||||
</launch>
|
||||
@ -0,0 +1,71 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>min_snap</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The min_snap package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="mywang@todo.todo">mywang</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/min_snap</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<build_depend>mini_snap_traj_utils</build_depend>
|
||||
<exec_depend>mini_snap_traj_utils</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,237 @@
|
||||
#include <min_snap/min_snap_closeform.h>
|
||||
|
||||
namespace my_planner
|
||||
{
|
||||
minsnapCloseform::minsnapCloseform(const vector<Eigen::Vector3d> &waypoints, double meanvel)
|
||||
{
|
||||
n_order = 7;
|
||||
wps = waypoints;
|
||||
n_seg = int(wps.size()) - 1;
|
||||
n_per_seg = n_order + 1;
|
||||
mean_vel = meanvel;
|
||||
}
|
||||
|
||||
void minsnapCloseform::Init(const vector<Eigen::Vector3d> &waypoints, double meanvel)
|
||||
{
|
||||
n_order = 7;
|
||||
wps = waypoints;
|
||||
n_seg = int(wps.size()) - 1;
|
||||
n_per_seg = n_order + 1;
|
||||
mean_vel = meanvel;
|
||||
}
|
||||
|
||||
// 0 means each segment time is one second.
|
||||
void minsnapCloseform::init_ts(int init_type)
|
||||
{
|
||||
const double dist_min = 2.0;
|
||||
ts = Eigen::VectorXd::Zero(n_seg);
|
||||
if (init_type)
|
||||
{
|
||||
Eigen::VectorXd dist(n_seg);
|
||||
double dist_sum = 0, t_sum = 0;
|
||||
for (int i = 0; i < n_seg; i++)
|
||||
{
|
||||
dist(i) = 0;
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
dist(i) += pow(wps[i + 1](j) - wps[i](j), 2);
|
||||
}
|
||||
dist(i) = sqrt(dist(i));
|
||||
if ((dist(i)) < dist_min)
|
||||
{
|
||||
dist(i) = sqrt(dist(i)) * 2;
|
||||
}
|
||||
dist_sum += dist(i);
|
||||
}
|
||||
dist(0) += 1;
|
||||
dist(n_seg - 1) += 1;
|
||||
dist_sum += 2;
|
||||
double T = dist_sum / mean_vel;
|
||||
for (int i = 0; i < n_seg - 1; i++)
|
||||
{
|
||||
ts(i) = dist(i) / dist_sum * T;
|
||||
t_sum += ts(i);
|
||||
}
|
||||
ts(n_seg - 1) = T - t_sum;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < n_seg; i++)
|
||||
{
|
||||
ts(i) = 1;
|
||||
}
|
||||
}
|
||||
cout << "ts: " << ts.transpose() << endl;
|
||||
}
|
||||
|
||||
int minsnapCloseform::fact(int n)
|
||||
{
|
||||
if (n < 0)
|
||||
{
|
||||
cout << "ERROR fact(" << n << ")" << endl;
|
||||
return 0;
|
||||
}
|
||||
else if (n == 0)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
return n * fact(n - 1);
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::VectorXd minsnapCloseform::calDecVel(const Eigen::VectorXd decvel)
|
||||
{
|
||||
Eigen::VectorXd temp(Eigen::VectorXd::Zero((n_seg + 1) * 4));
|
||||
for (int i = 0; i < (n_seg + 1) / 2; i++)
|
||||
{
|
||||
temp.segment(i * 8, 8) = decvel.segment((i * 2) * 8, 8);
|
||||
}
|
||||
if (n_seg % 2 != 1)
|
||||
{
|
||||
temp.tail(4) = decvel.tail(4);
|
||||
}
|
||||
|
||||
return temp;
|
||||
}
|
||||
|
||||
void minsnapCloseform::calQ()
|
||||
{
|
||||
Q = Eigen::MatrixXd::Zero(n_seg * (n_order + 1), n_seg * (n_order + 1));
|
||||
for (int k = 0; k < n_seg; k++)
|
||||
{
|
||||
Eigen::MatrixXd Q_k(Eigen::MatrixXd::Zero(n_order + 1, n_order + 1));
|
||||
for (int i = 5; i <= n_order + 1; i++)
|
||||
{
|
||||
for (int j = 5; j <= n_order + 1; j++)
|
||||
{
|
||||
Q_k(i - 1, j - 1) = fact(i) / fact(i - 4) *
|
||||
fact(j) / fact(j - 4) /
|
||||
(i + j - 7) * pow(ts(k), i + j - 7);
|
||||
}
|
||||
}
|
||||
Q.block(k * (n_order + 1), k * (n_order + 1), n_order + 1, n_order + 1) = Q_k;
|
||||
}
|
||||
}
|
||||
|
||||
void minsnapCloseform::calM()
|
||||
{
|
||||
M = Eigen::MatrixXd::Zero(n_seg * (n_order + 1), n_seg * (n_order + 1));
|
||||
for (int k = 0; k < n_seg; k++)
|
||||
{
|
||||
Eigen::MatrixXd M_k(Eigen::MatrixXd::Zero(n_order + 1, n_order + 1));
|
||||
M_k(0, 0) = 1;
|
||||
M_k(1, 1) = 1;
|
||||
M_k(2, 2) = 2;
|
||||
M_k(3, 3) = 6;
|
||||
for (int i = 0; i <= n_order; i++)
|
||||
{
|
||||
for (int j = 0; j <= 3; j++)
|
||||
{
|
||||
if (i >= j)
|
||||
{
|
||||
M_k(j + 4, i) = fact(i) / fact(i - j) * pow(ts(k), i - j);
|
||||
}
|
||||
}
|
||||
}
|
||||
M.block(k * (n_order + 1), k * (n_order + 1), n_order + 1, n_order + 1) = M_k;
|
||||
}
|
||||
}
|
||||
|
||||
void minsnapCloseform::calCt()
|
||||
{
|
||||
int m = n_seg * (n_order + 1);
|
||||
int n = 4 * (n_seg + 1);
|
||||
Ct = Eigen::MatrixXd::Zero(m, n);
|
||||
for (int i = 0; i < n_seg; i++)
|
||||
{
|
||||
Ct.block(i * (n_order + 1), i * 4, n_order + 1, n_order + 1) =
|
||||
Eigen::MatrixXd::Identity(n_order + 1, n_order + 1);
|
||||
}
|
||||
|
||||
Eigen::MatrixXd dF_Ct = Eigen::MatrixXd::Zero(m, 8 + (n_seg - 1));
|
||||
Eigen::MatrixXd dP_Ct = Eigen::MatrixXd::Zero(m, (n_seg - 1) * 3);
|
||||
dF_Ct.leftCols(4) = Ct.leftCols(4);
|
||||
for (int i = 0; i < n_seg - 1; i++)
|
||||
{
|
||||
dF_Ct.col(i + 4) = Ct.col(i * 4 + 4);
|
||||
dP_Ct.middleCols(i * 3, 3) = Ct.middleCols(i * 4 + 5, 3);
|
||||
}
|
||||
dF_Ct.rightCols(4) = Ct.rightCols(4);
|
||||
Ct << dF_Ct, dP_Ct;
|
||||
}
|
||||
|
||||
std::pair<Eigen::VectorXd, Eigen::VectorXd> minsnapCloseform::MinSnapCloseFormServer(const Eigen::VectorXd &wp)
|
||||
{
|
||||
std::pair<Eigen::VectorXd, Eigen::MatrixXd> return_vel;
|
||||
Eigen::VectorXd poly_coef, dec_vel;
|
||||
Eigen::VectorXd dF(n_seg + 7), dP(3 * (n_seg - 1));
|
||||
Eigen::VectorXd start_cond(4), end_cond(4), d(4 * n_seg + 4);
|
||||
Eigen::MatrixXd R, R_pp, R_fp;
|
||||
start_cond << wp.head(1), 0, 0, 0;
|
||||
end_cond << wp.tail(1), 0, 0, 0;
|
||||
init_ts(1);
|
||||
|
||||
calQ();
|
||||
calM();
|
||||
calCt();
|
||||
|
||||
|
||||
R = Ct.transpose() * M.inverse().transpose() * Q * M.inverse() * Ct;
|
||||
R_pp = R.bottomRightCorner(3 * (n_seg - 1), 3 * (n_seg - 1));
|
||||
R_fp = R.topRightCorner(n_seg + 7, 3 * (n_seg - 1));
|
||||
|
||||
dF << start_cond, wp.segment(1, n_seg - 1), end_cond;
|
||||
dP = -R_pp.inverse() * R_fp.transpose() * dF;
|
||||
d << dF, dP;
|
||||
|
||||
dec_vel = Ct * d;
|
||||
poly_coef = M.inverse() * dec_vel;
|
||||
|
||||
return_vel.first = poly_coef;
|
||||
return_vel.second = dec_vel;
|
||||
return return_vel;
|
||||
}
|
||||
|
||||
void minsnapCloseform::calMinsnap_polycoef()
|
||||
{
|
||||
Eigen::VectorXd wps_x(n_seg + 1), wps_y(n_seg + 1), wps_z(n_seg + 1);
|
||||
for (int i = 0; i < n_seg + 1; i++)
|
||||
{
|
||||
wps_x(i) = wps[i](0);
|
||||
wps_y(i) = wps[i](1);
|
||||
wps_z(i) = wps[i](2);
|
||||
}
|
||||
std::pair<Eigen::VectorXd, Eigen::VectorXd> return_vel;
|
||||
return_vel = MinSnapCloseFormServer(wps_x);
|
||||
poly_coef_x = return_vel.first;
|
||||
dec_vel_x = calDecVel(return_vel.second);
|
||||
return_vel = MinSnapCloseFormServer(wps_y);
|
||||
poly_coef_y = return_vel.first;
|
||||
dec_vel_y = calDecVel(return_vel.second);
|
||||
return_vel = MinSnapCloseFormServer(wps_z);
|
||||
poly_coef_z = return_vel.first;
|
||||
dec_vel_z = calDecVel(return_vel.second);
|
||||
}
|
||||
|
||||
Eigen::MatrixXd minsnapCloseform::getPolyCoef()
|
||||
{
|
||||
Eigen::MatrixXd poly_coef(poly_coef_x.size(), 3);
|
||||
poly_coef << poly_coef_x, poly_coef_y, poly_coef_z;
|
||||
return poly_coef;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd minsnapCloseform::getDecVel()
|
||||
{
|
||||
Eigen::MatrixXd dec_vel(dec_vel_x.size(), 3);
|
||||
dec_vel << dec_vel_x, dec_vel_y, dec_vel_z;
|
||||
return dec_vel;
|
||||
}
|
||||
|
||||
Eigen::VectorXd minsnapCloseform::getTime()
|
||||
{
|
||||
return ts;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,137 @@
|
||||
#include <ros/ros.h>
|
||||
#include <iostream>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/PoseArray.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <quadrotor_msgs/PolynomialTrajectory.h>
|
||||
#include <quadrotor_msgs/PositionCommand.h>
|
||||
#include "min_snap/min_snap_closeform.h"
|
||||
|
||||
ros::Publisher goal_list_pub;
|
||||
ros::Publisher poly_coef_pub;
|
||||
ros::Subscriber rviz_goal_sub;
|
||||
ros::Subscriber odom_sub;
|
||||
|
||||
int id = 0;
|
||||
double mean_vel = 0;
|
||||
const int GOAL_HEIGHT = 1;
|
||||
nav_msgs::Odometry odom;
|
||||
geometry_msgs::Pose goal_pt;
|
||||
geometry_msgs::PoseArray goal_list;
|
||||
my_planner::minsnapCloseform minsnap_solver;
|
||||
std::vector<Eigen::Vector3d> waypoints;
|
||||
quadrotor_msgs::PolynomialTrajectory poly_pub_topic;
|
||||
|
||||
void pub_poly_coefs()
|
||||
{
|
||||
Eigen::MatrixXd poly_coef = minsnap_solver.getPolyCoef();
|
||||
Eigen::MatrixXd dec_vel = minsnap_solver.getDecVel();
|
||||
Eigen::VectorXd time = minsnap_solver.getTime();
|
||||
|
||||
poly_pub_topic.num_segment = goal_list.poses.size() - 1;
|
||||
poly_pub_topic.coef_x.clear();
|
||||
poly_pub_topic.coef_y.clear();
|
||||
poly_pub_topic.coef_z.clear();
|
||||
poly_pub_topic.time.clear();
|
||||
poly_pub_topic.trajectory_id = id;
|
||||
|
||||
// display decision variable
|
||||
ROS_WARN("decision variable:");
|
||||
for (int i = 0; i < goal_list.poses.size(); i++)
|
||||
{
|
||||
cout << "Point number = " << i + 1 << endl
|
||||
<< dec_vel.middleRows(i * 4, 4) << endl;
|
||||
}
|
||||
|
||||
for (int i = 0; i < time.size(); i++)
|
||||
{
|
||||
for (int j = (i + 1) * 8 - 1; j >= i * 8; j--)
|
||||
{
|
||||
poly_pub_topic.coef_x.push_back(poly_coef(j, 0));
|
||||
poly_pub_topic.coef_y.push_back(poly_coef(j, 1));
|
||||
poly_pub_topic.coef_z.push_back(poly_coef(j, 2));
|
||||
}
|
||||
poly_pub_topic.time.push_back(time(i));
|
||||
}
|
||||
|
||||
poly_pub_topic.header.frame_id = "world";
|
||||
poly_pub_topic.header.stamp = ros::Time::now();
|
||||
|
||||
poly_coef_pub.publish(poly_pub_topic);
|
||||
}
|
||||
|
||||
void solve_min_snap()
|
||||
{
|
||||
Eigen::Vector3d wp;
|
||||
waypoints.clear();
|
||||
for (int i = 0; i < int(goal_list.poses.size()); i++)
|
||||
{
|
||||
wp << goal_list.poses[i].position.x, goal_list.poses[i].position.y, goal_list.poses[i].position.z;
|
||||
waypoints.push_back(wp);
|
||||
}
|
||||
minsnap_solver.Init(waypoints, mean_vel);
|
||||
ROS_INFO("Init success");
|
||||
minsnap_solver.calMinsnap_polycoef();
|
||||
pub_poly_coefs();
|
||||
}
|
||||
|
||||
void rviz_goal_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
|
||||
{
|
||||
goal_pt = msg->pose;
|
||||
if (goal_pt.position.z < 0)
|
||||
{
|
||||
goal_pt.position.z = GOAL_HEIGHT;
|
||||
goal_list.poses.push_back(goal_pt);
|
||||
goal_pt.position = odom.pose.pose.position;
|
||||
goal_list.poses.insert(goal_list.poses.begin(), goal_pt);
|
||||
goal_list.header.stamp = ros::Time::now();
|
||||
goal_list.header.frame_id = "world";
|
||||
goal_list.header.seq = id++;
|
||||
goal_list_pub.publish(goal_list);
|
||||
solve_min_snap();
|
||||
ROS_INFO("solver finished");
|
||||
goal_list.poses.clear();
|
||||
}
|
||||
else
|
||||
{
|
||||
goal_pt.position.z = GOAL_HEIGHT;
|
||||
goal_list.poses.push_back(goal_pt);
|
||||
}
|
||||
}
|
||||
|
||||
void odom_goal_cb(const nav_msgs::Odometry::ConstPtr &msg)
|
||||
{
|
||||
odom = *msg;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "min_snap_generator");
|
||||
ros::NodeHandle nh("~");
|
||||
ros::Rate rate(10);
|
||||
|
||||
// 【订阅】里程计（此处注意话题名称替换）
|
||||
odom_sub = nh.subscribe("/odom_topic", 10, odom_goal_cb);
|
||||
// 【订阅】RVIZ目标点
|
||||
rviz_goal_sub = nh.subscribe("/rviz_goal", 10, rviz_goal_cb);
|
||||
// 【发布】目标点，用于目标点显示
|
||||
goal_list_pub = nh.advertise<geometry_msgs::PoseArray>("/goal_list", 10);
|
||||
// 发布多项式轨迹
|
||||
poly_coef_pub = nh.advertise<quadrotor_msgs::PolynomialTrajectory>("/poly_coefs", 10);
|
||||
|
||||
poly_pub_topic.num_order = 7;
|
||||
poly_pub_topic.start_yaw = 0;
|
||||
poly_pub_topic.final_yaw = 0;
|
||||
poly_pub_topic.mag_coeff = 0;
|
||||
poly_pub_topic.order.push_back(0);
|
||||
|
||||
ros::param::get("/min_snap_generator/meanvel", mean_vel);
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
ros::spinOnce();
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,163 @@
|
||||
#include <ros/ros.h>
|
||||
#include <iostream>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/PoseArray.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <quadrotor_msgs/PolynomialTrajectory.h>
|
||||
#include <quadrotor_msgs/PositionCommand.h>
|
||||
#include "min_snap/min_snap_closeform.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace my_planner;
|
||||
|
||||
ros::Publisher poly_coef_pub,way_point_list_pub;
|
||||
ros::Subscriber odom_sub;
|
||||
|
||||
|
||||
bool get_goal{false};
|
||||
bool odom_valid;
|
||||
geometry_msgs::PoseArray way_point_list;
|
||||
minsnapCloseform minsnap_solver;
|
||||
quadrotor_msgs::PolynomialTrajectory poly_pub_topic;
|
||||
|
||||
void mainloop(const ros::TimerEvent &e);
|
||||
|
||||
void cal_poly_coefs(geometry_msgs::PoseArray goal_list, double mean_vel)
|
||||
{
|
||||
// 航点容器
|
||||
std::vector<Eigen::Vector3d> waypoints;
|
||||
Eigen::Vector3d wp;
|
||||
waypoints.clear();
|
||||
|
||||
for (int i = 0; i < int(goal_list.poses.size()); i++)
|
||||
{
|
||||
wp << goal_list.poses[i].position.x, goal_list.poses[i].position.y, goal_list.poses[i].position.z;
|
||||
waypoints.push_back(wp);
|
||||
}
|
||||
// 初始化
|
||||
minsnap_solver.Init(waypoints, mean_vel);
|
||||
// 计算轨迹
|
||||
minsnap_solver.calMinsnap_polycoef();
|
||||
|
||||
// 读取轨迹
|
||||
Eigen::MatrixXd poly_coef = minsnap_solver.getPolyCoef();
|
||||
Eigen::MatrixXd dec_vel = minsnap_solver.getDecVel();
|
||||
Eigen::VectorXd time = minsnap_solver.getTime();
|
||||
|
||||
// poly_pub_topic.num_segment = 1;
|
||||
poly_pub_topic.num_segment = goal_list.poses.size() - 1;
|
||||
poly_pub_topic.coef_x.clear();
|
||||
poly_pub_topic.coef_y.clear();
|
||||
poly_pub_topic.coef_z.clear();
|
||||
poly_pub_topic.time.clear();
|
||||
poly_pub_topic.trajectory_id = 0;
|
||||
|
||||
for (int i = 0; i < time.size(); i++)
|
||||
{
|
||||
for (int j = (i + 1) * 8 - 1; j >= i * 8; j--)
|
||||
{
|
||||
poly_pub_topic.coef_x.push_back(poly_coef(j, 0));
|
||||
poly_pub_topic.coef_y.push_back(poly_coef(j, 1));
|
||||
poly_pub_topic.coef_z.push_back(poly_coef(j, 2));
|
||||
}
|
||||
poly_pub_topic.time.push_back(time(i));
|
||||
}
|
||||
|
||||
poly_pub_topic.header.frame_id = "world";
|
||||
poly_pub_topic.header.stamp = ros::Time::now();
|
||||
|
||||
// 发布轨迹多项式
|
||||
poly_coef_pub.publish(poly_pub_topic);
|
||||
}
|
||||
|
||||
void goal_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
|
||||
{
|
||||
goal_pt = msg->pose;
|
||||
if (goal_pt.position.z < 0)
|
||||
{
|
||||
goal_pt.position.z = GOAL_HEIGHT;
|
||||
goal_list.poses.push_back(goal_pt);
|
||||
goal_pt.position = odom.pose.pose.position;
|
||||
goal_list.poses.insert(goal_list.poses.begin(), goal_pt);
|
||||
goal_list.header.stamp = ros::Time::now();
|
||||
goal_list.header.frame_id = "world";
|
||||
goal_list.header.seq = id++;
|
||||
goal_list_pub.publish(goal_list);
|
||||
solve_min_snap();
|
||||
ROS_INFO("solver finished");
|
||||
goal_list.poses.clear();
|
||||
}
|
||||
else
|
||||
{
|
||||
goal_pt.position.z = GOAL_HEIGHT;
|
||||
goal_list.poses.push_back(goal_pt);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "mini_snap");
|
||||
ros::NodeHandle nh("~");
|
||||
ros::Rate rate(200.0);
|
||||
|
||||
//【订阅】odom
|
||||
uav_odom_sub = nh.subscribe<nav_msgs::Odometry>("/prometheus/uav_odom_ned", 10, uav_odom_cb);
|
||||
|
||||
rviz_goal_sub = nh.subscribe("/prometheus/goal", 10, goal_cb);
|
||||
|
||||
way_point_list_pub = nh.advertise<geometry_msgs::PoseArray>("/goal_list", 10);
|
||||
|
||||
poly_coef_pub = nh.advertise<quadrotor_msgs::PolynomialTrajectory>("/poly_coefs", 10);
|
||||
|
||||
// 【定时器】mainloop
|
||||
mainloop_timer = nh.createTimer(ros::Duration(0.01), mainloop);
|
||||
|
||||
odom_valid = false;
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void mainloop(const ros::TimerEvent &e)
|
||||
{
|
||||
if(!odom_valid)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if(!get_goal)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// 航点清空
|
||||
way_point_list.poses.clear();
|
||||
geometry_msgs::Pose goal_pt;
|
||||
|
||||
// 加入当前位置
|
||||
goal_pt.position = uav_odom_enu.pose.pose.position;
|
||||
way_point_list.poses.push_back(goal_pt);
|
||||
|
||||
goal_pt.position.x = 1.2;
|
||||
goal_pt.position.y = -1.2;
|
||||
goal_pt.position.z = 2.0;
|
||||
way_point_list.poses.push_back(goal_pt);
|
||||
|
||||
goal_pt.position.x = circle_origin_pos_1[0] - 5.0*cos(circle_origin_yaw_1);
|
||||
goal_pt.position.y = circle_origin_pos_1[1] - 5.0*sin(circle_origin_yaw_1);
|
||||
goal_pt.position.z = circle_origin_pos_1[2] - 0.3; // circle_origin_pos_1[2]:2.77
|
||||
way_point_list.poses.push_back(goal_pt);
|
||||
|
||||
goal_pt.position.x = circle_origin_pos_1[0] - 3.0*cos(circle_origin_yaw_1);
|
||||
goal_pt.position.y = circle_origin_pos_1[1] - 3.0*sin(circle_origin_yaw_1);
|
||||
goal_pt.position.z = circle_origin_pos_1[2];
|
||||
way_point_list.poses.push_back(goal_pt);
|
||||
|
||||
way_point_list.header.stamp = ros::Time::now();
|
||||
way_point_list.header.frame_id = "world";
|
||||
way_point_list.header.seq = 1;
|
||||
way_point_list_pub.publish(way_point_list);
|
||||
|
||||
cal_poly_coefs(way_point_list, 4.0f);
|
||||
}
|
||||
Binary file not shown.
@ -0,0 +1,217 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(traj_server)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
quadrotor_msgs
|
||||
mini_snap_traj_utils
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a exec_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
# generate_messages(
|
||||
# DEPENDENCIES
|
||||
# std_msgs
|
||||
# )
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES traj_server
|
||||
# CATKIN_DEPENDS roscpp rospy std_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
# include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/traj_server.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
# add_executable(${PROJECT_NAME}_node src/traj_server_node.cpp)
|
||||
|
||||
## Rename C++ executable without prefix
|
||||
## The above recommended prefix causes long target names, the following renames the
|
||||
## target back to the shorter version for ease of user use
|
||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(${PROJECT_NAME}_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
add_executable(my_traj_server src/my_traj_server.cpp)
|
||||
target_link_libraries(my_traj_server ${catkin_LIBRARIES})
|
||||
# add_dependencies(my_traj_server ${${PROJECT_NAME}_EXPORTED_TARGETS})
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# catkin_install_python(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
|
||||
# install(TARGETS ${PROJECT_NAME}_node
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark libraries for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
|
||||
# install(TARGETS ${PROJECT_NAME}
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_traj_server.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
@ -0,0 +1,70 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>traj_server</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The traj_server package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="mywang@todo.todo">mywang</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/traj_server</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<build_depend>mini_snap_traj_utils</build_depend>
|
||||
<exec_depend>mini_snap_traj_utils</exec_depend>
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,264 @@
|
||||
#include <ros/ros.h>
|
||||
#include "quadrotor_msgs/PositionCommand.h"
|
||||
#include "quadrotor_msgs/PolynomialTrajectory.h"
|
||||
#include "mini_snap_traj_utils/polynomial_traj.h"
|
||||
#include <geometry_msgs/PoseArray.h>
|
||||
|
||||
ros::Publisher pose_cmd_pub;
|
||||
ros::Publisher traj_pts_pub;
|
||||
ros::Subscriber poly_traj_sub;
|
||||
|
||||
PolynomialTraj Poly_traj;
|
||||
quadrotor_msgs::PositionCommand cmd;
|
||||
ros::Time start_time;
|
||||
double pos_gain[3] = {0, 0, 0};
|
||||
double vel_gain[3] = {0, 0, 0};
|
||||
|
||||
double last_yaw_, last_yaw_dot_;
|
||||
bool receive_traj_ = false;
|
||||
int traj_id_;
|
||||
double traj_duration_, time_forward_;
|
||||
|
||||
std::pair<double, double> calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last)
|
||||
{
|
||||
constexpr double PI = 3.1415926;
|
||||
constexpr double YAW_DOT_MAX_PER_SEC = PI;
|
||||
// constexpr double YAW_DOT_DOT_MAX_PER_SEC = PI;
|
||||
std::pair<double, double> yaw_yawdot(0, 0);
|
||||
double yaw = 0;
|
||||
double yawdot = 0;
|
||||
|
||||
// Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? traj_[0].evaluateDeBoorT(t_cur + time_forward_) - pos : traj_[0].evaluateDeBoorT(traj_duration_) - pos;
|
||||
Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? Poly_traj.evaluate(t_cur + time_forward_) - pos : Poly_traj.evaluate(traj_duration_) - pos;
|
||||
double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_;
|
||||
double max_yaw_change = YAW_DOT_MAX_PER_SEC * (time_now - time_last).toSec();
|
||||
if (yaw_temp - last_yaw_ > PI)
|
||||
{
|
||||
if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ - max_yaw_change;
|
||||
if (yaw < -PI)
|
||||
yaw += 2 * PI;
|
||||
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = yaw_temp;
|
||||
if (yaw - last_yaw_ > PI)
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
else
|
||||
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
|
||||
}
|
||||
}
|
||||
else if (yaw_temp - last_yaw_ < -PI)
|
||||
{
|
||||
if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ + max_yaw_change;
|
||||
if (yaw > PI)
|
||||
yaw -= 2 * PI;
|
||||
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = yaw_temp;
|
||||
if (yaw - last_yaw_ < -PI)
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
else
|
||||
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (yaw_temp - last_yaw_ < -max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ - max_yaw_change;
|
||||
if (yaw < -PI)
|
||||
yaw += 2 * PI;
|
||||
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else if (yaw_temp - last_yaw_ > max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ + max_yaw_change;
|
||||
if (yaw > PI)
|
||||
yaw -= 2 * PI;
|
||||
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = yaw_temp;
|
||||
if (yaw - last_yaw_ > PI)
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
else if (yaw - last_yaw_ < -PI)
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
else
|
||||
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
|
||||
}
|
||||
}
|
||||
|
||||
if (fabs(yaw - last_yaw_) <= max_yaw_change)
|
||||
yaw = 0.5 * last_yaw_ + 0.5 * yaw; // nieve LPF
|
||||
yawdot = 0.5 * last_yaw_dot_ + 0.5 * yawdot;
|
||||
last_yaw_ = yaw;
|
||||
last_yaw_dot_ = yawdot;
|
||||
|
||||
yaw_yawdot.first = yaw;
|
||||
yaw_yawdot.second = yawdot;
|
||||
|
||||
return yaw_yawdot;
|
||||
}
|
||||
|
||||
void polyTrajCallback(quadrotor_msgs::PolynomialTrajectory::ConstPtr msg)
|
||||
{
|
||||
ROS_INFO("[my Traj server]:receive poly_traj");
|
||||
|
||||
Poly_traj.reset();
|
||||
int idx;
|
||||
for (int i = 0; i < msg->num_segment; i++)
|
||||
{
|
||||
vector<double> cx, cy, cz;
|
||||
for (int j = 0; j < msg->num_order + 1; j++)
|
||||
{
|
||||
idx = i * (msg->num_order + 1) + j;
|
||||
cx.push_back(msg->coef_x[idx]);
|
||||
cy.push_back(msg->coef_y[idx]);
|
||||
cz.push_back(msg->coef_z[idx]);
|
||||
}
|
||||
Poly_traj.addSegment(cx, cy, cz, msg->time[i]);
|
||||
}
|
||||
Poly_traj.init();
|
||||
|
||||
// ROS_INFO("[my traj server]:time=%f", Poly_traj.getTimes().front());
|
||||
|
||||
start_time = ros::Time::now();
|
||||
traj_id_ = msg->trajectory_id;
|
||||
traj_duration_ = Poly_traj.getTimeSum();
|
||||
receive_traj_ = true;
|
||||
}
|
||||
|
||||
void pub_traj(double t_cur)
|
||||
{
|
||||
static int old_cnt = 0;
|
||||
int cnt = (int)((traj_duration_ - t_cur) * 2) + 1;
|
||||
|
||||
if (cnt != old_cnt)
|
||||
{
|
||||
geometry_msgs::PoseArray traj_pts;
|
||||
geometry_msgs::Pose traj_pt;
|
||||
Eigen::Vector3d opt_pt(Eigen::Vector3d::Zero());
|
||||
|
||||
traj_pts.header.stamp = ros::Time::now();
|
||||
for (int i = 0; i < cnt + 1; i++)
|
||||
{
|
||||
opt_pt = Poly_traj.evaluate(std::min(t_cur + i * 0.5, traj_duration_));
|
||||
traj_pt.orientation.w = 1.0;
|
||||
traj_pt.position.x = opt_pt(0);
|
||||
traj_pt.position.y = opt_pt(1);
|
||||
traj_pt.position.z = opt_pt(2);
|
||||
traj_pts.poses.push_back(traj_pt);
|
||||
}
|
||||
traj_pts_pub.publish(traj_pts);
|
||||
}
|
||||
old_cnt = cnt;
|
||||
}
|
||||
|
||||
void cmdCallback(const ros::TimerEvent &e)
|
||||
{
|
||||
if (!receive_traj_)
|
||||
return;
|
||||
|
||||
Eigen::Vector3d pos(Eigen::Vector3d::Zero());
|
||||
Eigen::Vector3d vel(Eigen::Vector3d::Zero());
|
||||
Eigen::Vector3d acc(Eigen::Vector3d::Zero());
|
||||
Eigen::Vector3d jerk(Eigen::Vector3d::Zero());
|
||||
std::pair<double, double> yaw_yawdot(0, 0);
|
||||
|
||||
ros::Time time_now = ros::Time::now();
|
||||
double t_cur = (time_now - start_time).toSec();
|
||||
// ROS_INFO("time = %f", t_cur);
|
||||
static ros::Time time_last = ros::Time::now();
|
||||
|
||||
if (t_cur < traj_duration_ && t_cur >= 0.0)
|
||||
{
|
||||
pos = Poly_traj.evaluate(t_cur);
|
||||
vel = Poly_traj.evaluateVel(t_cur);
|
||||
acc = Poly_traj.evaluateAcc(t_cur);
|
||||
jerk = Poly_traj.evaluateJerk(t_cur);
|
||||
yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last);
|
||||
pub_traj(t_cur + 0.5);
|
||||
}
|
||||
else if (t_cur >= traj_duration_)
|
||||
{
|
||||
pos = Poly_traj.evaluate(traj_duration_);
|
||||
yaw_yawdot.first = last_yaw_;
|
||||
yaw_yawdot.second = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("[my Traj server]: invalid time.");
|
||||
}
|
||||
time_last = time_now;
|
||||
|
||||
cmd.header.stamp = time_now;
|
||||
cmd.header.frame_id = "world";
|
||||
cmd.trajectory_id = traj_id_;
|
||||
|
||||
cmd.position.x = pos(0);
|
||||
cmd.position.y = pos(1);
|
||||
cmd.position.z = pos(2);
|
||||
|
||||
cmd.velocity.x = vel(0);
|
||||
cmd.velocity.y = vel(1);
|
||||
cmd.velocity.z = vel(2);
|
||||
|
||||
cmd.acceleration.x = acc(0);
|
||||
cmd.acceleration.y = acc(1);
|
||||
cmd.acceleration.z = acc(2);
|
||||
|
||||
cmd.jerk.x = jerk(0);
|
||||
cmd.jerk.y = jerk(1);
|
||||
cmd.jerk.z = jerk(2);
|
||||
|
||||
cmd.yaw = yaw_yawdot.first;
|
||||
cmd.yaw_dot = yaw_yawdot.second;
|
||||
|
||||
last_yaw_ = cmd.yaw;
|
||||
|
||||
pose_cmd_pub.publish(cmd);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "my_traj_server");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
pose_cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);
|
||||
traj_pts_pub = nh.advertise<geometry_msgs::PoseArray>("/traj_pts", 50);
|
||||
poly_traj_sub = nh.subscribe("/poly_coefs", 10, polyTrajCallback);
|
||||
|
||||
ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback);
|
||||
|
||||
/* control parameter */
|
||||
cmd.kx[0] = pos_gain[0];
|
||||
cmd.kx[1] = pos_gain[1];
|
||||
cmd.kx[2] = pos_gain[2];
|
||||
|
||||
cmd.kv[0] = vel_gain[0];
|
||||
cmd.kv[1] = vel_gain[1];
|
||||
cmd.kv[2] = vel_gain[2];
|
||||
last_yaw_ = 0.0;
|
||||
last_yaw_dot_ = 0.0;
|
||||
time_forward_ = 1.0;
|
||||
|
||||
ros::Duration(1.0).sleep();
|
||||
|
||||
ROS_INFO("[my Traj server]: ready.");
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,324 @@
|
||||
#include <ros/ros.h>
|
||||
#include "quadrotor_msgs/PositionCommand.h"
|
||||
#include "quadrotor_msgs/PolynomialTrajectory.h"
|
||||
#include "mini_snap_traj_utils/polynomial_traj.h"
|
||||
#include <geometry_msgs/PoseArray.h>
|
||||
#include "geometry_utils.h"
|
||||
#include <prometheus_msgs/UAVCommand.h>
|
||||
|
||||
ros::Publisher pose_cmd_pub;
|
||||
ros::Publisher traj_pts_pub;
|
||||
ros::Subscriber poly_traj_sub;
|
||||
ros::Publisher uav_cmd_pub;
|
||||
|
||||
PolynomialTraj Poly_traj;
|
||||
quadrotor_msgs::PositionCommand cmd;
|
||||
ros::Time start_time;
|
||||
double pos_gain[3] = {0, 0, 0};
|
||||
double vel_gain[3] = {0, 0, 0};
|
||||
|
||||
double last_yaw_, last_yaw_dot_;
|
||||
bool receive_traj_ = false;
|
||||
int traj_id_;
|
||||
double traj_duration_, time_forward_;
|
||||
|
||||
double time_traj;
|
||||
|
||||
std::pair<double, double> calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last)
|
||||
{
|
||||
constexpr double PI = 3.1415926;
|
||||
constexpr double YAW_DOT_MAX_PER_SEC = PI;
|
||||
// constexpr double YAW_DOT_DOT_MAX_PER_SEC = PI;
|
||||
std::pair<double, double> yaw_yawdot(0, 0);
|
||||
double yaw = 0;
|
||||
double yawdot = 0;
|
||||
|
||||
// Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? traj_[0].evaluateDeBoorT(t_cur + time_forward_) - pos : traj_[0].evaluateDeBoorT(traj_duration_) - pos;
|
||||
Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? Poly_traj.evaluate(t_cur + time_forward_) - pos : Poly_traj.evaluate(traj_duration_) - pos;
|
||||
double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_;
|
||||
double max_yaw_change = YAW_DOT_MAX_PER_SEC * (time_now - time_last).toSec();
|
||||
if (yaw_temp - last_yaw_ > PI)
|
||||
{
|
||||
if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ - max_yaw_change;
|
||||
if (yaw < -PI)
|
||||
yaw += 2 * PI;
|
||||
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = yaw_temp;
|
||||
if (yaw - last_yaw_ > PI)
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
else
|
||||
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
|
||||
}
|
||||
}
|
||||
else if (yaw_temp - last_yaw_ < -PI)
|
||||
{
|
||||
if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ + max_yaw_change;
|
||||
if (yaw > PI)
|
||||
yaw -= 2 * PI;
|
||||
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = yaw_temp;
|
||||
if (yaw - last_yaw_ < -PI)
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
else
|
||||
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (yaw_temp - last_yaw_ < -max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ - max_yaw_change;
|
||||
if (yaw < -PI)
|
||||
yaw += 2 * PI;
|
||||
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else if (yaw_temp - last_yaw_ > max_yaw_change)
|
||||
{
|
||||
yaw = last_yaw_ + max_yaw_change;
|
||||
if (yaw > PI)
|
||||
yaw -= 2 * PI;
|
||||
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
}
|
||||
else
|
||||
{
|
||||
yaw = yaw_temp;
|
||||
if (yaw - last_yaw_ > PI)
|
||||
yawdot = -YAW_DOT_MAX_PER_SEC;
|
||||
else if (yaw - last_yaw_ < -PI)
|
||||
yawdot = YAW_DOT_MAX_PER_SEC;
|
||||
else
|
||||
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
|
||||
}
|
||||
}
|
||||
|
||||
if (fabs(yaw - last_yaw_) <= max_yaw_change)
|
||||
yaw = 0.5 * last_yaw_ + 0.5 * yaw; // nieve LPF
|
||||
yawdot = 0.5 * last_yaw_dot_ + 0.5 * yawdot;
|
||||
last_yaw_ = yaw;
|
||||
last_yaw_dot_ = yawdot;
|
||||
|
||||
yaw_yawdot.first = yaw;
|
||||
yaw_yawdot.second = yawdot;
|
||||
|
||||
return yaw_yawdot;
|
||||
}
|
||||
|
||||
void polyTrajCallback(quadrotor_msgs::PolynomialTrajectory::ConstPtr msg)
|
||||
{
|
||||
ROS_INFO("[my Traj server]:receive poly_traj");
|
||||
|
||||
Poly_traj.reset();
|
||||
int idx;
|
||||
for (int i = 0; i < msg->num_segment; i++)
|
||||
{
|
||||
vector<double> cx, cy, cz;
|
||||
for (int j = 0; j < msg->num_order + 1; j++)
|
||||
{
|
||||
idx = i * (msg->num_order + 1) + j;
|
||||
cx.push_back(msg->coef_x[idx]);
|
||||
cy.push_back(msg->coef_y[idx]);
|
||||
cz.push_back(msg->coef_z[idx]);
|
||||
}
|
||||
// 前面三个轨迹参数，后面是这段轨迹的时间
|
||||
Poly_traj.addSegment(cx, cy, cz, msg->time[i]);
|
||||
}
|
||||
Poly_traj.init();
|
||||
|
||||
|
||||
|
||||
start_time = ros::Time::now();
|
||||
traj_id_ = msg->trajectory_id;
|
||||
traj_duration_ = Poly_traj.getTimeSum();
|
||||
receive_traj_ = true;
|
||||
|
||||
// 打印第一段的时间
|
||||
ROS_INFO("[my traj server]:time=%f", Poly_traj.getTimes().front());
|
||||
// 打印总时间
|
||||
ROS_INFO("[my traj server]:traj_duration_=%f", traj_duration_);
|
||||
|
||||
}
|
||||
|
||||
void pub_traj(double t_cur)
|
||||
{
|
||||
static int old_cnt = 0;
|
||||
int cnt = (int)((traj_duration_ - t_cur) * 2) + 1;
|
||||
|
||||
if (cnt != old_cnt)
|
||||
{
|
||||
geometry_msgs::PoseArray traj_pts;
|
||||
geometry_msgs::Pose traj_pt;
|
||||
Eigen::Vector3d opt_pt(Eigen::Vector3d::Zero());
|
||||
|
||||
traj_pts.header.stamp = ros::Time::now();
|
||||
for (int i = 0; i < cnt + 1; i++)
|
||||
{
|
||||
opt_pt = Poly_traj.evaluate(std::min(t_cur + i * 0.5, traj_duration_));
|
||||
traj_pt.orientation.w = 1.0;
|
||||
traj_pt.position.x = opt_pt(0);
|
||||
traj_pt.position.y = opt_pt(1);
|
||||
traj_pt.position.z = opt_pt(2);
|
||||
traj_pts.poses.push_back(traj_pt);
|
||||
}
|
||||
traj_pts_pub.publish(traj_pts);
|
||||
}
|
||||
old_cnt = cnt;
|
||||
}
|
||||
|
||||
|
||||
void pub_prometheus_command(quadrotor_msgs::PositionCommand ego_traj_cmd)
|
||||
{
|
||||
prometheus_msgs::UAVCommand uav_command;
|
||||
uav_command.header.stamp = ros::Time::now();
|
||||
uav_command.source = prometheus_msgs::UAVCommand::EGO;
|
||||
uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
|
||||
if(ego_traj_cmd.velocity.x == 0.0 && ego_traj_cmd.velocity.y == 0.0)
|
||||
{
|
||||
uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS;
|
||||
}
|
||||
else{
|
||||
uav_command.Move_mode = prometheus_msgs::UAVCommand::TRAJECTORY;
|
||||
}
|
||||
uav_command.position_ref[0] = ego_traj_cmd.position.x;
|
||||
uav_command.position_ref[1] = ego_traj_cmd.position.y;
|
||||
uav_command.position_ref[2] = ego_traj_cmd.position.z;
|
||||
uav_command.velocity_ref[0] = ego_traj_cmd.velocity.x;
|
||||
uav_command.velocity_ref[1] = ego_traj_cmd.velocity.y;
|
||||
uav_command.velocity_ref[2] = ego_traj_cmd.velocity.z;
|
||||
uav_command.acceleration_ref[0] = ego_traj_cmd.acceleration.x;
|
||||
uav_command.acceleration_ref[1] = ego_traj_cmd.acceleration.y;
|
||||
uav_command.acceleration_ref[2] = ego_traj_cmd.acceleration.z;
|
||||
uav_command.yaw_ref = geometry_utils::normalize_angle(ego_traj_cmd.yaw);
|
||||
// uav_command.yaw_rate_ref = ego_traj_cmd.yaw_dot;
|
||||
uav_cmd_pub.publish(uav_command);
|
||||
}
|
||||
void debug_cb(const ros::TimerEvent &e)
|
||||
{
|
||||
if (!receive_traj_)
|
||||
return;
|
||||
|
||||
|
||||
ROS_INFO("time = %f", time_traj);
|
||||
}
|
||||
|
||||
void cmdCallback(const ros::TimerEvent &e)
|
||||
{
|
||||
if (!receive_traj_)
|
||||
return;
|
||||
|
||||
Eigen::Vector3d pos(Eigen::Vector3d::Zero());
|
||||
Eigen::Vector3d vel(Eigen::Vector3d::Zero());
|
||||
Eigen::Vector3d acc(Eigen::Vector3d::Zero());
|
||||
Eigen::Vector3d jerk(Eigen::Vector3d::Zero());
|
||||
std::pair<double, double> yaw_yawdot(0, 0);
|
||||
|
||||
ros::Time time_now = ros::Time::now();
|
||||
double t_cur = (time_now - start_time).toSec();
|
||||
time_traj = t_cur;
|
||||
// ROS_INFO("time = %f", t_cur);
|
||||
static ros::Time time_last = ros::Time::now();
|
||||
|
||||
if (t_cur < traj_duration_ && t_cur >= 0.0)
|
||||
{
|
||||
pos = Poly_traj.evaluate(t_cur);
|
||||
vel = Poly_traj.evaluateVel(t_cur);
|
||||
acc = Poly_traj.evaluateAcc(t_cur);
|
||||
jerk = Poly_traj.evaluateJerk(t_cur);
|
||||
yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last);
|
||||
pub_traj(t_cur + 0.5);
|
||||
}
|
||||
else if (t_cur >= traj_duration_)
|
||||
{
|
||||
pos = Poly_traj.evaluate(traj_duration_);
|
||||
vel.setZero();
|
||||
acc.setZero();
|
||||
yaw_yawdot.first = last_yaw_;
|
||||
yaw_yawdot.second = 0;
|
||||
receive_traj_ = false;
|
||||
ROS_WARN("[my Traj server]: traj over.");
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("[my Traj server]: invalid time.");
|
||||
}
|
||||
time_last = time_now;
|
||||
|
||||
cmd.header.stamp = time_now;
|
||||
cmd.header.frame_id = "world";
|
||||
cmd.trajectory_id = traj_id_;
|
||||
|
||||
cmd.position.x = pos(0);
|
||||
cmd.position.y = pos(1);
|
||||
cmd.position.z = pos(2);
|
||||
|
||||
cmd.velocity.x = vel(0);
|
||||
cmd.velocity.y = vel(1);
|
||||
cmd.velocity.z = vel(2);
|
||||
|
||||
cmd.acceleration.x = acc(0);
|
||||
cmd.acceleration.y = acc(1);
|
||||
cmd.acceleration.z = acc(2);
|
||||
|
||||
cmd.jerk.x = jerk(0);
|
||||
cmd.jerk.y = jerk(1);
|
||||
cmd.jerk.z = jerk(2);
|
||||
|
||||
cmd.yaw = yaw_yawdot.first;
|
||||
cmd.yaw_dot = yaw_yawdot.second;
|
||||
|
||||
last_yaw_ = cmd.yaw;
|
||||
|
||||
pose_cmd_pub.publish(cmd);
|
||||
|
||||
pub_prometheus_command(cmd);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "traj_server_pengyu");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
pose_cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);
|
||||
traj_pts_pub = nh.advertise<geometry_msgs::PoseArray>("/traj_pts", 50);
|
||||
poly_traj_sub = nh.subscribe("/poly_coefs", 10, polyTrajCallback);
|
||||
|
||||
// [订阅] EGO规划结果 - to Prometheus uav_control
|
||||
uav_cmd_pub = nh.advertise<prometheus_msgs::UAVCommand>("/prometheus/command", 50);
|
||||
|
||||
ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback);
|
||||
|
||||
ros::Timer debug_timer = nh.createTimer(ros::Duration(1.0), debug_cb);
|
||||
|
||||
/* control parameter */
|
||||
cmd.kx[0] = pos_gain[0];
|
||||
cmd.kx[1] = pos_gain[1];
|
||||
cmd.kx[2] = pos_gain[2];
|
||||
|
||||
cmd.kv[0] = vel_gain[0];
|
||||
cmd.kv[1] = vel_gain[1];
|
||||
cmd.kv[2] = vel_gain[2];
|
||||
last_yaw_ = 0.0;
|
||||
last_yaw_dot_ = 0.0;
|
||||
time_forward_ = 1.0;
|
||||
|
||||
ros::Duration(1.0).sleep();
|
||||
|
||||
ROS_INFO("[my Traj server]: ready.");
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,39 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(mini_snap_traj_utils)
|
||||
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
set(CMAKE_CXX_FLAGS "-std=c++11")
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
std_msgs
|
||||
cv_bridge
|
||||
)
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL 1.7 REQUIRED)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES mini_snap_traj_utils
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
include_directories(
|
||||
SYSTEM
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Eigen3_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
link_directories(${PCL_LIBRARY_DIRS})
|
||||
|
||||
add_library( mini_snap_traj_utils
|
||||
src/planning_visualization.cpp
|
||||
src/polynomial_traj.cpp
|
||||
)
|
||||
target_link_libraries( mini_snap_traj_utils
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
@ -0,0 +1,54 @@
|
||||
#ifndef _PLANNING_VISUALIZATION_H_
|
||||
#define _PLANNING_VISUALIZATION_H_
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <mini_snap_traj_utils/polynomial_traj.h>
|
||||
#include <ros/ros.h>
|
||||
#include <vector>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
using std::vector;
|
||||
namespace my_planner
|
||||
{
|
||||
class PlanningVisualization
|
||||
{
|
||||
private:
|
||||
ros::NodeHandle node;
|
||||
|
||||
ros::Publisher goal_point_pub;
|
||||
ros::Publisher global_list_pub;
|
||||
ros::Publisher init_list_pub;
|
||||
ros::Publisher optimal_list_pub;
|
||||
ros::Publisher a_star_list_pub;
|
||||
ros::Publisher guide_vector_pub;
|
||||
ros::Publisher intermediate_state_pub;
|
||||
|
||||
public:
|
||||
PlanningVisualization(/* args */) {}
|
||||
~PlanningVisualization() {}
|
||||
PlanningVisualization(ros::NodeHandle &nh);
|
||||
|
||||
void Init(ros::NodeHandle &nh);
|
||||
typedef std::shared_ptr<PlanningVisualization> Ptr;
|
||||
|
||||
void displayMarkerList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale,
|
||||
Eigen::Vector4d color, int id);
|
||||
void generatePathDisplayArray(visualization_msgs::MarkerArray &array,
|
||||
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
|
||||
void generateArrowDisplayArray(visualization_msgs::MarkerArray &array,
|
||||
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
|
||||
void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id);
|
||||
void displayGlobalPathList(vector<Eigen::Vector3d> global_pts, const double scale, int id);
|
||||
void displayInitPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id);
|
||||
void displayOptimalList(Eigen::MatrixXd optimal_pts, int id);
|
||||
void displayAStarList(std::vector<std::vector<Eigen::Vector3d>> a_star_paths, int id);
|
||||
void displayArrowList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
|
||||
// void displayIntermediateState(ros::Publisher& intermediate_pub, ego_planner::BsplineOptimizer::Ptr optimizer, double sleep_time, const int start_iteration);
|
||||
// void displayNewArrow(ros::Publisher& guide_vector_pub, ego_planner::BsplineOptimizer::Ptr optimizer);
|
||||
};
|
||||
} // namespace ego_planner
|
||||
#endif
|
||||
@ -0,0 +1,368 @@
|
||||
#ifndef _POLYNOMIAL_TRAJ_H
|
||||
#define _POLYNOMIAL_TRAJ_H
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#include <vector>
|
||||
|
||||
using std::vector;
|
||||
|
||||
class PolynomialTraj
|
||||
{
|
||||
private:
|
||||
vector<double> times; // time of each segment
|
||||
vector<vector<double>> cxs; // coefficient of x of each segment, from n-1 -> 0
|
||||
vector<vector<double>> cys; // coefficient of y of each segment
|
||||
vector<vector<double>> czs; // coefficient of z of each segment
|
||||
|
||||
double time_sum;
|
||||
int num_seg;
|
||||
|
||||
/* evaluation */
|
||||
vector<Eigen::Vector3d> traj_vec3d;
|
||||
double length;
|
||||
|
||||
public:
|
||||
PolynomialTraj(/* args */)
|
||||
{
|
||||
}
|
||||
~PolynomialTraj()
|
||||
{
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
times.clear(), cxs.clear(), cys.clear(), czs.clear();
|
||||
time_sum = 0.0, num_seg = 0;
|
||||
}
|
||||
|
||||
void addSegment(vector<double> cx, vector<double> cy, vector<double> cz, double t)
|
||||
{
|
||||
cxs.push_back(cx), cys.push_back(cy), czs.push_back(cz), times.push_back(t);
|
||||
}
|
||||
|
||||
void init()
|
||||
{
|
||||
num_seg = times.size();
|
||||
time_sum = 0.0;
|
||||
for (int i = 0; i < times.size(); ++i)
|
||||
{
|
||||
time_sum += times[i];
|
||||
}
|
||||
}
|
||||
|
||||
vector<double> getTimes()
|
||||
{
|
||||
return times;
|
||||
}
|
||||
|
||||
vector<vector<double>> getCoef(int axis)
|
||||
{
|
||||
switch (axis)
|
||||
{
|
||||
case 0:
|
||||
return cxs;
|
||||
case 1:
|
||||
return cys;
|
||||
case 2:
|
||||
return czs;
|
||||
default:
|
||||
std::cout << "\033[31mIllegal axis!\033[0m" << std::endl;
|
||||
}
|
||||
|
||||
vector<vector<double>> empty;
|
||||
return empty;
|
||||
}
|
||||
|
||||
Eigen::Vector3d evaluate(double t)
|
||||
{
|
||||
/* detetrmine segment num */
|
||||
int idx = 0;
|
||||
while (times[idx] + 1e-4 < t)
|
||||
{
|
||||
t -= times[idx];
|
||||
++idx;
|
||||
}
|
||||
|
||||
/* evaluation */
|
||||
int order = cxs[idx].size();
|
||||
Eigen::VectorXd cx(order), cy(order), cz(order), tv(order);
|
||||
for (int i = 0; i < order; ++i)
|
||||
{
|
||||
cx(i) = cxs[idx][i], cy(i) = cys[idx][i], cz(i) = czs[idx][i];
|
||||
tv(order - 1 - i) = std::pow(t, double(i));
|
||||
}
|
||||
|
||||
Eigen::Vector3d pt;
|
||||
pt(0) = tv.dot(cx), pt(1) = tv.dot(cy), pt(2) = tv.dot(cz);
|
||||
return pt;
|
||||
}
|
||||
|
||||
Eigen::Vector3d evaluateVel(double t)
|
||||
{
|
||||
/* detetrmine segment num */
|
||||
int idx = 0;
|
||||
while (times[idx] + 1e-4 < t)
|
||||
{
|
||||
t -= times[idx];
|
||||
++idx;
|
||||
}
|
||||
|
||||
/* evaluation */
|
||||
int order = cxs[idx].size();
|
||||
Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1);
|
||||
|
||||
/* coef of vel */
|
||||
for (int i = 0; i < order - 1; ++i)
|
||||
{
|
||||
vx(i) = double(i + 1) * cxs[idx][order - 2 - i];
|
||||
vy(i) = double(i + 1) * cys[idx][order - 2 - i];
|
||||
vz(i) = double(i + 1) * czs[idx][order - 2 - i];
|
||||
}
|
||||
double ts = t;
|
||||
Eigen::VectorXd tv(order - 1);
|
||||
for (int i = 0; i < order - 1; ++i)
|
||||
tv(i) = pow(ts, i);
|
||||
|
||||
Eigen::Vector3d vel;
|
||||
vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz);
|
||||
return vel;
|
||||
}
|
||||
|
||||
Eigen::Vector3d evaluateAcc(double t)
|
||||
{
|
||||
/* detetrmine segment num */
|
||||
int idx = 0;
|
||||
while (times[idx] + 1e-4 < t)
|
||||
{
|
||||
t -= times[idx];
|
||||
++idx;
|
||||
}
|
||||
|
||||
/* evaluation */
|
||||
int order = cxs[idx].size();
|
||||
Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2);
|
||||
|
||||
/* coef of vel */
|
||||
for (int i = 0; i < order - 2; ++i)
|
||||
{
|
||||
ax(i) = double((i + 2) * (i + 1)) * cxs[idx][order - 3 - i];
|
||||
ay(i) = double((i + 2) * (i + 1)) * cys[idx][order - 3 - i];
|
||||
az(i) = double((i + 2) * (i + 1)) * czs[idx][order - 3 - i];
|
||||
}
|
||||
double ts = t;
|
||||
Eigen::VectorXd tv(order - 2);
|
||||
for (int i = 0; i < order - 2; ++i)
|
||||
tv(i) = pow(ts, i);
|
||||
|
||||
Eigen::Vector3d acc;
|
||||
acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az);
|
||||
return acc;
|
||||
}
|
||||
|
||||
Eigen::Vector3d evaluateJerk(double t)
|
||||
{
|
||||
/* detetrmine segment num */
|
||||
int idx = 0;
|
||||
while (times[idx] + 1e-4 < t)
|
||||
{
|
||||
t -= times[idx];
|
||||
++idx;
|
||||
}
|
||||
|
||||
/* evaluation */
|
||||
int order = cxs[idx].size();
|
||||
Eigen::VectorXd jx(order - 3), jy(order - 3), jz(order - 3);
|
||||
|
||||
/* coef of vel */
|
||||
for (int i = 0; i < order - 3; ++i)
|
||||
{
|
||||
jx(i) = double((i + 3) * (i + 2) * (i + 1)) * cxs[idx][order - 4 - i];
|
||||
jy(i) = double((i + 3) * (i + 2) * (i + 1)) * cys[idx][order - 4 - i];
|
||||
jz(i) = double((i + 3) * (i + 2) * (i + 1)) * czs[idx][order - 4 - i];
|
||||
}
|
||||
double ts = t;
|
||||
Eigen::VectorXd tv(order - 3);
|
||||
for (int i = 0; i < order - 3; ++i)
|
||||
tv(i) = pow(ts, i);
|
||||
|
||||
Eigen::Vector3d jerk;
|
||||
jerk(0) = tv.dot(jx), jerk(1) = tv.dot(jy), jerk(2) = tv.dot(jz);
|
||||
return jerk;
|
||||
}
|
||||
|
||||
/* for evaluating traj, should be called in sequence!!! */
|
||||
double getTimeSum()
|
||||
{
|
||||
return this->time_sum;
|
||||
}
|
||||
|
||||
vector<Eigen::Vector3d> getTraj()
|
||||
{
|
||||
double eval_t = 0.0;
|
||||
traj_vec3d.clear();
|
||||
while (eval_t < time_sum)
|
||||
{
|
||||
Eigen::Vector3d pt = evaluate(eval_t);
|
||||
traj_vec3d.push_back(pt);
|
||||
eval_t += 0.01;
|
||||
}
|
||||
return traj_vec3d;
|
||||
}
|
||||
|
||||
double getLength()
|
||||
{
|
||||
length = 0.0;
|
||||
|
||||
Eigen::Vector3d p_l = traj_vec3d[0], p_n;
|
||||
for (int i = 1; i < traj_vec3d.size(); ++i)
|
||||
{
|
||||
p_n = traj_vec3d[i];
|
||||
length += (p_n - p_l).norm();
|
||||
p_l = p_n;
|
||||
}
|
||||
return length;
|
||||
}
|
||||
|
||||
double getMeanVel()
|
||||
{
|
||||
double mean_vel = length / time_sum;
|
||||
return mean_vel;
|
||||
}
|
||||
|
||||
double getAccCost()
|
||||
{
|
||||
double cost = 0.0;
|
||||
int order = cxs[0].size();
|
||||
|
||||
for (int s = 0; s < times.size(); ++s)
|
||||
{
|
||||
Eigen::Vector3d um;
|
||||
um(0) = 2 * cxs[s][order - 3], um(1) = 2 * cys[s][order - 3], um(2) = 2 * czs[s][order - 3];
|
||||
cost += um.squaredNorm() * times[s];
|
||||
}
|
||||
|
||||
return cost;
|
||||
}
|
||||
|
||||
double getJerk()
|
||||
{
|
||||
double jerk = 0.0;
|
||||
|
||||
/* evaluate jerk */
|
||||
for (int s = 0; s < times.size(); ++s)
|
||||
{
|
||||
Eigen::VectorXd cxv(cxs[s].size()), cyv(cys[s].size()), czv(czs[s].size());
|
||||
/* convert coefficient */
|
||||
int order = cxs[s].size();
|
||||
for (int j = 0; j < order; ++j)
|
||||
{
|
||||
cxv(j) = cxs[s][order - 1 - j], cyv(j) = cys[s][order - 1 - j], czv(j) = czs[s][order - 1 - j];
|
||||
}
|
||||
double ts = times[s];
|
||||
|
||||
/* jerk matrix */
|
||||
Eigen::MatrixXd mat_jerk(order, order);
|
||||
mat_jerk.setZero();
|
||||
for (double i = 3; i < order; i += 1)
|
||||
for (double j = 3; j < order; j += 1)
|
||||
{
|
||||
mat_jerk(i, j) =
|
||||
i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) * pow(ts, i + j - 5) / (i + j - 5);
|
||||
}
|
||||
|
||||
jerk += (cxv.transpose() * mat_jerk * cxv)(0, 0);
|
||||
jerk += (cyv.transpose() * mat_jerk * cyv)(0, 0);
|
||||
jerk += (czv.transpose() * mat_jerk * czv)(0, 0);
|
||||
}
|
||||
|
||||
return jerk;
|
||||
}
|
||||
|
||||
void getMeanAndMaxVel(double &mean_v, double &max_v)
|
||||
{
|
||||
int num = 0;
|
||||
mean_v = 0.0, max_v = -1.0;
|
||||
for (int s = 0; s < times.size(); ++s)
|
||||
{
|
||||
int order = cxs[s].size();
|
||||
Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1);
|
||||
|
||||
/* coef of vel */
|
||||
for (int i = 0; i < order - 1; ++i)
|
||||
{
|
||||
vx(i) = double(i + 1) * cxs[s][order - 2 - i];
|
||||
vy(i) = double(i + 1) * cys[s][order - 2 - i];
|
||||
vz(i) = double(i + 1) * czs[s][order - 2 - i];
|
||||
}
|
||||
double ts = times[s];
|
||||
|
||||
double eval_t = 0.0;
|
||||
while (eval_t < ts)
|
||||
{
|
||||
Eigen::VectorXd tv(order - 1);
|
||||
for (int i = 0; i < order - 1; ++i)
|
||||
tv(i) = pow(ts, i);
|
||||
Eigen::Vector3d vel;
|
||||
vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz);
|
||||
double vn = vel.norm();
|
||||
mean_v += vn;
|
||||
if (vn > max_v)
|
||||
max_v = vn;
|
||||
++num;
|
||||
|
||||
eval_t += 0.01;
|
||||
}
|
||||
}
|
||||
|
||||
mean_v = mean_v / double(num);
|
||||
}
|
||||
|
||||
void getMeanAndMaxAcc(double &mean_a, double &max_a)
|
||||
{
|
||||
int num = 0;
|
||||
mean_a = 0.0, max_a = -1.0;
|
||||
for (int s = 0; s < times.size(); ++s)
|
||||
{
|
||||
int order = cxs[s].size();
|
||||
Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2);
|
||||
|
||||
/* coef of acc */
|
||||
for (int i = 0; i < order - 2; ++i)
|
||||
{
|
||||
ax(i) = double((i + 2) * (i + 1)) * cxs[s][order - 3 - i];
|
||||
ay(i) = double((i + 2) * (i + 1)) * cys[s][order - 3 - i];
|
||||
az(i) = double((i + 2) * (i + 1)) * czs[s][order - 3 - i];
|
||||
}
|
||||
double ts = times[s];
|
||||
|
||||
double eval_t = 0.0;
|
||||
while (eval_t < ts)
|
||||
{
|
||||
Eigen::VectorXd tv(order - 2);
|
||||
for (int i = 0; i < order - 2; ++i)
|
||||
tv(i) = pow(ts, i);
|
||||
Eigen::Vector3d acc;
|
||||
acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az);
|
||||
double an = acc.norm();
|
||||
mean_a += an;
|
||||
if (an > max_a)
|
||||
max_a = an;
|
||||
++num;
|
||||
|
||||
eval_t += 0.01;
|
||||
}
|
||||
}
|
||||
|
||||
mean_a = mean_a / double(num);
|
||||
}
|
||||
|
||||
static PolynomialTraj minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel,
|
||||
const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc,
|
||||
const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time);
|
||||
|
||||
static PolynomialTraj one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
|
||||
const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc,
|
||||
double t);
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,67 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>mini_snap_traj_utils</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The mini_snap_traj_utils package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="bzhouai@todo.todo">bzhouai</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/mini_snap_traj_utils</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,253 @@
|
||||
#include <mini_snap_traj_utils/planning_visualization.h>
|
||||
|
||||
using std::cout;
|
||||
using std::endl;
|
||||
namespace my_planner
|
||||
{
|
||||
PlanningVisualization::PlanningVisualization(ros::NodeHandle &nh)
|
||||
{
|
||||
node = nh;
|
||||
|
||||
goal_point_pub = nh.advertise<visualization_msgs::Marker>("goal_point", 2);
|
||||
global_list_pub = nh.advertise<visualization_msgs::Marker>("global_list", 2);
|
||||
init_list_pub = nh.advertise<visualization_msgs::Marker>("init_list", 2);
|
||||
optimal_list_pub = nh.advertise<visualization_msgs::Marker>("optimal_list", 2);
|
||||
a_star_list_pub = nh.advertise<visualization_msgs::Marker>("a_star_list", 20);
|
||||
}
|
||||
|
||||
void PlanningVisualization::Init(ros::NodeHandle &nh)
|
||||
{
|
||||
node = nh;
|
||||
|
||||
goal_point_pub = nh.advertise<visualization_msgs::Marker>("goal_point", 2);
|
||||
global_list_pub = nh.advertise<visualization_msgs::Marker>("global_list", 2);
|
||||
init_list_pub = nh.advertise<visualization_msgs::Marker>("init_list", 2);
|
||||
optimal_list_pub = nh.advertise<visualization_msgs::Marker>("optimal_list", 2);
|
||||
a_star_list_pub = nh.advertise<visualization_msgs::Marker>("a_star_list", 20);
|
||||
}
|
||||
|
||||
// // real ids used: {id, id+1000}
|
||||
void PlanningVisualization::displayMarkerList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale,
|
||||
Eigen::Vector4d color, int id)
|
||||
{
|
||||
visualization_msgs::Marker sphere, line_strip;
|
||||
sphere.header.frame_id = line_strip.header.frame_id = "world";
|
||||
sphere.header.stamp = line_strip.header.stamp = ros::Time::now();
|
||||
sphere.type = visualization_msgs::Marker::SPHERE_LIST;
|
||||
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
sphere.action = line_strip.action = visualization_msgs::Marker::ADD;
|
||||
sphere.id = id;
|
||||
line_strip.id = id + 1000;
|
||||
|
||||
sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
|
||||
sphere.color.r = line_strip.color.r = color(0);
|
||||
sphere.color.g = line_strip.color.g = color(1);
|
||||
sphere.color.b = line_strip.color.b = color(2);
|
||||
sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0;
|
||||
sphere.scale.x = scale;
|
||||
sphere.scale.y = scale;
|
||||
sphere.scale.z = scale;
|
||||
line_strip.scale.x = scale / 2;
|
||||
geometry_msgs::Point pt;
|
||||
for (int i = 0; i < int(list.size()); i++)
|
||||
{
|
||||
pt.x = list[i](0);
|
||||
pt.y = list[i](1);
|
||||
pt.z = list[i](2);
|
||||
sphere.points.push_back(pt);
|
||||
line_strip.points.push_back(pt);
|
||||
}
|
||||
pub.publish(sphere);
|
||||
pub.publish(line_strip);
|
||||
}
|
||||
|
||||
// real ids used: {id, id+1}
|
||||
void PlanningVisualization::generatePathDisplayArray(visualization_msgs::MarkerArray &array,
|
||||
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
|
||||
{
|
||||
visualization_msgs::Marker sphere, line_strip;
|
||||
sphere.header.frame_id = line_strip.header.frame_id = "map";
|
||||
sphere.header.stamp = line_strip.header.stamp = ros::Time::now();
|
||||
sphere.type = visualization_msgs::Marker::SPHERE_LIST;
|
||||
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
sphere.action = line_strip.action = visualization_msgs::Marker::ADD;
|
||||
sphere.id = id;
|
||||
line_strip.id = id + 1;
|
||||
|
||||
sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
|
||||
sphere.color.r = line_strip.color.r = color(0);
|
||||
sphere.color.g = line_strip.color.g = color(1);
|
||||
sphere.color.b = line_strip.color.b = color(2);
|
||||
sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0;
|
||||
sphere.scale.x = scale;
|
||||
sphere.scale.y = scale;
|
||||
sphere.scale.z = scale;
|
||||
line_strip.scale.x = scale / 3;
|
||||
geometry_msgs::Point pt;
|
||||
for (int i = 0; i < int(list.size()); i++)
|
||||
{
|
||||
pt.x = list[i](0);
|
||||
pt.y = list[i](1);
|
||||
pt.z = list[i](2);
|
||||
sphere.points.push_back(pt);
|
||||
line_strip.points.push_back(pt);
|
||||
}
|
||||
array.markers.push_back(sphere);
|
||||
array.markers.push_back(line_strip);
|
||||
}
|
||||
|
||||
// real ids used: {1000*id ~ (arrow nums)+1000*id}
|
||||
void PlanningVisualization::generateArrowDisplayArray(visualization_msgs::MarkerArray &array,
|
||||
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
|
||||
{
|
||||
visualization_msgs::Marker arrow;
|
||||
arrow.header.frame_id = "map";
|
||||
arrow.header.stamp = ros::Time::now();
|
||||
arrow.type = visualization_msgs::Marker::ARROW;
|
||||
arrow.action = visualization_msgs::Marker::ADD;
|
||||
|
||||
// geometry_msgs::Point start, end;
|
||||
// arrow.points
|
||||
|
||||
arrow.color.r = color(0);
|
||||
arrow.color.g = color(1);
|
||||
arrow.color.b = color(2);
|
||||
arrow.color.a = color(3) > 1e-5 ? color(3) : 1.0;
|
||||
arrow.scale.x = scale;
|
||||
arrow.scale.y = 2 * scale;
|
||||
arrow.scale.z = 2 * scale;
|
||||
|
||||
geometry_msgs::Point start, end;
|
||||
for (int i = 0; i < int(list.size() / 2); i++)
|
||||
{
|
||||
// arrow.color.r = color(0) / (1+i);
|
||||
// arrow.color.g = color(1) / (1+i);
|
||||
// arrow.color.b = color(2) / (1+i);
|
||||
|
||||
start.x = list[2 * i](0);
|
||||
start.y = list[2 * i](1);
|
||||
start.z = list[2 * i](2);
|
||||
end.x = list[2 * i + 1](0);
|
||||
end.y = list[2 * i + 1](1);
|
||||
end.z = list[2 * i + 1](2);
|
||||
arrow.points.clear();
|
||||
arrow.points.push_back(start);
|
||||
arrow.points.push_back(end);
|
||||
arrow.id = i + id * 1000;
|
||||
|
||||
array.markers.push_back(arrow);
|
||||
}
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id)
|
||||
{
|
||||
visualization_msgs::Marker sphere;
|
||||
sphere.header.frame_id = "world";
|
||||
sphere.header.stamp = ros::Time::now();
|
||||
sphere.type = visualization_msgs::Marker::SPHERE;
|
||||
sphere.action = visualization_msgs::Marker::ADD;
|
||||
sphere.id = id;
|
||||
|
||||
sphere.pose.orientation.w = 1.0;
|
||||
sphere.color.r = color(0);
|
||||
sphere.color.g = color(1);
|
||||
sphere.color.b = color(2);
|
||||
sphere.color.a = color(3);
|
||||
sphere.scale.x = scale;
|
||||
sphere.scale.y = scale;
|
||||
sphere.scale.z = scale;
|
||||
sphere.pose.position.x = goal_point(0);
|
||||
sphere.pose.position.y = goal_point(1);
|
||||
sphere.pose.position.z = goal_point(2);
|
||||
|
||||
goal_point_pub.publish(sphere);
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayGlobalPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id)
|
||||
{
|
||||
|
||||
if (global_list_pub.getNumSubscribers() == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Eigen::Vector4d color(0, 0.5, 0.5, 1);
|
||||
displayMarkerList(global_list_pub, init_pts, scale, color, id);
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayInitPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id)
|
||||
{
|
||||
|
||||
if (init_list_pub.getNumSubscribers() == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Eigen::Vector4d color(0, 0, 1, 1);
|
||||
displayMarkerList(init_list_pub, init_pts, scale, color, id);
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayOptimalList(Eigen::MatrixXd optimal_pts, int id)
|
||||
{
|
||||
|
||||
if (optimal_list_pub.getNumSubscribers() == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
vector<Eigen::Vector3d> list;
|
||||
for (int i = 0; i < optimal_pts.cols(); i++)
|
||||
{
|
||||
Eigen::Vector3d pt = optimal_pts.col(i).transpose();
|
||||
list.push_back(pt);
|
||||
}
|
||||
Eigen::Vector4d color(1, 0, 0, 1);
|
||||
displayMarkerList(optimal_list_pub, list, 0.15, color, id);
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayAStarList(std::vector<std::vector<Eigen::Vector3d>> a_star_paths, int id /* = Eigen::Vector4d(0.5,0.5,0,1)*/)
|
||||
{
|
||||
|
||||
if (a_star_list_pub.getNumSubscribers() == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
int i = 0;
|
||||
vector<Eigen::Vector3d> list;
|
||||
|
||||
Eigen::Vector4d color = Eigen::Vector4d(0.5 + ((double)rand() / RAND_MAX / 2), 0.5 + ((double)rand() / RAND_MAX / 2), 0, 1); // make the A star pathes different every time.
|
||||
double scale = 0.05 + (double)rand() / RAND_MAX / 10;
|
||||
|
||||
// for ( int i=0; i<10; i++ )
|
||||
// {
|
||||
// //Eigen::Vector4d color(1,1,0,0);
|
||||
// displayMarkerList(a_star_list_pub, list, scale, color, id+i);
|
||||
// }
|
||||
|
||||
for (auto block : a_star_paths)
|
||||
{
|
||||
list.clear();
|
||||
for (auto pt : block)
|
||||
{
|
||||
list.push_back(pt);
|
||||
}
|
||||
//Eigen::Vector4d color(0.5,0.5,0,1);
|
||||
displayMarkerList(a_star_list_pub, list, scale, color, id + i); // real ids used: [ id ~ id+a_star_paths.size() ]
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
void PlanningVisualization::displayArrowList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
|
||||
{
|
||||
visualization_msgs::MarkerArray array;
|
||||
// clear
|
||||
pub.publish(array);
|
||||
|
||||
generateArrowDisplayArray(array, list, scale, color, id);
|
||||
|
||||
pub.publish(array);
|
||||
}
|
||||
|
||||
// PlanningVisualization::
|
||||
} // namespace ego_planner
|
||||
@ -0,0 +1,224 @@
|
||||
#include <iostream>
|
||||
#include <mini_snap_traj_utils/polynomial_traj.h>
|
||||
|
||||
PolynomialTraj PolynomialTraj::minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel,
|
||||
const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc,
|
||||
const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time)
|
||||
{
|
||||
int seg_num = Time.size();
|
||||
Eigen::MatrixXd poly_coeff(seg_num, 3 * 6);
|
||||
Eigen::VectorXd Px(6 * seg_num), Py(6 * seg_num), Pz(6 * seg_num);
|
||||
|
||||
int num_f, num_p; // number of fixed and free variables
|
||||
int num_d; // number of all segments' derivatives
|
||||
|
||||
const static auto Factorial = [](int x) {
|
||||
int fac = 1;
|
||||
for (int i = x; i > 0; i--)
|
||||
fac = fac * i;
|
||||
return fac;
|
||||
};
|
||||
|
||||
/* ---------- end point derivative ---------- */
|
||||
Eigen::VectorXd Dx = Eigen::VectorXd::Zero(seg_num * 6);
|
||||
Eigen::VectorXd Dy = Eigen::VectorXd::Zero(seg_num * 6);
|
||||
Eigen::VectorXd Dz = Eigen::VectorXd::Zero(seg_num * 6);
|
||||
|
||||
for (int k = 0; k < seg_num; k++)
|
||||
{
|
||||
/* position to derivative */
|
||||
Dx(k * 6) = Pos(0, k);
|
||||
Dx(k * 6 + 1) = Pos(0, k + 1);
|
||||
Dy(k * 6) = Pos(1, k);
|
||||
Dy(k * 6 + 1) = Pos(1, k + 1);
|
||||
Dz(k * 6) = Pos(2, k);
|
||||
Dz(k * 6 + 1) = Pos(2, k + 1);
|
||||
|
||||
if (k == 0)
|
||||
{
|
||||
Dx(k * 6 + 2) = start_vel(0);
|
||||
Dy(k * 6 + 2) = start_vel(1);
|
||||
Dz(k * 6 + 2) = start_vel(2);
|
||||
|
||||
Dx(k * 6 + 4) = start_acc(0);
|
||||
Dy(k * 6 + 4) = start_acc(1);
|
||||
Dz(k * 6 + 4) = start_acc(2);
|
||||
}
|
||||
else if (k == seg_num - 1)
|
||||
{
|
||||
Dx(k * 6 + 3) = end_vel(0);
|
||||
Dy(k * 6 + 3) = end_vel(1);
|
||||
Dz(k * 6 + 3) = end_vel(2);
|
||||
|
||||
Dx(k * 6 + 5) = end_acc(0);
|
||||
Dy(k * 6 + 5) = end_acc(1);
|
||||
Dz(k * 6 + 5) = end_acc(2);
|
||||
}
|
||||
}
|
||||
|
||||
/* ---------- Mapping Matrix A ---------- */
|
||||
Eigen::MatrixXd Ab;
|
||||
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);
|
||||
|
||||
for (int k = 0; k < seg_num; k++)
|
||||
{
|
||||
Ab = Eigen::MatrixXd::Zero(6, 6);
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
Ab(2 * i, i) = Factorial(i);
|
||||
for (int j = i; j < 6; j++)
|
||||
Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i);
|
||||
}
|
||||
A.block(k * 6, k * 6, 6, 6) = Ab;
|
||||
}
|
||||
|
||||
/* ---------- Produce Selection Matrix C' ---------- */
|
||||
Eigen::MatrixXd Ct, C;
|
||||
|
||||
num_f = 2 * seg_num + 4; // 3 + 3 + (seg_num - 1) * 2 = 2m + 4
|
||||
num_p = 2 * seg_num - 2; //(seg_num - 1) * 2 = 2m - 2
|
||||
num_d = 6 * seg_num;
|
||||
Ct = Eigen::MatrixXd::Zero(num_d, num_f + num_p);
|
||||
Ct(0, 0) = 1;
|
||||
Ct(2, 1) = 1;
|
||||
Ct(4, 2) = 1; // stack the start point
|
||||
Ct(1, 3) = 1;
|
||||
Ct(3, 2 * seg_num + 4) = 1;
|
||||
Ct(5, 2 * seg_num + 5) = 1;
|
||||
|
||||
Ct(6 * (seg_num - 1) + 0, 2 * seg_num + 0) = 1;
|
||||
Ct(6 * (seg_num - 1) + 1, 2 * seg_num + 1) = 1; // Stack the end point
|
||||
Ct(6 * (seg_num - 1) + 2, 4 * seg_num + 0) = 1;
|
||||
Ct(6 * (seg_num - 1) + 3, 2 * seg_num + 2) = 1; // Stack the end point
|
||||
Ct(6 * (seg_num - 1) + 4, 4 * seg_num + 1) = 1;
|
||||
Ct(6 * (seg_num - 1) + 5, 2 * seg_num + 3) = 1; // Stack the end point
|
||||
|
||||
for (int j = 2; j < seg_num; j++)
|
||||
{
|
||||
Ct(6 * (j - 1) + 0, 2 + 2 * (j - 1) + 0) = 1;
|
||||
Ct(6 * (j - 1) + 1, 2 + 2 * (j - 1) + 1) = 1;
|
||||
Ct(6 * (j - 1) + 2, 2 * seg_num + 4 + 2 * (j - 2) + 0) = 1;
|
||||
Ct(6 * (j - 1) + 3, 2 * seg_num + 4 + 2 * (j - 1) + 0) = 1;
|
||||
Ct(6 * (j - 1) + 4, 2 * seg_num + 4 + 2 * (j - 2) + 1) = 1;
|
||||
Ct(6 * (j - 1) + 5, 2 * seg_num + 4 + 2 * (j - 1) + 1) = 1;
|
||||
}
|
||||
|
||||
C = Ct.transpose();
|
||||
|
||||
Eigen::VectorXd Dx1 = C * Dx;
|
||||
Eigen::VectorXd Dy1 = C * Dy;
|
||||
Eigen::VectorXd Dz1 = C * Dz;
|
||||
|
||||
/* ---------- minimum snap matrix ---------- */
|
||||
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);
|
||||
|
||||
for (int k = 0; k < seg_num; k++)
|
||||
{
|
||||
for (int i = 3; i < 6; i++)
|
||||
{
|
||||
for (int j = 3; j < 6; j++)
|
||||
{
|
||||
Q(k * 6 + i, k * 6 + j) =
|
||||
i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ---------- R matrix ---------- */
|
||||
Eigen::MatrixXd R = C * A.transpose().inverse() * Q * A.inverse() * Ct;
|
||||
|
||||
Eigen::VectorXd Dxf(2 * seg_num + 4), Dyf(2 * seg_num + 4), Dzf(2 * seg_num + 4);
|
||||
|
||||
Dxf = Dx1.segment(0, 2 * seg_num + 4);
|
||||
Dyf = Dy1.segment(0, 2 * seg_num + 4);
|
||||
Dzf = Dz1.segment(0, 2 * seg_num + 4);
|
||||
|
||||
Eigen::MatrixXd Rff(2 * seg_num + 4, 2 * seg_num + 4);
|
||||
Eigen::MatrixXd Rfp(2 * seg_num + 4, 2 * seg_num - 2);
|
||||
Eigen::MatrixXd Rpf(2 * seg_num - 2, 2 * seg_num + 4);
|
||||
Eigen::MatrixXd Rpp(2 * seg_num - 2, 2 * seg_num - 2);
|
||||
|
||||
Rff = R.block(0, 0, 2 * seg_num + 4, 2 * seg_num + 4);
|
||||
Rfp = R.block(0, 2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2);
|
||||
Rpf = R.block(2 * seg_num + 4, 0, 2 * seg_num - 2, 2 * seg_num + 4);
|
||||
Rpp = R.block(2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2, 2 * seg_num - 2);
|
||||
|
||||
/* ---------- close form solution ---------- */
|
||||
|
||||
Eigen::VectorXd Dxp(2 * seg_num - 2), Dyp(2 * seg_num - 2), Dzp(2 * seg_num - 2);
|
||||
Dxp = -(Rpp.inverse() * Rfp.transpose()) * Dxf;
|
||||
Dyp = -(Rpp.inverse() * Rfp.transpose()) * Dyf;
|
||||
Dzp = -(Rpp.inverse() * Rfp.transpose()) * Dzf;
|
||||
|
||||
Dx1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dxp;
|
||||
Dy1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dyp;
|
||||
Dz1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dzp;
|
||||
|
||||
Px = (A.inverse() * Ct) * Dx1;
|
||||
Py = (A.inverse() * Ct) * Dy1;
|
||||
Pz = (A.inverse() * Ct) * Dz1;
|
||||
|
||||
for (int i = 0; i < seg_num; i++)
|
||||
{
|
||||
poly_coeff.block(i, 0, 1, 6) = Px.segment(i * 6, 6).transpose();
|
||||
poly_coeff.block(i, 6, 1, 6) = Py.segment(i * 6, 6).transpose();
|
||||
poly_coeff.block(i, 12, 1, 6) = Pz.segment(i * 6, 6).transpose();
|
||||
}
|
||||
|
||||
/* ---------- use polynomials ---------- */
|
||||
PolynomialTraj poly_traj;
|
||||
for (int i = 0; i < poly_coeff.rows(); ++i)
|
||||
{
|
||||
vector<double> cx(6), cy(6), cz(6);
|
||||
for (int j = 0; j < 6; ++j)
|
||||
{
|
||||
cx[j] = poly_coeff(i, j), cy[j] = poly_coeff(i, j + 6), cz[j] = poly_coeff(i, j + 12);
|
||||
}
|
||||
reverse(cx.begin(), cx.end());
|
||||
reverse(cy.begin(), cy.end());
|
||||
reverse(cz.begin(), cz.end());
|
||||
double ts = Time(i);
|
||||
poly_traj.addSegment(cx, cy, cz, ts);
|
||||
}
|
||||
|
||||
return poly_traj;
|
||||
}
|
||||
|
||||
PolynomialTraj PolynomialTraj::one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
|
||||
const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc,
|
||||
double t)
|
||||
{
|
||||
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(6, 6), Crow(1, 6);
|
||||
Eigen::VectorXd Bx(6), By(6), Bz(6);
|
||||
|
||||
C(0, 5) = 1;
|
||||
C(1, 4) = 1;
|
||||
C(2, 3) = 2;
|
||||
Crow << pow(t, 5), pow(t, 4), pow(t, 3), pow(t, 2), t, 1;
|
||||
C.row(3) = Crow;
|
||||
Crow << 5 * pow(t, 4), 4 * pow(t, 3), 3 * pow(t, 2), 2 * t, 1, 0;
|
||||
C.row(4) = Crow;
|
||||
Crow << 20 * pow(t, 3), 12 * pow(t, 2), 6 * t, 2, 0, 0;
|
||||
C.row(5) = Crow;
|
||||
|
||||
Bx << start_pt(0), start_vel(0), start_acc(0), end_pt(0), end_vel(0), end_acc(0);
|
||||
By << start_pt(1), start_vel(1), start_acc(1), end_pt(1), end_vel(1), end_acc(1);
|
||||
Bz << start_pt(2), start_vel(2), start_acc(2), end_pt(2), end_vel(2), end_acc(2);
|
||||
|
||||
Eigen::VectorXd Cofx = C.colPivHouseholderQr().solve(Bx);
|
||||
Eigen::VectorXd Cofy = C.colPivHouseholderQr().solve(By);
|
||||
Eigen::VectorXd Cofz = C.colPivHouseholderQr().solve(Bz);
|
||||
|
||||
vector<double> cx(6), cy(6), cz(6);
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
cx[i] = Cofx(i);
|
||||
cy[i] = Cofy(i);
|
||||
cz[i] = Cofz(i);
|
||||
}
|
||||
|
||||
PolynomialTraj poly_traj;
|
||||
poly_traj.addSegment(cx, cy, cz, t);
|
||||
|
||||
return poly_traj;
|
||||
}
|
||||
@ -0,0 +1,13 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(cmake_utils)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS roscpp)
|
||||
|
||||
set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake)
|
||||
|
||||
file(GLOB CMAKE_UILTS_FILES cmake/*.cmake)
|
||||
|
||||
catkin_package(
|
||||
CFG_EXTRAS ${CMAKE_UILTS_FILES}
|
||||
)
|
||||
|
||||
@ -0,0 +1,126 @@
|
||||
set(archdetect_c_code "
|
||||
#if defined(__arm__) || defined(__TARGET_ARCH_ARM)
|
||||
#if defined(__ARM_ARCH_7__) \\
|
||||
|| defined(__ARM_ARCH_7A__) \\
|
||||
|| defined(__ARM_ARCH_7R__) \\
|
||||
|| defined(__ARM_ARCH_7M__) \\
|
||||
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 7)
|
||||
#error cmake_ARCH armv7
|
||||
#elif defined(__ARM_ARCH_6__) \\
|
||||
|| defined(__ARM_ARCH_6J__) \\
|
||||
|| defined(__ARM_ARCH_6T2__) \\
|
||||
|| defined(__ARM_ARCH_6Z__) \\
|
||||
|| defined(__ARM_ARCH_6K__) \\
|
||||
|| defined(__ARM_ARCH_6ZK__) \\
|
||||
|| defined(__ARM_ARCH_6M__) \\
|
||||
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 6)
|
||||
#error cmake_ARCH armv6
|
||||
#elif defined(__ARM_ARCH_5TEJ__) \\
|
||||
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 5)
|
||||
#error cmake_ARCH armv5
|
||||
#else
|
||||
#error cmake_ARCH arm
|
||||
#endif
|
||||
#elif defined(__i386) || defined(__i386__) || defined(_M_IX86)
|
||||
#error cmake_ARCH i386
|
||||
#elif defined(__x86_64) || defined(__x86_64__) || defined(__amd64) || defined(_M_X64)
|
||||
#error cmake_ARCH x86_64
|
||||
#elif defined(__ia64) || defined(__ia64__) || defined(_M_IA64)
|
||||
#error cmake_ARCH ia64
|
||||
#elif defined(__ppc__) || defined(__ppc) || defined(__powerpc__) \\
|
||||
|| defined(_ARCH_COM) || defined(_ARCH_PWR) || defined(_ARCH_PPC) \\
|
||||
|| defined(_M_MPPC) || defined(_M_PPC)
|
||||
#if defined(__ppc64__) || defined(__powerpc64__) || defined(__64BIT__)
|
||||
#error cmake_ARCH ppc64
|
||||
#else
|
||||
#error cmake_ARCH ppc
|
||||
#endif
|
||||
#endif
|
||||
#error cmake_ARCH unknown
|
||||
")
|
||||
|
||||
# Set ppc_support to TRUE before including this file or ppc and ppc64
|
||||
# will be treated as invalid architectures since they are no longer supported by Apple
|
||||
|
||||
function(target_architecture output_var)
|
||||
if(APPLE AND CMAKE_OSX_ARCHITECTURES)
|
||||
# On OS X we use CMAKE_OSX_ARCHITECTURES *if* it was set
|
||||
# First let's normalize the order of the values
|
||||
|
||||
# Note that it's not possible to compile PowerPC applications if you are using
|
||||
# the OS X SDK version 10.6 or later - you'll need 10.4/10.5 for that, so we
|
||||
# disable it by default
|
||||
# See this page for more information:
|
||||
# http://stackoverflow.com/questions/5333490/how-can-we-restore-ppc-ppc64-as-well-as-full-10-4-10-5-sdk-support-to-xcode-4
|
||||
|
||||
# Architecture defaults to i386 or ppc on OS X 10.5 and earlier, depending on the CPU type detected at runtime.
|
||||
# On OS X 10.6+ the default is x86_64 if the CPU supports it, i386 otherwise.
|
||||
|
||||
foreach(osx_arch ${CMAKE_OSX_ARCHITECTURES})
|
||||
if("${osx_arch}" STREQUAL "ppc" AND ppc_support)
|
||||
set(osx_arch_ppc TRUE)
|
||||
elseif("${osx_arch}" STREQUAL "i386")
|
||||
set(osx_arch_i386 TRUE)
|
||||
elseif("${osx_arch}" STREQUAL "x86_64")
|
||||
set(osx_arch_x86_64 TRUE)
|
||||
elseif("${osx_arch}" STREQUAL "ppc64" AND ppc_support)
|
||||
set(osx_arch_ppc64 TRUE)
|
||||
else()
|
||||
message(FATAL_ERROR "Invalid OS X arch name: ${osx_arch}")
|
||||
endif()
|
||||
endforeach()
|
||||
|
||||
# Now add all the architectures in our normalized order
|
||||
if(osx_arch_ppc)
|
||||
list(APPEND ARCH ppc)
|
||||
endif()
|
||||
|
||||
if(osx_arch_i386)
|
||||
list(APPEND ARCH i386)
|
||||
endif()
|
||||
|
||||
if(osx_arch_x86_64)
|
||||
list(APPEND ARCH x86_64)
|
||||
endif()
|
||||
|
||||
if(osx_arch_ppc64)
|
||||
list(APPEND ARCH ppc64)
|
||||
endif()
|
||||
else()
|
||||
file(WRITE "${CMAKE_BINARY_DIR}/arch.c" "${archdetect_c_code}")
|
||||
|
||||
enable_language(C)
|
||||
|
||||
# Detect the architecture in a rather creative way...
|
||||
# This compiles a small C program which is a series of ifdefs that selects a
|
||||
# particular #error preprocessor directive whose message string contains the
|
||||
# target architecture. The program will always fail to compile (both because
|
||||
# file is not a valid C program, and obviously because of the presence of the
|
||||
# #error preprocessor directives... but by exploiting the preprocessor in this
|
||||
# way, we can detect the correct target architecture even when cross-compiling,
|
||||
# since the program itself never needs to be run (only the compiler/preprocessor)
|
||||
try_run(
|
||||
run_result_unused
|
||||
compile_result_unused
|
||||
"${CMAKE_BINARY_DIR}"
|
||||
"${CMAKE_BINARY_DIR}/arch.c"
|
||||
COMPILE_OUTPUT_VARIABLE ARCH
|
||||
CMAKE_FLAGS CMAKE_OSX_ARCHITECTURES=${CMAKE_OSX_ARCHITECTURES}
|
||||
)
|
||||
|
||||
# Parse the architecture name from the compiler output
|
||||
string(REGEX MATCH "cmake_ARCH ([a-zA-Z0-9_]+)" ARCH "${ARCH}")
|
||||
|
||||
# Get rid of the value marker leaving just the architecture name
|
||||
string(REPLACE "cmake_ARCH " "" ARCH "${ARCH}")
|
||||
|
||||
# If we are compiling with an unknown architecture this variable should
|
||||
# already be set to "unknown" but in the case that it's empty (i.e. due
|
||||
# to a typo in the code), then set it to unknown
|
||||
if (NOT ARCH)
|
||||
set(ARCH unknown)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
set(${output_var} "${ARCH}" PARENT_SCOPE)
|
||||
endfunction()
|
||||
@ -0,0 +1,2 @@
|
||||
list(APPEND CMAKE_MODULE_PATH "${cmake_utils_SOURCE_PREFIX}/cmake_modules")
|
||||
link_directories( ${cmake_utils_SOURCE_PREFIX}/lib/mosek8 )
|
||||
@ -0,0 +1,17 @@
|
||||
string(ASCII 27 Esc)
|
||||
set(ColourReset "${Esc}[m")
|
||||
set(ColourBold "${Esc}[1m")
|
||||
set(Red "${Esc}[31m")
|
||||
set(Green "${Esc}[32m")
|
||||
set(Yellow "${Esc}[33m")
|
||||
set(Blue "${Esc}[34m")
|
||||
set(Magenta "${Esc}[35m")
|
||||
set(Cyan "${Esc}[36m")
|
||||
set(White "${Esc}[37m")
|
||||
set(BoldRed "${Esc}[1;31m")
|
||||
set(BoldGreen "${Esc}[1;32m")
|
||||
set(BoldYellow "${Esc}[1;33m")
|
||||
set(BoldBlue "${Esc}[1;34m")
|
||||
set(BoldMagenta "${Esc}[1;35m")
|
||||
set(BoldCyan "${Esc}[1;36m")
|
||||
set(BoldWhite "${Esc}[1;37m")
|
||||
@ -0,0 +1,173 @@
|
||||
# Ceres Solver - A fast non-linear least squares minimizer
|
||||
# Copyright 2015 Google Inc. All rights reserved.
|
||||
# http://ceres-solver.org/
|
||||
#
|
||||
# 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 Google Inc. nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software without
|
||||
# specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
# Author: alexs.mac@gmail.com (Alex Stewart)
|
||||
#
|
||||
|
||||
# FindEigen.cmake - Find Eigen library, version >= 3.
|
||||
#
|
||||
# This module defines the following variables:
|
||||
#
|
||||
# EIGEN_FOUND: TRUE iff Eigen is found.
|
||||
# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
|
||||
#
|
||||
# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
|
||||
# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
|
||||
# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
|
||||
# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
|
||||
#
|
||||
# The following variables control the behaviour of this module:
|
||||
#
|
||||
# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
|
||||
# search for eigen includes, e.g: /timbuktu/eigen3.
|
||||
#
|
||||
# The following variables are also defined by this module, but in line with
|
||||
# CMake recommended FindPackage() module style should NOT be referenced directly
|
||||
# by callers (use the plural variables detailed above instead). These variables
|
||||
# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
|
||||
# are NOT re-called (i.e. search for library is not repeated) if these variables
|
||||
# are set with valid values _in the CMake cache_. This means that if these
|
||||
# variables are set directly in the cache, either by the user in the CMake GUI,
|
||||
# or by the user passing -DVAR=VALUE directives to CMake when called (which
|
||||
# explicitly defines a cache variable), then they will be used verbatim,
|
||||
# bypassing the HINTS variables and other hard-coded search locations.
|
||||
#
|
||||
# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
|
||||
# include directory of any dependencies.
|
||||
|
||||
# Called if we failed to find Eigen or any of it's required dependencies,
|
||||
# unsets all public (designed to be used externally) variables and reports
|
||||
# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
|
||||
macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
|
||||
unset(EIGEN_FOUND)
|
||||
unset(EIGEN_INCLUDE_DIRS)
|
||||
# Make results of search visible in the CMake GUI if Eigen has not
|
||||
# been found so that user does not have to toggle to advanced view.
|
||||
mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
|
||||
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
|
||||
# use the camelcase library name, not uppercase.
|
||||
if (Eigen_FIND_QUIETLY)
|
||||
message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
|
||||
elseif (Eigen_FIND_REQUIRED)
|
||||
message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
|
||||
else()
|
||||
# Neither QUIETLY nor REQUIRED, use no priority which emits a message
|
||||
# but continues configuration and allows generation.
|
||||
message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
|
||||
endif ()
|
||||
return()
|
||||
endmacro(EIGEN_REPORT_NOT_FOUND)
|
||||
|
||||
# Protect against any alternative find_package scripts for this library having
|
||||
# been called previously (in a client project) which set EIGEN_FOUND, but not
|
||||
# the other variables we require / set here which could cause the search logic
|
||||
# here to fail.
|
||||
unset(EIGEN_FOUND)
|
||||
|
||||
# Search user-installed locations first, so that we prefer user installs
|
||||
# to system installs where both exist.
|
||||
#
|
||||
# TODO: Add standard Windows search locations for Eigen.
|
||||
list(APPEND EIGEN_CHECK_INCLUDE_DIRS
|
||||
/usr/local/include
|
||||
/usr/local/homebrew/include # Mac OS X
|
||||
/opt/local/var/macports/software # Mac OS X.
|
||||
/opt/local/include
|
||||
/usr/include)
|
||||
# Additional suffixes to try appending to each search path.
|
||||
list(APPEND EIGEN_CHECK_PATH_SUFFIXES
|
||||
eigen3 # Default root directory for Eigen.
|
||||
Eigen/include/eigen3 ) # Windows (for C:/Program Files prefix).
|
||||
|
||||
# Search supplied hint directories first if supplied.
|
||||
find_path(EIGEN_INCLUDE_DIR
|
||||
NAMES Eigen/Core
|
||||
PATHS ${EIGEN_INCLUDE_DIR_HINTS}
|
||||
${EIGEN_CHECK_INCLUDE_DIRS}
|
||||
PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
|
||||
|
||||
if (NOT EIGEN_INCLUDE_DIR OR
|
||||
NOT EXISTS ${EIGEN_INCLUDE_DIR})
|
||||
eigen_report_not_found(
|
||||
"Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
|
||||
"path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
|
||||
endif (NOT EIGEN_INCLUDE_DIR OR
|
||||
NOT EXISTS ${EIGEN_INCLUDE_DIR})
|
||||
|
||||
# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
|
||||
# if called.
|
||||
set(EIGEN_FOUND TRUE)
|
||||
|
||||
# Extract Eigen version from Eigen/src/Core/util/Macros.h
|
||||
if (EIGEN_INCLUDE_DIR)
|
||||
set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
|
||||
if (NOT EXISTS ${EIGEN_VERSION_FILE})
|
||||
eigen_report_not_found(
|
||||
"Could not find file: ${EIGEN_VERSION_FILE} "
|
||||
"containing version information in Eigen install located at: "
|
||||
"${EIGEN_INCLUDE_DIR}.")
|
||||
else (NOT EXISTS ${EIGEN_VERSION_FILE})
|
||||
file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
|
||||
|
||||
string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
|
||||
EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
|
||||
string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
|
||||
EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
|
||||
|
||||
string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
|
||||
EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
|
||||
string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
|
||||
EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
|
||||
|
||||
string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
|
||||
EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
|
||||
string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
|
||||
EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
|
||||
|
||||
# This is on a single line s/t CMake does not interpret it as a list of
|
||||
# elements and insert ';' separators which would result in 3.;2.;0 nonsense.
|
||||
set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
|
||||
endif (NOT EXISTS ${EIGEN_VERSION_FILE})
|
||||
endif (EIGEN_INCLUDE_DIR)
|
||||
|
||||
# Set standard CMake FindPackage variables if found.
|
||||
if (EIGEN_FOUND)
|
||||
set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
|
||||
endif (EIGEN_FOUND)
|
||||
|
||||
# Handle REQUIRED / QUIET optional arguments and version.
|
||||
include(FindPackageHandleStandardArgs)
|
||||
find_package_handle_standard_args(Eigen
|
||||
REQUIRED_VARS EIGEN_INCLUDE_DIRS
|
||||
VERSION_VAR EIGEN_VERSION)
|
||||
|
||||
# Only mark internal variables as advanced if we found Eigen, otherwise
|
||||
# leave it visible in the standard GUI for the user to set manually.
|
||||
if (EIGEN_FOUND)
|
||||
mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)
|
||||
endif (EIGEN_FOUND)
|
||||
@ -0,0 +1,135 @@
|
||||
# Try to find gnu scientific library GSL
|
||||
# See
|
||||
# http://www.gnu.org/software/gsl/ and
|
||||
# http://gnuwin32.sourceforge.net/packages/gsl.htm
|
||||
#
|
||||
# Based on a script of Felix Woelk and Jan Woetzel
|
||||
# (www.mip.informatik.uni-kiel.de)
|
||||
#
|
||||
# It defines the following variables:
|
||||
# GSL_FOUND - system has GSL lib
|
||||
# GSL_INCLUDE_DIRS - where to find headers
|
||||
# GSL_LIBRARIES - full path to the libraries
|
||||
# GSL_LIBRARY_DIRS, the directory where the PLplot library is found.
|
||||
|
||||
# CMAKE_GSL_CXX_FLAGS = Unix compiler flags for GSL, essentially "`gsl-config --cxxflags`"
|
||||
# GSL_LINK_DIRECTORIES = link directories, useful for rpath on Unix
|
||||
# GSL_EXE_LINKER_FLAGS = rpath on Unix
|
||||
|
||||
set( GSL_FOUND OFF )
|
||||
set( GSL_CBLAS_FOUND OFF )
|
||||
|
||||
# Windows, but not for Cygwin and MSys where gsl-config is available
|
||||
if( WIN32 AND NOT CYGWIN AND NOT MSYS )
|
||||
# look for headers
|
||||
find_path( GSL_INCLUDE_DIR
|
||||
NAMES gsl/gsl_cdf.h gsl/gsl_randist.h
|
||||
)
|
||||
if( GSL_INCLUDE_DIR )
|
||||
# look for gsl library
|
||||
find_library( GSL_LIBRARY
|
||||
NAMES gsl
|
||||
)
|
||||
if( GSL_LIBRARY )
|
||||
set( GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR} )
|
||||
get_filename_component( GSL_LIBRARY_DIRS ${GSL_LIBRARY} PATH )
|
||||
set( GSL_FOUND ON )
|
||||
endif( GSL_LIBRARY )
|
||||
|
||||
# look for gsl cblas library
|
||||
find_library( GSL_CBLAS_LIBRARY
|
||||
NAMES gslcblas
|
||||
)
|
||||
if( GSL_CBLAS_LIBRARY )
|
||||
set( GSL_CBLAS_FOUND ON )
|
||||
endif( GSL_CBLAS_LIBRARY )
|
||||
|
||||
set( GSL_LIBRARIES ${GSL_LIBRARY} ${GSL_CBLAS_LIBRARY} )
|
||||
endif( GSL_INCLUDE_DIR )
|
||||
|
||||
mark_as_advanced(
|
||||
GSL_INCLUDE_DIR
|
||||
GSL_LIBRARY
|
||||
GSL_CBLAS_LIBRARY
|
||||
)
|
||||
else( WIN32 AND NOT CYGWIN AND NOT MSYS )
|
||||
if( UNIX OR MSYS )
|
||||
find_program( GSL_CONFIG_EXECUTABLE gsl-config
|
||||
/usr/bin/
|
||||
/usr/local/bin
|
||||
)
|
||||
|
||||
if( GSL_CONFIG_EXECUTABLE )
|
||||
set( GSL_FOUND ON )
|
||||
|
||||
# run the gsl-config program to get cxxflags
|
||||
execute_process(
|
||||
COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --cflags
|
||||
OUTPUT_VARIABLE GSL_CFLAGS
|
||||
RESULT_VARIABLE RET
|
||||
ERROR_QUIET
|
||||
)
|
||||
if( RET EQUAL 0 )
|
||||
string( STRIP "${GSL_CFLAGS}" GSL_CFLAGS )
|
||||
separate_arguments( GSL_CFLAGS )
|
||||
|
||||
# parse definitions from cflags; drop -D* from CFLAGS
|
||||
string( REGEX MATCHALL "-D[^;]+"
|
||||
GSL_DEFINITIONS "${GSL_CFLAGS}" )
|
||||
string( REGEX REPLACE "-D[^;]+;" ""
|
||||
GSL_CFLAGS "${GSL_CFLAGS}" )
|
||||
|
||||
# parse include dirs from cflags; drop -I prefix
|
||||
string( REGEX MATCHALL "-I[^;]+"
|
||||
GSL_INCLUDE_DIRS "${GSL_CFLAGS}" )
|
||||
string( REPLACE "-I" ""
|
||||
GSL_INCLUDE_DIRS "${GSL_INCLUDE_DIRS}")
|
||||
string( REGEX REPLACE "-I[^;]+;" ""
|
||||
GSL_CFLAGS "${GSL_CFLAGS}")
|
||||
|
||||
message("GSL_DEFINITIONS=${GSL_DEFINITIONS}")
|
||||
message("GSL_INCLUDE_DIRS=${GSL_INCLUDE_DIRS}")
|
||||
message("GSL_CFLAGS=${GSL_CFLAGS}")
|
||||
else( RET EQUAL 0 )
|
||||
set( GSL_FOUND FALSE )
|
||||
endif( RET EQUAL 0 )
|
||||
|
||||
# run the gsl-config program to get the libs
|
||||
execute_process(
|
||||
COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --libs
|
||||
OUTPUT_VARIABLE GSL_LIBRARIES
|
||||
RESULT_VARIABLE RET
|
||||
ERROR_QUIET
|
||||
)
|
||||
if( RET EQUAL 0 )
|
||||
string(STRIP "${GSL_LIBRARIES}" GSL_LIBRARIES )
|
||||
separate_arguments( GSL_LIBRARIES )
|
||||
|
||||
# extract linkdirs (-L) for rpath (i.e., LINK_DIRECTORIES)
|
||||
string( REGEX MATCHALL "-L[^;]+"
|
||||
GSL_LIBRARY_DIRS "${GSL_LIBRARIES}" )
|
||||
string( REPLACE "-L" ""
|
||||
GSL_LIBRARY_DIRS "${GSL_LIBRARY_DIRS}" )
|
||||
else( RET EQUAL 0 )
|
||||
set( GSL_FOUND FALSE )
|
||||
endif( RET EQUAL 0 )
|
||||
|
||||
MARK_AS_ADVANCED(
|
||||
GSL_CFLAGS
|
||||
)
|
||||
message( STATUS "Using GSL from ${GSL_PREFIX}" )
|
||||
else( GSL_CONFIG_EXECUTABLE )
|
||||
message( STATUS "FindGSL: gsl-config not found.")
|
||||
endif( GSL_CONFIG_EXECUTABLE )
|
||||
endif( UNIX OR MSYS )
|
||||
endif( WIN32 AND NOT CYGWIN AND NOT MSYS )
|
||||
|
||||
if( GSL_FOUND )
|
||||
if( NOT GSL_FIND_QUIETLY )
|
||||
message( STATUS "FindGSL: Found both GSL headers and library" )
|
||||
endif( NOT GSL_FIND_QUIETLY )
|
||||
else( GSL_FOUND )
|
||||
if( GSL_FIND_REQUIRED )
|
||||
message( FATAL_ERROR "FindGSL: Could not find GSL headers or library" )
|
||||
endif( GSL_FIND_REQUIRED )
|
||||
endif( GSL_FOUND )
|
||||
@ -0,0 +1,124 @@
|
||||
macro(mvIMPACT_REPORT_NOT_FOUND REASON_MSG)
|
||||
unset(mvIMPACT_FOUND)
|
||||
unset(mvIMPACT_INCLUDE_DIRS)
|
||||
unset(mvIMPACT_LIBRARIES)
|
||||
unset(mvIMPACT_WORLD_VERSION)
|
||||
unset(mvIMPACT_MAJOR_VERSION)
|
||||
unset(mvIMPACT_MINOR_VERSION)
|
||||
# Make results of search visible in the CMake GUI if mvimpact has not
|
||||
# been found so that user does not have to toggle to advanced view.
|
||||
mark_as_advanced(CLEAR mvIMPACT_INCLUDE_DIR)
|
||||
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
|
||||
# use the camelcase library name, not uppercase.
|
||||
if(Mvimpact_FIND_QUIETLY)
|
||||
message(STATUS "Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
|
||||
elseif(Mvimpact_FIND_REQUIRED)
|
||||
message(FATAL_ERROR "Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
|
||||
else()
|
||||
# Neither QUIETLY nor REQUIRED, use no priority which emits a message
|
||||
# but continues configuration and allows generation.
|
||||
message("-- Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
|
||||
endif()
|
||||
endmacro(mvIMPACT_REPORT_NOT_FOUND)
|
||||
|
||||
# Search user-installed locations first, so that we prefer user installs
|
||||
# to system installs where both exist.
|
||||
get_filename_component(BLUEFOX2_DIR ${CMAKE_CURRENT_SOURCE_DIR} REALPATH)
|
||||
list(APPEND mvIMPACT_CHECK_INCLUDE_DIRS
|
||||
/opt/mvIMPACT_acquire
|
||||
/opt/mvIMPACT_Acquire
|
||||
)
|
||||
execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCH)
|
||||
|
||||
if(NOT ARCH)
|
||||
set(ARCH "arm64")
|
||||
elseif(${ARCH} STREQUAL "aarch64")
|
||||
set(ARCH "arm64")
|
||||
endif()
|
||||
|
||||
list(APPEND mvIMPACT_CHECK_LIBRARY_DIRS
|
||||
/opt/mvIMPACT_acquire/lib/${ARCH}
|
||||
/opt/mvIMPACT_Acquire/lib/${ARCH}
|
||||
)
|
||||
|
||||
# Check general hints
|
||||
if(mvIMPACT_HINTS AND EXISTS ${mvIMPACT_HINTS})
|
||||
set(mvIMPACT_INCLUDE_DIR_HINTS ${mvIMPACT_HINTS}/include)
|
||||
set(mvIMPACT_LIBRARY_DIR_HINTS ${mvIMPACT_HINTS}/lib)
|
||||
endif()
|
||||
|
||||
# Search supplied hint directories first if supplied.
|
||||
# Find include directory for mvimpact
|
||||
find_path(mvIMPACT_INCLUDE_DIR
|
||||
NAMES mvIMPACT_CPP/mvIMPACT_acquire.h
|
||||
PATHS ${mvIMPACT_INCLUDE_DIR_HINTS}
|
||||
${mvIMPACT_CHECK_INCLUDE_DIRS}
|
||||
NO_DEFAULT_PATH)
|
||||
if(NOT mvIMPACT_INCLUDE_DIR OR NOT EXISTS ${mvIMPACT_INCLUDE_DIR})
|
||||
mvIMPACT_REPORT_NOT_FOUND(
|
||||
"Could not find mvimpact include directory, set mvIMPACT_INCLUDE_DIR to "
|
||||
"path to mvimpact include directory,"
|
||||
"e.g. /opt/mvIMPACT_acquire.")
|
||||
else()
|
||||
message(STATUS "mvimpact include dir found: " ${mvIMPACT_INCLUDE_DIR})
|
||||
endif()
|
||||
|
||||
# Find library directory for mvimpact
|
||||
find_library(mvIMPACT_LIBRARY
|
||||
NAMES libmvBlueFOX.so
|
||||
PATHS ${mvIMPACT_LIBRARY_DIR_HINTS}
|
||||
${mvIMPACT_CHECK_LIBRARY_DIRS}
|
||||
NO_DEFAULT_PATH)
|
||||
if(NOT mvIMPACT_LIBRARY OR NOT EXISTS ${mvIMPACT_LIBRARY})
|
||||
mvIMPACT_REPORT_NOT_FOUND(
|
||||
"Could not find mvimpact library, set mvIMPACT_LIBRARY "
|
||||
"to full path to mvimpact library direcotory.${mvIMPACT_CHECK_LIBRARY_DIRS}~~")
|
||||
else()
|
||||
# TODO: need to fix this hacky solution for getting mvIMPACT_LIBRARY_DIR
|
||||
string(REGEX MATCH ".*/" mvIMPACT_LIBRARY_DIR ${mvIMPACT_LIBRARY})
|
||||
message(STATUS "mvimpact library dir found: " ${mvIMPACT_LIBRARY_DIR})
|
||||
|
||||
endif()
|
||||
|
||||
# Mark internally as found, then verify. mvIMPACT_REPORT_NOT_FOUND() unsets if
|
||||
# called.
|
||||
set(mvIMPACT_FOUND TRUE)
|
||||
|
||||
# Extract mvimpact version
|
||||
if(mvIMPACT_LIBRARY_DIR)
|
||||
file(GLOB mvIMPACT_LIBS
|
||||
RELATIVE ${mvIMPACT_LIBRARY_DIR}
|
||||
${mvIMPACT_LIBRARY_DIR}/libmvBlueFOX.so.[0-9].[0-9].[0-9])
|
||||
# TODO: add version support
|
||||
# string(REGEX MATCH ""
|
||||
# mvIMPACT_WORLD_VERSION ${mvIMPACT_PVBASE})
|
||||
# message(STATUS "mvimpact world version: " ${mvIMPACT_WORLD_VERSION})
|
||||
endif()
|
||||
|
||||
# Catch case when caller has set mvIMPACT_INCLUDE_DIR in the cache / GUI and
|
||||
# thus FIND_[PATH/LIBRARY] are not called, but specified locations are
|
||||
# invalid, otherwise we would report the library as found.
|
||||
if(NOT mvIMPACT_INCLUDE_DIR AND NOT EXISTS ${mvIMPACT_INCLUDE_DIR}/mvIMPACT_CPP/mvIMPACT_acquire.h)
|
||||
mvIMPACT_REPORT_NOT_FOUND("Caller defined mvIMPACT_INCLUDE_DIR: "
|
||||
${mvIMPACT_INCLUDE_DIR}
|
||||
" does not contain mvIMPACT_CPP/mvIMPACT_acquire.h header.")
|
||||
endif()
|
||||
|
||||
# Set standard CMake FindPackage variables if found.
|
||||
if(mvIMPACT_FOUND)
|
||||
set(mvIMPACT_INCLUDE_DIRS ${mvIMPACT_INCLUDE_DIR})
|
||||
file(GLOB mvIMPACT_LIBRARIES ${mvIMPACT_LIBRARY_DIR}lib*.so)
|
||||
endif()
|
||||
|
||||
# Handle REQUIRED / QUIET optional arguments.
|
||||
include(FindPackageHandleStandardArgs)
|
||||
if(mvIMPACT_FOUND)
|
||||
FIND_PACKAGE_HANDLE_STANDARD_ARGS(Mvimpact DEFAULT_MSG
|
||||
mvIMPACT_INCLUDE_DIRS mvIMPACT_LIBRARIES)
|
||||
endif()
|
||||
|
||||
# Only mark internal variables as advanced if we found mvimpact, otherwise
|
||||
# leave it visible in the standard GUI for the user to set manually.
|
||||
if(mvIMPACT_FOUND)
|
||||
mark_as_advanced(FORCE mvIMPACT_INCLUDE_DIR mvIMPACT_LIBRARY)
|
||||
endif()
|
||||
@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>cmake_utils</name>
|
||||
<version>0.0.0</version>
|
||||
<description>
|
||||
Once you used this package,
|
||||
then you do not need to copy cmake files among packages
|
||||
</description>
|
||||
|
||||
<maintainer email="william.wu@dji.com"> William.Wu </maintainer>
|
||||
|
||||
<license>LGPLv3</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>cmake_modules</depend>
|
||||
|
||||
</package>
|
||||
@ -0,0 +1,222 @@
|
||||
#cmake_minimum_required(VERSION 2.4.6)
|
||||
#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||
|
||||
# Set the build type. Options are:
|
||||
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
|
||||
# Debug : w/ debug symbols, w/o optimization
|
||||
# Release : w/o debug symbols, w/ optimization
|
||||
# RelWithDebInfo : w/ debug symbols, w/ optimization
|
||||
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
|
||||
#set(ROS_BUILD_TYPE Release)
|
||||
|
||||
#rosbuild_init()
|
||||
|
||||
#set the default path for built executables to the "bin" directory
|
||||
#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
|
||||
#set the default path for built libraries to the "lib" directory
|
||||
#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||
|
||||
#uncomment if you have defined messages
|
||||
#rosbuild_genmsg()
|
||||
#uncomment if you have defined services
|
||||
#rosbuild_gensrv()
|
||||
|
||||
#common commands for building c++ executables and libraries
|
||||
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
|
||||
#target_link_libraries(${PROJECT_NAME} another_library)
|
||||
#rosbuild_add_executable(example examples/example.cpp)
|
||||
#target_link_libraries(example ${PROJECT_NAME})
|
||||
#rosbuild_add_boost_directories()
|
||||
|
||||
|
||||
#rosbuild_add_executable(multi_map_server src/multi_map_server.cc)
|
||||
#target_link_libraries(multi_map_server pose_utils)
|
||||
|
||||
#rosbuild_add_executable(multi_map_visualization src/multi_map_visualization.cc)
|
||||
#target_link_libraries(multi_map_visualization pose_utils)
|
||||
|
||||
#----------------------------------
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(multi_map_server)
|
||||
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
set(CMAKE_CXX_FLAGS "-std=c++11")
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
pose_utils
|
||||
message_generation
|
||||
roscpp
|
||||
visualization_msgs
|
||||
tf
|
||||
std_srvs
|
||||
laser_geometry
|
||||
pose_utils
|
||||
quadrotor_msgs
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependencies might have been
|
||||
## pulled in transitively but can be declared for certainty nonetheless:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a run_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
add_message_files(
|
||||
FILES
|
||||
MultiOccupancyGrid.msg
|
||||
MultiSparseMap3D.msg
|
||||
SparseMap3D.msg
|
||||
VerticalOccupancyGridList.msg
|
||||
#plan_cmd.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
)
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if you package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES irobot_msgs
|
||||
CATKIN_DEPENDS geometry_msgs nav_msgs quadrotor_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
find_package(Armadillo REQUIRED)
|
||||
include_directories(${ARMADILLO_INCLUDE_DIRS})
|
||||
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(include)
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a cpp library
|
||||
# add_library(irobot_msgs
|
||||
# src/${PROJECT_NAME}/irobot_msgs.cpp
|
||||
# )
|
||||
|
||||
## Declare a cpp executable
|
||||
add_executable(multi_map_visualization src/multi_map_visualization.cc)
|
||||
|
||||
## Add cmake target dependencies of the executable/library
|
||||
## as an example, message headers may need to be generated before nodes
|
||||
add_dependencies(multi_map_visualization multi_map_server_messages_cpp)
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
target_link_libraries(multi_map_visualization
|
||||
${catkin_LIBRARIES}
|
||||
${ARMADILLO_LIBRARIES}
|
||||
pose_utils
|
||||
)
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS irobot_msgs irobot_msgs_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_irobot_msgs.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
@ -0,0 +1 @@
|
||||
include $(shell rospack find mk)/cmake.mk
|
||||
@ -0,0 +1,242 @@
|
||||
#ifndef MAP2D_H
|
||||
#define MAP2D_H
|
||||
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <tf/tf.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
class Map2D
|
||||
{
|
||||
|
||||
private:
|
||||
nav_msgs::OccupancyGrid map;
|
||||
int expandStep;
|
||||
int binning;
|
||||
bool isBinningSet;
|
||||
bool updated;
|
||||
|
||||
public:
|
||||
Map2D()
|
||||
{
|
||||
map.data.resize(0);
|
||||
map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);
|
||||
expandStep = 200;
|
||||
binning = 1;
|
||||
isBinningSet = false;
|
||||
updated = false;
|
||||
}
|
||||
|
||||
Map2D(int _binning)
|
||||
{
|
||||
map.data.resize(0);
|
||||
map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);
|
||||
expandStep = 200;
|
||||
binning = _binning;
|
||||
isBinningSet = true;
|
||||
updated = false;
|
||||
}
|
||||
|
||||
~Map2D() {}
|
||||
|
||||
double GetResolution() { return map.info.resolution; }
|
||||
double GetMinX() { return map.info.origin.position.x; }
|
||||
double GetMinY() { return map.info.origin.position.y; }
|
||||
double GetMaxX() { return map.info.origin.position.x + map.info.width * map.info.resolution; }
|
||||
double GetMaxY() { return map.info.origin.position.y + map.info.height * map.info.resolution; }
|
||||
bool Updated() { return updated; }
|
||||
void Reset() { map = nav_msgs::OccupancyGrid(); }
|
||||
|
||||
void SetBinning(int _binning)
|
||||
{
|
||||
if (!isBinningSet)
|
||||
binning = _binning;
|
||||
}
|
||||
|
||||
// Get occupancy value, 0: unknown; +ve: occupied; -ve: free
|
||||
signed char GetOccupiedFromWorldFrame(double x, double y)
|
||||
{
|
||||
int xm = (x - map.info.origin.position.x) / map.info.resolution;
|
||||
int ym = (y - map.info.origin.position.y) / map.info.resolution;
|
||||
if (xm < 0 || xm > (int)(map.info.width-1) || ym < 0 || ym > (int)(map.info.height-1))
|
||||
return 0;
|
||||
else
|
||||
return map.data[ym*map.info.width+xm];
|
||||
}
|
||||
|
||||
void Replace(nav_msgs::OccupancyGrid m)
|
||||
{
|
||||
// Check data
|
||||
if (m.data.size() == 0)
|
||||
return;
|
||||
isBinningSet = true;
|
||||
// Binning, conservative, take maximum
|
||||
if (binning > 1)
|
||||
{
|
||||
int _width = m.info.width;
|
||||
m.info.width /= binning;
|
||||
m.info.height /= binning;
|
||||
m.info.resolution *= binning;
|
||||
vector<signed char> data(m.info.width * m.info.height);
|
||||
for (uint32_t i = 0; i < m.info.height; i++)
|
||||
{
|
||||
for (uint32_t j = 0; j < m.info.width; j++)
|
||||
{
|
||||
int val = -0xff;
|
||||
for (int _i = 0; _i < binning; _i++)
|
||||
{
|
||||
for (int _j = 0; _j < binning; _j++)
|
||||
{
|
||||
int v = m.data[(i*binning + _i) * _width + (j*binning + _j)];
|
||||
val = (v > val)?v:val;
|
||||
}
|
||||
}
|
||||
data[i * m.info.width + j] = val;
|
||||
}
|
||||
}
|
||||
m.data = data;
|
||||
}
|
||||
// Replace map
|
||||
map = m;
|
||||
updated = true;
|
||||
}
|
||||
|
||||
// Merge submap
|
||||
void Update(nav_msgs::OccupancyGrid m)
|
||||
{
|
||||
// Check data
|
||||
if (m.data.size() == 0)
|
||||
return;
|
||||
isBinningSet = true;
|
||||
// Binning, conservative, take maximum
|
||||
if (binning > 1)
|
||||
{
|
||||
int _width = m.info.width;
|
||||
m.info.width /= binning;
|
||||
m.info.height /= binning;
|
||||
m.info.resolution *= binning;
|
||||
vector<signed char> data(m.info.width * m.info.height);
|
||||
for (uint32_t i = 0; i < m.info.height; i++)
|
||||
{
|
||||
for (uint32_t j = 0; j < m.info.width; j++)
|
||||
{
|
||||
int val = -0xff;
|
||||
for (int _i = 0; _i < binning; _i++)
|
||||
{
|
||||
for (int _j = 0; _j < binning; _j++)
|
||||
{
|
||||
int v = m.data[(i*binning + _i) * _width + (j*binning + _j)];
|
||||
val = (v > val)?v:val;
|
||||
}
|
||||
}
|
||||
data[i * m.info.width + j] = val;
|
||||
}
|
||||
}
|
||||
m.data = data;
|
||||
}
|
||||
// Initialize and check resolution
|
||||
if (map.info.resolution == 0)
|
||||
map.info.resolution = m.info.resolution;
|
||||
else if (m.info.resolution != map.info.resolution)
|
||||
return;
|
||||
// Get Info
|
||||
double ox = m.info.origin.position.x;
|
||||
double oy = m.info.origin.position.y;
|
||||
double oyaw = tf::getYaw(m.info.origin.orientation);
|
||||
double syaw = sin(oyaw);
|
||||
double cyaw = cos(oyaw);
|
||||
int mx = m.info.width;
|
||||
int my = m.info.height;
|
||||
double xs[4];
|
||||
double ys[4];
|
||||
xs[0] = cyaw * (0) - syaw * (0) + ox;
|
||||
xs[1] = cyaw * (mx*map.info.resolution) - syaw * (0) + ox;
|
||||
xs[2] = cyaw * (0) - syaw * (mx*map.info.resolution) + ox;
|
||||
xs[3] = cyaw * (mx*map.info.resolution) - syaw * (my*map.info.resolution) + ox;
|
||||
ys[0] = syaw * (0) + cyaw * (0) + oy;
|
||||
ys[1] = syaw * (mx*map.info.resolution) + cyaw * (0) + oy;
|
||||
ys[2] = syaw * (0) + cyaw * (my*map.info.resolution) + oy;
|
||||
ys[3] = syaw * (mx*map.info.resolution) + cyaw * (my*map.info.resolution) + oy;
|
||||
double minx = xs[0];
|
||||
double maxx = xs[0];
|
||||
double miny = ys[0];
|
||||
double maxy = ys[0];
|
||||
for (int k = 0; k < 4; k++)
|
||||
{
|
||||
minx = (xs[k] < minx)?xs[k]:minx;
|
||||
miny = (ys[k] < miny)?ys[k]:miny;
|
||||
maxx = (xs[k] > maxx)?xs[k]:maxx;
|
||||
maxy = (ys[k] > maxy)?ys[k]:maxy;
|
||||
}
|
||||
// Check whether resize map is needed
|
||||
bool mapResetFlag = false;
|
||||
double prevOriginX = map.info.origin.position.x;
|
||||
double prevOriginY = map.info.origin.position.y;
|
||||
int prevMapX = map.info.width;
|
||||
int prevMapY = map.info.height;
|
||||
while (map.info.origin.position.x > minx)
|
||||
{
|
||||
map.info.width += expandStep;
|
||||
map.info.origin.position.x -= expandStep*map.info.resolution;
|
||||
mapResetFlag = true;
|
||||
}
|
||||
while (map.info.origin.position.y > miny)
|
||||
{
|
||||
map.info.height += expandStep;
|
||||
map.info.origin.position.y -= expandStep*map.info.resolution;
|
||||
mapResetFlag = true;
|
||||
}
|
||||
while (map.info.origin.position.x + map.info.width*map.info.resolution < maxx)
|
||||
{
|
||||
map.info.width += expandStep;
|
||||
mapResetFlag = true;
|
||||
}
|
||||
while (map.info.origin.position.y + map.info.height*map.info.resolution < maxy)
|
||||
{
|
||||
map.info.height += expandStep;
|
||||
mapResetFlag = true;
|
||||
}
|
||||
// Resize map
|
||||
if (mapResetFlag)
|
||||
{
|
||||
mapResetFlag = false;
|
||||
vector<signed char> _data = map.data;
|
||||
map.data.clear();
|
||||
map.data.resize(map.info.width*map.info.height,0);
|
||||
for (int x = 0; x < prevMapX; x++)
|
||||
{
|
||||
for(int y = 0; y < prevMapY; y++)
|
||||
{
|
||||
int xn = x + round((prevOriginX - map.info.origin.position.x) / map.info.resolution);
|
||||
int yn = y + round((prevOriginY - map.info.origin.position.y) / map.info.resolution);
|
||||
map.data[yn*map.info.width+xn] = _data[y*prevMapX+x];
|
||||
}
|
||||
}
|
||||
}
|
||||
// Merge map
|
||||
for (int x = 0; x < mx; x++)
|
||||
{
|
||||
for(int y = 0; y < my; y++)
|
||||
{
|
||||
int xn = (cyaw*(x*map.info.resolution)-syaw*(y*map.info.resolution)+ox-map.info.origin.position.x) / map.info.resolution;
|
||||
int yn = (syaw*(x*map.info.resolution)+cyaw*(y*map.info.resolution)+oy-map.info.origin.position.y) / map.info.resolution;
|
||||
if (abs((int)(map.data[yn*map.info.width+xn]) + (int)(m.data[y*mx+x])) <= 127)
|
||||
map.data[yn*map.info.width+xn] += m.data[y*mx+x];
|
||||
}
|
||||
}
|
||||
updated = true;
|
||||
}
|
||||
|
||||
const nav_msgs::OccupancyGrid& GetMap()
|
||||
{
|
||||
map.header.stamp = ros::Time::now();
|
||||
map.info.map_load_time = ros::Time::now();
|
||||
map.header.frame_id = string("/map");
|
||||
updated = false;
|
||||
return map;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,608 @@
|
||||
#ifndef MAP3D_H
|
||||
#define MAP3D_H
|
||||
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <tf/tf.h>
|
||||
#include <armadillo>
|
||||
#include <multi_map_server/SparseMap3D.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
// Occupancy probability of a sensor
|
||||
#define PROB_OCCUPIED 0.95
|
||||
|
||||
// Free space probability of a sensor
|
||||
#define PROB_FREE 0.01
|
||||
|
||||
// Threshold that determine a cell is occupied
|
||||
#define PROB_OCCUPIED_THRESHOLD 0.75
|
||||
|
||||
// If beyond this threshold, the occupancy(occupied) of this cell is fixed, no decay
|
||||
#define PROB_OCCUPIED_FIXED_THRESHOLD 0.95
|
||||
|
||||
// Threshold that determine a cell is free
|
||||
#define PROB_FREE_THRESHOLD 0.25
|
||||
|
||||
// If bwlow this threshold, the occupancy(free) of this cell is fixed, no decay
|
||||
#define PROB_FREE_FIXED_THRESHOLD 0.05
|
||||
|
||||
// Used integer value to store occupancy, create a large scale factor to enable small scale decay
|
||||
#define LOG_ODD_SCALE_FACTOR 10000
|
||||
|
||||
// Decay factor per second
|
||||
#define LOG_ODD_DECAY_RATE 1.00
|
||||
|
||||
// Binary value of the occupancy output
|
||||
#define OCCUPIED 1
|
||||
#define FREE -1
|
||||
|
||||
// Cell Struct -----------------------------------------
|
||||
struct OccupancyGrid
|
||||
{
|
||||
int upper;
|
||||
int lower;
|
||||
int mass;
|
||||
};
|
||||
|
||||
// Occupancy Grids List --------------------------------
|
||||
class OccupancyGridList
|
||||
{
|
||||
public:
|
||||
|
||||
OccupancyGridList() { updateCounter = 0; }
|
||||
|
||||
~OccupancyGridList() { }
|
||||
|
||||
void PackMsg(multi_map_server::VerticalOccupancyGridList &msg)
|
||||
{
|
||||
msg.x = x;
|
||||
msg.y = y;
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
{
|
||||
msg.upper.push_back(k->upper);
|
||||
msg.lower.push_back(k->lower);
|
||||
msg.mass.push_back(k->mass);
|
||||
}
|
||||
}
|
||||
|
||||
void UnpackMsg(const multi_map_server::VerticalOccupancyGridList &msg)
|
||||
{
|
||||
x = msg.x;
|
||||
y = msg.y;
|
||||
updateCounter = 0;
|
||||
grids.clear();
|
||||
for (unsigned int k = 0; k < msg.mass.size(); k++)
|
||||
{
|
||||
OccupancyGrid c;
|
||||
c.upper = msg.upper[k];
|
||||
c.lower = msg.lower[k];
|
||||
c.mass = msg.mass[k];
|
||||
grids.push_back(c);
|
||||
}
|
||||
}
|
||||
|
||||
void GetOccupancyGrids(vector<OccupancyGrid>& _grids)
|
||||
{
|
||||
_grids.clear();
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
_grids.push_back((*k));
|
||||
}
|
||||
|
||||
inline int GetUpdateCounter() { return updateCounter; }
|
||||
|
||||
inline void SetUpdateCounterXY(int _updateCounter, double _x, double _y)
|
||||
{
|
||||
updateCounter = _updateCounter;
|
||||
x = _x;
|
||||
y = _y;
|
||||
}
|
||||
|
||||
inline int GetOccupancyValue(int mz)
|
||||
{
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
if (mz <= k->upper && mz >= k->lower)
|
||||
return k->mass / (k->upper - k->lower + 1);
|
||||
return 0;
|
||||
}
|
||||
|
||||
inline void DeleteOccupancyGrid(int mz)
|
||||
{
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
{
|
||||
if (mz <= k->upper && mz >= k->lower)
|
||||
{
|
||||
grids.erase(k);
|
||||
return;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
inline void SetOccupancyValue(int mz, int value)
|
||||
{
|
||||
OccupancyGrid grid;
|
||||
grid.upper = mz;
|
||||
grid.lower = mz;
|
||||
grid.mass = value;
|
||||
|
||||
list<OccupancyGrid>::iterator gend = grids.end();
|
||||
gend--;
|
||||
|
||||
if (grids.size() == 0) // Empty case
|
||||
{
|
||||
grids.push_back(grid);
|
||||
return;
|
||||
}
|
||||
else if (mz - grids.begin()->upper > 1) // Beyond highest
|
||||
{
|
||||
grids.push_front(grid);
|
||||
return;
|
||||
}
|
||||
else if (mz - grids.begin()->upper == 1) // Next to highest
|
||||
{
|
||||
grids.begin()->upper += 1;
|
||||
grids.begin()->mass += value;
|
||||
return;
|
||||
}
|
||||
else if (gend->lower - mz > 1) // Below lowest
|
||||
{
|
||||
grids.push_back(grid);
|
||||
return;
|
||||
}
|
||||
else if (gend->lower - mz == 1) // Next to lowest
|
||||
{
|
||||
grids.end()->lower -= 1;
|
||||
grids.end()->mass += value;
|
||||
return;
|
||||
}
|
||||
else // General case
|
||||
{
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
{
|
||||
if (mz <= k->upper && mz >= k->lower) // Within a grid
|
||||
{
|
||||
k->mass += value;
|
||||
return;
|
||||
}
|
||||
else if (k != gend)
|
||||
{
|
||||
list<OccupancyGrid>::iterator j = k;
|
||||
j++;
|
||||
if (k->lower - mz == 1 && mz - j->upper > 1) // ###*--###
|
||||
{
|
||||
k->lower -= 1;
|
||||
k->mass += value;
|
||||
return;
|
||||
}
|
||||
else if (k->lower - mz > 1 && mz - j->upper == 1) // ###--*###
|
||||
{
|
||||
j->upper += 1;
|
||||
j->mass += value;
|
||||
return;
|
||||
}
|
||||
else if (k->lower - mz == 1 && mz - j->upper == 1) // ###*###
|
||||
{
|
||||
k->lower = j->lower;
|
||||
k->mass += j->mass + value;
|
||||
grids.erase(j);
|
||||
return;
|
||||
}
|
||||
else if (k->lower - mz > 1 && mz - j->upper > 1) // ###-*-###
|
||||
{
|
||||
grids.insert(j, grid);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Merging two columns, merge the grids in input "gridList" into current column
|
||||
inline void Merge(const OccupancyGridList& gridsIn)
|
||||
{
|
||||
// Create a sorted list containing both upper and lower values
|
||||
list<pair<int, int> > lp;
|
||||
for (list<OccupancyGrid>::const_iterator k = grids.begin(); k != grids.end(); k++)
|
||||
{
|
||||
lp.push_back( make_pair(k->upper, k->mass));
|
||||
lp.push_back( make_pair(k->lower, -1));
|
||||
}
|
||||
list<pair<int, int> > lp2;
|
||||
for (list<OccupancyGrid>::const_iterator k = gridsIn.grids.begin(); k != gridsIn.grids.end(); k++)
|
||||
{
|
||||
lp2.push_back( make_pair(k->upper, k->mass));
|
||||
lp2.push_back( make_pair(k->lower, -1));
|
||||
}
|
||||
lp.merge(lp2, ComparePair());
|
||||
// Manipulate this list to get a minimum size list
|
||||
grids.clear();
|
||||
int currUpper = 0;
|
||||
int currLower = 0;
|
||||
int currMass = 0;
|
||||
int upperCnt = 0;
|
||||
int lowerCnt = 0;
|
||||
list<pair<int, int> >::iterator lend = lp.end();
|
||||
lend--;
|
||||
for (list<pair<int, int> >::iterator k = lp.begin(); k != lp.end(); k++)
|
||||
{
|
||||
if (k->second > 0)
|
||||
{
|
||||
if (upperCnt == 0) currUpper = k->first;
|
||||
currMass = (k->second > currMass)?k->second:currMass;
|
||||
upperCnt++;
|
||||
}
|
||||
if (k->second < 0)
|
||||
{
|
||||
currLower = k->first;
|
||||
lowerCnt++;
|
||||
}
|
||||
if (lowerCnt == upperCnt && k != lend)
|
||||
{
|
||||
list<pair<int, int> >::iterator j = k;
|
||||
if (k->first - (++j)->first == 1) continue;
|
||||
}
|
||||
if (lowerCnt == upperCnt)
|
||||
{
|
||||
OccupancyGrid c;
|
||||
c.upper = currUpper;
|
||||
c.lower = currLower;
|
||||
c.mass = currMass;
|
||||
grids.push_back(c);
|
||||
upperCnt = lowerCnt = currUpper = currLower = currMass = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void Decay(int upThr, int lowThr, double factor)
|
||||
{
|
||||
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
|
||||
{
|
||||
int val = k->mass / (k->upper - k->lower + 1);
|
||||
if (val < upThr && val > lowThr)
|
||||
k->mass *= factor;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
struct ComparePair
|
||||
{
|
||||
bool operator()(pair<int, int> p1, pair<int, int> p2)
|
||||
{
|
||||
if (p1.first != p2.first)
|
||||
return (p1.first > p2.first);
|
||||
else
|
||||
return (p1.second > p2.second);
|
||||
}
|
||||
};
|
||||
|
||||
// List of vertical occupancy values
|
||||
list<OccupancyGrid> grids;
|
||||
// Location of the list in world frame
|
||||
double x;
|
||||
double y;
|
||||
// Update indicator
|
||||
int updateCounter;
|
||||
|
||||
};
|
||||
|
||||
// 3D Map Object ---------------------------------
|
||||
class Map3D
|
||||
{
|
||||
public:
|
||||
|
||||
Map3D()
|
||||
{
|
||||
resolution = 0.1;
|
||||
decayInterval = -1;
|
||||
originX = -5;
|
||||
originY = -5;
|
||||
originZ = 0;
|
||||
mapX = 200;
|
||||
mapY = 200;
|
||||
expandStep = 200;
|
||||
updated = false;
|
||||
updateCounter = 1;
|
||||
updateList.clear();
|
||||
mapBase.clear();
|
||||
mapBase.resize(mapX*mapY, NULL);
|
||||
logOddOccupied = log(PROB_OCCUPIED/(1.0-PROB_OCCUPIED)) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFree = log(PROB_FREE/(1.0-PROB_FREE)) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddOccupiedThr = log(1.0/(1.0-PROB_OCCUPIED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddOccupiedFixedThr = log(1.0/(1.0-PROB_OCCUPIED_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFreeThr = log(1.0/(1.0-PROB_FREE_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFreeFixedThr = log(1.0/(1.0-PROB_FREE_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
}
|
||||
|
||||
Map3D(const Map3D& _map3d)
|
||||
{
|
||||
resolution = _map3d.resolution;
|
||||
decayInterval = _map3d.decayInterval;
|
||||
originX = _map3d.originX;
|
||||
originY = _map3d.originY;
|
||||
originZ = _map3d.originZ;
|
||||
mapX = _map3d.mapX;
|
||||
mapY = _map3d.mapY;
|
||||
expandStep = _map3d.expandStep;
|
||||
updated = _map3d.updated;
|
||||
updateCounter = _map3d.updateCounter;
|
||||
updateList = _map3d.updateList;
|
||||
mapBase = _map3d.mapBase;
|
||||
for (unsigned int k = 0; k < mapBase.size(); k++)
|
||||
{
|
||||
if (mapBase[k])
|
||||
{
|
||||
mapBase[k] = new OccupancyGridList;
|
||||
*mapBase[k] = *_map3d.mapBase[k];
|
||||
}
|
||||
}
|
||||
logOddOccupied = log(PROB_OCCUPIED/(1.0-PROB_OCCUPIED)) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFree = log(PROB_FREE/(1.0-PROB_FREE)) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddOccupiedThr = log(1.0/(1.0-PROB_OCCUPIED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddOccupiedFixedThr = log(1.0/(1.0-PROB_OCCUPIED_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFreeThr = log(1.0/(1.0-PROB_FREE_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
logOddFreeFixedThr = log(1.0/(1.0-PROB_FREE_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
|
||||
}
|
||||
|
||||
~Map3D()
|
||||
{
|
||||
for (unsigned int k = 0; k < mapBase.size(); k++)
|
||||
{
|
||||
if (mapBase[k])
|
||||
{
|
||||
delete mapBase[k];
|
||||
mapBase[k] = NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PackMsg(multi_map_server::SparseMap3D &msg)
|
||||
{
|
||||
// Basic map info
|
||||
msg.header.stamp = ros::Time::now();
|
||||
msg.header.frame_id = string("/map");
|
||||
msg.info.map_load_time = ros::Time::now();
|
||||
msg.info.resolution = resolution;
|
||||
msg.info.origin.position.x = originX;
|
||||
msg.info.origin.position.y = originY;
|
||||
msg.info.origin.position.z = originZ;
|
||||
msg.info.width = mapX;
|
||||
msg.info.height = mapY;
|
||||
msg.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);
|
||||
// Pack columns into message
|
||||
msg.lists.clear();
|
||||
for (unsigned int k = 0; k < updateList.size(); k++)
|
||||
{
|
||||
multi_map_server::VerticalOccupancyGridList c;
|
||||
updateList[k]->PackMsg(c);
|
||||
msg.lists.push_back(c);
|
||||
}
|
||||
updateList.clear();
|
||||
updateCounter++;
|
||||
}
|
||||
|
||||
void UnpackMsg(const multi_map_server::SparseMap3D &msg)
|
||||
{
|
||||
// Unpack column msgs, Replace the whole column
|
||||
for (unsigned int k = 0; k < msg.lists.size(); k++)
|
||||
{
|
||||
int mx, my, mz;
|
||||
WorldFrameToMapFrame(msg.lists[k].x, msg.lists[k].y, 0, mx, my, mz);
|
||||
ResizeMapBase(mx, my);
|
||||
if (mapBase[my*mapX+mx]) delete mapBase[my*mapX+mx];
|
||||
mapBase[my*mapX+mx] = new OccupancyGridList;
|
||||
mapBase[my*mapX+mx]->UnpackMsg(msg.lists[k]);
|
||||
}
|
||||
CheckDecayMap();
|
||||
updated = true;
|
||||
}
|
||||
|
||||
inline void SetOccupancyFromWorldFrame(double x, double y, double z, double p = PROB_OCCUPIED)
|
||||
{
|
||||
// Find occupancy value to be set
|
||||
int value = 0;
|
||||
if (p == PROB_OCCUPIED) value = logOddOccupied;
|
||||
else if (p == PROB_FREE) value = logOddFree;
|
||||
else return;
|
||||
// Update occupancy value, return the column that have been changed
|
||||
int mx, my, mz;
|
||||
WorldFrameToMapFrame(x, y, z, mx, my, mz);
|
||||
ResizeMapBase(mx, my);
|
||||
if (!mapBase[my*mapX+mx])
|
||||
mapBase[my*mapX+mx] = new OccupancyGridList;
|
||||
mapBase[my*mapX+mx]->SetOccupancyValue(mz, value);
|
||||
// Also record the column that have been changed in another list, for publish incremental map
|
||||
if (mapBase[my*mapX+mx]->GetUpdateCounter() != updateCounter)
|
||||
{
|
||||
updateList.push_back(mapBase[my*mapX+mx]);
|
||||
mapBase[my*mapX+mx]->SetUpdateCounterXY(updateCounter, x, y);
|
||||
}
|
||||
updated = true;
|
||||
}
|
||||
|
||||
inline int GetOccupancyFromWorldFrame(double x, double y, double z)
|
||||
{
|
||||
int mx, my, mz;
|
||||
WorldFrameToMapFrame(x, y, z, mx, my, mz);
|
||||
if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
|
||||
return 0;
|
||||
if (!mapBase[my*mapX+mx])
|
||||
return 0;
|
||||
else
|
||||
{
|
||||
if (mapBase[my*mapX+mx]->GetOccupancyValue(mz) > logOddOccupiedThr)
|
||||
return 1;
|
||||
else if (mapBase[my*mapX+mx]->GetOccupancyValue(mz) < logOddFreeThr)
|
||||
return -1;
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
inline void DeleteFromWorldFrame(double x, double y, double z)
|
||||
{
|
||||
int mx, my, mz;
|
||||
WorldFrameToMapFrame(x, y, z, mx, my, mz);
|
||||
if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
|
||||
return;
|
||||
if (mapBase[my*mapX+mx])
|
||||
mapBase[my*mapX+mx]->DeleteOccupancyGrid(mz);
|
||||
}
|
||||
|
||||
vector<arma::colvec>& GetOccupancyWorldFrame(int type = OCCUPIED)
|
||||
{
|
||||
pts.clear();
|
||||
for (int mx = 0; mx < mapX; mx++)
|
||||
{
|
||||
for (int my = 0; my < mapY; my++)
|
||||
{
|
||||
if (mapBase[my*mapX+mx])
|
||||
{
|
||||
vector<OccupancyGrid> grids;
|
||||
mapBase[my*mapX+mx]->GetOccupancyGrids(grids);
|
||||
for (unsigned int k = 0; k < grids.size(); k++)
|
||||
{
|
||||
if ( (grids[k].mass / (grids[k].upper - grids[k].lower + 1) > logOddOccupiedThr && type == OCCUPIED) ||
|
||||
(grids[k].mass / (grids[k].upper - grids[k].lower + 1) < logOddFreeThr && type == FREE) )
|
||||
{
|
||||
for (int mz = grids[k].lower; mz <= grids[k].upper; mz++)
|
||||
{
|
||||
double x, y, z;
|
||||
MapFrameToWorldFrame(mx, my, mz, x, y, z);
|
||||
arma::colvec pt(3);
|
||||
pt(0) = x;
|
||||
pt(1) = y;
|
||||
pt(2) = z;
|
||||
pts.push_back(pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
// Do not allow setting parameters if at least one update received
|
||||
void SetResolution(double _resolution)
|
||||
{
|
||||
if (!updated)
|
||||
resolution = _resolution;
|
||||
}
|
||||
|
||||
void SetDecayInterval(double _decayInterval)
|
||||
{
|
||||
if (!updated && _decayInterval > 0)
|
||||
decayInterval = _decayInterval;
|
||||
}
|
||||
|
||||
inline double GetResolution() { return resolution; }
|
||||
inline double GetMaxX() { return originX + mapX*resolution; }
|
||||
inline double GetMinX() { return originX; }
|
||||
inline double GetMaxY() { return originY + mapY*resolution; }
|
||||
inline double GetMinY() { return originY; }
|
||||
inline bool Updated() { return updated; }
|
||||
|
||||
private:
|
||||
|
||||
inline void WorldFrameToMapFrame(double x, double y, double z, int& mx, int& my, int& mz)
|
||||
{
|
||||
mx = (x - originX) / resolution;
|
||||
my = (y - originY) / resolution;
|
||||
mz = (z - originZ) / resolution;
|
||||
}
|
||||
|
||||
inline void MapFrameToWorldFrame(int mx, int my, int mz, double& x, double& y, double& z)
|
||||
{
|
||||
double r = 0.5*resolution;
|
||||
x = mx * resolution + r + originX;
|
||||
y = my * resolution + r + originY;
|
||||
z = mz * resolution + r + originZ;
|
||||
}
|
||||
|
||||
inline void ResizeMapBase(int& mx, int& my)
|
||||
{
|
||||
if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
|
||||
{
|
||||
double prevOriginX = originX;
|
||||
double prevOriginY = originY;
|
||||
int prevMapX = mapX;
|
||||
int prevMapY = mapY;
|
||||
while(mx < 0)
|
||||
{
|
||||
mapX += expandStep;
|
||||
mx += expandStep;
|
||||
originX -= expandStep*resolution;
|
||||
}
|
||||
while(my < 0)
|
||||
{
|
||||
mapY += expandStep;
|
||||
my += expandStep;
|
||||
originY -= expandStep*resolution;
|
||||
}
|
||||
while(mx >= mapX)
|
||||
{
|
||||
mapX += expandStep;
|
||||
}
|
||||
while(my >= mapY)
|
||||
{
|
||||
mapY += expandStep;
|
||||
}
|
||||
vector<OccupancyGridList*> _mapBase = mapBase;
|
||||
mapBase.clear();
|
||||
mapBase.resize(mapX*mapY,NULL);
|
||||
for (int _x = 0; _x < prevMapX; _x++)
|
||||
{
|
||||
for(int _y = 0; _y < prevMapY; _y++)
|
||||
{
|
||||
int x = _x + round((prevOriginX - originX) / resolution);
|
||||
int y = _y + round((prevOriginY - originY) / resolution);
|
||||
mapBase[y*mapX+x] = _mapBase[_y*prevMapX+_x];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CheckDecayMap()
|
||||
{
|
||||
if (decayInterval < 0)
|
||||
return;
|
||||
// Check whether to decay
|
||||
static ros::Time prevDecayT = ros::Time::now();
|
||||
ros::Time t = ros::Time::now();
|
||||
double dt = (t - prevDecayT).toSec();
|
||||
if (dt > decayInterval)
|
||||
{
|
||||
double r = pow(LOG_ODD_DECAY_RATE, dt);
|
||||
for (int mx = 0; mx < mapX; mx++)
|
||||
for (int my = 0; my < mapY; my++)
|
||||
if (mapBase[my*mapX+mx])
|
||||
mapBase[my*mapX+mx]->Decay(logOddOccupiedFixedThr, logOddFreeFixedThr, r);
|
||||
prevDecayT = t;
|
||||
}
|
||||
}
|
||||
|
||||
double resolution;
|
||||
double decayInterval;
|
||||
|
||||
int logOddOccupied;
|
||||
int logOddFree;
|
||||
int logOddOccupiedThr;
|
||||
int logOddOccupiedFixedThr;
|
||||
int logOddFreeThr;
|
||||
int logOddFreeFixedThr;
|
||||
|
||||
bool updated;
|
||||
int updateCounter;
|
||||
vector<OccupancyGridList*> updateList;
|
||||
|
||||
double originX, originY, originZ;
|
||||
int mapX, mapY;
|
||||
int expandStep;
|
||||
vector<OccupancyGridList*> mapBase;
|
||||
|
||||
vector<arma::colvec> pts;
|
||||
|
||||
};
|
||||
#endif
|
||||
@ -0,0 +1,26 @@
|
||||
/**
|
||||
\mainpage
|
||||
\htmlinclude manifest.html
|
||||
|
||||
\b multi_map_server is ...
|
||||
|
||||
<!--
|
||||
Provide an overview of your package.
|
||||
-->
|
||||
|
||||
|
||||
\section codeapi Code API
|
||||
|
||||
<!--
|
||||
Provide links to specific auto-generated API documentation within your
|
||||
package that is of particular interest to a reader. Doxygen will
|
||||
document pretty much every part of your code, so do your best here to
|
||||
point the reader to the actual API.
|
||||
|
||||
If your codebase is fairly large or has different sets of APIs, you
|
||||
should use the doxygen 'group' tag to keep these APIs together. For
|
||||
example, the roscpp documentation has 'libros' group.
|
||||
-->
|
||||
|
||||
|
||||
*/
|
||||
@ -0,0 +1,2 @@
|
||||
nav_msgs/OccupancyGrid[] maps
|
||||
geometry_msgs/Pose[] origins
|
||||
@ -0,0 +1,2 @@
|
||||
SparseMap3D[] maps
|
||||
geometry_msgs/Pose[] origins
|
||||
@ -0,0 +1,4 @@
|
||||
Header header
|
||||
nav_msgs/MapMetaData info
|
||||
VerticalOccupancyGridList[] lists
|
||||
|
||||
@ -0,0 +1,6 @@
|
||||
float32 x
|
||||
float32 y
|
||||
int32[] upper
|
||||
int32[] lower
|
||||
int32[] mass
|
||||
|
||||
@ -0,0 +1,38 @@
|
||||
<package>
|
||||
<description>multi_map_server</description>
|
||||
<name>multi_map_server</name>
|
||||
<version>0.0.0</version>
|
||||
<maintainer email="eeshaojie@todo.todo">Shaojie Shen</maintainer>
|
||||
<license>BSD</license>
|
||||
|
||||
<url>http://ros.org/wiki/multi_map_server</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>visualization_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>std_srvs</build_depend>
|
||||
<build_depend>laser_geometry</build_depend>
|
||||
<build_depend>pose_utils</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>quadrotor_msgs</build_depend>
|
||||
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>visualization_msgs</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>tf</run_depend>
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>std_srvs</run_depend>
|
||||
<run_depend>laser_geometry</run_depend>
|
||||
<run_depend>pose_utils</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<run_depend>quadrotor_msgs</run_depend>
|
||||
|
||||
<export>
|
||||
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -0,0 +1 @@
|
||||
#autogenerated by ROS python message generators
|
||||
@ -0,0 +1,404 @@
|
||||
"""autogenerated by genpy from multi_map_server/MultiOccupancyGrid.msg. Do not edit."""
|
||||
import sys
|
||||
python3 = True if sys.hexversion > 0x03000000 else False
|
||||
import genpy
|
||||
import struct
|
||||
|
||||
import geometry_msgs.msg
|
||||
import nav_msgs.msg
|
||||
import genpy
|
||||
import std_msgs.msg
|
||||
|
||||
class MultiOccupancyGrid(genpy.Message):
|
||||
_md5sum = "61e63a291f11a6b1796a1edf79f34f72"
|
||||
_type = "multi_map_server/MultiOccupancyGrid"
|
||||
_has_header = False #flag to mark the presence of a Header object
|
||||
_full_text = """nav_msgs/OccupancyGrid[] maps
|
||||
geometry_msgs/Pose[] origins
|
||||
|
||||
================================================================================
|
||||
MSG: nav_msgs/OccupancyGrid
|
||||
# This represents a 2-D grid map, in which each cell represents the probability of
|
||||
# occupancy.
|
||||
|
||||
Header header
|
||||
|
||||
#MetaData for the map
|
||||
MapMetaData info
|
||||
|
||||
# The map data, in row-major order, starting with (0,0). Occupancy
|
||||
# probabilities are in the range [0,100]. Unknown is -1.
|
||||
int8[] data
|
||||
|
||||
================================================================================
|
||||
MSG: std_msgs/Header
|
||||
# Standard metadata for higher-level stamped data types.
|
||||
# This is generally used to communicate timestamped data
|
||||
# in a particular coordinate frame.
|
||||
#
|
||||
# sequence ID: consecutively increasing ID
|
||||
uint32 seq
|
||||
#Two-integer timestamp that is expressed as:
|
||||
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
|
||||
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
|
||||
# time-handling sugar is provided by the client library
|
||||
time stamp
|
||||
#Frame this data is associated with
|
||||
# 0: no frame
|
||||
# 1: global frame
|
||||
string frame_id
|
||||
|
||||
================================================================================
|
||||
MSG: nav_msgs/MapMetaData
|
||||
# This hold basic information about the characterists of the OccupancyGrid
|
||||
|
||||
# The time at which the map was loaded
|
||||
time map_load_time
|
||||
# The map resolution [m/cell]
|
||||
float32 resolution
|
||||
# Map width [cells]
|
||||
uint32 width
|
||||
# Map height [cells]
|
||||
uint32 height
|
||||
# The origin of the map [m, m, rad]. This is the real-world pose of the
|
||||
# cell (0,0) in the map.
|
||||
geometry_msgs/Pose origin
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Pose
|
||||
# A representation of pose in free space, composed of postion and orientation.
|
||||
Point position
|
||||
Quaternion orientation
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Point
|
||||
# This contains the position of a point in free space
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Quaternion
|
||||
# This represents an orientation in free space in quaternion form.
|
||||
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
|
||||
"""
|
||||
__slots__ = ['maps','origins']
|
||||
_slot_types = ['nav_msgs/OccupancyGrid[]','geometry_msgs/Pose[]']
|
||||
|
||||
def __init__(self, *args, **kwds):
|
||||
"""
|
||||
Constructor. Any message fields that are implicitly/explicitly
|
||||
set to None will be assigned a default value. The recommend
|
||||
use is keyword arguments as this is more robust to future message
|
||||
changes. You cannot mix in-order arguments and keyword arguments.
|
||||
|
||||
The available fields are:
|
||||
maps,origins
|
||||
|
||||
:param args: complete set of field values, in .msg order
|
||||
:param kwds: use keyword arguments corresponding to message field names
|
||||
to set specific fields.
|
||||
"""
|
||||
if args or kwds:
|
||||
super(MultiOccupancyGrid, self).__init__(*args, **kwds)
|
||||
#message fields cannot be None, assign default values for those that are
|
||||
if self.maps is None:
|
||||
self.maps = []
|
||||
if self.origins is None:
|
||||
self.origins = []
|
||||
else:
|
||||
self.maps = []
|
||||
self.origins = []
|
||||
|
||||
def _get_types(self):
|
||||
"""
|
||||
internal API method
|
||||
"""
|
||||
return self._slot_types
|
||||
|
||||
def serialize(self, buff):
|
||||
"""
|
||||
serialize message into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
"""
|
||||
try:
|
||||
length = len(self.maps)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.maps:
|
||||
_v1 = val1.header
|
||||
buff.write(_struct_I.pack(_v1.seq))
|
||||
_v2 = _v1.stamp
|
||||
_x = _v2
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v1.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_v3 = val1.info
|
||||
_v4 = _v3.map_load_time
|
||||
_x = _v4
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v3
|
||||
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
|
||||
_v5 = _v3.origin
|
||||
_v6 = _v5.position
|
||||
_x = _v6
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v7 = _v5.orientation
|
||||
_x = _v7
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
length = len(val1.data)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%sb'%length
|
||||
buff.write(struct.pack(pattern, *val1.data))
|
||||
length = len(self.origins)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.origins:
|
||||
_v8 = val1.position
|
||||
_x = _v8
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v9 = val1.orientation
|
||||
_x = _v9
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize(self, str):
|
||||
"""
|
||||
unpack serialized message in str into this message instance
|
||||
:param str: byte array of serialized message, ``str``
|
||||
"""
|
||||
try:
|
||||
if self.maps is None:
|
||||
self.maps = None
|
||||
if self.origins is None:
|
||||
self.origins = None
|
||||
end = 0
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.maps = []
|
||||
for i in range(0, length):
|
||||
val1 = nav_msgs.msg.OccupancyGrid()
|
||||
_v10 = val1.header
|
||||
start = end
|
||||
end += 4
|
||||
(_v10.seq,) = _struct_I.unpack(str[start:end])
|
||||
_v11 = _v10.stamp
|
||||
_x = _v11
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
_v10.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
_v10.frame_id = str[start:end]
|
||||
_v12 = val1.info
|
||||
_v13 = _v12.map_load_time
|
||||
_x = _v13
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
_x = _v12
|
||||
start = end
|
||||
end += 12
|
||||
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
|
||||
_v14 = _v12.origin
|
||||
_v15 = _v14.position
|
||||
_x = _v15
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v16 = _v14.orientation
|
||||
_x = _v16
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%sb'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.data = struct.unpack(pattern, str[start:end])
|
||||
self.maps.append(val1)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.origins = []
|
||||
for i in range(0, length):
|
||||
val1 = geometry_msgs.msg.Pose()
|
||||
_v17 = val1.position
|
||||
_x = _v17
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v18 = val1.orientation
|
||||
_x = _v18
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
self.origins.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
|
||||
def serialize_numpy(self, buff, numpy):
|
||||
"""
|
||||
serialize message with numpy array types into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
length = len(self.maps)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.maps:
|
||||
_v19 = val1.header
|
||||
buff.write(_struct_I.pack(_v19.seq))
|
||||
_v20 = _v19.stamp
|
||||
_x = _v20
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v19.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_v21 = val1.info
|
||||
_v22 = _v21.map_load_time
|
||||
_x = _v22
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v21
|
||||
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
|
||||
_v23 = _v21.origin
|
||||
_v24 = _v23.position
|
||||
_x = _v24
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v25 = _v23.orientation
|
||||
_x = _v25
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
length = len(val1.data)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%sb'%length
|
||||
buff.write(val1.data.tostring())
|
||||
length = len(self.origins)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.origins:
|
||||
_v26 = val1.position
|
||||
_x = _v26
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v27 = val1.orientation
|
||||
_x = _v27
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize_numpy(self, str, numpy):
|
||||
"""
|
||||
unpack serialized message in str into this message instance using numpy for array types
|
||||
:param str: byte array of serialized message, ``str``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
if self.maps is None:
|
||||
self.maps = None
|
||||
if self.origins is None:
|
||||
self.origins = None
|
||||
end = 0
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.maps = []
|
||||
for i in range(0, length):
|
||||
val1 = nav_msgs.msg.OccupancyGrid()
|
||||
_v28 = val1.header
|
||||
start = end
|
||||
end += 4
|
||||
(_v28.seq,) = _struct_I.unpack(str[start:end])
|
||||
_v29 = _v28.stamp
|
||||
_x = _v29
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
_v28.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
_v28.frame_id = str[start:end]
|
||||
_v30 = val1.info
|
||||
_v31 = _v30.map_load_time
|
||||
_x = _v31
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
_x = _v30
|
||||
start = end
|
||||
end += 12
|
||||
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
|
||||
_v32 = _v30.origin
|
||||
_v33 = _v32.position
|
||||
_x = _v33
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v34 = _v32.orientation
|
||||
_x = _v34
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%sb'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.data = numpy.frombuffer(str[start:end], dtype=numpy.int8, count=length)
|
||||
self.maps.append(val1)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.origins = []
|
||||
for i in range(0, length):
|
||||
val1 = geometry_msgs.msg.Pose()
|
||||
_v35 = val1.position
|
||||
_x = _v35
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v36 = val1.orientation
|
||||
_x = _v36
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
self.origins.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
_struct_I = genpy.struct_I
|
||||
_struct_4d = struct.Struct("<4d")
|
||||
_struct_f2I = struct.Struct("<f2I")
|
||||
_struct_2I = struct.Struct("<2I")
|
||||
_struct_3d = struct.Struct("<3d")
|
||||
@ -0,0 +1,484 @@
|
||||
"""autogenerated by genpy from multi_map_server/MultiSparseMap3D.msg. Do not edit."""
|
||||
import sys
|
||||
python3 = True if sys.hexversion > 0x03000000 else False
|
||||
import genpy
|
||||
import struct
|
||||
|
||||
import geometry_msgs.msg
|
||||
import multi_map_server.msg
|
||||
import nav_msgs.msg
|
||||
import genpy
|
||||
import std_msgs.msg
|
||||
|
||||
class MultiSparseMap3D(genpy.Message):
|
||||
_md5sum = "2e3d76c98ee3e2b23a422f64965f6418"
|
||||
_type = "multi_map_server/MultiSparseMap3D"
|
||||
_has_header = False #flag to mark the presence of a Header object
|
||||
_full_text = """SparseMap3D[] maps
|
||||
geometry_msgs/Pose[] origins
|
||||
|
||||
================================================================================
|
||||
MSG: multi_map_server/SparseMap3D
|
||||
Header header
|
||||
nav_msgs/MapMetaData info
|
||||
VerticalOccupancyGridList[] lists
|
||||
|
||||
|
||||
================================================================================
|
||||
MSG: std_msgs/Header
|
||||
# Standard metadata for higher-level stamped data types.
|
||||
# This is generally used to communicate timestamped data
|
||||
# in a particular coordinate frame.
|
||||
#
|
||||
# sequence ID: consecutively increasing ID
|
||||
uint32 seq
|
||||
#Two-integer timestamp that is expressed as:
|
||||
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
|
||||
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
|
||||
# time-handling sugar is provided by the client library
|
||||
time stamp
|
||||
#Frame this data is associated with
|
||||
# 0: no frame
|
||||
# 1: global frame
|
||||
string frame_id
|
||||
|
||||
================================================================================
|
||||
MSG: nav_msgs/MapMetaData
|
||||
# This hold basic information about the characterists of the OccupancyGrid
|
||||
|
||||
# The time at which the map was loaded
|
||||
time map_load_time
|
||||
# The map resolution [m/cell]
|
||||
float32 resolution
|
||||
# Map width [cells]
|
||||
uint32 width
|
||||
# Map height [cells]
|
||||
uint32 height
|
||||
# The origin of the map [m, m, rad]. This is the real-world pose of the
|
||||
# cell (0,0) in the map.
|
||||
geometry_msgs/Pose origin
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Pose
|
||||
# A representation of pose in free space, composed of postion and orientation.
|
||||
Point position
|
||||
Quaternion orientation
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Point
|
||||
# This contains the position of a point in free space
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Quaternion
|
||||
# This represents an orientation in free space in quaternion form.
|
||||
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
|
||||
================================================================================
|
||||
MSG: multi_map_server/VerticalOccupancyGridList
|
||||
float32 x
|
||||
float32 y
|
||||
int32[] upper
|
||||
int32[] lower
|
||||
int32[] mass
|
||||
|
||||
|
||||
"""
|
||||
__slots__ = ['maps','origins']
|
||||
_slot_types = ['multi_map_server/SparseMap3D[]','geometry_msgs/Pose[]']
|
||||
|
||||
def __init__(self, *args, **kwds):
|
||||
"""
|
||||
Constructor. Any message fields that are implicitly/explicitly
|
||||
set to None will be assigned a default value. The recommend
|
||||
use is keyword arguments as this is more robust to future message
|
||||
changes. You cannot mix in-order arguments and keyword arguments.
|
||||
|
||||
The available fields are:
|
||||
maps,origins
|
||||
|
||||
:param args: complete set of field values, in .msg order
|
||||
:param kwds: use keyword arguments corresponding to message field names
|
||||
to set specific fields.
|
||||
"""
|
||||
if args or kwds:
|
||||
super(MultiSparseMap3D, self).__init__(*args, **kwds)
|
||||
#message fields cannot be None, assign default values for those that are
|
||||
if self.maps is None:
|
||||
self.maps = []
|
||||
if self.origins is None:
|
||||
self.origins = []
|
||||
else:
|
||||
self.maps = []
|
||||
self.origins = []
|
||||
|
||||
def _get_types(self):
|
||||
"""
|
||||
internal API method
|
||||
"""
|
||||
return self._slot_types
|
||||
|
||||
def serialize(self, buff):
|
||||
"""
|
||||
serialize message into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
"""
|
||||
try:
|
||||
length = len(self.maps)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.maps:
|
||||
_v1 = val1.header
|
||||
buff.write(_struct_I.pack(_v1.seq))
|
||||
_v2 = _v1.stamp
|
||||
_x = _v2
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v1.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_v3 = val1.info
|
||||
_v4 = _v3.map_load_time
|
||||
_x = _v4
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v3
|
||||
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
|
||||
_v5 = _v3.origin
|
||||
_v6 = _v5.position
|
||||
_x = _v6
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v7 = _v5.orientation
|
||||
_x = _v7
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
length = len(val1.lists)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val2 in val1.lists:
|
||||
_x = val2
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(val2.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val2.upper))
|
||||
length = len(val2.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val2.lower))
|
||||
length = len(val2.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val2.mass))
|
||||
length = len(self.origins)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.origins:
|
||||
_v8 = val1.position
|
||||
_x = _v8
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v9 = val1.orientation
|
||||
_x = _v9
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize(self, str):
|
||||
"""
|
||||
unpack serialized message in str into this message instance
|
||||
:param str: byte array of serialized message, ``str``
|
||||
"""
|
||||
try:
|
||||
if self.maps is None:
|
||||
self.maps = None
|
||||
if self.origins is None:
|
||||
self.origins = None
|
||||
end = 0
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.maps = []
|
||||
for i in range(0, length):
|
||||
val1 = multi_map_server.msg.SparseMap3D()
|
||||
_v10 = val1.header
|
||||
start = end
|
||||
end += 4
|
||||
(_v10.seq,) = _struct_I.unpack(str[start:end])
|
||||
_v11 = _v10.stamp
|
||||
_x = _v11
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
_v10.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
_v10.frame_id = str[start:end]
|
||||
_v12 = val1.info
|
||||
_v13 = _v12.map_load_time
|
||||
_x = _v13
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
_x = _v12
|
||||
start = end
|
||||
end += 12
|
||||
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
|
||||
_v14 = _v12.origin
|
||||
_v15 = _v14.position
|
||||
_x = _v15
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v16 = _v14.orientation
|
||||
_x = _v16
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
val1.lists = []
|
||||
for i in range(0, length):
|
||||
val2 = multi_map_server.msg.VerticalOccupancyGridList()
|
||||
_x = val2
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.upper = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.lower = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.mass = struct.unpack(pattern, str[start:end])
|
||||
val1.lists.append(val2)
|
||||
self.maps.append(val1)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.origins = []
|
||||
for i in range(0, length):
|
||||
val1 = geometry_msgs.msg.Pose()
|
||||
_v17 = val1.position
|
||||
_x = _v17
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v18 = val1.orientation
|
||||
_x = _v18
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
self.origins.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
|
||||
def serialize_numpy(self, buff, numpy):
|
||||
"""
|
||||
serialize message with numpy array types into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
length = len(self.maps)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.maps:
|
||||
_v19 = val1.header
|
||||
buff.write(_struct_I.pack(_v19.seq))
|
||||
_v20 = _v19.stamp
|
||||
_x = _v20
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v19.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_v21 = val1.info
|
||||
_v22 = _v21.map_load_time
|
||||
_x = _v22
|
||||
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
|
||||
_x = _v21
|
||||
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
|
||||
_v23 = _v21.origin
|
||||
_v24 = _v23.position
|
||||
_x = _v24
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v25 = _v23.orientation
|
||||
_x = _v25
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
length = len(val1.lists)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val2 in val1.lists:
|
||||
_x = val2
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(val2.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val2.upper.tostring())
|
||||
length = len(val2.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val2.lower.tostring())
|
||||
length = len(val2.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val2.mass.tostring())
|
||||
length = len(self.origins)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.origins:
|
||||
_v26 = val1.position
|
||||
_x = _v26
|
||||
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
|
||||
_v27 = val1.orientation
|
||||
_x = _v27
|
||||
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize_numpy(self, str, numpy):
|
||||
"""
|
||||
unpack serialized message in str into this message instance using numpy for array types
|
||||
:param str: byte array of serialized message, ``str``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
if self.maps is None:
|
||||
self.maps = None
|
||||
if self.origins is None:
|
||||
self.origins = None
|
||||
end = 0
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.maps = []
|
||||
for i in range(0, length):
|
||||
val1 = multi_map_server.msg.SparseMap3D()
|
||||
_v28 = val1.header
|
||||
start = end
|
||||
end += 4
|
||||
(_v28.seq,) = _struct_I.unpack(str[start:end])
|
||||
_v29 = _v28.stamp
|
||||
_x = _v29
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
_v28.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
_v28.frame_id = str[start:end]
|
||||
_v30 = val1.info
|
||||
_v31 = _v30.map_load_time
|
||||
_x = _v31
|
||||
start = end
|
||||
end += 8
|
||||
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
|
||||
_x = _v30
|
||||
start = end
|
||||
end += 12
|
||||
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
|
||||
_v32 = _v30.origin
|
||||
_v33 = _v32.position
|
||||
_x = _v33
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v34 = _v32.orientation
|
||||
_x = _v34
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
val1.lists = []
|
||||
for i in range(0, length):
|
||||
val2 = multi_map_server.msg.VerticalOccupancyGridList()
|
||||
_x = val2
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val2.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
val1.lists.append(val2)
|
||||
self.maps.append(val1)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.origins = []
|
||||
for i in range(0, length):
|
||||
val1 = geometry_msgs.msg.Pose()
|
||||
_v35 = val1.position
|
||||
_x = _v35
|
||||
start = end
|
||||
end += 24
|
||||
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
|
||||
_v36 = val1.orientation
|
||||
_x = _v36
|
||||
start = end
|
||||
end += 32
|
||||
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
|
||||
self.origins.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
_struct_I = genpy.struct_I
|
||||
_struct_f2I = struct.Struct("<f2I")
|
||||
_struct_2f = struct.Struct("<2f")
|
||||
_struct_4d = struct.Struct("<4d")
|
||||
_struct_2I = struct.Struct("<2I")
|
||||
_struct_3d = struct.Struct("<3d")
|
||||
@ -0,0 +1,340 @@
|
||||
"""autogenerated by genpy from multi_map_server/SparseMap3D.msg. Do not edit."""
|
||||
import sys
|
||||
python3 = True if sys.hexversion > 0x03000000 else False
|
||||
import genpy
|
||||
import struct
|
||||
|
||||
import geometry_msgs.msg
|
||||
import multi_map_server.msg
|
||||
import nav_msgs.msg
|
||||
import genpy
|
||||
import std_msgs.msg
|
||||
|
||||
class SparseMap3D(genpy.Message):
|
||||
_md5sum = "a20102f0b3a02e95070dab4140b78fb5"
|
||||
_type = "multi_map_server/SparseMap3D"
|
||||
_has_header = True #flag to mark the presence of a Header object
|
||||
_full_text = """Header header
|
||||
nav_msgs/MapMetaData info
|
||||
VerticalOccupancyGridList[] lists
|
||||
|
||||
|
||||
================================================================================
|
||||
MSG: std_msgs/Header
|
||||
# Standard metadata for higher-level stamped data types.
|
||||
# This is generally used to communicate timestamped data
|
||||
# in a particular coordinate frame.
|
||||
#
|
||||
# sequence ID: consecutively increasing ID
|
||||
uint32 seq
|
||||
#Two-integer timestamp that is expressed as:
|
||||
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
|
||||
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
|
||||
# time-handling sugar is provided by the client library
|
||||
time stamp
|
||||
#Frame this data is associated with
|
||||
# 0: no frame
|
||||
# 1: global frame
|
||||
string frame_id
|
||||
|
||||
================================================================================
|
||||
MSG: nav_msgs/MapMetaData
|
||||
# This hold basic information about the characterists of the OccupancyGrid
|
||||
|
||||
# The time at which the map was loaded
|
||||
time map_load_time
|
||||
# The map resolution [m/cell]
|
||||
float32 resolution
|
||||
# Map width [cells]
|
||||
uint32 width
|
||||
# Map height [cells]
|
||||
uint32 height
|
||||
# The origin of the map [m, m, rad]. This is the real-world pose of the
|
||||
# cell (0,0) in the map.
|
||||
geometry_msgs/Pose origin
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Pose
|
||||
# A representation of pose in free space, composed of postion and orientation.
|
||||
Point position
|
||||
Quaternion orientation
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Point
|
||||
# This contains the position of a point in free space
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
|
||||
================================================================================
|
||||
MSG: geometry_msgs/Quaternion
|
||||
# This represents an orientation in free space in quaternion form.
|
||||
|
||||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
float64 w
|
||||
|
||||
================================================================================
|
||||
MSG: multi_map_server/VerticalOccupancyGridList
|
||||
float32 x
|
||||
float32 y
|
||||
int32[] upper
|
||||
int32[] lower
|
||||
int32[] mass
|
||||
|
||||
|
||||
"""
|
||||
__slots__ = ['header','info','lists']
|
||||
_slot_types = ['std_msgs/Header','nav_msgs/MapMetaData','multi_map_server/VerticalOccupancyGridList[]']
|
||||
|
||||
def __init__(self, *args, **kwds):
|
||||
"""
|
||||
Constructor. Any message fields that are implicitly/explicitly
|
||||
set to None will be assigned a default value. The recommend
|
||||
use is keyword arguments as this is more robust to future message
|
||||
changes. You cannot mix in-order arguments and keyword arguments.
|
||||
|
||||
The available fields are:
|
||||
header,info,lists
|
||||
|
||||
:param args: complete set of field values, in .msg order
|
||||
:param kwds: use keyword arguments corresponding to message field names
|
||||
to set specific fields.
|
||||
"""
|
||||
if args or kwds:
|
||||
super(SparseMap3D, self).__init__(*args, **kwds)
|
||||
#message fields cannot be None, assign default values for those that are
|
||||
if self.header is None:
|
||||
self.header = std_msgs.msg.Header()
|
||||
if self.info is None:
|
||||
self.info = nav_msgs.msg.MapMetaData()
|
||||
if self.lists is None:
|
||||
self.lists = []
|
||||
else:
|
||||
self.header = std_msgs.msg.Header()
|
||||
self.info = nav_msgs.msg.MapMetaData()
|
||||
self.lists = []
|
||||
|
||||
def _get_types(self):
|
||||
"""
|
||||
internal API method
|
||||
"""
|
||||
return self._slot_types
|
||||
|
||||
def serialize(self, buff):
|
||||
"""
|
||||
serialize message into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
"""
|
||||
try:
|
||||
_x = self
|
||||
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
|
||||
_x = self.header.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_x = self
|
||||
buff.write(_struct_2If2I7d.pack(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w))
|
||||
length = len(self.lists)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.lists:
|
||||
_x = val1
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(val1.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val1.upper))
|
||||
length = len(val1.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val1.lower))
|
||||
length = len(val1.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *val1.mass))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize(self, str):
|
||||
"""
|
||||
unpack serialized message in str into this message instance
|
||||
:param str: byte array of serialized message, ``str``
|
||||
"""
|
||||
try:
|
||||
if self.header is None:
|
||||
self.header = std_msgs.msg.Header()
|
||||
if self.info is None:
|
||||
self.info = nav_msgs.msg.MapMetaData()
|
||||
if self.lists is None:
|
||||
self.lists = None
|
||||
end = 0
|
||||
_x = self
|
||||
start = end
|
||||
end += 12
|
||||
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
self.header.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
self.header.frame_id = str[start:end]
|
||||
_x = self
|
||||
start = end
|
||||
end += 76
|
||||
(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w,) = _struct_2If2I7d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.lists = []
|
||||
for i in range(0, length):
|
||||
val1 = multi_map_server.msg.VerticalOccupancyGridList()
|
||||
_x = val1
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.upper = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.lower = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.mass = struct.unpack(pattern, str[start:end])
|
||||
self.lists.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
|
||||
def serialize_numpy(self, buff, numpy):
|
||||
"""
|
||||
serialize message with numpy array types into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
_x = self
|
||||
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
|
||||
_x = self.header.frame_id
|
||||
length = len(_x)
|
||||
if python3 or type(_x) == unicode:
|
||||
_x = _x.encode('utf-8')
|
||||
length = len(_x)
|
||||
if python3:
|
||||
buff.write(struct.pack('<I%sB'%length, length, *_x))
|
||||
else:
|
||||
buff.write(struct.pack('<I%ss'%length, length, _x))
|
||||
_x = self
|
||||
buff.write(_struct_2If2I7d.pack(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w))
|
||||
length = len(self.lists)
|
||||
buff.write(_struct_I.pack(length))
|
||||
for val1 in self.lists:
|
||||
_x = val1
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(val1.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val1.upper.tostring())
|
||||
length = len(val1.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val1.lower.tostring())
|
||||
length = len(val1.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(val1.mass.tostring())
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize_numpy(self, str, numpy):
|
||||
"""
|
||||
unpack serialized message in str into this message instance using numpy for array types
|
||||
:param str: byte array of serialized message, ``str``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
if self.header is None:
|
||||
self.header = std_msgs.msg.Header()
|
||||
if self.info is None:
|
||||
self.info = nav_msgs.msg.MapMetaData()
|
||||
if self.lists is None:
|
||||
self.lists = None
|
||||
end = 0
|
||||
_x = self
|
||||
start = end
|
||||
end += 12
|
||||
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
start = end
|
||||
end += length
|
||||
if python3:
|
||||
self.header.frame_id = str[start:end].decode('utf-8')
|
||||
else:
|
||||
self.header.frame_id = str[start:end]
|
||||
_x = self
|
||||
start = end
|
||||
end += 76
|
||||
(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w,) = _struct_2If2I7d.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
self.lists = []
|
||||
for i in range(0, length):
|
||||
val1 = multi_map_server.msg.VerticalOccupancyGridList()
|
||||
_x = val1
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
val1.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
self.lists.append(val1)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
_struct_I = genpy.struct_I
|
||||
_struct_3I = struct.Struct("<3I")
|
||||
_struct_2f = struct.Struct("<2f")
|
||||
_struct_2If2I7d = struct.Struct("<2If2I7d")
|
||||
@ -0,0 +1,185 @@
|
||||
"""autogenerated by genpy from multi_map_server/VerticalOccupancyGridList.msg. Do not edit."""
|
||||
import sys
|
||||
python3 = True if sys.hexversion > 0x03000000 else False
|
||||
import genpy
|
||||
import struct
|
||||
|
||||
|
||||
class VerticalOccupancyGridList(genpy.Message):
|
||||
_md5sum = "7ef85cc95b82747f51eb01a16bd7c795"
|
||||
_type = "multi_map_server/VerticalOccupancyGridList"
|
||||
_has_header = False #flag to mark the presence of a Header object
|
||||
_full_text = """float32 x
|
||||
float32 y
|
||||
int32[] upper
|
||||
int32[] lower
|
||||
int32[] mass
|
||||
|
||||
|
||||
"""
|
||||
__slots__ = ['x','y','upper','lower','mass']
|
||||
_slot_types = ['float32','float32','int32[]','int32[]','int32[]']
|
||||
|
||||
def __init__(self, *args, **kwds):
|
||||
"""
|
||||
Constructor. Any message fields that are implicitly/explicitly
|
||||
set to None will be assigned a default value. The recommend
|
||||
use is keyword arguments as this is more robust to future message
|
||||
changes. You cannot mix in-order arguments and keyword arguments.
|
||||
|
||||
The available fields are:
|
||||
x,y,upper,lower,mass
|
||||
|
||||
:param args: complete set of field values, in .msg order
|
||||
:param kwds: use keyword arguments corresponding to message field names
|
||||
to set specific fields.
|
||||
"""
|
||||
if args or kwds:
|
||||
super(VerticalOccupancyGridList, self).__init__(*args, **kwds)
|
||||
#message fields cannot be None, assign default values for those that are
|
||||
if self.x is None:
|
||||
self.x = 0.
|
||||
if self.y is None:
|
||||
self.y = 0.
|
||||
if self.upper is None:
|
||||
self.upper = []
|
||||
if self.lower is None:
|
||||
self.lower = []
|
||||
if self.mass is None:
|
||||
self.mass = []
|
||||
else:
|
||||
self.x = 0.
|
||||
self.y = 0.
|
||||
self.upper = []
|
||||
self.lower = []
|
||||
self.mass = []
|
||||
|
||||
def _get_types(self):
|
||||
"""
|
||||
internal API method
|
||||
"""
|
||||
return self._slot_types
|
||||
|
||||
def serialize(self, buff):
|
||||
"""
|
||||
serialize message into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
"""
|
||||
try:
|
||||
_x = self
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(self.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *self.upper))
|
||||
length = len(self.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *self.lower))
|
||||
length = len(self.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(struct.pack(pattern, *self.mass))
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize(self, str):
|
||||
"""
|
||||
unpack serialized message in str into this message instance
|
||||
:param str: byte array of serialized message, ``str``
|
||||
"""
|
||||
try:
|
||||
end = 0
|
||||
_x = self
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.upper = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.lower = struct.unpack(pattern, str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.mass = struct.unpack(pattern, str[start:end])
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
|
||||
def serialize_numpy(self, buff, numpy):
|
||||
"""
|
||||
serialize message with numpy array types into buffer
|
||||
:param buff: buffer, ``StringIO``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
_x = self
|
||||
buff.write(_struct_2f.pack(_x.x, _x.y))
|
||||
length = len(self.upper)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(self.upper.tostring())
|
||||
length = len(self.lower)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(self.lower.tostring())
|
||||
length = len(self.mass)
|
||||
buff.write(_struct_I.pack(length))
|
||||
pattern = '<%si'%length
|
||||
buff.write(self.mass.tostring())
|
||||
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
|
||||
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
|
||||
|
||||
def deserialize_numpy(self, str, numpy):
|
||||
"""
|
||||
unpack serialized message in str into this message instance using numpy for array types
|
||||
:param str: byte array of serialized message, ``str``
|
||||
:param numpy: numpy python module
|
||||
"""
|
||||
try:
|
||||
end = 0
|
||||
_x = self
|
||||
start = end
|
||||
end += 8
|
||||
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
start = end
|
||||
end += 4
|
||||
(length,) = _struct_I.unpack(str[start:end])
|
||||
pattern = '<%si'%length
|
||||
start = end
|
||||
end += struct.calcsize(pattern)
|
||||
self.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
|
||||
return self
|
||||
except struct.error as e:
|
||||
raise genpy.DeserializationError(e) #most likely buffer underfill
|
||||
|
||||
_struct_I = genpy.struct_I
|
||||
_struct_2f = struct.Struct("<2f")
|
||||
@ -0,0 +1,4 @@
|
||||
from ._SparseMap3D import *
|
||||
from ._MultiOccupancyGrid import *
|
||||
from ._MultiSparseMap3D import *
|
||||
from ._VerticalOccupancyGridList import *
|
||||
@ -0,0 +1,90 @@
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <pose_utils.h>
|
||||
#include <multi_map_server/MultiOccupancyGrid.h>
|
||||
#include <multi_map_server/MultiSparseMap3D.h>
|
||||
#include <multi_map_server/Map2D.h>
|
||||
#include <multi_map_server/Map3D.h>
|
||||
|
||||
ros::Publisher pub1;
|
||||
ros::Publisher pub2;
|
||||
|
||||
vector<Map2D> maps2d;
|
||||
vector<geometry_msgs::Pose> origins2d;
|
||||
vector<Map3D> maps3d;
|
||||
vector<geometry_msgs::Pose> origins3d;
|
||||
|
||||
void maps2d_callback(const multi_map_server::MultiOccupancyGrid::ConstPtr &msg)
|
||||
{
|
||||
// Merge map
|
||||
maps2d.resize(msg->maps.size(), Map2D(4));
|
||||
for (unsigned int k = 0; k < msg->maps.size(); k++)
|
||||
maps2d[k].Replace(msg->maps[k]);
|
||||
origins2d = msg->origins;
|
||||
// Assemble and publish map
|
||||
multi_map_server::MultiOccupancyGrid m;
|
||||
m.maps.resize(maps2d.size());
|
||||
m.origins.resize(maps2d.size());
|
||||
for (unsigned int k = 0; k < maps2d.size(); k++)
|
||||
{
|
||||
m.maps[k] = maps2d[k].GetMap();
|
||||
m.origins[k] = origins2d[k];
|
||||
}
|
||||
pub1.publish(m);
|
||||
}
|
||||
|
||||
void maps3d_callback(const multi_map_server::MultiSparseMap3D::ConstPtr &msg)
|
||||
{
|
||||
// Update incremental map
|
||||
maps3d.resize(msg->maps.size());
|
||||
for (unsigned int k = 0; k < msg->maps.size(); k++)
|
||||
maps3d[k].UnpackMsg(msg->maps[k]);
|
||||
origins3d = msg->origins;
|
||||
// Publish
|
||||
sensor_msgs::PointCloud m;
|
||||
for (unsigned int k = 0; k < msg->maps.size(); k++)
|
||||
{
|
||||
colvec po(6);
|
||||
po(0) = origins3d[k].position.x;
|
||||
po(1) = origins3d[k].position.y;
|
||||
po(2) = origins3d[k].position.z;
|
||||
colvec poq(4);
|
||||
poq(0) = origins3d[k].orientation.w;
|
||||
poq(1) = origins3d[k].orientation.x;
|
||||
poq(2) = origins3d[k].orientation.y;
|
||||
poq(3) = origins3d[k].orientation.z;
|
||||
po.rows(3,5) = R_to_ypr(quaternion_to_R(poq));
|
||||
colvec tpo = po.rows(0,2);
|
||||
mat Rpo = ypr_to_R(po.rows(3,5));
|
||||
vector<colvec> pts = maps3d[k].GetOccupancyWorldFrame(OCCUPIED);
|
||||
for (unsigned int i = 0; i < pts.size(); i++)
|
||||
{
|
||||
colvec pt = Rpo * pts[i] + tpo;
|
||||
geometry_msgs::Point32 _pt;
|
||||
_pt.x = pt(0);
|
||||
_pt.y = pt(1);
|
||||
_pt.z = pt(2);
|
||||
m.points.push_back(_pt);
|
||||
}
|
||||
}
|
||||
// Publish
|
||||
m.header.stamp = ros::Time::now();
|
||||
m.header.frame_id = string("/map");
|
||||
pub2.publish(m);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "multi_map_visualization");
|
||||
ros::NodeHandle n("~");
|
||||
|
||||
ros::Subscriber sub1 = n.subscribe("dmaps2d", 1, maps2d_callback);
|
||||
ros::Subscriber sub2 = n.subscribe("dmaps3d", 1, maps3d_callback);
|
||||
pub1 = n.advertise<multi_map_server::MultiOccupancyGrid>("maps2d", 1, true);
|
||||
pub2 = n.advertise<sensor_msgs::PointCloud>("map3d", 1, true);
|
||||
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue