├── LICENSE ├── README.md ├── cloud_compression ├── CMakeLists.txt ├── include │ └── cloud_compression │ │ └── octree_pointcloud_compression.h ├── msg │ └── CompressedCloud.msg ├── nodelet_plugin.xml ├── package.xml └── src │ ├── compression.cpp │ ├── octree_pointcloud_compression.hpp │ ├── xyz_compression.cpp │ ├── xyz_compression.h │ ├── xyz_decompression.cpp │ └── xyz_decompression.h ├── config_server ├── CMakeLists.txt ├── include │ └── config_server │ │ ├── configserver.h │ │ ├── parameter.h │ │ ├── parameterclient.h │ │ └── variant.h ├── launch │ └── example.launch ├── msg │ ├── ParameterDescription.msg │ ├── ParameterList.msg │ ├── ParameterValue.msg │ └── ParameterValueList.msg ├── package.xml ├── src │ ├── configserver.cpp │ ├── parameter.cpp │ └── parameterclient.cpp └── srv │ ├── GetParameter.srv │ ├── Load.srv │ ├── Save.srv │ ├── SetParameter.srv │ ├── ShowDeadVars.srv │ ├── Subscribe.srv │ └── SubscribeList.srv ├── mod_laser_filters ├── CMakeLists.txt ├── config │ └── approx_self_filter.yaml ├── include │ ├── filters │ │ └── filter_base.h │ └── laser_filters │ │ ├── approx_self_filter.h │ │ ├── scan_self_shadows_filter.h │ │ └── scan_shadows_filter.h ├── laser_filters_plugins.xml ├── launch │ └── approx_self_filter.launch ├── mainpage.dox ├── package.xml └── src │ └── laser_scan_filters.cpp ├── mrs_laser_mapping ├── CMakeLists.txt ├── cmake │ └── Modules │ │ ├── FindBLAS.cmake │ │ ├── FindCSparse.cmake │ │ ├── FindCholmod.cmake │ │ ├── FindEigen3.cmake │ │ ├── FindG2O.cmake │ │ ├── FindGSL.cmake │ │ ├── FindLAPACK.cmake │ │ ├── FindSuiteSparse.cmake │ │ └── FindTBB.cmake ├── config │ ├── config_mav_inventairy.yaml │ ├── config_velodyne.yaml │ ├── laser_shadow_filter.yaml │ └── self_filter_frames.yaml ├── include │ ├── map_nodelet.h │ ├── mrs_laser_mapping │ │ ├── color_utils.h │ │ ├── graph_publisher.h │ │ ├── impl │ │ │ ├── graph_publisher.hpp │ │ │ ├── map_publisher.hpp │ │ │ └── surfelmap_publisher.hpp │ │ ├── map_publisher.h │ │ ├── slam_graph.h │ │ ├── surfelmap_publisher.h │ │ └── trajectory_publisher.h │ ├── scan_assembler_nodelet.h │ ├── slam_nodelet.h │ └── slam_visualizer_nodelet.h ├── launch │ ├── mav_mapping.launch │ ├── mav_mapping.rviz │ ├── momaro_mapping.launch │ ├── momaro_mapping.rviz │ ├── velodyne_mapping.launch │ └── velodyne_mapping.rviz ├── msg │ ├── KeyFrame.msg │ ├── KeyFrameTransforms.msg │ └── MultiResolutionMap.msg ├── nodelet_plugins.xml ├── package.xml ├── src │ ├── map_publisher.cpp │ ├── nodes │ │ ├── map_nodelet.cpp │ │ ├── scan_assembler_nodelet.cpp │ │ ├── slam_nodelet.cpp │ │ └── slam_visualizer_nodelet.cpp │ ├── slam_graph.cpp │ ├── surfelmap_publisher.cpp │ └── trajectory_publisher.cpp └── srv │ ├── AddKeyFramesByDistance.srv │ ├── AddPointsToMap.srv │ ├── DeleteMarkedPoints.srv │ ├── InterestPointsSwitchHand.srv │ └── SelfFilter.srv ├── mrs_laser_maps ├── CMakeLists.txt ├── include │ ├── impl │ │ ├── map_level.hpp │ │ └── map_multiresolution.hpp │ └── mrs_laser_maps │ │ ├── grid_cell.h │ │ ├── grid_cell_with_statistics.h │ │ ├── map_level.h │ │ ├── map_multiresolution.h │ │ ├── map_point_types.h │ │ ├── multiresolution_surfel_registration.h │ │ ├── surfel.h │ │ ├── surfel_map_interface.h │ │ └── synchronized_circular_buffer.h ├── package.xml └── src │ ├── map_multiresolution.cpp │ └── multiresolution_surfel_registration.cpp └── parameter_tuner ├── CMakeLists.txt ├── include ├── parameteritem.h └── parametertuner.h ├── package.xml ├── plugin_rqt_gui.xml └── src ├── parameteritem.cpp ├── parametertuner.cpp └── parametertuner.ui /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, University of Bonn, Autonomous Intelligent Systems. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation and/or 12 | other materials provided with the distribution. 13 | 14 | 3. Neither the name of the copyright holder nor the names of its contributors 15 | may be used to endorse or promote products derived from this software without 16 | specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 22 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 25 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | mrs_laser_map - Local Multiresolution Grids for Efficient 3D Laser Mapping and Localization 3 | ===================================================================== 4 | 5 | `mrs_laser_map` is a mapping system for mobile robots using laser range sensors. 6 | It provides efficient local mapping for obstacle perception, and allocentric mapping 7 | for autonomous navigation. It has been successfully used on different 8 | robotic platforms, such as micro aerial vehicles and ground robots, in 9 | different research projects and robotic competitions. 10 | 11 | 12 | Papers Describing the Approach 13 | -------- 14 | 15 | David Droeschel, Jörg Stückler, and Sven Behnke: [Local Multi-Resolution Representation for 6D Motion Estimation and Mapping with a Continuously Rotating 3D Laser Scanner](http://ais.uni-bonn.de/papers/ICRA_2014_Droeschel.pdf) IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, May 2014 16 | 17 | David Droeschel, Jörg Stückler, and Sven Behnke: [Local Multi-Resolution Surfel Grids for MAV Motion Estimation and 3D Mapping](http://ais.uni-bonn.de/papers/IAS_2014_Droeschel_MAV-Mapping.pdf) In Proceedings of 13th International Conference on Intelligent Autonomous Systems (IAS), Padova, Italy, July 2014. 18 | 19 | Overview 20 | -------- 21 | 22 | The repository contains the following ROS packages: 23 | 24 | * `mrs_laser_maps`: map data structure and surfel-based registration 25 | * `mrs_laser_mapping`: ROS nodes for local and allocentric mapping 26 | * `config_server`: parameter server 27 | * `parameter_tuner`: rqt plugin for config server 28 | * `mod_laser_filters`: laser filters for preprocessing 29 | * `cloud_compression`: modified pcl cloud_compression 30 | 31 | 32 | Dependencies 33 | --------------- 34 | 35 | * Ubuntu 14.04 36 | 37 | * [ROS Indigo](http://www.ros.org/wiki/indigo/Installation/Ubuntu): 38 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~{.sh} 39 | sudo apt-get install ros-indigo-desktop-full 40 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 41 | * [g2o](https://openslam.org/g2o.html): 42 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~{.sh} 43 | sudo apt-get install ros-indigo-libg2o 44 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 45 | 46 | Make sure you have the latest libg2o ROS package, previous versions were compiled without optimization. 47 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~{.sh} 48 | droeschel@flanders:~/catkin_ws$ apt-cache policy ros-indigo-libg2o 49 | ros-indigo-libg2o: 50 | Installed: 2014.2.18-0trusty-20160321-175501-0700 51 | Candidate: 2014.2.18-0trusty-20160321-175501-0700 52 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 53 | 54 | Getting started 55 | --------------- 56 | 57 | Clone the repository in your catkin workspace (here ~/catkin_ws) 58 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~{.sh} 59 | cd ~/catkin_ws/src 60 | git clone https://github.com/AIS-Bonn/mrs_laser_map mrs_laser_map 61 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 62 | 63 | and build it with catkin_make (or catkin tools): 64 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~{.sh} 65 | cd ~/catkin_ws/ 66 | catkin_make 67 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 68 | 69 | Download the sample [bagfile](http://www.ais.uni-bonn.de/~droeschel/bags/mav_2.bag) and run mav_mapping.launch 70 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~{.sh} 71 | roslaunch mrs_laser_mapping mav_mapping.launch 72 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 73 | 74 | License 75 | ------- 76 | 77 | `mrs_laser_map` is licensed under the BSD 3-clause license. 78 | 79 | Contact 80 | ----------------- 81 | 82 | ``` 83 | David Droeschel 84 | Institute of Computer Science VI 85 | Rheinische Friedrich-Wilhelms-Universität Bonn 86 | Friedrich Ebert-Allee 144 87 | 53113 Bonn 88 | ``` 89 | -------------------------------------------------------------------------------- /cloud_compression/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 2.8) 3 | project(cloud_compression) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | roscpp 7 | pcl_ros 8 | pcl_conversions 9 | nodelet 10 | message_generation 11 | std_msgs 12 | ) 13 | 14 | find_package(PCL REQUIRED) 15 | 16 | add_message_files(FILES 17 | CompressedCloud.msg 18 | ) 19 | 20 | generate_messages(DEPENDENCIES 21 | std_msgs 22 | ) 23 | 24 | catkin_package( 25 | INCLUDE_DIRS include 26 | LIBRARIES cloud_compression 27 | ) 28 | 29 | include_directories(${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} include) 30 | 31 | link_directories(${PCL_LIBRARY_DIRS}) 32 | add_definitions(${PCL_DEFINITIONS}) 33 | 34 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") 35 | 36 | add_library(cloud_compression 37 | src/compression.cpp 38 | ) 39 | target_link_libraries(cloud_compression 40 | ${catkin_LIBRARIES} 41 | ) 42 | 43 | add_library(xyz_compression 44 | src/xyz_compression.cpp 45 | ) 46 | target_link_libraries(xyz_compression 47 | ${catkin_LIBRARIES} 48 | ${PCL_LIBRARY_DIRS} 49 | cloud_compression 50 | ) 51 | add_dependencies(xyz_compression 52 | ${PROJECT_NAME}_generate_messages_cpp 53 | ) 54 | 55 | add_library(xyz_decompression 56 | src/xyz_decompression.cpp 57 | ) 58 | target_link_libraries(xyz_decompression 59 | ${catkin_LIBRARIES} 60 | ${PCL_LIBRARY_DIRS} 61 | cloud_compression 62 | ) 63 | add_dependencies(xyz_decompression 64 | ${PROJECT_NAME}_generate_messages_cpp 65 | ) 66 | -------------------------------------------------------------------------------- /cloud_compression/msg/CompressedCloud.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8[] data 3 | -------------------------------------------------------------------------------- /cloud_compression/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /cloud_compression/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cloud_compression 3 | Nodelets for compression of point clouds 4 | 0.0.1 5 | BSD 6 | Max Schwarz 7 | 8 | catkin 9 | roscpp 10 | pcl_ros 11 | nodelet 12 | 13 | laser_mapping 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /cloud_compression/src/compression.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | /* 4 | * Software License Agreement (BSD License) 5 | * 6 | * Copyright (c) 2011, Willow Garage, Inc. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of Willow Garage, Inc. nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Julius Kammerl (julius@kammerl.de) 37 | */ 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | 48 | #include 49 | #include "octree_pointcloud_compression.hpp" 50 | 51 | 52 | template class PCL_EXPORTS cloud_compression::OctreePointCloudCompression; 53 | template class PCL_EXPORTS cloud_compression::OctreePointCloudCompression; 54 | template class PCL_EXPORTS cloud_compression::OctreePointCloudCompression; 55 | // template class PCL_EXPORTS cloud_compression::OctreePointCloudCompression; -------------------------------------------------------------------------------- /cloud_compression/src/xyz_compression.cpp: -------------------------------------------------------------------------------- 1 | // Compression for clouds without color 2 | // Author: Max Schwarz 3 | 4 | #include "xyz_compression.h" 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | namespace cloud_compression 17 | { 18 | 19 | XYZCompression::XYZCompression() 20 | { 21 | } 22 | 23 | XYZCompression::~XYZCompression() 24 | { 25 | } 26 | 27 | void XYZCompression::onInit() 28 | { 29 | ros::NodeHandle& nh = getPrivateNodeHandle(); 30 | 31 | m_compression.reset( 32 | new Compression(pcl::io::HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR) 33 | ); 34 | 35 | m_sub = nh.subscribe("input", 1, &XYZCompression::handleCloud, this); 36 | m_pub = nh.advertise("output", 1); 37 | } 38 | 39 | class VectorStreamBuf : public std::streambuf 40 | { 41 | public: 42 | explicit VectorStreamBuf(std::vector* out) 43 | : m_out(out) 44 | {} 45 | 46 | virtual int overflow(int c) 47 | { 48 | m_out->push_back(c); 49 | return 0; 50 | } 51 | private: 52 | std::vector* m_out; 53 | }; 54 | 55 | class VectorStream : public std::ostream 56 | { 57 | public: 58 | explicit VectorStream(std::vector* out) 59 | : m_buf(out) 60 | { 61 | rdbuf(&m_buf); 62 | } 63 | private: 64 | VectorStreamBuf m_buf; 65 | }; 66 | 67 | void XYZCompression::handleCloud(const PointCloud::ConstPtr& cloud) 68 | { 69 | CompressedCloudPtr msg(new CompressedCloud); 70 | 71 | VectorStream stream(&msg->data); 72 | 73 | m_compression.reset( 74 | new Compression(pcl::io::HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR) 75 | ); 76 | m_compression->encodePointCloud(cloud, stream); 77 | 78 | pcl_conversions::fromPCL(cloud->header, msg->header); 79 | 80 | // ROS_INFO("Compressed %lu points to %lu bytes", cloud->size(), msg->data.size()); 81 | 82 | m_pub.publish(msg); 83 | } 84 | 85 | } 86 | 87 | PLUGINLIB_EXPORT_CLASS(cloud_compression::XYZCompression, nodelet::Nodelet) 88 | -------------------------------------------------------------------------------- /cloud_compression/src/xyz_compression.h: -------------------------------------------------------------------------------- 1 | // Compression for clouds without color 2 | // Author: Max Schwarz 3 | 4 | #ifndef XYZ_COMPRESSION_H 5 | #define XYZ_COMPRESSION_H 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace cloud_compression 17 | { 18 | 19 | class XYZCompression : public nodelet::Nodelet 20 | { 21 | public: 22 | XYZCompression(); 23 | virtual ~XYZCompression(); 24 | 25 | virtual void onInit() override; 26 | private: 27 | typedef pcl::PointXYZ Point; 28 | typedef pcl::PointCloud PointCloud; 29 | typedef cloud_compression::OctreePointCloudCompression Compression; 30 | 31 | void handleCloud(const PointCloud::ConstPtr& cloud); 32 | 33 | ros::Subscriber m_sub; 34 | ros::Publisher m_pub; 35 | Compression::Ptr m_compression; 36 | }; 37 | 38 | } 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /cloud_compression/src/xyz_decompression.cpp: -------------------------------------------------------------------------------- 1 | // Decompression for clouds without color 2 | // Author: Max Schwarz 3 | 4 | #include "xyz_decompression.h" 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace cloud_compression 19 | { 20 | 21 | XYZDecompression::XYZDecompression() 22 | { 23 | } 24 | 25 | XYZDecompression::~XYZDecompression() 26 | { 27 | } 28 | 29 | void XYZDecompression::onInit() 30 | { 31 | ros::NodeHandle& nh = getPrivateNodeHandle(); 32 | 33 | m_compression.reset( 34 | new Compression(pcl::io::HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR) 35 | ); 36 | 37 | m_sub = nh.subscribe("input", 1, &XYZDecompression::handleMsg, this); 38 | m_pub = nh.advertise("output", 1); 39 | } 40 | 41 | class VectorStreamBuf : public std::streambuf 42 | { 43 | public: 44 | explicit VectorStreamBuf(const std::vector* in) 45 | { 46 | setg( 47 | reinterpret_cast(const_cast(in->data())), 48 | reinterpret_cast(const_cast(in->data())), 49 | reinterpret_cast(const_cast(in->data() + in->size())) 50 | ); 51 | } 52 | }; 53 | 54 | class VectorStream : public std::istream 55 | { 56 | public: 57 | explicit VectorStream(const std::vector* in) 58 | : m_buf(in) 59 | { 60 | rdbuf(&m_buf); 61 | } 62 | private: 63 | VectorStreamBuf m_buf; 64 | }; 65 | 66 | void XYZDecompression::handleMsg(const CompressedCloud::ConstPtr& msg) 67 | { 68 | PointCloud::Ptr cloud(new PointCloud); 69 | 70 | VectorStream stream(&msg->data); 71 | 72 | m_compression->decodePointCloud(stream, cloud); 73 | 74 | pcl_conversions::toPCL(msg->header, cloud->header); 75 | 76 | // ROS_INFO("Decompression: got cloud with %lux%lu points (compressed size: %lu)", (long unsigned int)cloud->width, (long unsigned int)cloud->height, msg->data.size()); 77 | 78 | if (cloud->points.size() > 0) 79 | m_pub.publish(cloud); 80 | } 81 | 82 | } 83 | 84 | PLUGINLIB_EXPORT_CLASS(cloud_compression::XYZDecompression, nodelet::Nodelet) 85 | 86 | -------------------------------------------------------------------------------- /cloud_compression/src/xyz_decompression.h: -------------------------------------------------------------------------------- 1 | // Decompression for clouds without color 2 | // Author: Max Schwarz 3 | 4 | #ifndef XYZ_DECOMPRESSION_H 5 | #define XYZ_DECOMPRESSION_H 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace cloud_compression 19 | { 20 | 21 | class XYZDecompression : public nodelet::Nodelet 22 | { 23 | public: 24 | XYZDecompression(); 25 | virtual ~XYZDecompression(); 26 | 27 | virtual void onInit() override; 28 | private: 29 | typedef pcl::PointXYZ Point; 30 | typedef pcl::PointCloud PointCloud; 31 | typedef cloud_compression::OctreePointCloudCompression Compression; 32 | 33 | void handleMsg(const CompressedCloud::ConstPtr& msg); 34 | 35 | ros::Subscriber m_sub; 36 | ros::Publisher m_pub; 37 | Compression::Ptr m_compression; 38 | }; 39 | 40 | } 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /config_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(config_server) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp genmsg roslib) 5 | 6 | ####################################### 7 | ## Declare ROS messages and services ## 8 | ####################################### 9 | 10 | ## Generate messages in the 'msg' folder 11 | add_message_files( 12 | FILES 13 | ParameterDescription.msg 14 | ParameterList.msg 15 | ParameterValue.msg 16 | ParameterValueList.msg 17 | ) 18 | 19 | ## Generate services in the 'srv' folder 20 | add_service_files( 21 | FILES 22 | GetParameter.srv 23 | SetParameter.srv 24 | Subscribe.srv 25 | SubscribeList.srv 26 | Load.srv 27 | Save.srv 28 | ShowDeadVars.srv 29 | ) 30 | 31 | ## Generate added messages and services with any dependencies listed here 32 | generate_messages( 33 | DEPENDENCIES 34 | std_msgs # Or other packages containing msgs 35 | ) 36 | 37 | ################################################### 38 | ## Declare things to be passed to other projects ## 39 | ################################################### 40 | 41 | ## LIBRARIES: libraries you create in this project that dependent projects also need 42 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 43 | ## DEPENDS: system dependencies of this project that dependent projects also need 44 | catkin_package( 45 | INCLUDE_DIRS include 46 | LIBRARIES config_client 47 | # CATKIN_DEPENDS roscpp 48 | # DEPENDS system_lib 49 | ) 50 | 51 | ########### 52 | ## Build ## 53 | ########### 54 | 55 | ## Specify additional locations of header files 56 | include_directories(include 57 | ${catkin_INCLUDE_DIRS} 58 | ) 59 | 60 | ## Declare a cpp library 61 | add_library(config_client 62 | src/parameter.cpp 63 | src/parameterclient.cpp 64 | ) 65 | 66 | ## Declare a cpp executable 67 | add_executable(config_server 68 | src/configserver.cpp 69 | ) 70 | 71 | ## Specify libraries to link a library or executable target against 72 | target_link_libraries(config_server 73 | ${catkin_LIBRARIES} 74 | yaml-cpp 75 | ) 76 | 77 | target_link_libraries(config_client 78 | ${catkin_LIBRARIES} 79 | ) 80 | 81 | add_dependencies(config_server config_server_gencpp) 82 | add_dependencies(config_client config_server_gencpp) 83 | 84 | ############# 85 | ## Install ## 86 | ############# 87 | 88 | install(TARGETS config_server config_client 89 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 90 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 91 | ) 92 | 93 | ## Mark executable scripts (Python etc.) for installation 94 | ## not required for python when using catkin_python_setup() 95 | # install(PROGRAMS 96 | # scripts/my_python_script 97 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 98 | # ) 99 | 100 | ## Mark executables and/or libraries for installation 101 | # install(TARGETS config_server config_server_node 102 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 103 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 104 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 105 | # ) 106 | 107 | ## Mark cpp header files for installation 108 | # install(DIRECTORY include/${PROJECT_NAME}/ 109 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 110 | # FILES_MATCHING PATTERN "*.h" 111 | # PATTERN ".svn" EXCLUDE 112 | # ) 113 | 114 | ## Mark other files for installation (e.g. launch and bag files, etc.) 115 | # install(FILES 116 | # # myfile1 117 | # # myfile2 118 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 119 | # ) 120 | 121 | ############# 122 | ## Testing ## 123 | ############# 124 | 125 | ## Add gtest based cpp test target and link libraries 126 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_config_server.cpp) 127 | # if(TARGET ${PROJECT_NAME}-test) 128 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 129 | # endif() 130 | 131 | ## Add folders to be run by python nosetests 132 | # catkin_add_nosetests(test) 133 | -------------------------------------------------------------------------------- /config_server/include/config_server/configserver.h: -------------------------------------------------------------------------------- 1 | // Configuration server 2 | // Author: Max Schwarz 3 | 4 | #ifndef CONFIGSERVER_H 5 | #define CONFIGSERVER_H 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace YAML { class Node; } 21 | 22 | namespace config_server 23 | { 24 | 25 | struct Parameter 26 | { 27 | ParameterDescription desc; 28 | std::string value; 29 | 30 | pthread_mutex_t mutex; 31 | std::vector subscribers; 32 | 33 | void notify(const std::string& exclude = ""); 34 | }; 35 | 36 | class ConfigServer 37 | { 38 | public: 39 | ConfigServer(); 40 | virtual ~ConfigServer(); 41 | 42 | bool handleSave(SaveRequest& req, SaveResponse& resp); 43 | 44 | bool load(const std::string& filename); 45 | bool handleLoad(LoadRequest& req, LoadResponse& resp); 46 | private: 47 | typedef std::map ParameterMap; 48 | ParameterMap m_params; 49 | 50 | ros::NodeHandle m_nh; 51 | ros::ServiceServer m_srv_setParameter; 52 | ros::ServiceServer m_srv_getParameter; 53 | ros::ServiceServer m_srv_subscribe; 54 | ros::ServiceServer m_srv_subscribeList; 55 | ros::ServiceServer m_srv_showDeadVars; 56 | ros::Publisher m_pub_paramList; 57 | ros::Publisher m_pub_currentValues; 58 | 59 | ros::ServiceServer m_srv_save; 60 | ros::ServiceServer m_srv_load; 61 | 62 | bool handleSetParameter(SetParameterRequest& req, SetParameterResponse& resp); 63 | bool handleGetParameter(GetParameterRequest& req, GetParameterResponse& resp); 64 | 65 | bool doSubscribe(const std::string& callback, const ParameterDescription& desc, std::string* value, bool *changed = 0); 66 | bool handleSubscribe(SubscribeRequest& req, SubscribeResponse& resp); 67 | bool handleSubscribeList(SubscribeListRequest& req, SubscribeListResponse& resp); 68 | 69 | bool handleShowDeadVars(ShowDeadVarsRequest& req, ShowDeadVarsResponse& resp); 70 | 71 | void planUpdate(); 72 | void planValueUpdate(); 73 | void updateParameterList(); 74 | void updateParameterValueList(); 75 | 76 | void insertFromYAML(const YAML::Node& n, const std::string& path); 77 | 78 | std::string defaultConfigName(); 79 | 80 | ros::WallTimer m_publishParamsTimer; 81 | int m_publishParamsCounter; 82 | 83 | ros::WallTimer m_publishValuesTimer; 84 | int m_publishValuesCounter; 85 | }; 86 | 87 | } 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /config_server/include/config_server/parameter.h: -------------------------------------------------------------------------------- 1 | // Client object 2 | // Author: Max Schwarz 3 | 4 | #ifndef CONFIGSERVER_PARAMETER_H 5 | #define CONFIGSERVER_PARAMETER_H 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace config_server 20 | { 21 | 22 | /** 23 | * Base class for all parameters 24 | **/ 25 | class ParameterBase 26 | { 27 | public: 28 | ParameterBase() : m_haveDesc(false) {} 29 | virtual ~ParameterBase(); 30 | 31 | void reinit(); 32 | 33 | inline std::string name() const { return m_name; } 34 | 35 | protected: 36 | void notifyServer(); 37 | virtual void notifyClient(); 38 | 39 | virtual std::string serialize() const = 0; 40 | virtual bool deserialize(const std::string& value) = 0; 41 | 42 | void init(const ParameterDescription& desc, ros::NodeHandle* nh = NULL, bool create = true); 43 | 44 | bool handleSet(const std::string& value); 45 | private: 46 | friend class ParameterClient; 47 | 48 | std::string m_name; 49 | ParameterDescription m_desc; 50 | bool m_haveDesc; 51 | }; 52 | 53 | template 54 | class TypedParameter : public ParameterBase 55 | { 56 | public: 57 | void set(const T& value) 58 | { 59 | m_value = value; 60 | notifyServer(); 61 | notifyClient(); 62 | } 63 | 64 | inline T get() const { return m_value; } 65 | inline T operator()() const { return get(); } 66 | 67 | inline void setCallback(const boost::function& callback, bool callFunction = false) 68 | { 69 | m_callback = callback; 70 | if(callFunction && m_callback) 71 | m_callback(m_value); 72 | } 73 | inline void callCallback() 74 | { 75 | if(m_callback) 76 | m_callback(m_value); 77 | } 78 | 79 | protected: 80 | T m_value; 81 | boost::function m_callback; 82 | 83 | virtual void notifyClient() { callCallback(); } 84 | }; 85 | 86 | /** 87 | * User API for parameters on the config_server. 88 | * 89 | * Example usage: 90 | * @code 91 | * // Create a parameter with range [0.0, 1.0], step size 0.01, default value 0.5 92 | * Parameter myParam("myParam", 0.0, 0.01, 1.0, 0.5); 93 | * 94 | * // fetch its value 95 | * float value = myParam(); 96 | * 97 | * // and set it 98 | * myParam.set(0.8); 99 | * 100 | * // set a callback (use boost::bind for more complex setups) 101 | * void callback(float value) {} 102 | * myParam.setCallback(&callback); 103 | * @endcode 104 | **/ 105 | template 106 | class Parameter : TypedParameter 107 | { 108 | }; 109 | 110 | // Following are specializations for different value types. 111 | 112 | template<> 113 | class Parameter : public TypedParameter 114 | { 115 | public: 116 | Parameter(const std::string& name, const std::string& value = "") 117 | { 118 | config_server::ParameterDescription desc; 119 | desc.name = name; 120 | desc.type = "string"; 121 | desc.default_value = value; 122 | init(desc); 123 | } 124 | 125 | Parameter(const ParameterDescription& desc, ros::NodeHandle* nh = 0, bool create = true) 126 | { 127 | init(desc, nh, create); 128 | } 129 | 130 | protected: 131 | virtual bool deserialize(const std::string& value) 132 | { 133 | m_value = value; 134 | return true; 135 | } 136 | 137 | virtual std::string serialize() const 138 | { 139 | return m_value; 140 | } 141 | }; 142 | 143 | template 144 | inline std::string toString(const T& value) 145 | { 146 | std::stringstream ss; 147 | ss << value; 148 | return ss.str(); 149 | } 150 | 151 | template<> 152 | class Parameter : public TypedParameter 153 | { 154 | public: 155 | Parameter(const std::string& name, int min, int step, int max, int value) 156 | { 157 | ParameterDescription desc; 158 | desc.name = name; 159 | desc.type = "int"; 160 | desc.default_value = toString(value); 161 | desc.step = step; 162 | desc.min = min; 163 | desc.max = max; 164 | init(desc); 165 | } 166 | 167 | Parameter(const ParameterDescription& desc, ros::NodeHandle* nh = 0, bool create = true) 168 | { 169 | init(desc, nh, create); 170 | } 171 | protected: 172 | virtual bool deserialize(const std::string& value) 173 | { 174 | std::stringstream ss(value); 175 | 176 | int v; 177 | ss >> v; 178 | 179 | if(ss.fail() || ss.bad() || !ss.eof()) 180 | return false; 181 | 182 | m_value = v; 183 | return true; 184 | } 185 | virtual std::string serialize() const 186 | { 187 | std::stringstream ss; 188 | ss << m_value; 189 | 190 | return ss.str(); 191 | } 192 | }; 193 | 194 | template<> 195 | class Parameter : public TypedParameter 196 | { 197 | public: 198 | Parameter(const std::string& name, float min, float step, float max, float value) 199 | { 200 | ParameterDescription desc; 201 | desc.name = name; 202 | desc.type = "float"; 203 | desc.default_value = toString(value); 204 | desc.step = step; 205 | desc.min = min; 206 | desc.max = max; 207 | init(desc); 208 | } 209 | 210 | Parameter(const ParameterDescription& desc, ros::NodeHandle* nh = 0, bool create = true) 211 | { 212 | init(desc, nh, create); 213 | } 214 | protected: 215 | virtual bool deserialize(const std::string& value) 216 | { 217 | std::stringstream ss(value); 218 | 219 | float v; 220 | ss >> v; 221 | 222 | if(ss.fail() || ss.bad() || !ss.eof()) 223 | return false; 224 | 225 | m_value = v; 226 | return true; 227 | } 228 | virtual std::string serialize() const 229 | { 230 | std::stringstream ss; 231 | ss << m_value; 232 | 233 | return ss.str(); 234 | } 235 | }; 236 | 237 | template<> 238 | class Parameter : public TypedParameter 239 | { 240 | public: 241 | Parameter(const std::string& name, bool default_value) 242 | { 243 | ParameterDescription desc; 244 | desc.name = name; 245 | desc.type = "bool"; 246 | desc.default_value = default_value ? "1" : "0"; 247 | desc.step = 1; 248 | desc.min = 0; 249 | desc.max = 1; 250 | init(desc); 251 | } 252 | 253 | Parameter(const ParameterDescription& desc, ros::NodeHandle* nh = 0, bool create = true) 254 | { 255 | init(desc, nh, create); 256 | } 257 | protected: 258 | virtual bool deserialize(const std::string& value) 259 | { 260 | if(value.size() == 0 || value == "0") 261 | m_value = false; 262 | else 263 | m_value = true; 264 | 265 | return true; 266 | } 267 | 268 | virtual std::string serialize() const 269 | { 270 | return m_value ? "1" : "0"; 271 | } 272 | }; 273 | 274 | } 275 | 276 | #endif 277 | -------------------------------------------------------------------------------- /config_server/include/config_server/parameterclient.h: -------------------------------------------------------------------------------- 1 | // Config server client singleton 2 | // Author: Max Schwarz 3 | 4 | #ifndef CONFIGSERVER_PARAMETERCLIENT_H 5 | #define CONFIGSERVER_PARAMETERCLIENT_H 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace config_server 21 | { 22 | 23 | class ParameterBase; 24 | 25 | class ParameterClient 26 | { 27 | public: 28 | static void initialize(ros::NodeHandle& nh); 29 | static ParameterClient* instance(); 30 | 31 | void registerParameter(ParameterBase* param, const ParameterDescription& desc); 32 | void unregisterParameter(ParameterBase* param); 33 | 34 | void notify(ParameterBase* param, const std::string& value); 35 | 36 | void cork(); 37 | void uncork(); 38 | 39 | void sync(); 40 | private: 41 | ParameterClient(); 42 | ParameterClient(ros::NodeHandle& nh); 43 | ~ParameterClient(); 44 | 45 | void init(ros::NodeHandle& nh); 46 | 47 | bool handleSet(SetParameterRequest& req, SetParameterResponse& resp); 48 | 49 | static ParameterClient* g_instance; 50 | 51 | typedef std::multimap ParameterMap; 52 | ParameterMap m_parameters; 53 | 54 | ros::ServiceServer m_srv; 55 | std::string m_srv_name; 56 | 57 | int m_corked; 58 | config_server::SubscribeList m_subscribe; 59 | 60 | boost::recursive_mutex m_mutex; 61 | 62 | bool m_serverEnabled; 63 | ros::WallDuration m_waitDuration; 64 | }; 65 | 66 | } 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /config_server/include/config_server/variant.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AIS-Bonn/mrs_laser_map/efc0b4e215a7605608e607a85f5ea68eebb2c554/config_server/include/config_server/variant.h -------------------------------------------------------------------------------- /config_server/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /config_server/msg/ParameterDescription.msg: -------------------------------------------------------------------------------- 1 | string name 2 | string type 3 | float32 min 4 | float32 max 5 | float32 step 6 | string default_value -------------------------------------------------------------------------------- /config_server/msg/ParameterList.msg: -------------------------------------------------------------------------------- 1 | ParameterDescription[] parameters -------------------------------------------------------------------------------- /config_server/msg/ParameterValue.msg: -------------------------------------------------------------------------------- 1 | string name 2 | string type 3 | float32 min 4 | float32 max 5 | float32 step 6 | string value 7 | 8 | -------------------------------------------------------------------------------- /config_server/msg/ParameterValueList.msg: -------------------------------------------------------------------------------- 1 | ParameterValue[] parameters 2 | 3 | -------------------------------------------------------------------------------- /config_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | config_server 4 | 0.0.0 5 | The config_server package 6 | 7 | Max Schwarz 8 | 9 | TODO 10 | 11 | catkin 12 | roscpp 13 | roscpp 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /config_server/src/parameter.cpp: -------------------------------------------------------------------------------- 1 | // Client object 2 | // Author: Max Schwarz 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace config_server 11 | { 12 | 13 | ParameterBase::~ParameterBase() 14 | { 15 | if(!m_name.empty()) 16 | ParameterClient::instance()->unregisterParameter(this); 17 | m_haveDesc = false; 18 | } 19 | 20 | void ParameterBase::init(const ParameterDescription& desc, ros::NodeHandle* nh, bool create) 21 | { 22 | if(desc.name.empty()) return; 23 | m_name = desc.name; 24 | if(m_name[0] != '/') 25 | m_name = ros::this_node::getName() + "/" + desc.name; 26 | 27 | ParameterDescription createDesc; 28 | if(create) 29 | createDesc = desc; 30 | 31 | createDesc.name = m_name; 32 | 33 | ParameterClient::instance()->registerParameter(this, createDesc); 34 | m_desc = createDesc; 35 | m_haveDesc = true; 36 | } 37 | 38 | void ParameterBase::reinit() 39 | { 40 | if(!m_haveDesc) return; 41 | ParameterClient::instance()->unregisterParameter(this); 42 | ParameterClient::instance()->registerParameter(this, m_desc); 43 | } 44 | 45 | bool ParameterBase::handleSet(const std::string& value) 46 | { 47 | if(!deserialize(value)) 48 | return false; 49 | notifyClient(); 50 | return true; 51 | } 52 | 53 | void ParameterBase::notifyServer() 54 | { 55 | ParameterClient::instance()->notify(this, serialize()); 56 | } 57 | 58 | void ParameterBase::notifyClient() 59 | { 60 | } 61 | 62 | } 63 | // EOF -------------------------------------------------------------------------------- /config_server/src/parameterclient.cpp: -------------------------------------------------------------------------------- 1 | // Config server client singleton 2 | // Author: Max Schwarz 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace config_server 10 | { 11 | 12 | ParameterClient* ParameterClient::g_instance = 0; 13 | static bool g_waited = false; 14 | 15 | ParameterClient* ParameterClient::instance() 16 | { 17 | // FIXME: This is not thread-safe! 18 | if(!g_instance) 19 | g_instance = new ParameterClient(); 20 | 21 | return g_instance; 22 | } 23 | 24 | void ParameterClient::initialize(ros::NodeHandle& nh) 25 | { 26 | if(g_instance) 27 | return; 28 | 29 | g_instance = new ParameterClient(nh); 30 | } 31 | 32 | 33 | ParameterClient::ParameterClient(ros::NodeHandle& nh) 34 | : m_corked(0) 35 | { 36 | init(nh); 37 | } 38 | 39 | ParameterClient::ParameterClient() 40 | : m_corked(0) 41 | { 42 | ros::NodeHandle nh("~"); 43 | init(nh); 44 | } 45 | 46 | void ParameterClient::init(ros::NodeHandle& nh) 47 | { 48 | m_srv = nh.advertiseService("configure", &ParameterClient::handleSet, this); 49 | m_srv_name = nh.getNamespace() + "/configure"; 50 | 51 | m_subscribe.request.callback = m_srv_name; 52 | 53 | double secs; 54 | nh.param("/config_server/wait_duration", secs, 5.0); 55 | m_waitDuration = ros::WallDuration(secs); 56 | 57 | nh.param("/config_server/enabled", m_serverEnabled, true); 58 | } 59 | 60 | ParameterClient::~ParameterClient() 61 | { 62 | } 63 | 64 | 65 | void ParameterClient::registerParameter(ParameterBase* param, const ParameterDescription& desc) 66 | { 67 | boost::recursive_mutex::scoped_lock lock(m_mutex); 68 | 69 | m_parameters.insert( 70 | std::pair(param->name(), param) 71 | ); 72 | 73 | if(!m_serverEnabled) 74 | { 75 | // Quietly set to default... 76 | param->handleSet(desc.default_value); 77 | return; 78 | } 79 | 80 | if(m_corked) 81 | { 82 | for(size_t i = 0; i < m_subscribe.request.parameters.size(); ++i) 83 | { 84 | if(m_subscribe.request.parameters[i].name == desc.name) 85 | return; 86 | } 87 | 88 | m_subscribe.request.parameters.push_back(desc); 89 | } 90 | else 91 | { 92 | config_server::Subscribe srv; 93 | srv.request.callback = m_srv_name; 94 | srv.request.prop = param->name(); 95 | srv.request.desc = desc; 96 | srv.request.desc.name = param->name(); 97 | 98 | if(!g_waited) 99 | { 100 | // ros::service::waitForService() does a ros time sleep, which 101 | // hangs if time is simulated & stopped... 102 | ros::WallRate rate(std::min(1.0, 2.0 / m_waitDuration.toSec())); 103 | ros::WallTime start = ros::WallTime::now(); 104 | while(!ros::service::exists("/config_server/subscribe", false) && (ros::WallTime::now() - start) < m_waitDuration) 105 | { 106 | ROS_INFO_ONCE("Waiting for config_server..."); 107 | rate.sleep(); 108 | } 109 | 110 | g_waited = true; 111 | } 112 | 113 | if(!ros::service::call("/config_server/subscribe", srv)) 114 | { 115 | ROS_ERROR("Could not call /config_server/subscribe for parameter '%s'", param->name().c_str()); 116 | param->handleSet(srv.request.desc.default_value); 117 | } 118 | else 119 | param->handleSet(srv.response.value); 120 | } 121 | } 122 | 123 | void ParameterClient::cork() 124 | { 125 | boost::recursive_mutex::scoped_lock lock(m_mutex); 126 | m_corked++; 127 | } 128 | 129 | void ParameterClient::uncork() 130 | { 131 | boost::recursive_mutex::scoped_lock lock(m_mutex); 132 | 133 | if(m_corked == 0) 134 | { 135 | ROS_ERROR("ParameterClient::uncork() called while not corked!"); 136 | return; 137 | } 138 | 139 | if(--m_corked != 0) 140 | return; 141 | 142 | sync(); 143 | } 144 | 145 | void ParameterClient::sync() 146 | { 147 | boost::recursive_mutex::scoped_lock lock(m_mutex); 148 | 149 | if(!ros::service::call("/config_server/subscribe_list", m_subscribe)) 150 | { 151 | ROS_ERROR("Could not call /config_server/subscribe_list"); 152 | abort(); 153 | } 154 | 155 | if(m_subscribe.response.values.size() != m_subscribe.request.parameters.size()) 156 | { 157 | ROS_ERROR("Invalid response size from service call /config_server/subscribe_list"); 158 | ROS_ERROR("Initial parameter values will be wrong."); 159 | return; 160 | } 161 | 162 | for(size_t i = 0; i < m_subscribe.request.parameters.size(); ++i) 163 | { 164 | std::pair eq; 165 | eq = m_parameters.equal_range(m_subscribe.request.parameters[i].name); 166 | 167 | for(ParameterMap::iterator it = eq.first; it != eq.second; ++it) 168 | { 169 | it->second->handleSet(m_subscribe.response.values[i]); 170 | } 171 | } 172 | 173 | m_subscribe.request.parameters.clear(); 174 | } 175 | 176 | 177 | void ParameterClient::unregisterParameter(ParameterBase* param) 178 | { 179 | boost::recursive_mutex::scoped_lock lock(m_mutex); 180 | 181 | for(ParameterMap::iterator it = m_parameters.begin(); it != m_parameters.end(); ++it) 182 | { 183 | if(it->second == param) 184 | { 185 | m_parameters.erase(it); 186 | return; 187 | } 188 | } 189 | 190 | ROS_FATAL("config_server::ParameterClient: tried to unregister unknown parameter '%s'", param->name().c_str()); 191 | abort(); 192 | } 193 | 194 | bool ParameterClient::handleSet(SetParameterRequest& req, SetParameterResponse& resp) 195 | { 196 | boost::recursive_mutex::scoped_lock lock(m_mutex); 197 | 198 | std::pair eq; 199 | eq = m_parameters.equal_range(req.name); 200 | 201 | for(ParameterMap::iterator it = eq.first; it != eq.second; ++it) 202 | { 203 | if(!it->second->handleSet(req.value)) 204 | { 205 | resp.badValue = true; 206 | return true; 207 | } 208 | } 209 | 210 | resp.badValue = false; 211 | return true; 212 | } 213 | 214 | void ParameterClient::notify(ParameterBase* param, const std::string& value) 215 | { 216 | boost::recursive_mutex::scoped_lock lock(m_mutex); 217 | 218 | // Handle local notification 219 | std::pair eq; 220 | eq = m_parameters.equal_range(param->name()); 221 | 222 | for(ParameterMap::iterator it = eq.first; it != eq.second; ++it) 223 | { 224 | if(it->second == param) 225 | continue; 226 | 227 | it->second->handleSet(value); 228 | } 229 | 230 | // Notify the server 231 | config_server::SetParameter srv; 232 | srv.request.name = param->name(); 233 | srv.request.value = value; 234 | srv.request.no_notify = m_srv_name; 235 | 236 | if(!ros::service::call("/config_server/set_parameter", srv)) 237 | { 238 | ROS_ERROR("Could not set parameter '%s' on the parameter server", param->name().c_str()); 239 | } 240 | } 241 | 242 | } 243 | -------------------------------------------------------------------------------- /config_server/srv/GetParameter.srv: -------------------------------------------------------------------------------- 1 | string name 2 | --- 3 | string value -------------------------------------------------------------------------------- /config_server/srv/Load.srv: -------------------------------------------------------------------------------- 1 | string filename 2 | --- 3 | -------------------------------------------------------------------------------- /config_server/srv/Save.srv: -------------------------------------------------------------------------------- 1 | string filename 2 | --- 3 | -------------------------------------------------------------------------------- /config_server/srv/SetParameter.srv: -------------------------------------------------------------------------------- 1 | string name 2 | string value 3 | string no_notify 4 | --- 5 | bool badValue 6 | -------------------------------------------------------------------------------- /config_server/srv/ShowDeadVars.srv: -------------------------------------------------------------------------------- 1 | string configPath 2 | --- 3 | -------------------------------------------------------------------------------- /config_server/srv/Subscribe.srv: -------------------------------------------------------------------------------- 1 | string prop 2 | string callback 3 | ParameterDescription desc 4 | --- 5 | string value -------------------------------------------------------------------------------- /config_server/srv/SubscribeList.srv: -------------------------------------------------------------------------------- 1 | string callback 2 | ParameterDescription[] parameters 3 | --- 4 | string[] values -------------------------------------------------------------------------------- /mod_laser_filters/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mod_laser_filters) 3 | 4 | ############################################################################## 5 | # Find dependencies 6 | ############################################################################## 7 | 8 | set(THIS_PACKAGE_ROS_DEPS cmake_modules sensor_msgs roscpp pluginlib angles tf laser_geometry) 9 | 10 | find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_ROS_DEPS}) 11 | find_package(Boost REQUIRED COMPONENTS system signals) 12 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 13 | 14 | ############################################################################## 15 | # Define package 16 | ############################################################################## 17 | 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | LIBRARIES laser_scan_filters 21 | CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS} 22 | DEPENDS scan_to_scan_filter_chain 23 | ) 24 | 25 | ############################################################################## 26 | # Build 27 | ############################################################################## 28 | 29 | add_library(mod_laser_scan_filters src/laser_scan_filters.cpp) 30 | target_link_libraries(mod_laser_scan_filters ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 31 | 32 | 33 | ############################################################################## 34 | # Install 35 | ############################################################################## 36 | 37 | install(TARGETS mod_laser_scan_filters 38 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 40 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 41 | ) 42 | 43 | # Install headers 44 | install(DIRECTORY include/${PROJECT_NAME}/ 45 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 46 | 47 | install(FILES laser_filters_plugins.xml 48 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 49 | ) 50 | -------------------------------------------------------------------------------- /mod_laser_filters/config/approx_self_filter.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: self 3 | type: mod_laser_filters/ApproxSelfFilter 4 | params: 5 | radius: 0.7 6 | max_z: 0.2 7 | min_z: -0.3 8 | -------------------------------------------------------------------------------- /mod_laser_filters/include/laser_filters/approx_self_filter.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef APPROX_SELF_FILTER_H 3 | #define APPROX_SELF_FILTER_H 4 | 5 | #include 6 | 7 | #include "filters/filter_base.h" 8 | #include "sensor_msgs/LaserScan.h" 9 | #include "tf/transform_listener.h" 10 | #include "sensor_msgs/PointCloud.h" 11 | #include "ros/ros.h" 12 | #include "laser_geometry/laser_geometry.h" 13 | 14 | namespace mod_laser_filters 15 | { 16 | /** @b ApproxSelfFilter filters the robot/all points inside a cylinder in a laser scan line 17 | */ 18 | 19 | class ApproxSelfFilter : public filters::FilterBase 20 | { 21 | public: 22 | double laser_max_range_; // Used in laser scan projection 23 | double radius_, max_z_, min_z_; // parameters for selffilter 24 | 25 | //////////////////////////////////////////////////////////////////////////////// 26 | ApproxSelfFilter() : up_and_running_(false) 27 | { 28 | } 29 | 30 | /**@b Configure the filter from XML */ 31 | bool configure() 32 | { 33 | if (!filters::FilterBase::getParam(std::string("radius"), radius_)) 34 | { 35 | ROS_ERROR("Error: ApproxSelfFilter was not given radius.\n"); 36 | return false; 37 | } 38 | if (!filters::FilterBase::getParam(std::string("max_z"), max_z_)) 39 | { 40 | ROS_ERROR("Error: ApproxSelfFilter was not given max_z.\n"); 41 | return false; 42 | } 43 | if (!filters::FilterBase::getParam(std::string("min_z"), min_z_)) 44 | { 45 | ROS_ERROR("Error: ApproxSelfFilter was not given min_z.\n"); 46 | return false; 47 | } 48 | 49 | return true; 50 | } 51 | 52 | //////////////////////////////////////////////////////////////////////////////// 53 | virtual ~ApproxSelfFilter() 54 | { 55 | } 56 | 57 | //////////////////////////////////////////////////////////////////////////////// 58 | /** \brief Filter points inside a cylinder around the baselink 59 | * \param scan_in the input LaserScan message 60 | * \param scan_out the output LaserScan message 61 | */ 62 | bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan) 63 | { 64 | filtered_scan = input_scan; 65 | sensor_msgs::PointCloud laser_cloud; 66 | 67 | try 68 | { 69 | projector_.transformLaserScanToPointCloud("/base_link", input_scan, laser_cloud, tf_); 70 | } 71 | catch (tf::TransformException& ex) 72 | { 73 | if (up_and_running_) 74 | { 75 | ROS_WARN_THROTTLE(1, "Dropping Scan: Transform unavailable %s", ex.what()); 76 | } 77 | else 78 | { 79 | ROS_INFO_THROTTLE(.3, "Ignoring Scan: Waiting for TF"); 80 | } 81 | return false; 82 | } 83 | 84 | int c_idx = indexChannel(laser_cloud); 85 | 86 | if (c_idx == -1 || laser_cloud.channels[c_idx].values.size() == 0) 87 | { 88 | ROS_ERROR("We need an index channel to be able to filter out the footprint"); 89 | return false; 90 | } 91 | 92 | for (unsigned int i = 0; i < laser_cloud.points.size(); i++) 93 | { 94 | if (inFootprint(laser_cloud.points[i])) 95 | { 96 | int index = laser_cloud.channels[c_idx].values[i]; 97 | filtered_scan.ranges[index] = 98 | filtered_scan.range_max + 1.0; // If so, then make it bigger than max range 99 | } 100 | } 101 | 102 | up_and_running_ = true; 103 | return true; 104 | } 105 | 106 | int indexChannel(const sensor_msgs::PointCloud& scan_cloud) 107 | { 108 | int c_idx = -1; 109 | for (unsigned int d = 0; d < scan_cloud.channels.size(); d++) 110 | { 111 | if (scan_cloud.channels[d].name == "index") 112 | { 113 | c_idx = d; 114 | break; 115 | } 116 | } 117 | return c_idx; 118 | } 119 | 120 | bool inFootprint(const geometry_msgs::Point32& scan_pt) 121 | { 122 | if (sqrt(scan_pt.x * scan_pt.x + scan_pt.y * scan_pt.y) > radius_ || scan_pt.z > max_z_ || scan_pt.z < min_z_) 123 | return false; 124 | return true; 125 | } 126 | 127 | private: 128 | tf::TransformListener tf_; 129 | laser_geometry::LaserProjection projector_; 130 | double inscribed_radius_; 131 | bool up_and_running_; 132 | }; 133 | } 134 | 135 | #endif // APPROX_SELF_FILTER_H 136 | -------------------------------------------------------------------------------- /mod_laser_filters/include/laser_filters/scan_self_shadows_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008 Radu Bogdan Rusu 3 | * 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 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | * $Id: scan_shadows_filter.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $ 28 | * 29 | */ 30 | 31 | /* 32 | \author Radu Bogdan Rusu Tully Foote 33 | 34 | 35 | */ 36 | 37 | #ifndef LASER_SCAN_SELF_SHADOWS_FILTER_H 38 | #define LASER_SCAN_SELF_SHADOWS_FILTER_H 39 | 40 | #include 41 | 42 | #include "filters/filter_base.h" 43 | #include 44 | #include 45 | 46 | //#include 47 | //#include 48 | 49 | namespace mod_laser_filters 50 | { 51 | /** @b ScanShadowsFilter is a simple filter that filters shadow points in a laser scan line 52 | */ 53 | 54 | class ScanSelfShadowsFilter : public filters::FilterBase 55 | { 56 | public: 57 | double laser_max_range_; // Used in laser scan projection 58 | double min_angle_, max_angle_; // Filter angle threshold 59 | int window_, neighbors_; 60 | double self_radius_; 61 | double max_filter_radius_; 62 | 63 | // int seq_; 64 | // std::ofstream dist_f_; 65 | 66 | //////////////////////////////////////////////////////////////////////////////// 67 | ScanSelfShadowsFilter() : max_filter_radius_(std::numeric_limits::max()) 68 | //: seq_( 0 ) 69 | //, dist_f_( "/tmp/dist-filter.txt" ) 70 | { 71 | } 72 | 73 | /**@b Configure the filter from XML */ 74 | bool configure() 75 | { 76 | if (!filters::FilterBase::getParam(std::string("min_angle"), min_angle_)) 77 | { 78 | ROS_ERROR("Error: SelfShadowsFilter was not given min_angle.\n"); 79 | return false; 80 | } 81 | if (!filters::FilterBase::getParam(std::string("max_angle"), max_angle_)) 82 | { 83 | ROS_ERROR("Error: SelfShadowsFilter was not given min_angle.\n"); 84 | return false; 85 | } 86 | if (!filters::FilterBase::getParam(std::string("window"), window_)) 87 | { 88 | ROS_ERROR("Error: SelfShadowsFilter was not given window.\n"); 89 | return false; 90 | } 91 | neighbors_ = 0; // default value 92 | if (!filters::FilterBase::getParam(std::string("neighbors"), neighbors_)) 93 | { 94 | ROS_INFO("Error: SelfShadowsFilter was not given neighbors.\n"); 95 | } 96 | if (!filters::FilterBase::getParam(std::string("self_radius"), self_radius_)) 97 | { 98 | ROS_ERROR("Error: SelfShadowsFilter was not given self_radius.\n"); 99 | return false; 100 | } 101 | 102 | if (!filters::FilterBase::getParam(std::string("max_filter_radius"), max_filter_radius_)) 103 | { 104 | ROS_INFO("Error: SelfShadowsFilter was not max filter radius.\n"); 105 | } 106 | return true; 107 | } 108 | 109 | //////////////////////////////////////////////////////////////////////////////// 110 | virtual ~ScanSelfShadowsFilter() 111 | { 112 | } 113 | 114 | /** @brief calculate the perpendicular angle at the end of r1 to get to r2 115 | * See http://en.wikipedia.org/wiki/Law_of_cosines */ 116 | inline double getAngleWithViewpoint(float r1, float r2, float included_angle) 117 | { 118 | return atan2(r2 * sin(included_angle), r1 - r2 * cos(included_angle)); 119 | } 120 | 121 | //////////////////////////////////////////////////////////////////////////////// 122 | /** \brief Filter shadow points based on 3 global parameters: min_angle, max_angle 123 | * and window. {min,max}_angle specify the allowed angle interval (in degrees) 124 | * between the created lines (see getAngleWithViewPoint). Window specifies how many 125 | * consecutive measurements to take into account for one point. 126 | * \param scan_in the input LaserScan message 127 | * \param scan_out the output LaserScan message 128 | */ 129 | bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out) 130 | { 131 | // copy across all data first 132 | scan_out = scan_in; 133 | 134 | std::set indices_to_delete; 135 | // For each point in the current line scan 136 | for (unsigned int i = 0; i < scan_in.ranges.size(); i++) 137 | { 138 | if (scan_in.ranges[i] < self_radius_) 139 | { 140 | for (int y = -window_; y < window_ + 1; y++) 141 | { 142 | int j = i + y; 143 | if (j < 0 || j >= (int)scan_in.ranges.size() || (int)i == j) 144 | { // Out of scan bounds or itself 145 | continue; 146 | } 147 | 148 | double angle = abs(angles::to_degrees( 149 | getAngleWithViewpoint(scan_in.ranges[i], scan_in.ranges[j], y * scan_in.angle_increment))); 150 | if (angle < min_angle_ || angle > max_angle_) 151 | { 152 | for (int index = std::max(i - neighbors_, 0); 153 | index <= std::min(i + neighbors_, (int)scan_in.ranges.size() - 1); index++) 154 | { 155 | // if (scan_in.ranges[i] < scan_in.ranges[index]) // delete neighbor if they are farther away (note not 156 | //self) 157 | if (scan_in.ranges[index] < max_filter_radius_) 158 | indices_to_delete.insert(index); 159 | } 160 | } 161 | } 162 | } 163 | } 164 | 165 | ROS_DEBUG("ScanShadowsFilter removing %d Points from scan with min angle: %.2f, max angle: %.2f, neighbors: %d, " 166 | "and window: %d", 167 | (int)indices_to_delete.size(), min_angle_, max_angle_, neighbors_, window_); 168 | for (std::set::iterator it = indices_to_delete.begin(); it != indices_to_delete.end(); ++it) 169 | { 170 | scan_out.ranges[*it] = std::numeric_limits::quiet_NaN(); // Failed test to set the ranges to invalid value 171 | } 172 | return true; 173 | } 174 | 175 | //////////////////////////////////////////////////////////////////////////////// 176 | }; 177 | } 178 | 179 | #endif // LASER_SCAN_SELF_SHADOWS_FILTER_H 180 | -------------------------------------------------------------------------------- /mod_laser_filters/laser_filters_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | This is a filter which filters points from a laser scan that look like the veiling effect. 7 | 8 | 9 | 11 | 12 | This is a filter which filters points from a laser scan that look like the veiling effect. 13 | 14 | 15 | 17 | 18 | This is a filter which filters points on the robot (in a cylinder around the robot). 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mod_laser_filters/launch/approx_self_filter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /mod_laser_filters/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | This package is designed to provide filters for working with laser scans. 6 | 7 | \section filters Laser Scan Filters 8 | 9 | There are a number of filters that this class provides. There is also 10 | a factory class to provide easy creation and chaining of laser scan 11 | filters as a input to any process. 12 | 13 | \subsection median Median Filter 14 | laser_scan::LaserMedianFilter applies a median filter to scans both in range and intensity. 15 | 16 | \subsection mean Mean Filter 17 | \todo Document 18 | 19 | \subsection shadows Scan Shadows Filter 20 | \todo document 21 | 22 | \subsection intensity Intensity Filter 23 | \todo document 24 | 25 | */ 26 | -------------------------------------------------------------------------------- /mod_laser_filters/package.xml: -------------------------------------------------------------------------------- 1 | 2 | mod_laser_filters 3 | 4 | Assorted filters designed to operate on 2D planar laser scanners, 5 | which use the sensor_msgs/LaserScan type. 6 | 7 | 1.7.2 8 | Matthias Nieuwenhuisen 9 | Tully Foote 10 | BSD 11 | 12 | http://ros.org/wiki/laser_filters 13 | 14 | catkin 15 | 16 | cmake_modules 17 | sensor_msgs 18 | roscpp 19 | pluginlib 20 | angles 21 | tf 22 | laser_geometry 23 | 24 | cmake_modules 25 | sensor_msgs 26 | roscpp 27 | tf 28 | filters 29 | message_filters 30 | pluginlib 31 | angles 32 | laser_geometry 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /mod_laser_filters/src/laser_scan_filters.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "laser_filters/scan_shadows_filter.h" 31 | #include "laser_filters/scan_self_shadows_filter.h" 32 | #include "laser_filters/approx_self_filter.h" 33 | 34 | #include "sensor_msgs/LaserScan.h" 35 | #include "filters/filter_base.h" 36 | 37 | #include "pluginlib/class_list_macros.h" 38 | 39 | PLUGINLIB_EXPORT_CLASS(mod_laser_filters::ScanShadowsFilter, filters::FilterBase) 40 | PLUGINLIB_EXPORT_CLASS(mod_laser_filters::ScanSelfShadowsFilter, filters::FilterBase) 41 | PLUGINLIB_EXPORT_CLASS(mod_laser_filters::ApproxSelfFilter, filters::FilterBase) 42 | -------------------------------------------------------------------------------- /mrs_laser_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mrs_laser_mapping) 3 | 4 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 5 | 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") 7 | 8 | set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/Modules/" ${CMAKE_MODULE_PATH}) 9 | 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | sensor_msgs 14 | std_msgs 15 | pcl_conversions 16 | pcl_ros 17 | pcl_msgs 18 | laser_geometry 19 | tf 20 | geometry_msgs 21 | nodelet 22 | nav_msgs 23 | visualization_msgs 24 | tf_conversions 25 | cmake_modules 26 | message_generation 27 | config_server 28 | interactive_markers 29 | cloud_compression 30 | eigen_conversions 31 | mrs_laser_maps 32 | ) 33 | 34 | find_package(PCL REQUIRED) 35 | 36 | add_message_files(FILES 37 | MultiResolutionMap.msg 38 | KeyFrame.msg 39 | KeyFrameTransforms.msg 40 | ) 41 | 42 | add_service_files( 43 | FILES 44 | AddPointsToMap.srv 45 | #InterestPointsSwitchHand.srv 46 | #DeleteMarkedPoints.srv 47 | SelfFilter.srv 48 | AddKeyFramesByDistance.srv 49 | ) 50 | 51 | generate_messages( 52 | DEPENDENCIES 53 | std_msgs 54 | sensor_msgs 55 | geometry_msgs 56 | ) 57 | 58 | catkin_package( 59 | INCLUDE_DIRS include 60 | ) 61 | 62 | include_directories( 63 | include 64 | ${catkin_INCLUDE_DIRS} 65 | ${PCL_INCLUDE_DIRS} 66 | ) 67 | 68 | add_definitions(${PCL_DEFINITIONS}) 69 | 70 | #add_definitions(-DDEBUG_CELL_HITS) 71 | 72 | # boost required 73 | find_package(Boost REQUIRED COMPONENTS program_options thread system signals regex filesystem) 74 | include_directories(${Boost_INCLUDE_DIRS}) 75 | 76 | # G2O required 77 | find_package(G2O REQUIRED) 78 | include_directories(${G2O_INCLUDE_DIR}) 79 | 80 | # TBB required 81 | find_package(TBB REQUIRED) 82 | include_directories(${TBB_INCLUDE_DIRS}) 83 | 84 | # For building the CHOLMOD / CSPARSE solvers 85 | find_package(Cholmod) 86 | include_directories(${CHOLMOD_INCLUDE_DIR}) 87 | find_package(BLAS) 88 | find_package(LAPACK) 89 | find_package(CSparse) 90 | include_directories(${CSPARSE_INCLUDE_DIR}) 91 | 92 | find_package(mrs_laser_maps REQUIRED) 93 | 94 | #set the default path for built executables to the "bin" directory 95 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 96 | #set the default path for built libraries to the "lib" directory 97 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 98 | 99 | SET ( SRCS 100 | src/map_publisher.cpp 101 | ) 102 | 103 | SET ( SLAM_SRCS 104 | src/slam_graph.cpp 105 | src/map_publisher.cpp 106 | ) 107 | 108 | SET (SLAM_LIBS 109 | ${G2O_CORE_LIBRARY} 110 | ${G2O_STUFF_LIBRARY} 111 | ${G2O_SOLVER_CSPARSE} 112 | ${G2O_SOLVER_CSPARSE_EXTENSION} 113 | ${G2O_TYPES_SLAM3D} 114 | ) 115 | 116 | add_library(scan_assembler_nodelet src/nodes/scan_assembler_nodelet.cpp) 117 | target_link_libraries(scan_assembler_nodelet ${catkin_LIBRARIES}) 118 | add_dependencies(scan_assembler_nodelet 119 | ${PROJECT_NAME}_generate_messages_cpp 120 | ) 121 | 122 | add_library(map_nodelet src/nodes/map_nodelet.cpp ${SRCS} ) 123 | target_link_libraries(map_nodelet ${mrs_laser_maps_LIBRARIES} ${TBB_LIBRARIES} ${catkin_LIBRARIES}) 124 | add_dependencies(map_nodelet 125 | ${PROJECT_NAME}_generate_messages_cpp 126 | ) 127 | 128 | add_library(slam_nodelet src/nodes/slam_nodelet.cpp ${SLAM_SRCS}) 129 | target_link_libraries(slam_nodelet ${SLAM_LIBS} ${mrs_laser_maps_LIBRARIES} ${TBB_LIBRARIES} ${catkin_LIBRARIES} ) 130 | add_dependencies(slam_nodelet 131 | ${PROJECT_NAME}_generate_messages_cpp 132 | ) 133 | 134 | add_library(slam_visualizer_nodelet src/nodes/slam_visualizer_nodelet.cpp ) 135 | target_link_libraries(slam_visualizer_nodelet ${catkin_LIBRARIES}) 136 | add_dependencies(slam_visualizer_nodelet 137 | ${PROJECT_NAME}_generate_messages_cpp 138 | ) 139 | -------------------------------------------------------------------------------- /mrs_laser_mapping/cmake/Modules/FindCSparse.cmake: -------------------------------------------------------------------------------- 1 | # Look for csparse; note the difference in the directory specifications! 2 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 3 | PATHS 4 | /usr/include/suitesparse 5 | /usr/include 6 | /opt/local/include 7 | /usr/local/include 8 | /sw/include 9 | /usr/include/ufsparse 10 | /opt/local/include/ufsparse 11 | /usr/local/include/ufsparse 12 | /sw/include/ufsparse 13 | ${G2O_ROOT}/include/EXTERNAL/csparse 14 | ) 15 | 16 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 17 | PATHS 18 | /usr/lib 19 | /usr/local/lib 20 | /opt/local/lib 21 | /sw/lib 22 | ) 23 | 24 | include(FindPackageHandleStandardArgs) 25 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 26 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 27 | -------------------------------------------------------------------------------- /mrs_laser_mapping/cmake/Modules/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDE_DIR 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ufsparse 17 | ) 18 | 19 | find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 20 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) 21 | 22 | if(CHOLMOD_LIBRARIES) 23 | 24 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 25 | 26 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 27 | if (AMD_LIBRARY) 28 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 29 | else () 30 | set(CHOLMOD_LIBRARIES FALSE) 31 | endif () 32 | 33 | endif(CHOLMOD_LIBRARIES) 34 | 35 | if(CHOLMOD_LIBRARIES) 36 | 37 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 38 | if (COLAMD_LIBRARY) 39 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 40 | else () 41 | set(CHOLMOD_LIBRARIES FALSE) 42 | endif () 43 | 44 | endif(CHOLMOD_LIBRARIES) 45 | 46 | if(CHOLMOD_LIBRARIES) 47 | 48 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 49 | if (CAMD_LIBRARY) 50 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 51 | else () 52 | set(CHOLMOD_LIBRARIES FALSE) 53 | endif () 54 | 55 | endif(CHOLMOD_LIBRARIES) 56 | 57 | if(CHOLMOD_LIBRARIES) 58 | 59 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 60 | if (CCOLAMD_LIBRARY) 61 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 62 | else () 63 | set(CHOLMOD_LIBRARIES FALSE) 64 | endif () 65 | 66 | endif(CHOLMOD_LIBRARIES) 67 | 68 | if(CHOLMOD_LIBRARIES) 69 | 70 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 71 | if (CHOLMOD_METIS_LIBRARY) 72 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 73 | endif () 74 | 75 | endif(CHOLMOD_LIBRARIES) 76 | 77 | include(FindPackageHandleStandardArgs) 78 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 79 | CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 80 | 81 | mark_as_advanced(CHOLMOD_LIBRARIES) 82 | -------------------------------------------------------------------------------- /mrs_laser_mapping/cmake/Modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /mrs_laser_mapping/cmake/Modules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | /usr/local/include 7 | /usr/include 8 | /opt/local/include 9 | /opt/ros/indigo/include 10 | /sw/local/include 11 | /sw/include 12 | NO_DEFAULT_PATH 13 | ) 14 | 15 | # Macro to unify finding both the debug and release versions of the 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 17 | # macro. 18 | 19 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 20 | 21 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 22 | NAMES "g2o_${MYLIBRARYNAME}_d" 23 | PATHS 24 | ${G2O_ROOT}/lib/Debug 25 | ${G2O_ROOT}/lib 26 | $ENV{G2O_ROOT}/lib/Debug 27 | $ENV{G2O_ROOT}/lib 28 | NO_DEFAULT_PATH 29 | ) 30 | 31 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 32 | NAMES "g2o_${MYLIBRARYNAME}_d" 33 | PATHS 34 | ~/Library/Frameworks 35 | /Library/Frameworks 36 | /usr/local/lib 37 | /usr/local/lib64 38 | /usr/lib 39 | /usr/lib64 40 | /opt/local/lib 41 | /sw/local/lib 42 | /sw/lib 43 | ) 44 | 45 | FIND_LIBRARY(${MYLIBRARY} 46 | NAMES "g2o_${MYLIBRARYNAME}" 47 | PATHS 48 | ${G2O_ROOT}/lib/Release 49 | ${G2O_ROOT}/lib 50 | $ENV{G2O_ROOT}/lib/Release 51 | $ENV{G2O_ROOT}/lib 52 | NO_DEFAULT_PATH 53 | ) 54 | 55 | FIND_LIBRARY(${MYLIBRARY} 56 | NAMES "g2o_${MYLIBRARYNAME}" 57 | PATHS 58 | ~/Library/Frameworks 59 | /Library/Frameworks 60 | /usr/local/lib 61 | /usr/local/lib64 62 | /usr/lib 63 | /usr/lib64 64 | /opt/local/lib 65 | /sw/local/lib 66 | /sw/lib 67 | ) 68 | 69 | IF(NOT ${MYLIBRARY}_DEBUG) 70 | IF(MYLIBRARY) 71 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 72 | ENDIF(MYLIBRARY) 73 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 74 | 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 76 | 77 | # Find the core elements 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 80 | 81 | # Find the CLI library 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 83 | 84 | # Find the pluggable solvers 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 93 | 94 | # Find the predefined types 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 102 | 103 | # G2O solvers declared found if we found at least one solver 104 | SET(G2O_SOLVERS_FOUND "NO") 105 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 106 | SET(G2O_SOLVERS_FOUND "YES") 107 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 108 | 109 | # G2O itself declared found if we found the core libraries and at least one solver 110 | SET(G2O_FOUND "NO") 111 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 112 | SET(G2O_FOUND "YES") 113 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 114 | -------------------------------------------------------------------------------- /mrs_laser_mapping/cmake/Modules/FindGSL.cmake: -------------------------------------------------------------------------------- 1 | # Try to find gnu scientific library GSL 2 | # See 3 | # http://www.gnu.org/software/gsl/ and 4 | # http://gnuwin32.sourceforge.net/packages/gsl.htm 5 | # 6 | # Based on a script of Felix Woelk and Jan Woetzel 7 | # (www.mip.informatik.uni-kiel.de) 8 | # 9 | # It defines the following variables: 10 | # GSL_FOUND - system has GSL lib 11 | # GSL_INCLUDE_DIRS - where to find headers 12 | # GSL_LIBRARIES - full path to the libraries 13 | # GSL_LIBRARY_DIRS, the directory where the PLplot library is found. 14 | 15 | # CMAKE_GSL_CXX_FLAGS = Unix compiler flags for GSL, essentially "`gsl-config --cxxflags`" 16 | # GSL_LINK_DIRECTORIES = link directories, useful for rpath on Unix 17 | # GSL_EXE_LINKER_FLAGS = rpath on Unix 18 | 19 | set( GSL_FOUND OFF ) 20 | set( GSL_CBLAS_FOUND OFF ) 21 | 22 | # Windows, but not for Cygwin and MSys where gsl-config is available 23 | if( WIN32 AND NOT CYGWIN AND NOT MSYS ) 24 | # look for headers 25 | find_path( GSL_INCLUDE_DIR 26 | NAMES gsl/gsl_cdf.h gsl/gsl_randist.h 27 | ) 28 | if( GSL_INCLUDE_DIR ) 29 | # look for gsl library 30 | find_library( GSL_LIBRARY 31 | NAMES gsl 32 | ) 33 | if( GSL_LIBRARY ) 34 | set( GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR} ) 35 | get_filename_component( GSL_LIBRARY_DIRS ${GSL_LIBRARY} PATH ) 36 | set( GSL_FOUND ON ) 37 | endif( GSL_LIBRARY ) 38 | 39 | # look for gsl cblas library 40 | find_library( GSL_CBLAS_LIBRARY 41 | NAMES gslcblas 42 | ) 43 | if( GSL_CBLAS_LIBRARY ) 44 | set( GSL_CBLAS_FOUND ON ) 45 | endif( GSL_CBLAS_LIBRARY ) 46 | 47 | set( GSL_LIBRARIES ${GSL_LIBRARY} ${GSL_CBLAS_LIBRARY} ) 48 | endif( GSL_INCLUDE_DIR ) 49 | 50 | mark_as_advanced( 51 | GSL_INCLUDE_DIR 52 | GSL_LIBRARY 53 | GSL_CBLAS_LIBRARY 54 | ) 55 | else( WIN32 AND NOT CYGWIN AND NOT MSYS ) 56 | if( UNIX OR MSYS ) 57 | find_program( GSL_CONFIG_EXECUTABLE gsl-config 58 | /usr/bin/ 59 | /usr/local/bin 60 | ) 61 | 62 | if( GSL_CONFIG_EXECUTABLE ) 63 | set( GSL_FOUND ON ) 64 | 65 | # run the gsl-config program to get cxxflags 66 | execute_process( 67 | COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --cflags 68 | OUTPUT_VARIABLE GSL_CFLAGS 69 | RESULT_VARIABLE RET 70 | ERROR_QUIET 71 | ) 72 | if( RET EQUAL 0 ) 73 | string( STRIP "${GSL_CFLAGS}" GSL_CFLAGS ) 74 | separate_arguments( GSL_CFLAGS ) 75 | 76 | # parse definitions from cflags; drop -D* from CFLAGS 77 | string( REGEX MATCHALL "-D[^;]+" 78 | GSL_DEFINITIONS "${GSL_CFLAGS}" ) 79 | string( REGEX REPLACE "-D[^;]+;" "" 80 | GSL_CFLAGS "${GSL_CFLAGS}" ) 81 | 82 | # parse include dirs from cflags; drop -I prefix 83 | string( REGEX MATCHALL "-I[^;]+" 84 | GSL_INCLUDE_DIRS "${GSL_CFLAGS}" ) 85 | string( REPLACE "-I" "" 86 | GSL_INCLUDE_DIRS "${GSL_INCLUDE_DIRS}") 87 | string( REGEX REPLACE "-I[^;]+;" "" 88 | GSL_CFLAGS "${GSL_CFLAGS}") 89 | 90 | message("GSL_DEFINITIONS=${GSL_DEFINITIONS}") 91 | message("GSL_INCLUDE_DIRS=${GSL_INCLUDE_DIRS}") 92 | message("GSL_CFLAGS=${GSL_CFLAGS}") 93 | else( RET EQUAL 0 ) 94 | set( GSL_FOUND FALSE ) 95 | endif( RET EQUAL 0 ) 96 | 97 | # run the gsl-config program to get the libs 98 | execute_process( 99 | COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --libs 100 | OUTPUT_VARIABLE GSL_LIBRARIES 101 | RESULT_VARIABLE RET 102 | ERROR_QUIET 103 | ) 104 | if( RET EQUAL 0 ) 105 | string(STRIP "${GSL_LIBRARIES}" GSL_LIBRARIES ) 106 | separate_arguments( GSL_LIBRARIES ) 107 | 108 | # extract linkdirs (-L) for rpath (i.e., LINK_DIRECTORIES) 109 | string( REGEX MATCHALL "-L[^;]+" 110 | GSL_LIBRARY_DIRS "${GSL_LIBRARIES}" ) 111 | string( REPLACE "-L" "" 112 | GSL_LIBRARY_DIRS "${GSL_LIBRARY_DIRS}" ) 113 | else( RET EQUAL 0 ) 114 | set( GSL_FOUND FALSE ) 115 | endif( RET EQUAL 0 ) 116 | 117 | MARK_AS_ADVANCED( 118 | GSL_CFLAGS 119 | ) 120 | message( STATUS "Using GSL from ${GSL_PREFIX}" ) 121 | else( GSL_CONFIG_EXECUTABLE ) 122 | message( STATUS "FindGSL: gsl-config not found.") 123 | endif( GSL_CONFIG_EXECUTABLE ) 124 | endif( UNIX OR MSYS ) 125 | endif( WIN32 AND NOT CYGWIN AND NOT MSYS ) 126 | 127 | if( GSL_FOUND ) 128 | if( NOT GSL_FIND_QUIETLY ) 129 | message( STATUS "FindGSL: Found both GSL headers and library" ) 130 | endif( NOT GSL_FIND_QUIETLY ) 131 | else( GSL_FOUND ) 132 | if( GSL_FIND_REQUIRED ) 133 | message( FATAL_ERROR "FindGSL: Could not find GSL headers or library" ) 134 | endif( GSL_FIND_REQUIRED ) 135 | endif( GSL_FOUND ) 136 | 137 | -------------------------------------------------------------------------------- /mrs_laser_mapping/cmake/Modules/FindSuiteSparse.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h 2 | PATHS 3 | ${SUITE_SPARSE_ROOT}/include 4 | /usr/include/suitesparse 5 | /usr/include/ufsparse 6 | /opt/local/include/ufsparse 7 | /usr/local/include/ufsparse 8 | /sw/include/ufsparse 9 | ) 10 | 11 | FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod 12 | PATHS 13 | ${SUITE_SPARSE_ROOT}/lib 14 | /usr/lib 15 | /usr/local/lib 16 | /opt/local/lib 17 | /sw/lib 18 | ) 19 | 20 | FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd 21 | PATHS 22 | ${SUITE_SPARSE_ROOT}/lib 23 | /usr/lib 24 | /usr/local/lib 25 | /opt/local/lib 26 | /sw/lib 27 | ) 28 | 29 | FIND_LIBRARY(CAMD_LIBRARY NAMES camd 30 | PATHS 31 | ${SUITE_SPARSE_ROOT}/lib 32 | /usr/lib 33 | /usr/local/lib 34 | /opt/local/lib 35 | /sw/lib 36 | ) 37 | 38 | # Different platforms seemingly require linking against different sets of libraries 39 | IF(CYGWIN) 40 | FIND_PACKAGE(PkgConfig) 41 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 42 | PATHS 43 | /usr/lib 44 | /usr/local/lib 45 | /opt/local/lib 46 | /sw/lib 47 | ) 48 | PKG_CHECK_MODULES(LAPACK lapack) 49 | 50 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) 51 | 52 | # MacPorts build of the SparseSuite requires linking against extra libraries 53 | 54 | ELSEIF(APPLE) 55 | 56 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 57 | PATHS 58 | /usr/lib 59 | /usr/local/lib 60 | /opt/local/lib 61 | /sw/lib 62 | ) 63 | 64 | FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd 65 | PATHS 66 | /usr/lib 67 | /usr/local/lib 68 | /opt/local/lib 69 | /sw/lib 70 | ) 71 | 72 | FIND_LIBRARY(METIS_LIBRARY NAMES metis 73 | PATHS 74 | /usr/lib 75 | /usr/local/lib 76 | /opt/local/lib 77 | /sw/lib 78 | ) 79 | 80 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") 81 | ELSE(APPLE) 82 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) 83 | ENDIF(CYGWIN) 84 | 85 | IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 86 | SET(CHOLMOD_FOUND TRUE) 87 | ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 88 | SET(CHOLMOD_FOUND FALSE) 89 | ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 90 | 91 | # Look for csparse; note the difference in the directory specifications! 92 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 93 | PATHS 94 | /usr/include/suitesparse 95 | /usr/include 96 | /opt/local/include 97 | /usr/local/include 98 | /sw/include 99 | /usr/include/ufsparse 100 | /opt/local/include/ufsparse 101 | /usr/local/include/ufsparse 102 | /sw/include/ufsparse 103 | ) 104 | 105 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 106 | PATHS 107 | /usr/lib 108 | /usr/local/lib 109 | /opt/local/lib 110 | /sw/lib 111 | ) 112 | 113 | IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 114 | SET(CSPARSE_FOUND TRUE) 115 | ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 116 | SET(CSPARSE_FOUND FALSE) 117 | ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 118 | -------------------------------------------------------------------------------- /mrs_laser_mapping/config/config_mav_inventairy.yaml: -------------------------------------------------------------------------------- 1 | mapping_nodelet: 2 | assemble_half_scans: 1 3 | median_filter: 1 4 | occ_clamping_thresh_max: 3.5 5 | occ_clamping_thresh_min: -2 6 | occ_prob_hit: 0.85 7 | occ_prob_miss: -0.4 8 | occ_update: 1 9 | shadow_filter_cutoff: 1.5 10 | show_self_filter_markers: 0 11 | torso_cutoff_max_x: 0.65 12 | torso_cutoff_max_y: 0.65 13 | torso_cutoff_min_x: -0.65 14 | torso_cutoff_min_y: -0.65 15 | refine_local_map: 0 16 | -------------------------------------------------------------------------------- /mrs_laser_mapping/config/config_velodyne.yaml: -------------------------------------------------------------------------------- 1 | mapping_nodelet: 2 | assemble_half_scans: 1 3 | median_filter: 1 4 | occ_clamping_thresh_max: 3.5 5 | occ_clamping_thresh_min: -2 6 | occ_prob_hit: 0.85 7 | occ_prob_miss: -0.4 8 | occ_update: 1 9 | shadow_filter_cutoff: 1.5 10 | show_self_filter_markers: 0 11 | torso_cutoff_max_x: 0.65 12 | torso_cutoff_max_y: 0.65 13 | torso_cutoff_min_x: -0.65 14 | torso_cutoff_min_y: -0.65 15 | refine_local_map: 0 16 | -------------------------------------------------------------------------------- /mrs_laser_mapping/config/laser_shadow_filter.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: shadows 3 | type: mod_laser_filters/ScanShadowsFilter 4 | params: 5 | min_angle: 0.5 6 | max_angle: 179.5 7 | neighbors: 2 8 | window: 1 9 | max_filter_radius: 3.0 10 | - name: self 11 | type: mod_laser_filters/ApproxSelfFilter 12 | params: 13 | radius: 1.2 14 | max_z: 1.5 15 | min_z: -1.5 -------------------------------------------------------------------------------- /mrs_laser_mapping/config/self_filter_frames.yaml: -------------------------------------------------------------------------------- 1 | filter_frames: 2 | - frame_id: "/base_link" 3 | radius: 0.5 4 | # - frame_id: "/front_right_ankle_link" 5 | # radius: 0.15 6 | # - frame_id: "/front_left_ankle_link" 7 | # radius: 0.15 8 | # - frame_id: "/front_right_foot_link" 9 | # radius: 0.15 10 | # - frame_id: "/front_left_foot_link" 11 | # radius: 0.15 12 | # - frame_id: "/left_arm_hand_link" 13 | # radius: 0.25 14 | # - frame_id: "/left_arm_elbow_pitch_link" 15 | # radius: 0.25 16 | # - frame_id: "/right_arm_hand_link" 17 | # radius: 0.25 18 | # - frame_id: "/right_arm_elbow_pitch_link" 19 | # radius: 0.25 20 | -------------------------------------------------------------------------------- /mrs_laser_mapping/include/map_nodelet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _MAP_NODELET_H_ 38 | #define _MAP_NODELET_H_ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | #include 60 | 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | 69 | namespace mrs_laser_mapping 70 | { 71 | 72 | class MapNodelet : public nodelet::Nodelet 73 | { 74 | public: 75 | 76 | typedef PointXYZRGBScanLabel MapPointType; 77 | typedef PointXYZScanLine InputPointType; 78 | 79 | typedef mrs_laser_maps::MultiResolutionalMap MapType; 80 | 81 | 82 | MapNodelet(); 83 | virtual ~MapNodelet(); 84 | virtual void onInit(); 85 | 86 | bool clearMapServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 87 | bool resetServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 88 | bool decreaseOnceServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 89 | bool addPointsToMapServiceCall(AddPointsToMapRequest& req, AddPointsToMapResponse& res); 90 | 91 | void receivedCloud(const sensor_msgs::PointCloud2ConstPtr& msg); 92 | 93 | protected: 94 | void processScans(); 95 | 96 | void registerScan(pcl::PointCloud::Ptr cloud); // TODO 97 | 98 | void broadcastTf(); 99 | void updateTransforms(ros::Time time); 100 | 101 | bool checkTorsoRotation(ros::Time time); 102 | 103 | bool getTranform(const std::string& target_frame, const std::string& source_frame, ros::Time time, Eigen::Matrix4f& transform); 104 | bool getTranform(const std::string& target_frame, const std::string& source_frame, ros::Time time, tf::StampedTransform& transform); 105 | 106 | void clearMap(); 107 | 108 | private: 109 | message_filters::Subscriber sub_cloud_; 110 | boost::shared_ptr> cloud_message_filter_; 111 | 112 | ros::ServiceServer service_clear_map_; 113 | ros::ServiceServer service_reset_; 114 | ros::ServiceServer service_decrease_once_; 115 | ros::ServiceServer service_add_points_; 116 | 117 | tf::TransformListener tf_listener_; 118 | tf::TransformBroadcaster tf_broadcaster_; 119 | 120 | volatile bool running_; 121 | bool first_scan_; 122 | int scan_number_; 123 | 124 | int map_size_; 125 | int map_levels_; 126 | int map_cell_capacity_; 127 | double map_resolution_; 128 | 129 | int map_downsampled_size_; 130 | int map_downsampled_levels_; 131 | int map_downsampled_cell_capacity_; 132 | double map_downsampled_resolution_; 133 | 134 | std::string scan_assembler_frame_id_; 135 | const std::string map_frame_id_ = "base_link_oriented"; 136 | std::string sensor_frame_id_; 137 | 138 | int num_scans_registration_; 139 | int num_scans_for_map_publishing_; 140 | 141 | bool add_new_points_; 142 | double transform_wait_duration_; 143 | 144 | boost::shared_ptr< MapType > multiresolution_map_; 145 | 146 | boost::mutex mutex_map_; 147 | boost::mutex mutex_correction_transform_; 148 | 149 | Eigen::Matrix4d correction_transform_; 150 | Eigen::Matrix4d map_orientation_; 151 | 152 | tf::StampedTransform last_base_link_transform_; 153 | tf::StampedTransform last_base_link_oriented_transform_; 154 | 155 | ros::Time last_scan_stamp_; 156 | ros::Time last_scan_received_stamp_; 157 | ros::Duration scan_stamp_delta_; 158 | 159 | double decrease_rate_; 160 | bool decrease_once_; 161 | 162 | config_server::Parameter param_update_occupancy_; 163 | config_server::Parameter param_clamping_thresh_min_; 164 | config_server::Parameter param_clamping_thresh_max_; 165 | config_server::Parameter param_prob_hit_; 166 | config_server::Parameter param_prob_miss_; 167 | 168 | bool add_scans_when_torso_rotated_; 169 | double last_torso_yaw_; 170 | double const TORSO_YAW_THRESH = (M_PI / 180.0); 171 | 172 | mrs_laser_maps::synchronized_circular_buffer::Ptr> scan_buffer_; 173 | boost::shared_ptr process_scan_thread_; 174 | boost::shared_ptr tf_broadcaster_thread_; 175 | 176 | mrs_laser_maps::MultiResolutionSurfelRegistration surfel_registration_; 177 | 178 | }; 179 | } 180 | 181 | #endif 182 | -------------------------------------------------------------------------------- /mrs_laser_mapping/include/mrs_laser_mapping/color_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _COLOR_UTILS_H_ 38 | #define _COLOR_UTILS_H_ 39 | 40 | namespace mrs_laser_mapping 41 | { 42 | class ColorMapJet 43 | { 44 | public: 45 | static inline double red(double gray) 46 | { 47 | return base(gray - 0.5); 48 | } 49 | static inline double green(double gray) 50 | { 51 | return base(gray); 52 | } 53 | static inline double blue(double gray) 54 | { 55 | return base(gray + 0.5); 56 | } 57 | 58 | static inline double base(double val) 59 | { 60 | if (val <= -0.75) 61 | return 0; 62 | else if (val <= -0.25) 63 | return interpolate(val, 0.0, -0.75, 1.0, -0.25); 64 | else if (val <= 0.25) 65 | return 1.0; 66 | else if (val <= 0.75) 67 | return interpolate(val, 1.0, 0.25, 0.0, 0.75); 68 | else 69 | return 0.0; 70 | } 71 | 72 | static void getRainbowColor(float value, float& r, float& g, float& b) 73 | { 74 | // this is HSV color palette with hue values going only from 0.0 to 0.833333. 75 | value = std::min(value, 1.0f); 76 | value = std::max(value, 0.0f); 77 | 78 | float color[3]; 79 | 80 | float h = value * 5.0f + 1.0f; 81 | int i = floor(h); 82 | float f = h - i; 83 | if (!(i & 1)) 84 | f = 1 - f; // if i is even 85 | float n = 1 - f; 86 | 87 | if (i <= 1) 88 | color[0] = n, color[1] = 0, color[2] = 1; 89 | else if (i == 2) 90 | color[0] = 0, color[1] = n, color[2] = 1; 91 | else if (i == 3) 92 | color[0] = 0, color[1] = 1, color[2] = n; 93 | else if (i == 4) 94 | color[0] = n, color[1] = 1, color[2] = 0; 95 | else if (i >= 5) 96 | color[0] = 1, color[1] = n, color[2] = 0; 97 | 98 | r = color[0]; 99 | g = color[1]; 100 | b = color[2]; 101 | } 102 | 103 | private: 104 | static inline double interpolate(double val, double y0, double x0, double y1, double x1) 105 | { 106 | return (val - x0) * (y1 - y0) / (x1 - x0) + y0; 107 | } 108 | }; 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /mrs_laser_mapping/include/mrs_laser_mapping/graph_publisher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _GRAPH_PUBLISHER_H_ 38 | #define _GRAPH_PUBLISHER_H_ 39 | 40 | #include 41 | #include 42 | 43 | // 44 | // #include 45 | // #include 46 | 47 | #include 48 | 49 | #include 50 | 51 | 52 | #include 53 | 54 | #include 55 | #include 56 | 57 | namespace mrs_laser_mapping 58 | { 59 | 60 | template 61 | class GraphPublisher 62 | { 63 | public: 64 | 65 | typedef boost::shared_ptr MapPtr; 66 | 67 | typedef typename pcl::PointCloud PointCloud; 68 | typedef typename PointCloud::Ptr PointCloudPtr; 69 | 70 | typedef mrs_laser_mapping::SlamGraph GraphType; 71 | typedef GraphType::Ptr GraphPtr; 72 | GraphPublisher(); 73 | 74 | ~GraphPublisher(); 75 | 76 | static GraphPublisher* getInstance(); 77 | 78 | void publishOdometryGraph(GraphPtr graph, nav_msgs::Odometry odometry_msg ); 79 | 80 | void publishSLAMGraph(GraphPtr graph); 81 | 82 | void publishGraph(GraphPtr graph); 83 | 84 | void resetNodeCounter() 85 | { 86 | node_counter_ = 0; 87 | } 88 | 89 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 90 | 91 | protected: 92 | 93 | private: 94 | static GraphPublisher* instance_; 95 | 96 | ros::Publisher pub_slam_graph_marker_; 97 | ros::Publisher pub_map_; 98 | ros::Publisher pub_map_downsampled_; 99 | ros::Publisher pub_reference_keyframe_; 100 | ros::Publisher pub_keyframes_; 101 | ros::Publisher pub_keyframe_transforms_; 102 | ros::Publisher pub_odometry_; 103 | ros::Publisher pub_local_map_; 104 | 105 | std::string frame_id_slam_map_; 106 | std::string frame_id_odometry_; 107 | 108 | uint32_t run_id_; 109 | 110 | int node_counter_; 111 | 112 | std::vector odometry_msgs_; 113 | 114 | ros::Time start_time_; 115 | 116 | typedef cloud_compression::OctreePointCloudCompression Compression; 117 | }; 118 | } 119 | 120 | #include 121 | 122 | #endif 123 | -------------------------------------------------------------------------------- /mrs_laser_mapping/include/mrs_laser_mapping/map_publisher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _MAP_PUBLISHER_H_ 38 | #define _MAP_PUBLISHER_H_ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | 49 | 50 | namespace mrs_laser_mapping 51 | { 52 | class MapPublisher 53 | { 54 | public: 55 | MapPublisher(); 56 | 57 | ~MapPublisher(); 58 | 59 | static MapPublisher* getInstance(); 60 | 61 | template 62 | void publishOccupiedCells(const boost::shared_ptr& map); 63 | 64 | template 65 | void publishCellsWithOccupancy(const boost::shared_ptr& map); 66 | 67 | template 68 | void publishMap(const boost::shared_ptr& map); 69 | 70 | template 71 | void publishMapLevelColor(const boost::shared_ptr& map); 72 | 73 | template 74 | void publishMapScanColor(const boost::shared_ptr& map); 75 | 76 | private: 77 | static MapPublisher* instance_; 78 | 79 | ros::NodeHandle node_handle_; 80 | ros::Publisher pub_marker_; 81 | ros::Publisher pub_map_msg_; 82 | ros::Publisher pub_cloud_level_color_; 83 | ros::Publisher pub_cloud_scan_color_; 84 | }; 85 | } 86 | 87 | #include 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /mrs_laser_mapping/include/mrs_laser_mapping/slam_graph.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: David Droeschel (droeschel@ais.uni-bonn.de), Jörg Stückler 36 | */ 37 | 38 | #ifndef SLAM_GRAPH_H_ 39 | #define SLAM_GRAPH_H_ 40 | 41 | #include 42 | 43 | #include 44 | 45 | #include 46 | #include 47 | 48 | #include "g2o/core/sparse_optimizer.h" 49 | 50 | #include "g2o/types/slam3d/types_slam3d.h" 51 | 52 | #include "g2o/core/block_solver.h" 53 | #include "g2o/solvers/csparse/linear_solver_csparse.h" 54 | #include "g2o/solvers/cholmod/linear_solver_cholmod.h" 55 | 56 | typedef g2o::BlockSolver> SlamBlockSolver; 57 | typedef g2o::LinearSolverCSparse SlamLinearSolver; 58 | typedef g2o::LinearSolverCholmod SlamLinearCholmodSolver; 59 | typedef std::tr1::unordered_map VertexIDMap; 60 | typedef std::set EdgeSet; 61 | 62 | #include 63 | 64 | namespace mrs_laser_mapping 65 | { 66 | 67 | class GraphNode 68 | { 69 | public: 70 | 71 | typedef boost::shared_ptr MapPtr; 72 | 73 | typedef boost::shared_ptr< GraphNode > Ptr; 74 | typedef boost::shared_ptr< GraphNode const> ConstPtr; 75 | 76 | GraphNode() 77 | { 78 | sum_log_likelihood_ = 0.0; 79 | num_edges_ = 0.0; 80 | } 81 | ~GraphNode() 82 | { 83 | } 84 | 85 | MapPtr map_; 86 | unsigned int node_id_; 87 | 88 | double sum_log_likelihood_; 89 | double num_edges_; 90 | 91 | private: 92 | }; 93 | 94 | class SlamGraph 95 | { 96 | public: 97 | typedef boost::shared_ptr Ptr; 98 | 99 | typedef boost::shared_ptr MapPtr; 100 | 101 | typedef GraphNode::Ptr GraphNodePtr; 102 | typedef GraphNode::ConstPtr GraphNodeConstPtr; 103 | 104 | SlamGraph(); 105 | ~SlamGraph(); 106 | 107 | void setRegistrationParameters(const mrs_laser_maps::RegistrationParameters& params); 108 | 109 | unsigned int addKeyFrame(unsigned int v_prev_id, GraphNodePtr& keyFrame, 110 | const Eigen::Matrix4d& transform); 111 | 112 | bool addEdge(unsigned int v1_id, unsigned int v2_id); 113 | bool addEdge(unsigned int v1_id, unsigned int v2_id, const Eigen::Matrix4d& transform_guess); 114 | bool addEdge(unsigned int v1_id, unsigned int v2_id, const Eigen::Matrix4d& transform, 115 | const Eigen::Matrix& covariance); 116 | 117 | bool poseIsClose(const Eigen::Matrix4d& transform); 118 | bool poseIsFar(const Eigen::Matrix4d& transform); 119 | 120 | bool update(MapPtr view); 121 | bool setPose(const Eigen::Matrix4d& pose_update); 122 | 123 | void connectClosePoses(bool random = false); 124 | 125 | bool refineEdge(g2o::EdgeSE3* edge, float register_start_resolution, float register_stop_resolution); 126 | void refine(unsigned int refineIterations, unsigned int optimizeIterations, float register_start_resolution, 127 | float register_stop_resolution); 128 | 129 | void dumpError(); 130 | 131 | g2o::SparseOptimizer* optimizer_; 132 | 133 | std::vector< GraphNodePtr > graph_nodes_; 134 | std::map< unsigned int, GraphNodePtr > graph_nodes_map_; 135 | 136 | unsigned int reference_node_id_; 137 | 138 | double pose_is_close_dist_; 139 | double pose_is_close_angle_; 140 | 141 | double pose_is_far_dist_; 142 | 143 | bool add_nodes_by_distance_; 144 | bool add_node_manual_; 145 | bool first_frame_; 146 | 147 | // wrt reference key frame pose 148 | Eigen::Matrix4d last_transform_; 149 | 150 | 151 | mrs_laser_maps::MultiResolutionSurfelRegistration surfel_registration_; 152 | 153 | public: 154 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 155 | 156 | private: 157 | }; 158 | } 159 | 160 | #endif 161 | -------------------------------------------------------------------------------- /mrs_laser_mapping/include/mrs_laser_mapping/surfelmap_publisher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _SURFEL_MAP_PUBLISHER_H_ 38 | #define _SURFEL_MAP_PUBLISHER_H_ 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | 51 | namespace mrs_laser_mapping 52 | { 53 | class SurfelMapPublisher 54 | { 55 | public: 56 | SurfelMapPublisher(); 57 | 58 | ~SurfelMapPublisher(); 59 | 60 | static SurfelMapPublisher* getInstance(); 61 | 62 | 63 | 64 | template 65 | void publishSurfelMarkers(const boost::shared_ptr& map); 66 | 67 | template 68 | void publishPointCloud(const boost::shared_ptr& map); 69 | 70 | template 71 | void publishDownsampledPointCloud(const boost::shared_ptr& map, int mapSizeDownsampled, 72 | double resolutionDownsampled, int levelsDownsampled, int cellCapacityDownsampled); 73 | 74 | void publishScenePointCloud(pcl::PointCloud::Ptr cloud); 75 | void publishTransformedScenePointCloud(pcl::PointCloud::Ptr cloud); 76 | 77 | template 78 | void publishCorrespondences(pcl::PointCloud::Ptr source, 79 | pcl::PointCloud::Ptr target, 80 | const boost::shared_ptr& map); 81 | 82 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 83 | 84 | protected: 85 | 86 | private: 87 | static SurfelMapPublisher* m_instance; 88 | 89 | ros::NodeHandle m_nodeHandle; 90 | 91 | ros::Publisher m_markerPublisher; 92 | 93 | ros::Publisher m_pointCloudPublisher; 94 | ros::Publisher m_pointCloudDownsampledPublisher; 95 | 96 | ros::Publisher m_scenePointCloudPublisher; 97 | ros::Publisher m_scenePointCloudTransformedPublisher; 98 | 99 | ros::Publisher m_correspondencePublisher; 100 | 101 | unsigned int m_lastSurfelMarkerCount; 102 | }; 103 | } 104 | 105 | #include 106 | 107 | #endif 108 | -------------------------------------------------------------------------------- /mrs_laser_mapping/include/mrs_laser_mapping/trajectory_publisher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _TRAJECTORY_PUBLISHER_H_ 38 | #define _TRAJECTORY_PUBLISHER_H_ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #include 48 | 49 | #include 50 | #include 51 | 52 | using namespace std; 53 | 54 | namespace mrs_laser_mapping 55 | { 56 | class TrajectoryPublisher 57 | { 58 | public: 59 | TrajectoryPublisher(); 60 | // Jan : Parameter hinzugefügt für Parallelisierung 61 | TrajectoryPublisher(int bagPlayID); 62 | 63 | ~TrajectoryPublisher(); 64 | 65 | static TrajectoryPublisher* getInstance(); 66 | 67 | // Jan : Parameter hinzugefügt für Parallelisierung 68 | static TrajectoryPublisher* getInstance(int bagPlayID); 69 | 70 | void publishTrajectoryPoint(float x, float y, float z, float q_x, float q_y, float q_z, float q_w, ros::Time stamp, 71 | std::string frameId, std::string childFrameId); 72 | 73 | void publishTrajectoryPoint(float x, float y, float z, float roll, float pitch, float yaw, ros::Time stamp, 74 | std::string frameId, std::string childFrameId); 75 | 76 | void publishTrajectoryPoint(float x, float y, float z, tf::Quaternion q, ros::Time stamp, std::string frameId, 77 | std::string childFrameId); 78 | 79 | void publishTrajectoryPoint(tf::Transform transform, ros::Time stamp, std::string frameId, std::string childFrameId, 80 | const Eigen::Matrix& poseCov = Eigen::Matrix::Zero()); 81 | 82 | void publishTrajectoryPoint(Eigen::Matrix4f transform, ros::Time stamp, std::string frameId, std::string childFrameId, 83 | const Eigen::Matrix& poseCov = Eigen::Matrix::Zero()); 84 | 85 | void publishOdometryMsg(tf::Transform transform, ros::Time stamp, std::string frameId, std::string childFrameId, 86 | const Eigen::Matrix& poseCov = Eigen::Matrix::Zero()); 87 | 88 | Eigen::Matrix4f getMatrixForTf(const tf::TransformListener& tfListener, const std::string& target_frame, 89 | const std::string& source_frame, const ros::Time& time); 90 | 91 | void publishTransform(tf::Transform transform, ros::Time stamp, std::string frameId, std::string childFrameId); 92 | 93 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 94 | protected: 95 | inline float euclDist(const pcl::PointXYZ& p1, const pcl::PointXYZ& p2) 96 | { 97 | return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z)); 98 | } 99 | 100 | private: 101 | static TrajectoryPublisher* m_instance; 102 | 103 | ros::NodeHandle m_nodeHandle; 104 | 105 | tf::TransformBroadcaster m_transformBroadcaster; 106 | 107 | tf::TransformListener m_transformListener; 108 | 109 | ros::Publisher m_odometryPublisher; 110 | 111 | std::map> m_transformsMap; 112 | 113 | std::map>> m_covariancesMap; 114 | 115 | std::map m_odometryPublisherMap; 116 | 117 | std::map m_markerPublisherMap; 118 | 119 | std::map m_fileStreamMap; 120 | 121 | std::ofstream m_visOdomDistPlot; 122 | std::ofstream m_laserDistPlot; 123 | }; 124 | } 125 | 126 | #endif 127 | -------------------------------------------------------------------------------- /mrs_laser_mapping/include/scan_assembler_nodelet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _SCAN_ASSEMBLER_NODELET_H_ 38 | #define _SCAN_ASSEMBLER_NODELET_H_ 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | 58 | #include 59 | #include 60 | 61 | namespace mrs_laser_mapping 62 | { 63 | class ScanAssemblerNodelet : public nodelet::Nodelet 64 | { 65 | public: 66 | typedef pcl::PointXYZ PointT; 67 | typedef PointXYZScanLine OutputPointType; 68 | ScanAssemblerNodelet(); 69 | virtual ~ScanAssemblerNodelet(); 70 | virtual void onInit(); 71 | 72 | //! Save new scan lines to scan_line_buffer_ 73 | /*! 74 | \param msg a laser scan. 75 | */ 76 | void receivedLaserScan(const sensor_msgs::LaserScanConstPtr& msg); 77 | 78 | protected: 79 | //! Accumulate scan lines to generate 360 degrees point clouds 80 | /*! 81 | Transforms scan lines from the scan_line_buffer_ to point clouds in the frame_id. 82 | Accumulates those point clouds until the laser scanner made a 360 degrees rotation and then publishes this point 83 | cloud. 84 | */ 85 | void processScans(); 86 | 87 | //! Check if a scan is complete 88 | /*! 89 | \param laser_angle the current angle of the scanner. 90 | \return true if laser_angle and last_laser_yaw_angle_ belong to different rotations 91 | */ 92 | bool isScanComplete(float laser_angle); 93 | 94 | private: 95 | // a fixed frame (something like the world frame), transforms between this frame and the base_link are important 96 | std::string frame_id_; 97 | 98 | ros::Duration wait_duration_; 99 | 100 | mrs_laser_maps::synchronized_circular_buffer scan_line_buffer_; 101 | 102 | bool is_running_; 103 | bool is_first_scan_line_; 104 | bool is_first_scan_; 105 | 106 | boost::thread process_scan_thread_; 107 | 108 | ros::Publisher scan_publisher_; 109 | ros::Time first_stamp_; 110 | 111 | tf::TransformListener tf_listener_; 112 | 113 | // TF synchronized subscriber for laser scans 114 | message_filters::Subscriber sub_laser_scan_; 115 | boost::shared_ptr> message_filter_laser_scan_; 116 | 117 | laser_geometry::LaserProjection scan_projector_; 118 | pcl::PointCloud::Ptr cloud_for_assembler_; 119 | 120 | double last_laser_yaw_angle_; 121 | 122 | std::string scan_header_frame_id_; 123 | 124 | bool half_rotation_; 125 | bool add_invalid_points_; 126 | bool scan_line_number_by_stamp_; 127 | }; 128 | } 129 | 130 | #endif 131 | -------------------------------------------------------------------------------- /mrs_laser_mapping/include/slam_nodelet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _SLAM_NODELET_H_ 38 | #define _SLAM_NODELET_H_ 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include 55 | #include 56 | #include 57 | 58 | #include 59 | 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | 66 | #include 67 | #include 68 | #include 69 | 70 | 71 | namespace mrs_laser_mapping 72 | { 73 | class SlamNodelet : public nodelet::Nodelet 74 | { 75 | public: 76 | typedef PointXYZRGBScanLabel MapPointType; 77 | typedef mrs_laser_maps::MultiResolutionalMap MapType; 78 | typedef SlamGraph::Ptr GraphPtr; 79 | 80 | SlamNodelet(); 81 | virtual ~SlamNodelet(); 82 | virtual void onInit(); 83 | 84 | void initSLAMGraph(); 85 | 86 | bool addKeyFrameServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 87 | bool addKeyFramesByDistanceServiceCall(mrs_laser_mapping::AddKeyFramesByDistance::Request& req, 88 | mrs_laser_mapping::AddKeyFramesByDistance::Response& res); 89 | bool clearSlamGraphServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 90 | bool resendKeyframesServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 91 | 92 | void processMapQueue(); 93 | void receivedMap(const mrs_laser_mapping::MultiResolutionMapConstPtr& msg); 94 | void update(MapType::Ptr map); 95 | 96 | void receivedPoseUpdate(const geometry_msgs::PoseStamped& msg); 97 | 98 | void broadcastTf(); 99 | 100 | bool getTranform(const std::string& target_frame, const std::string& source_frame, ros::Time time, Eigen::Matrix4d& transform); 101 | bool getTranform(const std::string& target_frame, const std::string& source_frame, ros::Time time, tf::StampedTransform& transform); 102 | 103 | protected: 104 | 105 | 106 | private: 107 | volatile bool is_running_; 108 | 109 | boost::shared_ptr tf_broadcaster_thread_; 110 | boost::mutex correction_transform_mutex_; 111 | 112 | ros::Subscriber sub_map_; 113 | ros::Subscriber sub_pose_update_; 114 | 115 | ros::ServiceServer service_clear_slam_graph_; 116 | ros::ServiceServer service_add_keyframe_; 117 | ros::ServiceServer service_add_keyframes_by_distance_; 118 | ros::ServiceServer service_resend_keyframes_; 119 | 120 | tf::TransformListener tf_listener_; 121 | tf::TransformBroadcaster tf_broadcaster_; 122 | 123 | double transform_wait_duration_; 124 | 125 | Eigen::Matrix4d last_base_link_oriented_transform_; 126 | tf::StampedTransform transform_world_corrected_slam_; 127 | 128 | std::string frame_id_slam_map_; 129 | std::string frame_id_odometry_; 130 | 131 | GraphPtr slam_; 132 | 133 | double pose_is_close_dist_; 134 | double pose_is_close_angle_; 135 | 136 | double pose_is_far_dist_; 137 | 138 | bool first_map_; 139 | bool add_vertex_by_distance_; 140 | 141 | ros::Time last_update_time_; 142 | 143 | mrs_laser_maps::synchronized_circular_buffer map_buffer_; 144 | boost::mutex map_buffer_mutex_; 145 | boost::shared_ptr process_map_thread_; 146 | }; 147 | } 148 | 149 | #endif 150 | -------------------------------------------------------------------------------- /mrs_laser_mapping/include/slam_visualizer_nodelet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _SLAM_VISUALIZER_NODELET_H_ 38 | #define _SLAM_VISUALIZER_NODELET_H_ 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | #include 46 | 47 | #include 48 | #include 49 | 50 | #include 51 | 52 | #include 53 | #include 54 | #include 55 | 56 | #include 57 | 58 | #include 59 | 60 | #include 61 | #include 62 | #include 63 | #include 64 | 65 | #include 66 | 67 | #include 68 | 69 | namespace mrs_laser_mapping 70 | { 71 | class SlamVisualizerNodelet : public nodelet::Nodelet 72 | { 73 | public: 74 | SlamVisualizerNodelet(); 75 | virtual ~SlamVisualizerNodelet(); 76 | virtual void onInit(); 77 | 78 | void receivedKeyFrame(const mrs_laser_mapping::KeyFrameConstPtr& msg); 79 | void receivedKeyFrameTransforms(const mrs_laser_mapping::KeyFrameTransformsConstPtr& msg); 80 | 81 | void processKeyframeQueue(); 82 | void processKeyframeTransformQueue(); 83 | 84 | void processKeyFrame(const mrs_laser_mapping::KeyFrameConstPtr& msg); 85 | void processKeyFrameTransforms(const mrs_laser_mapping::KeyFrameTransformsConstPtr& msg); 86 | 87 | protected: 88 | void HSVtoRGB(float h, float s, float v, float* r, float* g, float* b) 89 | { 90 | int i; 91 | float f, p, q, t; 92 | if (s == 0) 93 | { 94 | // achromatic (grey) 95 | *r = *g = *b = v; 96 | return; 97 | } 98 | h /= 60; // sector 0 to 5 99 | i = floor(h); 100 | f = h - i; // factorial part of h 101 | p = v * (1 - s); 102 | q = v * (1 - s * f); 103 | t = v * (1 - s * (1 - f)); 104 | switch (i) 105 | { 106 | case 0: 107 | *r = v; 108 | *g = t; 109 | *b = p; 110 | break; 111 | case 1: 112 | *r = q; 113 | *g = v; 114 | *b = p; 115 | break; 116 | case 2: 117 | *r = p; 118 | *g = v; 119 | *b = t; 120 | break; 121 | case 3: 122 | *r = p; 123 | *g = q; 124 | *b = v; 125 | break; 126 | case 4: 127 | *r = t; 128 | *g = p; 129 | *b = v; 130 | break; 131 | default: // case 5: 132 | *r = v; 133 | *g = p; 134 | *b = q; 135 | break; 136 | } 137 | } 138 | 139 | private: 140 | typedef pcl::PointXYZRGB PointType; 141 | typedef pcl::PointCloud PointCloud; 142 | typedef PointCloud::Ptr PointCloudPtr; 143 | 144 | volatile bool is_running_; 145 | 146 | ros::Subscriber key_frame_subscriber_; 147 | ros::Subscriber key_frame_transforms_subscriber_; 148 | 149 | ros::Publisher slam_graph_marker_publisher_; 150 | ros::Publisher map_publisher_; 151 | ros::Publisher map_downsampled_publisher_; 152 | ros::Publisher map_color_shading_publisher_; 153 | 154 | std::string slam_map_frame_id_; 155 | 156 | double voxel_leaf_size_; 157 | 158 | float filter_limit_min_x_; 159 | float filter_limit_max_x_; 160 | float filter_limit_min_y_; 161 | float filter_limit_max_y_; 162 | float filter_limit_min_z_; 163 | float filter_limit_max_z_; 164 | 165 | uint32_t run_id_; 166 | uint32_t key_frame_counter_; 167 | 168 | typedef cloud_compression::OctreePointCloudCompression Compression; 169 | Compression::Ptr compression_; 170 | 171 | std::vector key_frame_clouds_; 172 | std::vector key_frame_clouds_downsampled_; 173 | 174 | std::vector::Ptr> key_frame_clouds_downsampled_normals_; 175 | 176 | float min_z_; 177 | float max_z_; 178 | 179 | std::vector keyframe_transforms_; 180 | 181 | mrs_laser_maps::synchronized_circular_buffer keyframe_buffer_; 182 | boost::shared_ptr process_keyframe_thread_; 183 | 184 | mrs_laser_maps::synchronized_circular_buffer keyframe_transform_buffer_; 185 | boost::shared_ptr process_keyframe_transform_thread_; 186 | 187 | boost::mutex keyframe_mutex_; 188 | }; 189 | } 190 | 191 | #endif 192 | -------------------------------------------------------------------------------- /mrs_laser_mapping/launch/mav_mapping.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 554 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Global Map 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz/Grid 35 | Color: 160; 160; 164 36 | Enabled: false 37 | Line Style: 38 | Line Width: 0.03 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: false 50 | - Alpha: 1 51 | Autocompute Intensity Bounds: true 52 | Autocompute Value Bounds: 53 | Max Value: 2.83743 54 | Min Value: -1.6652 55 | Value: true 56 | Axis: Z 57 | Channel Name: intensity 58 | Class: rviz/PointCloud2 59 | Color: 255; 255; 255 60 | Color Transformer: AxisColor 61 | Decay Time: 0 62 | Enabled: false 63 | Invert Rainbow: false 64 | Max Color: 255; 255; 255 65 | Max Intensity: 4096 66 | Min Color: 0; 0; 0 67 | Min Intensity: 0 68 | Name: Local Map 69 | Position Transformer: XYZ 70 | Queue Size: 10 71 | Selectable: true 72 | Size (Pixels): 3 73 | Size (m): 0.01 74 | Style: Points 75 | Topic: /mapping_nodelet/pointcloud_cut 76 | Unreliable: false 77 | Use Fixed Frame: true 78 | Use rainbow: true 79 | Value: false 80 | - Class: rviz/Marker 81 | Enabled: true 82 | Marker Topic: /slam_visualizer_nodelet/slam_graph_marker 83 | Name: Graph 84 | Namespaces: 85 | points_and_lines: true 86 | Queue Size: 100 87 | Value: true 88 | - Alpha: 1 89 | Autocompute Intensity Bounds: true 90 | Autocompute Value Bounds: 91 | Max Value: 2.0917 92 | Min Value: -2.10396 93 | Value: true 94 | Axis: Z 95 | Channel Name: intensity 96 | Class: rviz/PointCloud2 97 | Color: 255; 0; 0 98 | Color Transformer: AxisColor 99 | Decay Time: 0 100 | Enabled: true 101 | Invert Rainbow: false 102 | Max Color: 255; 255; 255 103 | Max Intensity: 4096 104 | Min Color: 0; 0; 0 105 | Min Intensity: 0 106 | Name: Global Map 107 | Position Transformer: XYZ 108 | Queue Size: 10 109 | Selectable: true 110 | Size (Pixels): 3 111 | Size (m): 0.01 112 | Style: Points 113 | Topic: /slam_visualizer_nodelet/slam_map_downsampled 114 | Unreliable: false 115 | Use Fixed Frame: true 116 | Use rainbow: true 117 | Value: true 118 | - Class: rviz/Axes 119 | Enabled: true 120 | Length: 1 121 | Name: base_link 122 | Radius: 0.1 123 | Reference Frame: base_link 124 | Value: true 125 | - Class: rviz/Marker 126 | Enabled: false 127 | Marker Topic: /mapping_nodelet/occupied_cells_marker 128 | Name: Marker 129 | Namespaces: 130 | {} 131 | Queue Size: 100 132 | Value: false 133 | Enabled: true 134 | Global Options: 135 | Background Color: 48; 48; 48 136 | Fixed Frame: world_corrected_slam 137 | Frame Rate: 30 138 | Name: root 139 | Tools: 140 | - Class: rviz/Interact 141 | Hide Inactive Objects: true 142 | - Class: rviz/MoveCamera 143 | - Class: rviz/Select 144 | - Class: rviz/FocusCamera 145 | - Class: rviz/Measure 146 | - Class: rviz/SetInitialPose 147 | Topic: /initialpose 148 | - Class: rviz/SetGoal 149 | Topic: /move_base_simple/goal 150 | - Class: rviz/PublishPoint 151 | Single click: true 152 | Topic: /clicked_point 153 | Value: true 154 | Views: 155 | Current: 156 | Class: rviz/Orbit 157 | Distance: 29.766 158 | Enable Stereo Rendering: 159 | Stereo Eye Separation: 0.06 160 | Stereo Focal Distance: 1 161 | Swap Stereo Eyes: false 162 | Value: false 163 | Focal Point: 164 | X: -1.24874 165 | Y: 0.562861 166 | Z: -2.59748 167 | Name: Current View 168 | Near Clip Distance: 0.01 169 | Pitch: 1.5198 170 | Target Frame: 171 | Value: Orbit (rviz) 172 | Yaw: 5.59096 173 | Saved: ~ 174 | Window Geometry: 175 | Displays: 176 | collapsed: false 177 | Height: 846 178 | Hide Left Dock: false 179 | Hide Right Dock: false 180 | QMainWindow State: 000000ff00000000fd00000004000000000000013a000002b7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006901000005fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000035000002b7000000da01000005fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000035000002b7000000af01000005fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000040fc0100000002fb0000000800540069006d00650100000000000004b00000020701000005fb0000000800540069006d0065010000000000000450000000000000000000000261000002b700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 181 | Selection: 182 | collapsed: false 183 | Time: 184 | collapsed: false 185 | Tool Properties: 186 | collapsed: false 187 | Views: 188 | collapsed: false 189 | Width: 1200 190 | X: 712 191 | Y: 35 192 | -------------------------------------------------------------------------------- /mrs_laser_mapping/launch/momaro_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | filter_field_name: z 111 | filter_limit_min: -3.0 112 | filter_limit_max: 2.3 113 | filter_limit_negative: False 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | filter_field_name: z 122 | filter_limit_min: -3.0 123 | filter_limit_max: 2.3 124 | filter_limit_negative: False 125 | 126 | 127 | 128 | 129 | 130 | -------------------------------------------------------------------------------- /mrs_laser_mapping/launch/velodyne_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | filter_field_name: z 71 | filter_limit_min: -3.0 72 | filter_limit_max: 2.5 73 | filter_limit_negative: False 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | min_x: -2.0 83 | max_x: 2.0 84 | min_y: -1.0 85 | max_y: 1.0 86 | min_z: -1.0 87 | max_z: 1.0 88 | negative: True 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /mrs_laser_mapping/msg/KeyFrame.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # Random number identifying this run (e.g. when the sender is restarted, this 4 | # number changes) 5 | uint32 runID 6 | 7 | # ID of this keyframe (counter starting at 0) 8 | uint32 keyframeID 9 | 10 | # Compressed point cloud 11 | uint8[] compressedCloud 12 | -------------------------------------------------------------------------------- /mrs_laser_mapping/msg/KeyFrameTransforms.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # Random number identifying this run (e.g. when the sender is restarted, this 4 | # number changes) 5 | uint32 runID 6 | 7 | # Transform of the keyframe relative to /world_corrected_slam 8 | geometry_msgs/Transform[] transforms 9 | 10 | # start and end points of the graph edges (not same size as keyframe transforms) 11 | geometry_msgs/Point[] edgeStartPoints 12 | geometry_msgs/Point[] edgeEndPoints 13 | -------------------------------------------------------------------------------- /mrs_laser_mapping/msg/MultiResolutionMap.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # Number of levels of the map 4 | uint8 levels 5 | 6 | # Size of map in meters 7 | uint8 size_in_meters 8 | 9 | # Resolution of the map 10 | uint8 resolution 11 | 12 | # Number of points stored per cell 13 | int32 cell_capacity 14 | 15 | # Point cloud for each level 16 | sensor_msgs/PointCloud2[] cloud 17 | 18 | # Free cell centers 19 | geometry_msgs/Point[] free_cell_centers 20 | 21 | -------------------------------------------------------------------------------- /mrs_laser_mapping/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | scan assembler nodelet 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | map nodelet 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | slam nodelet 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | slam visalization nodelet 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /mrs_laser_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrs_laser_mapping 4 | 1.0.0 5 | 6 | mrs_laser_mapping 7 | 8 | David Droeschel 9 | Max Schwarz 10 | 11 | BSD 12 | 13 | http://ros.org/wiki/mrs_laser_mapping 14 | 15 | catkin 16 | 17 | roscpp 18 | sensor_msgs 19 | std_msgs 20 | tf 21 | libpcl-all-dev 22 | pcl_ros 23 | laser_geometry 24 | geometry_msgs 25 | nodelet 26 | nav_msgs 27 | visualization_msgs 28 | tf_conversions 29 | cmake_modules 30 | message_generation 31 | config_server 32 | cloud_compression 33 | eigen_conversions 34 | mrs_laser_maps 35 | 36 | message_runtime 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /mrs_laser_mapping/src/map_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #include 38 | 39 | namespace mrs_laser_mapping 40 | { 41 | MapPublisher* MapPublisher::instance_ = 0; 42 | 43 | MapPublisher::MapPublisher() : node_handle_("~") 44 | { 45 | pub_marker_ = node_handle_.advertise("occupied_cells_marker", 10); 46 | pub_map_msg_ = node_handle_.advertise("multires_map", 10); 47 | pub_cloud_level_color_ = node_handle_.advertise("pointcloud_level_colored", 10); 48 | pub_cloud_scan_color_ = node_handle_.advertise("pointcloud_scan_colored", 10); 49 | } 50 | 51 | MapPublisher::~MapPublisher() 52 | { 53 | } 54 | 55 | MapPublisher* MapPublisher::getInstance() 56 | { 57 | if (instance_ == 0) 58 | instance_ = new MapPublisher(); 59 | return instance_; 60 | } 61 | 62 | } 63 | -------------------------------------------------------------------------------- /mrs_laser_mapping/src/surfelmap_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #include 38 | 39 | #include 40 | 41 | namespace mrs_laser_mapping 42 | { 43 | SurfelMapPublisher* SurfelMapPublisher::m_instance = 0; 44 | 45 | SurfelMapPublisher::SurfelMapPublisher() : m_nodeHandle(), m_lastSurfelMarkerCount(0) 46 | { 47 | m_markerPublisher = m_nodeHandle.advertise("/surfel_map/surfels", 100); 48 | // Jan 49 | m_pointCloudPublisher = m_nodeHandle.advertise>("/surfel_map/pointcloud", 10); 50 | m_pointCloudDownsampledPublisher = 51 | m_nodeHandle.advertise>("/surfel_map/pointcloud_downsampled", 10); 52 | // m_pointCloudPublisher = m_nodeHandle.advertise("/surfel_map/pointcloud", 10); 53 | m_pointCloudCuttedPublisher = m_nodeHandle.advertise("/surfel_map/pointcloud_cut", 10); 54 | 55 | m_scenePointCloudPublisher = m_nodeHandle.advertise("/surfel_map/scene_pointcloud", 10); 56 | m_scenePointCloudTransformedPublisher = 57 | m_nodeHandle.advertise("/surfel_map/scene_pointcloud_transformed", 10); 58 | m_correspondencePublisher = m_nodeHandle.advertise("/surfel_map/correspondences", 10); 59 | } 60 | 61 | SurfelMapPublisher::~SurfelMapPublisher() 62 | { 63 | } 64 | 65 | SurfelMapPublisher* SurfelMapPublisher::getInstance() 66 | { 67 | if (m_instance == 0) 68 | m_instance = new SurfelMapPublisher(); 69 | return m_instance; 70 | } 71 | 72 | 73 | void SurfelMapPublisher::publishTransformedScenePointCloud(pcl::PointCloud::Ptr cloud) 74 | { 75 | m_scenePointCloudTransformedPublisher.publish(cloud); 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /mrs_laser_mapping/srv/AddKeyFramesByDistance.srv: -------------------------------------------------------------------------------- 1 | # automatically add key frames if turned on 2 | uint8 addKeyFrames 3 | --- 4 | 5 | -------------------------------------------------------------------------------- /mrs_laser_mapping/srv/AddPointsToMap.srv: -------------------------------------------------------------------------------- 1 | uint64 addPoints 2 | --- 3 | uint64 addPoints 4 | -------------------------------------------------------------------------------- /mrs_laser_mapping/srv/DeleteMarkedPoints.srv: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Point32 minPointToDelete 3 | geometry_msgs/Point32 maxPointToDelete 4 | 5 | --- 6 | -------------------------------------------------------------------------------- /mrs_laser_mapping/srv/InterestPointsSwitchHand.srv: -------------------------------------------------------------------------------- 1 | uint8 leftHand 2 | --- 3 | 4 | -------------------------------------------------------------------------------- /mrs_laser_mapping/srv/SelfFilter.srv: -------------------------------------------------------------------------------- 1 | time stamp 2 | sensor_msgs/PointCloud2 input_cloud 3 | --- 4 | sensor_msgs/PointCloud2 filtered_cloud 5 | -------------------------------------------------------------------------------- /mrs_laser_maps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mrs_laser_maps) 3 | 4 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 5 | 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") 7 | 8 | set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/Modules/" ${CMAKE_MODULE_PATH}) 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | cmake_modules 12 | eigen_conversions 13 | message_generation 14 | pcl_conversions 15 | roscpp 16 | rosunit 17 | ) 18 | 19 | find_package(PCL REQUIRED) 20 | 21 | catkin_package( 22 | INCLUDE_DIRS include 23 | LIBRARIES ${PROJECT_NAME} 24 | ) 25 | 26 | include_directories( 27 | include 28 | ${catkin_INCLUDE_DIRS} 29 | ${PCL_INCLUDE_DIRS} 30 | ) 31 | 32 | add_definitions(${PCL_DEFINITIONS}) 33 | 34 | # add_definitions(-DDEBUG_CELL_HITS) 35 | 36 | # boost required 37 | find_package(Boost REQUIRED COMPONENTS program_options thread system signals regex filesystem) 38 | include_directories(${Boost_INCLUDE_DIRS}) 39 | 40 | set (${PROJECT_NAME}_SRCS 41 | src/map_multiresolution.cpp 42 | src/multiresolution_surfel_registration.cpp 43 | ) 44 | 45 | add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SRCS} ) 46 | target_link_libraries(${PROJECT_NAME} ${TBB_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) 47 | 48 | -------------------------------------------------------------------------------- /mrs_laser_maps/include/mrs_laser_maps/grid_cell.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _GRID_CELL_H_ 38 | #define _GRID_CELL_H_ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | //#include 46 | 47 | //#define DEBUG_CELL_HITS 48 | 49 | namespace mrs_laser_maps 50 | { 51 | 52 | 53 | 54 | template > 55 | class GridCell 56 | { 57 | public: 58 | 59 | typedef typename boost::shared_ptr> Ptr; 60 | 61 | GridCell(int capacity = 100) 62 | : is_end_point_(false) 63 | , debug_state_(UNINITIALIZED) 64 | , points_(boost::make_shared(capacity)) 65 | , occupancy_(0.5f) 66 | { 67 | 68 | #ifdef DEBUG_CELL_HITS 69 | hits_ = 0; 70 | #endif 71 | } 72 | 73 | GridCell(const GridCell& cell) 74 | : is_end_point_(cell.is_end_point_) 75 | , debug_state_(cell.debug_state_) 76 | , points_(boost::make_shared(*cell.points_.get())) 77 | , occupancy_(cell.occupancy_) 78 | { 79 | 80 | #ifdef DEBUG_CELL_HITS 81 | hits_ = cell.hits_; 82 | #endif 83 | } 84 | 85 | 86 | virtual ~GridCell(){}; 87 | 88 | virtual inline float getOccupancy() 89 | { 90 | return occupancy_; 91 | } 92 | 93 | virtual inline void setOccupancy(const float &occupancy) 94 | { 95 | occupancy_ = occupancy; 96 | } 97 | 98 | virtual inline void substractOccupancy(const float &occupancy) 99 | { 100 | occupancy_ -= occupancy; 101 | } 102 | 103 | virtual inline void addOccupancy(const float &occupancy) 104 | { 105 | occupancy_ += occupancy; 106 | } 107 | 108 | virtual void addPoint(const PointType &p) 109 | { 110 | points_->push_front(p); 111 | #ifdef DEBUG_CELL_HITS 112 | hits_++; 113 | #endif 114 | } 115 | 116 | virtual inline void addPoint(const boost::shared_ptr &p) 117 | { 118 | } 119 | 120 | virtual void erasePoint(typename BufferType::iterator &it) 121 | { 122 | it = points_->erase(it); 123 | } 124 | 125 | virtual const boost::shared_ptr &getPoints() 126 | { 127 | return points_; 128 | } 129 | 130 | inline int getCellHits() 131 | { 132 | #ifdef DEBUG_CELL_HITS 133 | return hits_; 134 | #else 135 | return 0; 136 | #endif 137 | } 138 | 139 | bool is_end_point_; 140 | 141 | enum DebugState 142 | { 143 | OCCUPIED, 144 | FREE, 145 | UNKNOWN, 146 | UNINITIALIZED 147 | }; 148 | 149 | DebugState debug_state_; 150 | 151 | protected: 152 | boost::shared_ptr points_; 153 | float occupancy_; 154 | 155 | #ifdef DEBUG_CELL_HITS 156 | int hits_; 157 | #endif 158 | 159 | private: 160 | }; 161 | 162 | 163 | //template <> 164 | //inline void GridCell>::addPoint(const boost::shared_ptr& p) 165 | // { 166 | // points_->push_front(p); 167 | //#ifdef DEBUG_CELL_HITS 168 | // hits_++; 169 | //#endif 170 | // } 171 | 172 | // template <> 173 | // void GridCell>>::addPoint(const PointXYZRGBScanLabel& p) 174 | // { 175 | // points_->push_front(boost::make_shared(p)); 176 | // #ifdef DEBUG_CELL_HITS 177 | // hits_++; 178 | // #endif 179 | // } 180 | // 181 | // template <> 182 | // void GridCell>::addPoint(const PointXYZRGBScanLabel& p) 183 | // { 184 | // points_->push_front(p); 185 | // #ifdef DEBUG_CELL_HITS 186 | // hits_++; 187 | // #endif 188 | // } 189 | // 190 | // template <> 191 | // void GridCell>::addPoint(const pcl::PointXYZ& p) 192 | // { 193 | // points_->push_front(p); 194 | // #ifdef DEBUG_CELL_HITS 195 | // hits_++; 196 | // #endif 197 | // } 198 | 199 | 200 | } 201 | 202 | #endif 203 | -------------------------------------------------------------------------------- /mrs_laser_maps/include/mrs_laser_maps/grid_cell_with_statistics.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _GRID_CELL_WITH_STATISTICS_H_ 38 | #define _GRID_CELL_WITH_STATISTICS_H_ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | 50 | namespace mrs_laser_maps 51 | { 52 | 53 | template > 54 | class GridCellWithStatistics : public GridCell, public SurfelCellInterface 55 | { 56 | public: 57 | typedef typename boost::shared_ptr CircularBufferPtr; 58 | typedef typename BufferType::iterator CircularBufferIterator; 59 | 60 | GridCellWithStatistics(int capacity = 100) : GridCell(capacity) 61 | , not_added_(0) 62 | { 63 | } 64 | 65 | GridCellWithStatistics(const GridCellWithStatistics& cell) 66 | : GridCell(cell) 67 | , surfel_(cell.surfel_) 68 | , not_added_(cell.not_added_) 69 | { 70 | } 71 | 72 | ~GridCellWithStatistics() 73 | { 74 | } 75 | 76 | void evaluate() 77 | { 78 | mrs_laser_maps::Surfel surfel; 79 | CircularBufferPtr buffer = this->getPoints(); 80 | 81 | for (CircularBufferIterator it_cell = buffer->begin(); it_cell != buffer->end(); 82 | it_cell++) 83 | { 84 | Eigen::Matrix pos; 85 | pos(0) = (*it_cell).x; 86 | pos(1) = (*it_cell).y; 87 | pos(2) = (*it_cell).z; 88 | 89 | surfel.add(pos); 90 | } 91 | 92 | surfel_ += surfel; 93 | surfel_.evaluate(); 94 | } 95 | 96 | template ::value, int>::type = 0> 97 | void evaluate(unsigned int skipScan) 98 | { 99 | mrs_laser_maps::Surfel surfel; 100 | CircularBufferPtr buffer = this->getPoints(); 101 | 102 | for ( CircularBufferIterator it_cell = buffer->begin(); it_cell != buffer->end(); 103 | it_cell++) 104 | { 105 | if ((*it_cell).scanNr != skipScan) 106 | { 107 | Eigen::Matrix pos; 108 | pos(0) = (*it_cell).x; 109 | pos(1) = (*it_cell).y; 110 | pos(2) = (*it_cell).z; 111 | 112 | surfel.add(pos); 113 | } 114 | } 115 | 116 | surfel_ += surfel; 117 | surfel_.evaluate(); 118 | } 119 | 120 | void addPoint(const PointType &p) 121 | { 122 | GridCell::addPoint(p); 123 | surfel_.unevaluate(); 124 | } 125 | 126 | 127 | inline void addPoint(const boost::shared_ptr &p) 128 | { 129 | } 130 | 131 | /* void addPoint(const PointType &p) 132 | { 133 | if (surfel_.num_points_ > MIN_SURFEL_POINTS) 134 | { 135 | Eigen::Matrix point; 136 | point(0) = p.x; 137 | point(1) = p.y; 138 | point(2) = p.z; 139 | 140 | double mdist = sqrt((point - surfel_.mean_).transpose() * surfel_.cov_.inverse() * (point - surfel_.mean_)); 141 | if (mdist > 2.0) 142 | { 143 | ROS_INFO_STREAM_THROTTLE(0.25, "mahalanobis dist: " << mdist << " for point " << point << " and mean " << surfel_.mean_ << " with cov " << surfel_.cov_ << " not added " << not_added_); 144 | GridCell::addPoint(p); 145 | } 146 | else 147 | { 148 | not_added_++; 149 | } 150 | } 151 | else 152 | { 153 | GridCell::addPoint(p); 154 | } 155 | } 156 | */ 157 | 158 | 159 | inline bool getPointByLabel(unsigned int label, PointType& point ) 160 | { 161 | return false; 162 | } 163 | 164 | inline bool updateByLabel(const PointType& point ) 165 | { 166 | return false; 167 | } 168 | 169 | void erasePoint(CircularBufferIterator& it) 170 | { 171 | it = this->getPoints()->erase(it); 172 | surfel_.unevaluate(); 173 | } 174 | 175 | inline const mrs_laser_maps::Surfel& getSurfel() const 176 | { 177 | return surfel_; 178 | } 179 | 180 | Surfel surfel_; 181 | 182 | int not_added_; 183 | private: 184 | }; 185 | 186 | //template <> 187 | //inline void GridCellWithStatistics>::addPoint(const boost::shared_ptr& p) 188 | //{ 189 | // GridCell>::addPoint(p); 190 | // surfel_.unevaluate(); 191 | //} 192 | 193 | //template <> 194 | //inline bool GridCellWithStatistics>::getPointByLabel(unsigned int label, PointXYZRGBScanLabel& point ) 195 | //{ 196 | // 197 | // 198 | // return ( this->getPoints()->find(label, point) ); 199 | // 200 | //} 201 | 202 | //template <> 203 | //inline bool GridCellWithStatistics>::updateByLabel(const PointXYZRGBScanLabel& point ) 204 | //{ 205 | // return this->getPoints()->update(point); 206 | // 207 | //} 208 | 209 | } 210 | 211 | #endif 212 | -------------------------------------------------------------------------------- /mrs_laser_maps/include/mrs_laser_maps/map_point_types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _MAP_POINT_TYPES_H_ 38 | #define _MAP_POINT_TYPES_H_ 39 | 40 | #define PCL_NO_PRECOMPILE 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | 48 | struct PointXYZRGBScanLabel 49 | { 50 | PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding 51 | uint16_t scanlineNr; 52 | uint16_t scanNr; 53 | uint32_t pointNr; 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 55 | } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment 56 | 57 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZRGBScanLabel, 58 | (float, x, x)(float, y, y)(float, z, z)(uint16_t, scanlineNr, scanlineNr) 59 | (uint16_t, scanNr, scanNr)(uint32_t, pointNr, pointNr)) 60 | 61 | struct PointXYZScanLine 62 | { 63 | PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding 64 | uint16_t scanlineNr; 65 | uint32_t pointNr; 66 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 67 | } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment 68 | 69 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZScanLine, 70 | (float, x, x)(float, y, y)(float, z, z)(uint16_t, scanlineNr, 71 | scanlineNr)(uint32_t, pointNr, pointNr)) 72 | 73 | 74 | 75 | #endif -------------------------------------------------------------------------------- /mrs_laser_maps/include/mrs_laser_maps/surfel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _SURFEL_H_ 38 | #define _SURFEL_H_ 39 | 40 | #include 41 | 42 | namespace mrs_laser_maps 43 | { 44 | const unsigned int MAX_NUM_SURFELS = 6; 45 | const double MIN_SURFEL_POINTS = 10.0; 46 | const double MAX_SURFEL_POINTS = 10000.0; 47 | 48 | class Surfel 49 | { 50 | public: 51 | Surfel() 52 | { 53 | clear(); 54 | } 55 | 56 | Surfel(const Surfel& surfel) 57 | : first_view_dir_(surfel.first_view_dir_) 58 | , num_points_(surfel.num_points_) 59 | , curvature_(surfel.curvature_) 60 | , mean_(surfel.mean_) 61 | , sum_(surfel.sum_) 62 | , normal_(surfel.normal_) 63 | , cov_(surfel.cov_) 64 | , invcov_(surfel.invcov_) 65 | , sum_squares_(surfel.sum_squares_) 66 | , up_to_date_(surfel.up_to_date_) 67 | , evaluated_(surfel.evaluated_) 68 | { 69 | clear(); 70 | } 71 | 72 | ~Surfel() 73 | { 74 | } 75 | 76 | inline void clear() 77 | { 78 | num_points_ = 0.0; 79 | mean_.setZero(); 80 | cov_.setZero(); 81 | invcov_.setZero(); 82 | sum_.setZero(); 83 | sum_squares_.setZero(); 84 | first_view_dir_.setZero(); 85 | 86 | up_to_date_ = false; 87 | evaluated_ = false; 88 | } 89 | 90 | inline Surfel& operator+=(const Surfel& rhs) 91 | { 92 | if (rhs.num_points_ > 0 && num_points_ < MAX_SURFEL_POINTS) 93 | { 94 | // numerically stable one-pass update scheme 95 | if (num_points_ == 0) 96 | { 97 | sum_squares_ = rhs.sum_squares_; 98 | sum_ = rhs.sum_; 99 | num_points_ = rhs.num_points_; 100 | } 101 | else 102 | { 103 | const Eigen::Matrix deltaS = rhs.num_points_ * sum_ - num_points_ * rhs.sum_; 104 | sum_squares_ += 105 | rhs.sum_squares_ + 106 | 1.0 / (num_points_ * rhs.num_points_ * (rhs.num_points_ + num_points_)) * deltaS * deltaS.transpose(); 107 | sum_ += rhs.sum_; 108 | num_points_ += rhs.num_points_; 109 | } 110 | 111 | first_view_dir_ = rhs.first_view_dir_; 112 | up_to_date_ = false; 113 | } 114 | 115 | return *this; 116 | } 117 | 118 | inline void add(const Eigen::Matrix& point) 119 | { 120 | // numerically stable one-pass update scheme 121 | if (num_points_ < std::numeric_limits::epsilon()) 122 | { 123 | sum_ += point; 124 | num_points_ += 1.0; 125 | up_to_date_ = false; 126 | } 127 | else if (num_points_ < MAX_SURFEL_POINTS) 128 | { 129 | const Eigen::Matrix deltaS = (sum_ - num_points_ * point); 130 | sum_squares_ += 1.0 / (num_points_ * (num_points_ + 1.0)) * deltaS * deltaS.transpose(); 131 | sum_ += point; 132 | num_points_ += 1.0; 133 | up_to_date_ = false; 134 | } 135 | } 136 | 137 | inline void evaluate() 138 | { 139 | double det = 0.0; 140 | 141 | curvature_ = 1.0; 142 | 143 | if (num_points_ > 0) 144 | { 145 | mean_ = sum_ / num_points_; 146 | } 147 | 148 | if (num_points_ >= MIN_SURFEL_POINTS) 149 | { 150 | cov_ = sum_squares_ / (num_points_ - 1.0); 151 | 152 | // enforce symmetry.. 153 | cov_(1, 0) = cov_(0, 1); 154 | cov_(2, 0) = cov_(0, 2); 155 | cov_(2, 1) = cov_(1, 2); 156 | invcov_ = cov_.inverse(); 157 | } 158 | 159 | det = cov_.determinant(); 160 | 161 | // not enough points or surfel degenerate 162 | if (num_points_ < MIN_SURFEL_POINTS || det <= std::numeric_limits::epsilon()) 163 | { 164 | } 165 | else 166 | { 167 | Eigen::Matrix eigen_values_; 168 | Eigen::Matrix eigen_vectors_; 169 | 170 | // eigen vectors are stored in the columns 171 | pcl::eigen33(cov_, eigen_vectors_, eigen_values_); 172 | 173 | normal_ = eigen_vectors_.col(0); 174 | if (normal_.dot(first_view_dir_) > 0.0) 175 | normal_ *= -1.0; 176 | } 177 | 178 | up_to_date_ = true; 179 | evaluated_ = true; 180 | } 181 | 182 | inline void unevaluate() 183 | { 184 | evaluated_ = false; 185 | // if( num_points_ > 0.0 ) { 186 | // 187 | // mean_ *= num_points_; 188 | // cov_ *= (num_points_-1.0); 189 | // 190 | // unevaluated_ = true; 191 | // 192 | // } 193 | } 194 | 195 | Eigen::Matrix first_view_dir_; 196 | 197 | double num_points_; 198 | 199 | double curvature_; 200 | 201 | Eigen::Matrix mean_, sum_; 202 | Eigen::Matrix normal_; 203 | Eigen::Matrix cov_, invcov_, sum_squares_; 204 | bool up_to_date_, evaluated_; 205 | 206 | public: 207 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 208 | 209 | private: 210 | }; 211 | 212 | 213 | } 214 | 215 | #endif 216 | -------------------------------------------------------------------------------- /mrs_laser_maps/include/mrs_laser_maps/surfel_map_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _SURFEL_CELL_INTERFACE_H_ 38 | #define _SURFEL_CELL_INTERFACE_H_ 39 | 40 | #include 41 | 42 | #include 43 | 44 | namespace mrs_laser_maps 45 | { 46 | 47 | 48 | class SurfelCellInterface 49 | { 50 | public: 51 | 52 | virtual ~SurfelCellInterface(){}; 53 | 54 | virtual void evaluate() = 0; 55 | 56 | virtual const mrs_laser_maps::Surfel& getSurfel() const = 0; 57 | 58 | }; 59 | 60 | class SurfelMapInterface 61 | { 62 | public: 63 | 64 | typedef std::vector> AlignedCellVector; 65 | 66 | typedef std::vector CellPtrVector; 67 | 68 | 69 | virtual ~SurfelMapInterface() {}; 70 | 71 | virtual void evaluateAll() = 0; 72 | 73 | virtual void unEvaluateAll() = 0; 74 | 75 | virtual bool isEvaluated() const = 0; 76 | 77 | virtual unsigned int getLevels() const = 0; 78 | 79 | virtual double getResolution() const = 0; 80 | 81 | virtual double getCellSize(int level) const = 0; 82 | 83 | virtual double getCellSize() const = 0; 84 | 85 | virtual void getOccupiedCells(CellPtrVector& cells, int level) = 0; 86 | 87 | virtual void getOccupiedCellsWithOffset(CellPtrVector& cells, std::vector& offsets) = 0; 88 | 89 | virtual bool getCell(const Eigen::Vector3f& point, SurfelCellInterface*& cellPtr, Eigen::Vector3f& cellOffset, int& level) = 0; 90 | 91 | virtual bool getCell(const Eigen::Vector3f& point, CellPtrVector& cellPtrs, 92 | std::vector>& cellOffsets, 93 | std::vector& levels, int neighbors = 0, bool reverse = false) = 0; 94 | 95 | virtual unsigned int getNumCellPoints() = 0; 96 | 97 | virtual ros::Time getLastUpdateTimestamp() const = 0; 98 | protected: 99 | 100 | }; 101 | } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /mrs_laser_maps/include/mrs_laser_maps/synchronized_circular_buffer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #ifndef _SYNCHRONIZED_CIRCULAR_BUFFER_H_ 38 | #define _SYNCHRONIZED_CIRCULAR_BUFFER_H_ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include 51 | 52 | #include 53 | 54 | namespace mrs_laser_maps 55 | { 56 | template 57 | class synchronized_circular_buffer 58 | { 59 | public: 60 | typedef boost::circular_buffer container_type; 61 | typedef typename container_type::size_type size_type; 62 | typedef typename container_type::value_type value_type; 63 | // typedef typename boost::call_traits::param_type param_type; 64 | // 65 | explicit synchronized_circular_buffer(size_type capacity) : m_unread(0), m_container(capacity) 66 | { 67 | } 68 | 69 | void push_front(T item) 70 | { 71 | boost::mutex::scoped_lock lock(m_mutex); 72 | m_not_full.wait(lock, boost::bind(&synchronized_circular_buffer::is_not_full, this)); 73 | m_container.push_front(item); 74 | ++m_unread; 75 | lock.unlock(); 76 | m_not_empty.notify_one(); 77 | } 78 | 79 | void pop_back(T* pItem) 80 | { 81 | boost::mutex::scoped_lock lock(m_mutex); 82 | m_not_empty.wait(lock, boost::bind(&synchronized_circular_buffer::is_not_empty, this)); 83 | *pItem = m_container[--m_unread]; 84 | 85 | m_not_full.notify_one(); 86 | } 87 | 88 | T back() 89 | { 90 | boost::mutex::scoped_lock lock(m_mutex); 91 | 92 | return m_container.back(); 93 | } 94 | 95 | T front() 96 | { 97 | boost::mutex::scoped_lock lock(m_mutex); 98 | 99 | return m_container.front(); 100 | } 101 | 102 | void clear() 103 | { 104 | boost::mutex::scoped_lock lock(m_mutex); 105 | 106 | m_container.clear(); 107 | 108 | m_unread = 0; 109 | lock.unlock(); 110 | m_not_full.notify_one(); 111 | } 112 | 113 | size_t size() 114 | { 115 | return m_unread; 116 | } 117 | 118 | private: 119 | synchronized_circular_buffer(const synchronized_circular_buffer&); // Disabled copy constructor 120 | synchronized_circular_buffer& operator=(const synchronized_circular_buffer&); // Disabled assign operator 121 | 122 | bool is_not_empty() const 123 | { 124 | return m_unread > 0; 125 | } 126 | bool is_not_full() const 127 | { 128 | return m_unread < m_container.capacity(); 129 | } 130 | 131 | size_type m_unread; 132 | container_type m_container; 133 | boost::mutex m_mutex; 134 | boost::condition m_not_empty; 135 | boost::condition m_not_full; 136 | }; 137 | } 138 | 139 | #endif 140 | -------------------------------------------------------------------------------- /mrs_laser_maps/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrs_laser_maps 4 | 0.0.1 5 | 6 | mrs_laser_maps 7 | 8 | David Droeschel 9 | Max Schwarz 10 | 11 | BSD 12 | 13 | http://ros.org/wiki/MRSLaserMaps 14 | 15 | catkin 16 | 17 | roscpp 18 | 23 | 24 | rosunit 25 | cmake_modules 26 | message_generation 27 | eigen_conversions 28 | pcl_conversions 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /mrs_laser_maps/src/map_multiresolution.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Computer Science Institute VI, University of Bonn 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of University of Bonn, Computer Science Institute 18 | * VI nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #include 38 | 39 | // PCL_INSTANTIATE(mrs_laser_maps::MultiResolutionalMap, PCL_XYZ_POINT_TYPES); 40 | 41 | // template class PCL_EXPORTS mrs_laser_maps::MultiResolutionalMap; 42 | template class PCL_EXPORTS mrs_laser_maps::MultiResolutionalMap; 43 | template class PCL_EXPORTS mrs_laser_maps::MultiResolutionalMap; 44 | //template class PCL_EXPORTS mrs_laser_maps::MultiResolutionalMapRefineable; -------------------------------------------------------------------------------- /parameter_tuner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(parameter_tuner) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp rqt_gui rqt_gui_cpp config_server sensor_msgs) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | ## Uncomment this if the package has a setup.py. This macro ensures 13 | ## modules and scripts declared therein get installed 14 | # catkin_python_setup() 15 | 16 | find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) 17 | include(${QT_USE_FILE}) 18 | include_directories(${CMAKE_CURRENT_BINARY_DIR}) 19 | 20 | ####################################### 21 | ## Declare ROS messages and services ## 22 | ####################################### 23 | 24 | ## Generate messages in the 'msg' folder 25 | # add_message_files( 26 | # FILES 27 | # Message1.msg 28 | # Message2.msg 29 | # ) 30 | 31 | ## Generate services in the 'srv' folder 32 | # add_service_files( 33 | # FILES 34 | # Service1.srv 35 | # Service2.srv 36 | # ) 37 | 38 | ## Generate added messages and services with any dependencies listed here 39 | # generate_messages( 40 | # DEPENDENCIES 41 | # std_msgs 42 | # ) 43 | 44 | ################################################### 45 | ## Declare things to be passed to other projects ## 46 | ################################################### 47 | 48 | ## LIBRARIES: libraries you create in this project that dependent projects also need 49 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 50 | ## DEPENDS: system dependencies of this project that dependent projects also need 51 | catkin_package( 52 | INCLUDE_DIRS include 53 | # LIBRARIES parameter_tuner 54 | CATKIN_DEPENDS roscpp config_server 55 | # DEPENDS system_lib 56 | ) 57 | 58 | ########### 59 | ## Build ## 60 | ########### 61 | 62 | ## Specify additional locations of header files 63 | include_directories(include 64 | ${catkin_INCLUDE_DIRS} 65 | ) 66 | 67 | qt4_wrap_ui(UIFILES 68 | src/parametertuner.ui 69 | ) 70 | qt4_wrap_cpp(MOCFILES 71 | include/parametertuner.h 72 | include/parameteritem.h 73 | ) 74 | 75 | # Declare a cpp library 76 | add_library(parametertuner 77 | ${UIFILES} 78 | ${MOCFILES} 79 | src/parametertuner.cpp 80 | src/parameteritem.cpp 81 | ) 82 | 83 | ## Declare a cpp executable 84 | # add_executable(parameter_tuner 85 | # ${UIFILES} 86 | # ${MOCFILES} 87 | # src/parametertuner.cpp 88 | # ) 89 | 90 | ## Add dependencies to the executable 91 | # add_dependencies(parameter_tuner_node ${PROJECT_NAME}) 92 | 93 | ## Specify libraries to link a library or executable target against 94 | target_link_libraries(parametertuner 95 | ${catkin_LIBRARIES} 96 | ${QT_LIBRARIES} 97 | ) 98 | 99 | ############# 100 | ## Install ## 101 | ############# 102 | 103 | install(TARGETS parametertuner 104 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 105 | ) 106 | 107 | install(FILES plugin_rqt_gui.xml 108 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 109 | ) 110 | 111 | ## Mark executable scripts (Python etc.) for installation 112 | ## not required for python when using catkin_python_setup() 113 | # install(PROGRAMS 114 | # scripts/my_python_script 115 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 116 | # ) 117 | 118 | ## Mark executables and/or libraries for installation 119 | # install(TARGETS parameter_tuner parameter_tuner_node 120 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 121 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 122 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 123 | # ) 124 | 125 | ## Mark cpp header files for installation 126 | # install(DIRECTORY include/${PROJECT_NAME}/ 127 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 128 | # FILES_MATCHING PATTERN "*.h" 129 | # PATTERN ".svn" EXCLUDE 130 | # ) 131 | 132 | ## Mark other files for installation (e.g. launch and bag files, etc.) 133 | # install(FILES 134 | # # myfile1 135 | # # myfile2 136 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 137 | # ) 138 | 139 | ############# 140 | ## Testing ## 141 | ############# 142 | 143 | ## Add gtest based cpp test target and link libraries 144 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_parameter_tuner.cpp) 145 | # if(TARGET ${PROJECT_NAME}-test) 146 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 147 | # endif() 148 | 149 | ## Add folders to be run by python nosetests 150 | # catkin_add_nosetests(test) 151 | -------------------------------------------------------------------------------- /parameter_tuner/include/parameteritem.h: -------------------------------------------------------------------------------- 1 | //subclass for slider 2 | //Author: Sebastian Schüller 3 | 4 | #ifndef PARAMETERITEM_H 5 | #define PARAMETERITEM_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace parametertuner 15 | { 16 | 17 | class WheelFilter : public QObject 18 | { 19 | Q_OBJECT 20 | public: 21 | explicit WheelFilter(QObject* parent = 0); 22 | 23 | protected: 24 | virtual bool eventFilter(QObject* object, QEvent* event); 25 | }; 26 | 27 | 28 | 29 | class ParameterWidgetBase : public QWidget 30 | { 31 | Q_OBJECT 32 | public: 33 | virtual void IncValue() = 0; 34 | virtual void DecValue() = 0; 35 | }; 36 | 37 | class IntParameterWidget : public ParameterWidgetBase 38 | { 39 | Q_OBJECT 40 | public: 41 | explicit IntParameterWidget(ros::NodeHandle& nh, const config_server::ParameterDescription& description); 42 | virtual ~IntParameterWidget(); 43 | virtual void DecValue(); 44 | virtual void IncValue(); 45 | 46 | Q_SIGNALS: 47 | void called(int); 48 | 49 | private Q_SLOTS: 50 | void handleSlider(); 51 | void handleSpinbox(); 52 | void handleCallback(int value); 53 | 54 | private: 55 | config_server::Parameter m_parameter; 56 | 57 | QSlider* m_slider; 58 | QSpinBox* m_spinbox; 59 | }; 60 | 61 | 62 | class FloatParameterWidget : public ParameterWidgetBase 63 | { 64 | Q_OBJECT 65 | public: 66 | explicit FloatParameterWidget(ros::NodeHandle& nh, const config_server::ParameterDescription& description); 67 | virtual ~FloatParameterWidget(); 68 | virtual void DecValue(); 69 | virtual void IncValue(); 70 | 71 | Q_SIGNALS: 72 | void called(float); 73 | 74 | private Q_SLOTS: 75 | void handleSlider(); 76 | void handleSpinbox(); 77 | void handleCallback(float value); 78 | 79 | private: 80 | config_server::Parameter< float > m_parameter; 81 | 82 | double m_sliderStepRatio; 83 | QSlider* m_slider; 84 | QDoubleSpinBox* m_spinbox; 85 | }; 86 | 87 | 88 | class StringParameterWidget : public ParameterWidgetBase 89 | { 90 | Q_OBJECT 91 | public: 92 | explicit StringParameterWidget(ros::NodeHandle& nh, const config_server::ParameterDescription& description); 93 | virtual ~StringParameterWidget(); 94 | virtual void DecValue(); 95 | virtual void IncValue(); 96 | 97 | Q_SIGNALS: 98 | void called(std::string); 99 | 100 | private Q_SLOTS: 101 | void handleLineEdit(); 102 | void handleCallback(std::string text); 103 | 104 | private: 105 | config_server::Parameter< std::string > m_parameter; 106 | 107 | QLineEdit* m_lineEdit; 108 | }; 109 | 110 | class BoolParameterWidget : public ParameterWidgetBase 111 | { 112 | Q_OBJECT 113 | public: 114 | explicit BoolParameterWidget(ros::NodeHandle& nh, const config_server::ParameterDescription& description); 115 | virtual ~BoolParameterWidget(); 116 | virtual void DecValue(); 117 | virtual void IncValue(); 118 | 119 | Q_SIGNALS: 120 | void called(bool); 121 | 122 | private Q_SLOTS: 123 | void handleCheckbox(); 124 | void handleCallback(bool value); 125 | 126 | private: 127 | config_server::Parameter< bool > m_parameter; 128 | 129 | QCheckBox* m_checkBox; 130 | }; 131 | 132 | } 133 | 134 | #endif -------------------------------------------------------------------------------- /parameter_tuner/include/parametertuner.h: -------------------------------------------------------------------------------- 1 | //rqt plugin for tuning parameters 2 | //Author: Sebastian Schüller 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #ifndef PARAMETERTUNER_H 14 | #define PARAMETERTUNER_H 15 | 16 | namespace parametertuner 17 | { 18 | 19 | struct SavedTreeNode 20 | { 21 | QString name; 22 | bool selected; 23 | }; 24 | 25 | 26 | class Parametertuner : public rqt_gui_cpp::Plugin 27 | { 28 | Q_OBJECT 29 | public: 30 | Parametertuner(); 31 | virtual ~Parametertuner(); 32 | 33 | void handleList(const config_server::ParameterListConstPtr& list); 34 | void handleJoystickInput(const sensor_msgs::JoyConstPtr& joy); 35 | 36 | virtual void initPlugin(qt_gui_cpp::PluginContext& context); 37 | virtual void shutdownPlugin(); 38 | 39 | Q_SIGNALS: 40 | void updateRequested(); 41 | void moveSelectionRequested(int); 42 | void valueChangeRequested(int); 43 | void expansionRequested(int); 44 | 45 | public slots: 46 | void save(); 47 | void reset(); 48 | void update(); 49 | 50 | void handleJoystickButton(); 51 | void handleSearchClicked(); 52 | void handleEnterPressed(); 53 | 54 | QTreeWidgetItem* itemGetParent(const QTreeWidgetItem* item); 55 | bool itemHasParent(const QTreeWidgetItem* item); 56 | void moveSelection(int dir); 57 | void changeValue(int dir); 58 | void handleExpansion(int dir); 59 | 60 | private: 61 | void expandCollapseParents(QTreeWidgetItem *item, bool expand); // Recursively expands/collapses parents of 'item' 62 | 63 | void saveExpanded_and_Selected(QTreeWidgetItem *item); // Recursively saves all expanded children 64 | void restoreExpanded_and_Selected(); // Expands all previously saved (as expanded) items 65 | 66 | // Finds all items with 'text', expands the tree such as all items are visible, selects all found items 67 | void search(QString text); 68 | void unsearch(); // Removes all consequences of previous search (collapses and deselects all found items) 69 | 70 | QList > branches; 71 | void saveBranches(); 72 | void restoreBranches(); 73 | 74 | void getExpandedItems(QTreeWidgetItem* item, QList &list); 75 | void saveBranch(QTreeWidgetItem* item, QVector &branch); 76 | bool checkBranch(QTreeWidgetItem* item, const QVector &branch, int index); 77 | 78 | void restoreSelection(QTreeWidgetItem* item, const QVector &branch, int index); 79 | 80 | QList itemsSelected; 81 | QList itemsExpanded; 82 | 83 | QList itemsFound; 84 | bool isSearchTurn; // If true, search() is called, otherwise - unsearch() 85 | 86 | ros::NodeHandle m_nh; 87 | QMutex m_mutex; 88 | ros::Subscriber m_sub_paramList; 89 | config_server::ParameterListConstPtr m_list; 90 | 91 | Ui::parametertuner m_ui; 92 | 93 | ros::Subscriber m_sub_joystick; 94 | 95 | bool m_useJoystick; 96 | bool m_buttonUp; 97 | bool m_buttonDown; 98 | bool m_buttonInc; 99 | bool m_buttonDec; 100 | bool m_buttonUpLeft; 101 | 102 | ros::WallTimer m_updateTimer; 103 | int m_updateCounter; 104 | void handleUpdateTimer(); 105 | 106 | void insertParameter(QString param, QTreeWidgetItem* ancestor, const config_server::ParameterDescription& description); 107 | QTreeWidgetItem* insertParameterNode(const config_server::ParameterDescription& description, QTreeWidgetItem* ancestor); 108 | 109 | void updateSelection(QTreeWidgetItem* old, QTreeWidgetItem* next); 110 | QTreeWidgetItem* getSelectedItem(); 111 | 112 | void deleteWidget(QTreeWidgetItem* item); 113 | 114 | enum direction 115 | { 116 | UP = -1, 117 | DOWN = 1, 118 | LEFT = -1, 119 | RIGHT = 1, 120 | COLLAPSE = 0 121 | }; 122 | }; 123 | 124 | } 125 | #endif -------------------------------------------------------------------------------- /parameter_tuner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | parameter_tuner 4 | 0.0.0 5 | The parameter_tuner package 6 | 7 | 8 | 9 | 10 | Sebastian Schueller 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | catkin 41 | roscpp 42 | rqt_gui 43 | rqt_gui_cpp 44 | config_server 45 | sensor_msgs 46 | roscpp 47 | rqt_gui 48 | rqt_gui_cpp 49 | config_server 50 | sensor_msgs 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /parameter_tuner/plugin_rqt_gui.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Tool to set parameters 5 | 6 | 7 | 8 | hwinfo 9 | tip 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /parameter_tuner/src/parametertuner.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | parametertuner 4 | 5 | 6 | 7 | 0 8 | 0 9 | 691 10 | 663 11 | 12 | 13 | 14 | Parameter Tuner 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 100 26 | 0 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 0 36 | 0 37 | 38 | 39 | 40 | 41 | 70 42 | 0 43 | 44 | 45 | 46 | 47 | 70 48 | 16777215 49 | 50 | 51 | 52 | Search 53 | 54 | 55 | true 56 | 57 | 58 | 59 | 60 | 61 | 62 | Reset Config 63 | 64 | 65 | 66 | 67 | 68 | 69 | Save Config 70 | 71 | 72 | 73 | 74 | 75 | 76 | Joystick enabled 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 1 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | --------------------------------------------------------------------------------