├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── decomp_ros_msgs ├── CMakeLists.txt ├── msg │ ├── Ellipsoid.msg │ ├── EllipsoidArray.msg │ ├── Polyhedron.msg │ └── PolyhedronArray.msg └── package.xml ├── decomp_ros_utils ├── CMakeLists.txt ├── icons │ └── classes │ │ ├── EllipsoidArray.png │ │ └── PolyhedronArray.png ├── include │ └── decomp_ros_utils │ │ └── data_ros_utils.h ├── package.xml ├── plugin_description.xml └── src │ ├── bound_visual.cpp │ ├── bound_visual.h │ ├── ellipsoid_array_display.cpp │ ├── ellipsoid_array_display.h │ ├── ellipsoid_array_visual.cpp │ ├── ellipsoid_array_visual.h │ ├── mesh_visual.cpp │ ├── mesh_visual.h │ ├── polyhedron_array_display.cpp │ ├── polyhedron_array_display.h │ ├── vector_visual.cpp │ └── vector_visual.h ├── decomp_test_node ├── CMakeLists.txt ├── data │ ├── example1.png │ ├── example2.png │ ├── iss_obs.bag │ ├── path2d.txt │ ├── path3d.txt │ ├── seeds.txt │ └── simple.bag ├── launch │ ├── rviz.launch │ ├── test.rviz │ ├── test_path_decomp_2d.launch │ ├── test_path_decomp_3d.launch │ └── test_seed_decomp.launch ├── package.xml └── src │ ├── bag_reader.hpp │ ├── test_path_decomp_2d.cpp │ ├── test_path_decomp_3d.cpp │ ├── test_seed_decomp.cpp │ └── txt_reader.hpp └── wercker.yml /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "DecompUtil"] 2 | path = DecompUtil 3 | url = https://github.com/sikang/DecompUtil.git 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, sikang 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MRSL Decomp Util ROS 2 | [![wercker status](https://app.wercker.com/status/d7ea0616ed5e23113099e84ec4289f6b/s/master "wercker status")](https://app.wercker.com/project/byKey/d7ea0616ed5e23113099e84ec4289f6b) 3 | - - - 4 | A ROS wrapper for implementing [`DecompUtil`](https://github.com/sikang/DecompUtil.git), stacks include: 5 | - `DecompUtil`: convex decomposition of free space in a cluttered environment 6 | - `decomp_ros_msgs`: ROS msgs used for storing, visualizing and communicating 7 | - `decomp_ros_utils`: ROS utils for interfacing with `DecompUtil` 8 | - `decomp_test_node`: contains two example codes for testing 9 | 10 | ## Compilation 11 | #### Prerequisite: 12 | - `ROS`(Indigo+) 13 | - [`catkin_simple`](https://github.com/catkin/catkin_simple) 14 | - `QT`(4+) 15 | 16 | If the submodule `DecompUtil` is not initialized yet, run following commands at first: 17 | ```bash 18 | $ cd /PATH/TO/DecompROS 19 | $ git submodule update --init 20 | ``` 21 | 22 | #### 1) Using Catkin: 23 | ```bash 24 | $ mv decomp_ros ~/catkin_ws/src 25 | $ cd ~/catkin_ws & catkin_make_isolated -DCMAKE_BUILD_TYPE=Release 26 | ``` 27 | 28 | #### 2) Using Catkin Tools (Recommended): 29 | ```bash 30 | $ mv decomp_ros ~/catkin_ws/src 31 | $ catkin config -DCMAKE_BUILD_TYPE=Release 32 | $ cd ~/catkin_ws & catkin build 33 | ``` 34 | 35 | ## Usage 36 | Simple test using the built-in data can be applied through following commands: 37 | ```bash 38 | $ roscd test_node/launch 39 | $ roslaunch rviz.launch 40 | $ roslaunch test_path_decomp.launch 41 | ``` 42 | 43 | ## Examples 44 | The following images show results from `EllipsoidDecomp2D` and `SeedDecomp3D`: 45 | 46 | -------------------------------------------------------------------------------- /decomp_ros_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(decomp_ros_msgs) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple() 7 | 8 | cs_install() 9 | 10 | cs_export() 11 | -------------------------------------------------------------------------------- /decomp_ros_msgs/msg/Ellipsoid.msg: -------------------------------------------------------------------------------- 1 | float64[3] d 2 | float64[9] E 3 | -------------------------------------------------------------------------------- /decomp_ros_msgs/msg/EllipsoidArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | Ellipsoid[] ellipsoids 3 | -------------------------------------------------------------------------------- /decomp_ros_msgs/msg/Polyhedron.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point[] points 2 | geometry_msgs/Point[] normals #norm is an outer vector 3 | -------------------------------------------------------------------------------- /decomp_ros_msgs/msg/PolyhedronArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | Polyhedron[] polyhedrons 3 | -------------------------------------------------------------------------------- /decomp_ros_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | decomp_ros_msgs 4 | 0.0.0 5 | The decomp_ros_msgs package 6 | 7 | 8 | 9 | 10 | sikang 11 | 12 | TODO 13 | 14 | catkin 15 | catkin_simple 16 | geometry_msgs 17 | message_generation 18 | message_runtime 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /decomp_ros_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(decomp_ros_utils) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wno-deprecated-declarations") 5 | find_package(catkin REQUIRED COMPONENTS rviz roscpp) 6 | find_package(catkin_simple REQUIRED) 7 | find_package(cmake_modules) 8 | find_package(Eigen3 REQUIRED) 9 | find_package(decomp_util REQUIRED) 10 | include_directories(${EIGEN3_INCLUDE_DIRS} ${DECOMP_UTIL_INCLUDE_DIRS}) 11 | 12 | add_definitions(-DQT_NO_KEYWORDS) 13 | 14 | ## This setting causes Qt's "MOC" generation to happen automatically. 15 | ## this does not moc things in include!!!!!!! only in src 16 | set(CMAKE_AUTOMOC ON) 17 | 18 | ## We'll use the version that rviz used so they are compatible. 19 | if(rviz_QT_VERSION VERSION_LESS "5") 20 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 21 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 22 | include(${QT_USE_FILE}) 23 | else() 24 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 25 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 26 | set(QT_LIBRARIES Qt5::Widgets) 27 | endif() 28 | 29 | catkin_simple() 30 | 31 | set(SOURCE_FILES 32 | src/bound_visual.cpp 33 | src/mesh_visual.cpp 34 | src/vector_visual.cpp 35 | src/ellipsoid_array_visual.cpp 36 | src/ellipsoid_array_display.cpp 37 | src/polyhedron_array_display.cpp 38 | ${MOC_FILES}) 39 | 40 | cs_add_library(decomp_rviz_plugins ${SOURCE_FILES}) 41 | target_link_libraries(decomp_rviz_plugins ${QT_LIBRARIES} ${catkin_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES}) 42 | 43 | cs_install() 44 | 45 | #cs_export() 46 | cs_export(DEPENDS decomp_util) 47 | -------------------------------------------------------------------------------- /decomp_ros_utils/icons/classes/EllipsoidArray.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sikang/DecompROS/8bf10f41257646f6f0c2a9950437c99855838c8c/decomp_ros_utils/icons/classes/EllipsoidArray.png -------------------------------------------------------------------------------- /decomp_ros_utils/icons/classes/PolyhedronArray.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sikang/DecompROS/8bf10f41257646f6f0c2a9950437c99855838c8c/decomp_ros_utils/icons/classes/PolyhedronArray.png -------------------------------------------------------------------------------- /decomp_ros_utils/include/decomp_ros_utils/data_ros_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef DECOMP_ROS_UTILS_H 2 | #define DECOMP_ROS_UTILS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace DecompROS { 12 | 13 | template nav_msgs::Path vec_to_path(const vec_Vecf &vs) { 14 | nav_msgs::Path path; 15 | for (const auto& it : vs) { 16 | geometry_msgs::PoseStamped pose; 17 | pose.pose.position.x = it(0); 18 | pose.pose.position.y = it(1); 19 | pose.pose.position.z = Dim == 2 ? 0 : it(2); 20 | pose.pose.orientation.w = 1.0; 21 | pose.pose.orientation.x = 0.0; 22 | pose.pose.orientation.y = 0.0; 23 | pose.pose.orientation.z = 0.0; 24 | 25 | path.poses.push_back(pose); 26 | } 27 | 28 | return path; 29 | } 30 | 31 | inline sensor_msgs::PointCloud vec_to_cloud(const vec_Vec3f &pts) { 32 | sensor_msgs::PointCloud cloud; 33 | cloud.points.resize(pts.size()); 34 | 35 | for (unsigned int i = 0; i < pts.size(); i++) { 36 | cloud.points[i].x = pts[i](0); 37 | cloud.points[i].y = pts[i](1); 38 | cloud.points[i].z = pts[i](2); 39 | } 40 | return cloud; 41 | } 42 | 43 | inline vec_Vec3f cloud_to_vec(const sensor_msgs::PointCloud &cloud) { 44 | vec_Vec3f pts; 45 | pts.resize(cloud.points.size()); 46 | for (unsigned int i = 0; i < cloud.points.size(); i++) { 47 | pts[i](0) = cloud.points[i].x; 48 | pts[i](1) = cloud.points[i].y; 49 | pts[i](2) = cloud.points[i].z; 50 | } 51 | 52 | return pts; 53 | } 54 | 55 | inline Polyhedron3D ros_to_polyhedron(const decomp_ros_msgs::Polyhedron& msg){ 56 | Polyhedron3D poly; 57 | for(unsigned int i = 0; i < msg.points.size(); i++){ 58 | Vec3f pt(msg.points[i].x, 59 | msg.points[i].y, 60 | msg.points[i].z); 61 | Vec3f n(msg.normals[i].x, 62 | msg.normals[i].y, 63 | msg.normals[i].z); 64 | poly.add(Hyperplane3D(pt, n)); 65 | } 66 | return poly; 67 | } 68 | 69 | inline vec_E ros_to_polyhedron_array(const decomp_ros_msgs::PolyhedronArray& msg) { 70 | vec_E polys(msg.polyhedrons.size()); 71 | 72 | for(size_t i = 0; i < msg.polyhedrons.size(); i++) 73 | polys[i] = ros_to_polyhedron(msg.polyhedrons[i]); 74 | 75 | return polys; 76 | } 77 | 78 | inline decomp_ros_msgs::Polyhedron polyhedron_to_ros(const Polyhedron2D& poly){ 79 | decomp_ros_msgs::Polyhedron msg; 80 | for (const auto &p : poly.hyperplanes()) { 81 | geometry_msgs::Point pt, n; 82 | pt.x = p.p_(0); 83 | pt.y = p.p_(1); 84 | pt.z = 0; 85 | n.x = p.n_(0); 86 | n.y = p.n_(1); 87 | n.z = 0; 88 | msg.points.push_back(pt); 89 | msg.normals.push_back(n); 90 | } 91 | 92 | geometry_msgs::Point pt1, n1; 93 | pt1.x = 0, pt1.y = 0, pt1.z = 0.01; 94 | n1.x = 0, n1.y = 0, n1.z = 1; 95 | msg.points.push_back(pt1); 96 | msg.normals.push_back(n1); 97 | 98 | geometry_msgs::Point pt2, n2; 99 | pt2.x = 0, pt2.y = 0, pt2.z = -0.01; 100 | n2.x = 0, n2.y = 0, n2.z = -1; 101 | msg.points.push_back(pt2); 102 | msg.normals.push_back(n2); 103 | 104 | return msg; 105 | } 106 | 107 | inline decomp_ros_msgs::Polyhedron polyhedron_to_ros(const Polyhedron3D& poly){ 108 | decomp_ros_msgs::Polyhedron msg; 109 | for (const auto &p : poly.hyperplanes()) { 110 | geometry_msgs::Point pt, n; 111 | pt.x = p.p_(0); 112 | pt.y = p.p_(1); 113 | pt.z = p.p_(2); 114 | n.x = p.n_(0); 115 | n.y = p.n_(1); 116 | n.z = p.n_(2); 117 | msg.points.push_back(pt); 118 | msg.normals.push_back(n); 119 | } 120 | 121 | return msg; 122 | } 123 | 124 | 125 | template 126 | decomp_ros_msgs::PolyhedronArray polyhedron_array_to_ros(const vec_E>& vs){ 127 | decomp_ros_msgs::PolyhedronArray msg; 128 | for (const auto &v : vs) 129 | msg.polyhedrons.push_back(polyhedron_to_ros(v)); 130 | return msg; 131 | } 132 | 133 | template 134 | decomp_ros_msgs::EllipsoidArray ellipsoid_array_to_ros(const vec_E>& Es) { 135 | decomp_ros_msgs::EllipsoidArray ellipsoids; 136 | for (unsigned int i = 0; i < Es.size(); i++) { 137 | decomp_ros_msgs::Ellipsoid ellipsoid; 138 | auto d = Es[i].d(); 139 | ellipsoid.d[0] = d(0); 140 | ellipsoid.d[1] = d(1); 141 | ellipsoid.d[2] = Dim == 2 ? 0:d(2); 142 | 143 | auto C = Es[i].C(); 144 | for (int x = 0; x < 3; x++) { 145 | for (int y = 0; y < 3; y++) { 146 | if(x < Dim && y < Dim) 147 | ellipsoid.E[3 * x + y] = C(x, y); 148 | else 149 | ellipsoid.E[3 * x + y] = 0; 150 | } 151 | } 152 | ellipsoids.ellipsoids.push_back(ellipsoid); 153 | } 154 | 155 | return ellipsoids; 156 | } 157 | 158 | } 159 | 160 | #endif 161 | -------------------------------------------------------------------------------- /decomp_ros_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | decomp_ros_utils 4 | 0.0.0 5 | The decomp_ros_utils package 6 | 7 | sikang 8 | 9 | TODO 10 | 11 | catkin 12 | catkin_simple 13 | roscpp 14 | rviz 15 | decomp_util 16 | decomp_ros_msgs 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /decomp_ros_utils/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | visualize decomp_ros_msgs/EllipsoidArray msg. 7 | 8 | 9 | 10 | 13 | 14 | visualize decomp_ros_msgs/PolyhedronArray msg. 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/bound_visual.cpp: -------------------------------------------------------------------------------- 1 | #include "bound_visual.h" 2 | 3 | namespace decomp_rviz_plugins { 4 | BoundVisual::BoundVisual(Ogre::SceneManager *scene_manager, 5 | Ogre::SceneNode *parent_node) { 6 | scene_manager_ = scene_manager; 7 | frame_node_ = parent_node->createChildSceneNode(); 8 | } 9 | 10 | BoundVisual::~BoundVisual() { scene_manager_->destroySceneNode(frame_node_); } 11 | 12 | void BoundVisual::setMessage(const vec_E& bds) { 13 | objs_.clear(); 14 | 15 | if (bds.empty()) 16 | return; 17 | 18 | size_t num_faces = bds.size(); 19 | objs_.resize(num_faces); 20 | for (auto &it : objs_) 21 | it.reset(new rviz::BillboardLine(scene_manager_, frame_node_)); 22 | 23 | int cnt = 0; 24 | for (const auto &vs : bds) { 25 | for (unsigned int i = 0; i <= vs.size(); i++) { 26 | if (i < vs.size()) { 27 | if(!std::isnan(vs[i](0))) 28 | objs_[cnt]->addPoint(Ogre::Vector3(vs[i](0), vs[i](1), vs[i](2))); 29 | } 30 | else { 31 | if(!std::isnan(vs[0](0))) 32 | objs_[cnt]->addPoint(Ogre::Vector3(vs[0](0), vs[0](1), vs[0](2))); 33 | } 34 | } 35 | cnt++; 36 | } 37 | } 38 | 39 | void BoundVisual::setFramePosition(const Ogre::Vector3 &position) { 40 | frame_node_->setPosition(position); 41 | } 42 | 43 | void BoundVisual::setFrameOrientation(const Ogre::Quaternion &orientation) { 44 | frame_node_->setOrientation(orientation); 45 | } 46 | 47 | void BoundVisual::setColor(float r, float g, float b, float a) { 48 | for (auto &it : objs_) 49 | it->setColor(r, g, b, a); 50 | } 51 | 52 | void BoundVisual::setScale(float s) { 53 | for (auto &it : objs_) 54 | it->setLineWidth(s); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/bound_visual.h: -------------------------------------------------------------------------------- 1 | #ifndef BOUND_VISUAL_H 2 | #define BOUND_VISUAL_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | 13 | namespace decomp_rviz_plugins { 14 | class BoundVisual { 15 | public: 16 | BoundVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node); 17 | ~BoundVisual(); 18 | 19 | void setMessage(const vec_E &bds); 20 | void setFramePosition(const Ogre::Vector3 &position); 21 | void setFrameOrientation(const Ogre::Quaternion &orientation); 22 | 23 | void setColor(float r, float g, float b, float a); 24 | void setScale(float s); 25 | 26 | private: 27 | std::vector> objs_; 28 | 29 | Ogre::SceneNode *frame_node_; 30 | 31 | Ogre::SceneManager *scene_manager_; 32 | }; 33 | } 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/ellipsoid_array_display.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ellipsoid_array_display.h" 3 | 4 | namespace decomp_rviz_plugins { 5 | 6 | EllipsoidArrayDisplay::EllipsoidArrayDisplay() { 7 | color_property_ = new rviz::ColorProperty("Color", QColor(204, 51, 204), 8 | "Color of ellipsoids.", 9 | this, SLOT(updateColorAndAlpha())); 10 | alpha_property_ = new rviz::FloatProperty( 11 | "Alpha", 0.5, "0 is fully transparent, 1.0 is fully opaque.", this, 12 | SLOT(updateColorAndAlpha())); 13 | 14 | } 15 | 16 | void EllipsoidArrayDisplay::onInitialize() { 17 | MFDClass::onInitialize(); 18 | } 19 | 20 | EllipsoidArrayDisplay::~EllipsoidArrayDisplay() {} 21 | 22 | void EllipsoidArrayDisplay::reset() { 23 | MFDClass::reset(); 24 | visual_ = nullptr; 25 | } 26 | 27 | void EllipsoidArrayDisplay::updateColorAndAlpha() { 28 | float alpha = alpha_property_->getFloat(); 29 | Ogre::ColourValue color = color_property_->getOgreColor(); 30 | 31 | if (visual_) 32 | visual_->setColor(color.r, color.g, color.b, alpha); 33 | } 34 | 35 | void EllipsoidArrayDisplay::processMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg) { 36 | Ogre::Quaternion orientation; 37 | Ogre::Vector3 position; 38 | if (!context_->getFrameManager()->getTransform( 39 | msg->header.frame_id, msg->header.stamp, position, orientation)) { 40 | ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", 41 | msg->header.frame_id.c_str(), qPrintable(fixed_frame_)); 42 | return; 43 | } 44 | 45 | std::shared_ptr visual; 46 | visual.reset(new EllipsoidArrayVisual(context_->getSceneManager(), scene_node_)); 47 | 48 | visual->setMessage(msg); 49 | visual->setFramePosition(position); 50 | visual->setFrameOrientation(orientation); 51 | 52 | float alpha = alpha_property_->getFloat(); 53 | Ogre::ColourValue color = color_property_->getOgreColor(); 54 | visual->setColor(color.r, color.g, color.b, alpha); 55 | 56 | visual_ = visual; 57 | } 58 | 59 | } 60 | 61 | #include 62 | PLUGINLIB_EXPORT_CLASS(decomp_rviz_plugins::EllipsoidArrayDisplay, rviz::Display) 63 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/ellipsoid_array_display.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include "ellipsoid_array_visual.h" 13 | 14 | namespace decomp_rviz_plugins { 15 | 16 | class EllipsoidArrayVisual; 17 | 18 | class EllipsoidArrayDisplay 19 | : public rviz::MessageFilterDisplay { 20 | Q_OBJECT 21 | public: 22 | EllipsoidArrayDisplay(); 23 | ~EllipsoidArrayDisplay(); 24 | 25 | protected: 26 | void onInitialize(); 27 | 28 | void reset(); 29 | 30 | private Q_SLOTS: 31 | void updateColorAndAlpha(); 32 | 33 | private: 34 | void processMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg); 35 | 36 | std::shared_ptr visual_; 37 | 38 | rviz::ColorProperty *color_property_; 39 | rviz::FloatProperty *alpha_property_; 40 | }; 41 | } 42 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/ellipsoid_array_visual.cpp: -------------------------------------------------------------------------------- 1 | #include "ellipsoid_array_visual.h" 2 | 3 | namespace decomp_rviz_plugins { 4 | EllipsoidArrayVisual::EllipsoidArrayVisual(Ogre::SceneManager *scene_manager, 5 | Ogre::SceneNode *parent_node) { 6 | scene_manager_ = scene_manager; 7 | frame_node_ = parent_node->createChildSceneNode(); 8 | } 9 | 10 | EllipsoidArrayVisual::~EllipsoidArrayVisual() { 11 | scene_manager_->destroySceneNode(frame_node_); 12 | } 13 | 14 | void EllipsoidArrayVisual::setMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg) { 15 | objs_.clear(); 16 | 17 | if (msg->ellipsoids.empty()) 18 | return; 19 | 20 | for (const auto& it: msg->ellipsoids) { 21 | if(std::isnan(it.d[0]) || 22 | std::isnan(it.d[1]) || 23 | std::isnan(it.d[2])) 24 | return; 25 | for (int i = 0; i < 3; i++) 26 | for (int j = 0; j < 3; j++) 27 | if(std::isnan(it.E[3 * i + j])) 28 | return; 29 | } 30 | 31 | objs_.resize(msg->ellipsoids.size()); 32 | 33 | for (auto &it : objs_) 34 | it.reset(new rviz::Shape(rviz::Shape::Type::Sphere, scene_manager_, 35 | frame_node_)); 36 | 37 | int cnt = 0; 38 | for (const auto &it : msg->ellipsoids) { 39 | Mat3f E; 40 | for (int i = 0; i < 3; i++) 41 | for (int j = 0; j < 3; j++) 42 | E(i, j) = it.E[3 * i + j]; 43 | Eigen::SelfAdjointEigenSolver es(E); 44 | 45 | Ogre::Vector3 scale(2 * es.eigenvalues()[0], 2 * es.eigenvalues()[1], 46 | 2 * es.eigenvalues()[2]); 47 | objs_[cnt]->setScale(scale); 48 | 49 | Ogre::Vector3 d(it.d[0], it.d[1], it.d[2]); 50 | objs_[cnt]->setPosition(d); 51 | 52 | Quatf q(es.eigenvectors().determinant() * es.eigenvectors()); 53 | Ogre::Quaternion o(q.w(), q.x(), q.y(), q.z()); 54 | objs_[cnt]->setOrientation(o); 55 | cnt++; 56 | } 57 | } 58 | 59 | void EllipsoidArrayVisual::setFramePosition(const Ogre::Vector3 &position) { 60 | frame_node_->setPosition(position); 61 | } 62 | 63 | void EllipsoidArrayVisual::setFrameOrientation( 64 | const Ogre::Quaternion &orientation) { 65 | frame_node_->setOrientation(orientation); 66 | } 67 | 68 | void EllipsoidArrayVisual::setColor(float r, float g, float b, float a) { 69 | for (auto &it : objs_) 70 | it->setColor(r, g, b, a); 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/ellipsoid_array_visual.h: -------------------------------------------------------------------------------- 1 | #ifndef ELLIPSOIDS_VISUAL_H 2 | #define ELLIPSOIDS_VISUAL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace decomp_rviz_plugins { 15 | class EllipsoidArrayVisual { 16 | public: 17 | EllipsoidArrayVisual(Ogre::SceneManager *scene_manager, 18 | Ogre::SceneNode *parent_node); 19 | 20 | virtual ~EllipsoidArrayVisual(); 21 | 22 | void setMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg); 23 | 24 | void setFramePosition(const Ogre::Vector3 &position); 25 | void setFrameOrientation(const Ogre::Quaternion &orientation); 26 | 27 | void setColor(float r, float g, float b, float a); 28 | 29 | private: 30 | std::vector> objs_; 31 | 32 | Ogre::SceneNode *frame_node_; 33 | 34 | Ogre::SceneManager *scene_manager_; 35 | }; 36 | } 37 | #endif 38 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/mesh_visual.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_visual.h" 2 | 3 | namespace decomp_rviz_plugins { 4 | MeshVisual::MeshVisual(Ogre::SceneManager *scene_manager, 5 | Ogre::SceneNode *parent_node) { 6 | scene_manager_ = scene_manager; 7 | frame_node_ = parent_node->createChildSceneNode(); 8 | obj_.reset(new rviz::MeshShape(scene_manager_, frame_node_)); 9 | } 10 | 11 | MeshVisual::~MeshVisual() { scene_manager_->destroySceneNode(frame_node_); } 12 | 13 | void MeshVisual::setMessage(const vec_E &bds) { 14 | obj_->clear(); 15 | 16 | if (bds.empty()) 17 | return; 18 | 19 | obj_->beginTriangles(); 20 | int free_cnt = 0; 21 | for (const auto &vs: bds) { 22 | if (vs.size() > 2) { 23 | Vec3f p0 = vs[0]; 24 | Vec3f p1 = vs[1]; 25 | Vec3f p2 = vs[2]; 26 | Vec3f n = (p2-p0).cross(p1-p0); 27 | n = n.normalized(); 28 | if(std::isnan(n(0))) 29 | n = Vec3f(0, 0, -1); 30 | 31 | int ref_cnt = free_cnt; 32 | Ogre::Vector3 normal(n(0), n(1), n(2)); 33 | for (unsigned int i = 0; i < vs.size(); i++) { 34 | obj_->addVertex(Ogre::Vector3(vs[i](0), vs[i](1), vs[i](2)), normal); 35 | if (i > 1 && i < vs.size()) 36 | obj_->addTriangle(ref_cnt, free_cnt - 1, free_cnt); 37 | free_cnt++; 38 | } 39 | } 40 | } 41 | obj_->endTriangles(); 42 | } 43 | 44 | // Position and orientation are passed through to the SceneNode. 45 | void MeshVisual::setFramePosition(const Ogre::Vector3 &position) { 46 | frame_node_->setPosition(position); 47 | } 48 | 49 | void MeshVisual::setFrameOrientation(const Ogre::Quaternion &orientation) { 50 | frame_node_->setOrientation(orientation); 51 | } 52 | 53 | void MeshVisual::setColor(float r, float g, float b, float a) { 54 | obj_->setColor(r, g, b, a); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/mesh_visual.h: -------------------------------------------------------------------------------- 1 | #ifndef MESH_VISUAL_H 2 | #define MESH_VISUAL_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace decomp_rviz_plugins { 14 | class MeshVisual { 15 | public: 16 | MeshVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node); 17 | 18 | virtual ~MeshVisual(); 19 | 20 | void setMessage(const vec_E &bds); 21 | void setFramePosition(const Ogre::Vector3 &position); 22 | void setFrameOrientation(const Ogre::Quaternion &orientation); 23 | 24 | void setColor(float r, float g, float b, float a); 25 | 26 | private: 27 | std::unique_ptr obj_; 28 | 29 | Ogre::SceneNode *frame_node_; 30 | 31 | Ogre::SceneManager *scene_manager_; 32 | }; 33 | } 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/polyhedron_array_display.cpp: -------------------------------------------------------------------------------- 1 | #include "polyhedron_array_display.h" 2 | 3 | namespace decomp_rviz_plugins { 4 | 5 | PolyhedronArrayDisplay::PolyhedronArrayDisplay() { 6 | mesh_color_property_ = 7 | new rviz::ColorProperty("MeshColor", QColor(0, 170, 255), "Mesh color.", 8 | this, SLOT(updateMeshColorAndAlpha())); 9 | bound_color_property_ = 10 | new rviz::ColorProperty("BoundColor", QColor(255, 0, 0), "Bound color.", 11 | this, SLOT(updateBoundColorAndAlpha())); 12 | 13 | alpha_property_ = new rviz::FloatProperty( 14 | "Alpha", 0.2, 15 | "0 is fully transparent, 1.0 is fully opaque, only affect mesh", this, 16 | SLOT(updateMeshColorAndAlpha())); 17 | 18 | scale_property_ = new rviz::FloatProperty("Scale", 0.1, "bound scale.", this, 19 | SLOT(updateScale())); 20 | 21 | vs_scale_property_ = new rviz::FloatProperty("VsScale", 1.0, "Vs scale.", this, 22 | SLOT(updateVsScale())); 23 | 24 | vs_color_property_ = 25 | new rviz::ColorProperty("VsColor", QColor(0, 255, 0), "Vs color.", 26 | this, SLOT(updateVsColorAndAlpha())); 27 | 28 | 29 | 30 | state_property_ = new rviz::EnumProperty( 31 | "State", "Mesh", "A Polygon can be represented as two states: Mesh and " 32 | "Bound, this option allows selecting visualizing Polygon" 33 | "in corresponding state", 34 | this, SLOT(updateState())); 35 | state_property_->addOption("Mesh", 0); 36 | state_property_->addOption("Bound", 1); 37 | state_property_->addOption("Both", 2); 38 | state_property_->addOption("Vs", 3); 39 | 40 | } 41 | 42 | void PolyhedronArrayDisplay::onInitialize() { MFDClass::onInitialize(); } 43 | 44 | PolyhedronArrayDisplay::~PolyhedronArrayDisplay() {} 45 | 46 | void PolyhedronArrayDisplay::reset() { 47 | MFDClass::reset(); 48 | visual_mesh_ = nullptr; 49 | visual_bound_ = nullptr; 50 | visual_vector_ = nullptr; 51 | } 52 | 53 | void PolyhedronArrayDisplay::processMessage(const decomp_ros_msgs::PolyhedronArray::ConstPtr &msg) { 54 | if (!context_->getFrameManager()->getTransform( 55 | msg->header.frame_id, msg->header.stamp, position_, orientation_)) { 56 | ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", 57 | msg->header.frame_id.c_str(), qPrintable(fixed_frame_)); 58 | return; 59 | } 60 | 61 | vertices_.clear(); 62 | vs_.clear(); 63 | 64 | const auto polys = DecompROS::ros_to_polyhedron_array(*msg); 65 | 66 | for(const auto& polyhedron: polys){ 67 | vec_E bds = cal_vertices(polyhedron); 68 | vertices_.insert(vertices_.end(), bds.begin(), bds.end()); 69 | const auto vs = polyhedron.cal_normals(); 70 | vs_.insert(vs_.end(), vs.begin(), vs.end()); 71 | } 72 | 73 | int state = state_property_->getOptionInt(); 74 | visualizeMessage(state); 75 | } 76 | 77 | void PolyhedronArrayDisplay::visualizeMesh() { 78 | std::shared_ptr visual_mesh; 79 | visual_mesh.reset(new MeshVisual(context_->getSceneManager(), scene_node_)); 80 | 81 | visual_mesh->setMessage(vertices_); 82 | visual_mesh->setFramePosition(position_); 83 | visual_mesh->setFrameOrientation(orientation_); 84 | 85 | float alpha = alpha_property_->getFloat(); 86 | Ogre::ColourValue color = mesh_color_property_->getOgreColor(); 87 | visual_mesh->setColor(color.r, color.g, color.b, alpha); 88 | visual_mesh_ = visual_mesh; 89 | } 90 | 91 | void PolyhedronArrayDisplay::visualizeBound() { 92 | std::shared_ptr visual_bound; 93 | visual_bound.reset(new BoundVisual(context_->getSceneManager(), scene_node_)); 94 | 95 | visual_bound->setMessage(vertices_); 96 | visual_bound->setFramePosition(position_); 97 | visual_bound->setFrameOrientation(orientation_); 98 | 99 | Ogre::ColourValue color = bound_color_property_->getOgreColor(); 100 | visual_bound->setColor(color.r, color.g, color.b, 1.0); 101 | float scale = scale_property_->getFloat(); 102 | visual_bound->setScale(scale); 103 | 104 | visual_bound_ = visual_bound; 105 | } 106 | 107 | void PolyhedronArrayDisplay::visualizeVs() { 108 | std::shared_ptr visual_vector; 109 | visual_vector.reset(new VectorVisual(context_->getSceneManager(), scene_node_)); 110 | 111 | visual_vector->setMessage(vs_); 112 | visual_vector->setFramePosition(position_); 113 | visual_vector->setFrameOrientation(orientation_); 114 | 115 | Ogre::ColourValue color = vs_color_property_->getOgreColor(); 116 | visual_vector->setColor(color.r, color.g, color.b, 1.0); 117 | 118 | visual_vector_ = visual_vector; 119 | } 120 | 121 | void PolyhedronArrayDisplay::visualizeMessage(int state) { 122 | switch (state) { 123 | case 0: 124 | visual_bound_ = nullptr; 125 | visual_vector_ = nullptr; 126 | visualizeMesh(); 127 | break; 128 | case 1: 129 | visual_mesh_ = nullptr; 130 | visual_vector_ = nullptr; 131 | visualizeBound(); 132 | break; 133 | case 2: 134 | visual_vector_ = nullptr; 135 | visualizeMesh(); 136 | visualizeBound(); 137 | break; 138 | case 3: 139 | visualizeVs(); 140 | break; 141 | default: 142 | std::cout << "Invalid State: " << state << std::endl; 143 | } 144 | } 145 | 146 | void PolyhedronArrayDisplay::updateMeshColorAndAlpha() { 147 | float alpha = alpha_property_->getFloat(); 148 | Ogre::ColourValue color = mesh_color_property_->getOgreColor(); 149 | 150 | if(visual_mesh_) 151 | visual_mesh_->setColor(color.r, color.g, color.b, alpha); 152 | } 153 | 154 | void PolyhedronArrayDisplay::updateBoundColorAndAlpha() { 155 | Ogre::ColourValue color = bound_color_property_->getOgreColor(); 156 | if(visual_bound_) 157 | visual_bound_->setColor(color.r, color.g, color.b, 1.0); 158 | } 159 | 160 | 161 | void PolyhedronArrayDisplay::updateState() { 162 | int state = state_property_->getOptionInt(); 163 | visualizeMessage(state); 164 | } 165 | 166 | void PolyhedronArrayDisplay::updateScale() { 167 | float s = scale_property_->getFloat(); 168 | if(visual_bound_) 169 | visual_bound_->setScale(s); 170 | } 171 | 172 | void PolyhedronArrayDisplay::updateVsScale() { 173 | float s = vs_scale_property_->getFloat(); 174 | if(visual_vector_) 175 | visual_vector_->setScale(s); 176 | } 177 | 178 | void PolyhedronArrayDisplay::updateVsColorAndAlpha() { 179 | Ogre::ColourValue color = vs_color_property_->getOgreColor(); 180 | if(visual_vector_) 181 | visual_vector_->setColor(color.r, color.g, color.b, 1); 182 | } 183 | } 184 | 185 | #include 186 | PLUGINLIB_EXPORT_CLASS(decomp_rviz_plugins::PolyhedronArrayDisplay, rviz::Display) 187 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/polyhedron_array_display.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include "mesh_visual.h" 16 | #include "bound_visual.h" 17 | #include "vector_visual.h" 18 | #include 19 | #include 20 | 21 | namespace decomp_rviz_plugins { 22 | class PolyhedronArrayDisplay 23 | : public rviz::MessageFilterDisplay { 24 | Q_OBJECT 25 | public: 26 | PolyhedronArrayDisplay(); 27 | virtual ~PolyhedronArrayDisplay(); 28 | 29 | protected: 30 | virtual void onInitialize(); 31 | 32 | virtual void reset(); 33 | 34 | private Q_SLOTS: 35 | void updateMeshColorAndAlpha(); 36 | void updateBoundColorAndAlpha(); 37 | void updateVsColorAndAlpha(); 38 | void updateState(); 39 | void updateScale(); 40 | void updateVsScale(); 41 | 42 | private: 43 | void processMessage(const decomp_ros_msgs::PolyhedronArray::ConstPtr &msg); 44 | void visualizeMessage(int state); 45 | void visualizeMesh(); 46 | void visualizeBound(); 47 | void visualizeVs(); 48 | 49 | std::shared_ptr visual_mesh_; 50 | std::shared_ptr visual_bound_; 51 | std::shared_ptr visual_vector_; 52 | 53 | rviz::ColorProperty *mesh_color_property_; 54 | rviz::ColorProperty *bound_color_property_; 55 | rviz::ColorProperty *vs_color_property_; 56 | rviz::FloatProperty *alpha_property_; 57 | rviz::FloatProperty *scale_property_; 58 | rviz::FloatProperty *vs_scale_property_; 59 | rviz::EnumProperty *state_property_; 60 | 61 | Ogre::Vector3 position_; 62 | Ogre::Quaternion orientation_; 63 | 64 | vec_E vertices_; 65 | vec_E> vs_; 66 | }; 67 | 68 | } 69 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/vector_visual.cpp: -------------------------------------------------------------------------------- 1 | #include "vector_visual.h" 2 | 3 | namespace decomp_rviz_plugins { 4 | VectorVisual::VectorVisual(Ogre::SceneManager *scene_manager, 5 | Ogre::SceneNode *parent_node) { 6 | scene_manager_ = scene_manager; 7 | frame_node_ = parent_node->createChildSceneNode(); 8 | } 9 | 10 | VectorVisual::~VectorVisual() { scene_manager_->destroySceneNode(frame_node_); } 11 | 12 | void VectorVisual::setMessage(const vec_E> &vs) { 13 | objs_.clear(); 14 | vs_ = vs; 15 | if (vs.empty()) 16 | return; 17 | objs_.resize(vs.size()); 18 | for (auto &it : objs_) 19 | it.reset(new rviz::Arrow(scene_manager_, frame_node_)); 20 | 21 | int cnt = 0; 22 | for (const auto &v : vs) { 23 | Vec3f n = v.second.normalized(); 24 | objs_[cnt]->setDirection(Ogre::Vector3(n(0), n(1), n(2))); 25 | objs_[cnt]->setPosition(Ogre::Vector3(v.first(0), v.first(1), v.first(2))); 26 | objs_[cnt]->setScale(s_ * Ogre::Vector3(1.0, 1.0, 1.0)); 27 | 28 | float d = s_ * v.second.norm(); 29 | float shaft_length = 0.7 * d; 30 | float head_length = 0.3 * d; 31 | objs_[cnt]->set(shaft_length, d / 8, head_length, d / 5); 32 | cnt ++; 33 | } 34 | } 35 | 36 | void VectorVisual::setFramePosition(const Ogre::Vector3 &position) { 37 | frame_node_->setPosition(position); 38 | } 39 | 40 | void VectorVisual::setFrameOrientation(const Ogre::Quaternion &orientation) { 41 | frame_node_->setOrientation(orientation); 42 | } 43 | 44 | void VectorVisual::setColor(float r, float g, float b, float a) { 45 | for (auto &it : objs_) 46 | it->setColor(r, g, b, a); 47 | } 48 | 49 | void VectorVisual::setScale(float s) { 50 | s_ = s; 51 | for (unsigned int i = 0; i < objs_.size(); i++){ 52 | float d = s_ * vs_[i].second.norm(); 53 | float shaft_length = 0.7 * d; 54 | float head_length = 0.3 * d; 55 | objs_[i]->set(shaft_length, d / 8, head_length, d / 5); 56 | } 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /decomp_ros_utils/src/vector_visual.h: -------------------------------------------------------------------------------- 1 | #ifndef VECTOR_VISUAL_H 2 | #define VECTOR_VISUAL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace decomp_rviz_plugins { 11 | class VectorVisual { 12 | public: 13 | VectorVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node); 14 | ~VectorVisual(); 15 | 16 | void setMessage(const vec_E> &vs); 17 | void setFramePosition(const Ogre::Vector3 &position); 18 | void setFrameOrientation(const Ogre::Quaternion &orientation); 19 | 20 | void setColor(float r, float g, float b, float a); 21 | void setScale(float s); 22 | 23 | private: 24 | std::vector> objs_; 25 | 26 | Ogre::SceneNode *frame_node_; 27 | 28 | Ogre::SceneManager *scene_manager_; 29 | 30 | float s_ = 1.0; 31 | vec_E> vs_; 32 | }; 33 | } 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /decomp_test_node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(decomp_test_node) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wno-deprecated-declarations") 5 | 6 | find_package(catkin_simple REQUIRED) 7 | find_package(cmake_modules REQUIRED) 8 | find_package(Eigen3 REQUIRED) 9 | find_package(decomp_util REQUIRED) 10 | include_directories(${EIGEN3_INCLUDE_DIRS} ${DECOMP_UTIL_INCLUDE_DIRS}) 11 | 12 | catkin_simple() 13 | 14 | cs_add_executable(test_path_decomp_2d src/test_path_decomp_2d.cpp) 15 | 16 | cs_add_executable(test_path_decomp_3d src/test_path_decomp_3d.cpp) 17 | 18 | cs_add_executable(test_seed_decomp src/test_seed_decomp.cpp) 19 | 20 | cs_install() 21 | 22 | cs_export() 23 | 24 | -------------------------------------------------------------------------------- /decomp_test_node/data/example1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sikang/DecompROS/8bf10f41257646f6f0c2a9950437c99855838c8c/decomp_test_node/data/example1.png -------------------------------------------------------------------------------- /decomp_test_node/data/example2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sikang/DecompROS/8bf10f41257646f6f0c2a9950437c99855838c8c/decomp_test_node/data/example2.png -------------------------------------------------------------------------------- /decomp_test_node/data/iss_obs.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sikang/DecompROS/8bf10f41257646f6f0c2a9950437c99855838c8c/decomp_test_node/data/iss_obs.bag -------------------------------------------------------------------------------- /decomp_test_node/data/path2d.txt: -------------------------------------------------------------------------------- 1 | 5 11.5 2 | 13 11.5 3 | 14 10.5 4 | 14 5 5 | -------------------------------------------------------------------------------- /decomp_test_node/data/path3d.txt: -------------------------------------------------------------------------------- 1 | 5 11.5 0.5 2 | 13 11.5 3.0 3 | 14 10.5 1.5 4 | 14 5 2.5 5 | -------------------------------------------------------------------------------- /decomp_test_node/data/seeds.txt: -------------------------------------------------------------------------------- 1 | -0.00481166 0.00636635 -0.1 2 | 4.30424 -0.58639 -0.1 3 | 4.40424 -0.68639 -0.1 4 | 7.20424 -0.98639 -0.2 5 | 7.70424 -1.48639 -0.2 6 | 7.70424 -3.08639 0.2 7 | 8.00424 -9.98639 -0.76756 8 | -------------------------------------------------------------------------------- /decomp_test_node/data/simple.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sikang/DecompROS/8bf10f41257646f6f0c2a9950437c99855838c8c/decomp_test_node/data/simple.bag -------------------------------------------------------------------------------- /decomp_test_node/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /decomp_test_node/launch/test.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 1725 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: PointCloud 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 49 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz/MarkerArray 52 | Enabled: false 53 | Marker Topic: /test_decomp/markers 54 | Name: MarkerArray 55 | Namespaces: 56 | {} 57 | Queue Size: 100 58 | Value: false 59 | - Alpha: 1 60 | Buffer Length: 1 61 | Class: rviz/Path 62 | Color: 25; 255; 0 63 | Enabled: true 64 | Head Diameter: 0.300000012 65 | Head Length: 0.200000003 66 | Length: 0.300000012 67 | Line Style: Billboards 68 | Line Width: 0.100000001 69 | Name: Path 70 | Offset: 71 | X: 0 72 | Y: 0 73 | Z: 0 74 | Pose Color: 255; 85; 255 75 | Pose Style: None 76 | Radius: 0.0299999993 77 | Shaft Diameter: 0.100000001 78 | Shaft Length: 0.100000001 79 | Topic: /test_decomp/path 80 | Unreliable: false 81 | Value: true 82 | - Alpha: 1 83 | Autocompute Intensity Bounds: true 84 | Autocompute Value Bounds: 85 | Max Value: 0.200000003 86 | Min Value: -0.767560005 87 | Value: true 88 | Axis: Z 89 | Channel Name: intensity 90 | Class: rviz/PointCloud 91 | Color: 255; 255; 255 92 | Color Transformer: AxisColor 93 | Decay Time: 0 94 | Enabled: false 95 | Invert Rainbow: false 96 | Max Color: 255; 255; 255 97 | Max Intensity: 4096 98 | Min Color: 0; 0; 0 99 | Min Intensity: 0 100 | Name: Seed 101 | Position Transformer: XYZ 102 | Queue Size: 10 103 | Selectable: true 104 | Size (Pixels): 3 105 | Size (m): 0.100000001 106 | Style: Boxes 107 | Topic: /test_decomp/seed 108 | Unreliable: false 109 | Use Fixed Frame: true 110 | Use rainbow: true 111 | Value: false 112 | - Alpha: 1 113 | Autocompute Intensity Bounds: true 114 | Autocompute Value Bounds: 115 | Max Value: 4 116 | Min Value: 0 117 | Value: true 118 | Axis: Z 119 | Channel Name: intensity 120 | Class: rviz/PointCloud 121 | Color: 255; 255; 255 122 | Color Transformer: AxisColor 123 | Decay Time: 0 124 | Enabled: true 125 | Invert Rainbow: false 126 | Max Color: 255; 255; 255 127 | Max Intensity: 4096 128 | Min Color: 0; 0; 0 129 | Min Intensity: 0 130 | Name: PointCloud 131 | Position Transformer: XYZ 132 | Queue Size: 10 133 | Selectable: true 134 | Size (Pixels): 3 135 | Size (m): 0.0500000007 136 | Style: Flat Squares 137 | Topic: /test_decomp/cloud 138 | Unreliable: false 139 | Use Fixed Frame: true 140 | Use rainbow: true 141 | Value: true 142 | - Alpha: 0.200000003 143 | BoundColor: 255; 0; 0 144 | Class: decomp_rviz_plugins/PolyhedronArray 145 | Enabled: true 146 | MeshColor: 0; 0; 255 147 | Name: PolyhedronArray 148 | Scale: 0.100000001 149 | State: Mesh 150 | Topic: /test_decomp/polyhedron_array 151 | Unreliable: false 152 | Value: true 153 | VsColor: 0; 255; 0 154 | VsScale: 1 155 | - Alpha: 0.300000012 156 | Class: decomp_rviz_plugins/EllipsoidArray 157 | Color: 204; 51; 204 158 | Enabled: true 159 | Name: EllipsoidArray 160 | Topic: /test_decomp/ellipsoid_array 161 | Unreliable: false 162 | Value: true 163 | Enabled: true 164 | Global Options: 165 | Background Color: 255; 255; 255 166 | Default Light: true 167 | Fixed Frame: map 168 | Frame Rate: 30 169 | Name: root 170 | Tools: 171 | - Class: rviz/Interact 172 | Hide Inactive Objects: true 173 | - Class: rviz/MoveCamera 174 | - Class: rviz/Select 175 | - Class: rviz/FocusCamera 176 | - Class: rviz/Measure 177 | - Class: rviz/SetInitialPose 178 | Topic: /initialpose 179 | - Class: rviz/SetGoal 180 | Topic: /move_base_simple/goal 181 | - Class: rviz/PublishPoint 182 | Single click: true 183 | Topic: /clicked_point 184 | Value: true 185 | Views: 186 | Current: 187 | Angle: 0 188 | Class: rviz/TopDownOrtho 189 | Enable Stereo Rendering: 190 | Stereo Eye Separation: 0.0599999987 191 | Stereo Focal Distance: 1 192 | Swap Stereo Eyes: false 193 | Value: false 194 | Invert Z Axis: false 195 | Name: Current View 196 | Near Clip Distance: 0.00999999978 197 | Scale: 100.016861 198 | Target Frame: 199 | Value: TopDownOrtho (rviz) 200 | X: 9.32167625 201 | Y: 8.82361126 202 | Saved: ~ 203 | Window Geometry: 204 | Displays: 205 | collapsed: false 206 | Height: 2112 207 | Hide Left Dock: false 208 | Hide Right Dock: false 209 | QMainWindow State: 000000ff00000000fd0000000400000000000002080000076ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000400000076f000000fa00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000016c0000076ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000400000076f000000e500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000f0000000058fc0100000002fb0000000800540069006d0065010000000000000f000000057800fffffffb0000000800540069006d0065010000000000000450000000000000000000000b740000076f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 210 | Selection: 211 | collapsed: false 212 | Time: 213 | collapsed: false 214 | Tool Properties: 215 | collapsed: false 216 | Views: 217 | collapsed: false 218 | Width: 3840 219 | X: 0 220 | Y: 48 221 | -------------------------------------------------------------------------------- /decomp_test_node/launch/test_path_decomp_2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /decomp_test_node/launch/test_path_decomp_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /decomp_test_node/launch/test_seed_decomp.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /decomp_test_node/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | decomp_test_node 4 | 0.0.1 5 | The test_node package 6 | sikang 7 | TODO 8 | 9 | catkin 10 | catkin_simple 11 | 12 | roscpp 13 | rosbag 14 | decomp_util 15 | decomp_ros_utils 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /decomp_test_node/src/bag_reader.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | template 8 | T read_bag(std::string file_name, std::string topic) { 9 | rosbag::Bag bag; 10 | bag.open(file_name, rosbag::bagmode::Read); 11 | 12 | std::vector topics; 13 | topics.push_back(topic); 14 | 15 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 16 | 17 | bool find = false; 18 | T msg; 19 | BOOST_FOREACH (rosbag::MessageInstance const m, view) { 20 | if (m.instantiate() != NULL) { 21 | msg = *m.instantiate(); 22 | ROS_WARN("Get data!"); 23 | find = true; 24 | break; 25 | } 26 | } 27 | bag.close(); 28 | if (!find) 29 | ROS_WARN("Fail to find '%s' in '%s'", topic.c_str(), file_name.c_str()); 30 | return msg; 31 | } 32 | -------------------------------------------------------------------------------- /decomp_test_node/src/test_path_decomp_2d.cpp: -------------------------------------------------------------------------------- 1 | #include "bag_reader.hpp" 2 | #include "txt_reader.hpp" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | int main(int argc, char ** argv){ 11 | ros::init(argc, argv, "test"); 12 | ros::NodeHandle nh("~"); 13 | 14 | ros::Publisher cloud_pub = nh.advertise("cloud", 1, true); 15 | ros::Publisher path_pub = nh.advertise("path", 1, true); 16 | ros::Publisher es_pub = nh.advertise("ellipsoid_array", 1, true); 17 | ros::Publisher poly_pub = nh.advertise("polyhedron_array", 1, true); 18 | 19 | std::string file_name, topic_name, path_file; 20 | 21 | nh.param("path_file", path_file, std::string("path.txt")); 22 | nh.param("bag_file", file_name, std::string("voxel_map")); 23 | nh.param("bag_topic", topic_name, std::string("voxel_map")); 24 | //Read the point cloud from bag 25 | sensor_msgs::PointCloud cloud = read_bag(file_name, topic_name); 26 | cloud.header.frame_id = "map"; 27 | cloud_pub.publish(cloud); 28 | 29 | vec_Vec3f obs = DecompROS::cloud_to_vec(cloud); 30 | vec_Vec2f obs2d; 31 | for(const auto& it: obs) 32 | obs2d.push_back(it.topRows<2>()); 33 | 34 | //Read path from txt 35 | vec_Vec2f path; 36 | if(!read_path<2>(path_file, path)) 37 | ROS_ERROR("Fail to read a path!"); 38 | 39 | nav_msgs::Path path_msg = DecompROS::vec_to_path(path); 40 | path_msg.header.frame_id = "map"; 41 | path_pub.publish(path_msg); 42 | 43 | //Using ellipsoid decomposition 44 | EllipsoidDecomp2D decomp_util; 45 | decomp_util.set_obs(obs2d); 46 | decomp_util.set_local_bbox(Vec2f(1, 2)); 47 | decomp_util.dilate(path); //Set max iteration number of 10, do fix the path 48 | 49 | //Publish visualization msgs 50 | decomp_ros_msgs::EllipsoidArray es_msg = DecompROS::ellipsoid_array_to_ros(decomp_util.get_ellipsoids()); 51 | es_msg.header.frame_id = "map"; 52 | es_pub.publish(es_msg); 53 | 54 | decomp_ros_msgs::PolyhedronArray poly_msg = DecompROS::polyhedron_array_to_ros(decomp_util.get_polyhedrons()); 55 | poly_msg.header.frame_id = "map"; 56 | poly_pub.publish(poly_msg); 57 | 58 | 59 | //Convert to inequality constraints Ax < b 60 | auto polys = decomp_util.get_polyhedrons(); 61 | for(size_t i = 0; i < path.size() - 1; i++) { 62 | const auto pt_inside = (path[i] + path[i+1]) / 2; 63 | LinearConstraint2D cs(pt_inside, polys[i].hyperplanes()); 64 | printf("i: %zu\n", i); 65 | std::cout << "A: " << cs.A() << std::endl; 66 | std::cout << "b: " << cs.b() << std::endl; 67 | 68 | std::cout << "point: " << path[i].transpose(); 69 | if(cs.inside(path[i])) 70 | std::cout << " is inside!" << std::endl; 71 | else 72 | std::cout << " is outside!" << std::endl; 73 | std::cout << "point: " << path[i+1].transpose(); 74 | if(cs.inside(path[i+1])) 75 | std::cout << " is inside!" << std::endl; 76 | else 77 | std::cout << " is outside!" << std::endl; 78 | } 79 | 80 | 81 | ros::spin(); 82 | 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /decomp_test_node/src/test_path_decomp_3d.cpp: -------------------------------------------------------------------------------- 1 | #include "bag_reader.hpp" 2 | #include "txt_reader.hpp" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | int main(int argc, char ** argv){ 11 | ros::init(argc, argv, "test"); 12 | ros::NodeHandle nh("~"); 13 | 14 | ros::Publisher cloud_pub = nh.advertise("cloud", 1, true); 15 | ros::Publisher path_pub = nh.advertise("path", 1, true); 16 | ros::Publisher es_pub = nh.advertise("ellipsoid_array", 1, true); 17 | ros::Publisher poly_pub = nh.advertise("polyhedron_array", 1, true); 18 | 19 | std::string file_name, topic_name, path_file; 20 | 21 | nh.param("path_file", path_file, std::string("path.txt")); 22 | nh.param("bag_file", file_name, std::string("voxel_map")); 23 | nh.param("bag_topic", topic_name, std::string("voxel_map")); 24 | //Read the point cloud from bag 25 | sensor_msgs::PointCloud cloud = read_bag(file_name, topic_name); 26 | cloud.header.frame_id = "map"; 27 | cloud_pub.publish(cloud); 28 | 29 | vec_Vec3f obs = DecompROS::cloud_to_vec(cloud); 30 | 31 | //Read path from txt 32 | vec_Vec3f path; 33 | if(!read_path<3>(path_file, path)) 34 | ROS_ERROR("Fail to read a path!"); 35 | 36 | nav_msgs::Path path_msg = DecompROS::vec_to_path(path); 37 | path_msg.header.frame_id = "map"; 38 | path_pub.publish(path_msg); 39 | 40 | //Using ellipsoid decomposition 41 | EllipsoidDecomp3D decomp_util; 42 | decomp_util.set_obs(obs); 43 | decomp_util.set_local_bbox(Vec3f(1, 2, 1)); 44 | decomp_util.dilate(path); //Set max iteration number of 10, do fix the path 45 | 46 | //Publish visualization msgs 47 | decomp_ros_msgs::EllipsoidArray es_msg = DecompROS::ellipsoid_array_to_ros(decomp_util.get_ellipsoids()); 48 | es_msg.header.frame_id = "map"; 49 | es_pub.publish(es_msg); 50 | 51 | decomp_ros_msgs::PolyhedronArray poly_msg = DecompROS::polyhedron_array_to_ros(decomp_util.get_polyhedrons()); 52 | poly_msg.header.frame_id = "map"; 53 | poly_pub.publish(poly_msg); 54 | 55 | //Convert to inequality constraints Ax < b 56 | auto polys = decomp_util.get_polyhedrons(); 57 | for(size_t i = 0; i < path.size() - 1; i++) { 58 | const auto pt_inside = (path[i] + path[i+1]) / 2; 59 | LinearConstraint3D cs(pt_inside, polys[i].hyperplanes()); 60 | printf("i: %zu\n", i); 61 | std::cout << "A: " << cs.A() << std::endl; 62 | std::cout << "b: " << cs.b() << std::endl; 63 | std::cout << "point: " << path[i].transpose(); 64 | if(cs.inside(path[i])) 65 | std::cout << " is inside!" << std::endl; 66 | else 67 | std::cout << " is outside!" << std::endl; 68 | 69 | std::cout << "point: " << path[i+1].transpose(); 70 | if(cs.inside(path[i+1])) 71 | std::cout << " is inside!" << std::endl; 72 | else 73 | std::cout << " is outside!" << std::endl; 74 | } 75 | 76 | 77 | ros::spin(); 78 | 79 | return 0; 80 | } 81 | -------------------------------------------------------------------------------- /decomp_test_node/src/test_seed_decomp.cpp: -------------------------------------------------------------------------------- 1 | #include "bag_reader.hpp" 2 | #include "txt_reader.hpp" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | std_msgs::Header header_; 10 | 11 | int main(int argc, char ** argv){ 12 | ros::init(argc, argv, "test"); 13 | ros::NodeHandle nh("~"); 14 | 15 | ros::Publisher map_pub = nh.advertise("cloud", 1, true); 16 | ros::Publisher marker_pub = nh.advertise("markers", 1, true); 17 | ros::Publisher seed_pub = nh.advertise("seed", 1, true); 18 | ros::Publisher es_pub = nh.advertise("ellipsoid_array", 1, true); 19 | ros::Publisher poly_pub = nh.advertise("polyhedron_array", 1, true); 20 | 21 | header_.frame_id = std::string("map"); 22 | std::string file_name, topic_name, marker_name, seed_file; 23 | 24 | nh.param("seed_file", seed_file, std::string("seed.txt")); 25 | nh.param("bag_file", file_name, std::string("voxel_map")); 26 | nh.param("bag_topic", topic_name, std::string("voxel_map")); 27 | nh.param("bag_marker", marker_name, std::string("voxel_map")); 28 | //Read the point cloud from bag 29 | sensor_msgs::PointCloud2 cloud2 = read_bag(file_name, topic_name); 30 | //Convert into vector of Eigen 31 | sensor_msgs::PointCloud cloud; 32 | sensor_msgs::convertPointCloud2ToPointCloud(cloud2, cloud); 33 | cloud.header = header_; 34 | map_pub.publish(cloud); 35 | 36 | vec_Vec3f obs = DecompROS::cloud_to_vec(cloud); 37 | 38 | visualization_msgs::MarkerArray markers = read_bag(file_name, marker_name); 39 | for(auto & it: markers.markers) 40 | it.header = header_; 41 | marker_pub.publish(markers); 42 | 43 | //Read path from txt 44 | vec_Vec3f seeds; 45 | if(!read_path<3>(seed_file, seeds)) 46 | ROS_ERROR("Fail to read seeds!"); 47 | 48 | sensor_msgs::PointCloud seed_msg = DecompROS::vec_to_cloud(seeds); 49 | seed_msg.header = header_; 50 | seed_pub.publish(seed_msg); 51 | 52 | vec_E es; 53 | vec_E polys; 54 | 55 | for(const auto& it: seeds) { 56 | //Using seed decomposition 57 | SeedDecomp3D decomp_util(it); 58 | decomp_util.set_obs(obs); 59 | decomp_util.dilate(5.0); 60 | 61 | es.push_back(decomp_util.get_ellipsoid()); 62 | polys.push_back(decomp_util.get_polyhedron()); 63 | } 64 | 65 | //Publish visualization msgs 66 | decomp_ros_msgs::EllipsoidArray es_msg = DecompROS::ellipsoid_array_to_ros(es); 67 | es_msg.header = header_; 68 | es_pub.publish(es_msg); 69 | 70 | decomp_ros_msgs::PolyhedronArray poly_msg = DecompROS::polyhedron_array_to_ros(polys); 71 | poly_msg.header = header_; 72 | poly_pub.publish(poly_msg); 73 | 74 | 75 | /* 76 | vec_LinearConstraint3f cs = decomp_util.get_constraints(); 77 | for(int i = 0; i < cs.size(); i++) { 78 | MatD3f A = cs[i].first; 79 | VecDf b = cs[i].second; 80 | 81 | printf("i: %d\n", i); 82 | std::cout << "start: " << (A*path[i]-b).transpose() << std::endl; 83 | std::cout << "end: " << (A*path[i+1]-b).transpose() << std::endl; 84 | } 85 | */ 86 | 87 | 88 | ros::spin(); 89 | 90 | return 0; 91 | } 92 | -------------------------------------------------------------------------------- /decomp_test_node/src/txt_reader.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | template 7 | bool read_path(std::string file_name, vec_Vecf& path) { 8 | std::ifstream myfile(file_name); 9 | if (!myfile) { 10 | std::cout << "Unable to open file: " << file_name << std::endl; 11 | return false; 12 | } 13 | 14 | if (myfile.is_open()) { 15 | std::string line; 16 | while (getline(myfile, line)) { 17 | /// Seperate the line by single space 18 | std::istringstream buf(line); 19 | std::istream_iterator beg(buf), end; 20 | 21 | std::vector tokens(beg, end); 22 | 23 | if (tokens.size() != Dim) { 24 | std::cout << "Invalid format!" << std::endl; 25 | std::cout << line << '\n'; 26 | return false; 27 | } 28 | 29 | /// Extract the digit value 30 | Vecf pt; 31 | for(int i = 0; i < Dim; i++) 32 | pt(i) = atof(tokens[i].c_str()); 33 | path.push_back(pt); 34 | } 35 | myfile.close(); 36 | } 37 | 38 | return true; 39 | } 40 | -------------------------------------------------------------------------------- /wercker.yml: -------------------------------------------------------------------------------- 1 | box: osrf/ros:kinetic-desktop-full 2 | 3 | build: 4 | steps: 5 | - script: 6 | name: initialize git submodules 7 | code: | 8 | git submodule update --init --recursive 9 | - script: 10 | name: install dependencies 11 | code: | 12 | sudo apt-get update 13 | rosdep update 14 | rosdep install --from-paths . --ignore-src -y -r --as-root apt:false 15 | - script: 16 | name: build 17 | code: | 18 | cd .. 19 | mkdir -p catkin_ws/src 20 | mv source catkin_ws/src/ 21 | cd catkin_ws/src 22 | git clone https://github.com/catkin/catkin_simple.git 23 | catkin_init_workspace 24 | cd .. 25 | export ROS_PARALLEL_JOBS='-j4 -l4' # Limit parallel jobs 26 | catkin_make_isolated -DCMAKE_BUILD_TYPE=Release 27 | 28 | --------------------------------------------------------------------------------