├── .gitignore ├── README.txt ├── hector_compressed_map_transport ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── src │ └── map_to_image_node.cpp ├── hector_geotiff ├── CHANGELOG.rst ├── CMakeLists.txt ├── fonts │ ├── LICENSE.txt │ └── Roboto-Regular.ttf ├── include │ └── hector_geotiff │ │ ├── geotiff_writer.h │ │ ├── map_writer_interface.h │ │ └── map_writer_plugin_interface.h ├── maps │ └── .gitignore ├── package.xml └── src │ ├── geotiff_node.cpp │ ├── geotiff_saver.cpp │ └── geotiff_writer │ └── geotiff_writer.cpp ├── hector_geotiff_launch ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── geotiff_mapper.launch │ └── geotiff_mapper_only.launch └── package.xml ├── hector_geotiff_plugins ├── CHANGELOG.rst ├── CMakeLists.txt ├── hector_geotiff_plugins.xml ├── package.xml └── src │ └── trajectory_geotiff_plugin.cpp ├── hector_imu_attitude_to_tf ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ └── example.launch ├── package.xml └── src │ └── imu_attitude_to_tf_node.cpp ├── hector_imu_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── bertl_robot.launch │ └── jasmine_robot.launch ├── package.xml └── src │ └── pose_and_orientation_to_imu_node.cpp ├── hector_map_server ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── src │ └── hector_map_server.cpp ├── hector_map_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── hector_map_tools │ │ └── HectorMapTools.h └── package.xml ├── hector_mapping ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── hector_slam_lib │ │ ├── map │ │ ├── GridMap.h │ │ ├── GridMapBase.h │ │ ├── GridMapCacheArray.h │ │ ├── GridMapLogOdds.h │ │ ├── GridMapReflectanceCount.h │ │ ├── GridMapSimpleCount.h │ │ ├── MapDimensionProperties.h │ │ ├── OccGridMapBase.h │ │ ├── OccGridMapUtil.h │ │ └── OccGridMapUtilConfig.h │ │ ├── matcher │ │ └── ScanMatcher.h │ │ ├── scan │ │ └── DataPointContainer.h │ │ ├── slam_main │ │ ├── HectorSlamProcessor.h │ │ ├── MapProcContainer.h │ │ ├── MapRepMultiMap.h │ │ ├── MapRepSingleMap.h │ │ └── MapRepresentationInterface.h │ │ └── util │ │ ├── DrawInterface.h │ │ ├── HectorDebugInfoInterface.h │ │ ├── MapLockerInterface.h │ │ └── UtilFunctions.h ├── launch │ └── mapping_default.launch ├── msg │ ├── HectorDebugInfo.msg │ └── HectorIterData.msg ├── package.xml ├── src │ ├── HectorDebugInfoProvider.h │ ├── HectorDrawings.h │ ├── HectorMapMutex.h │ ├── HectorMappingRos.cpp │ ├── HectorMappingRos.h │ ├── PoseInfoContainer.cpp │ ├── PoseInfoContainer.h │ ├── main.cpp │ └── main_mapper.cpp └── srv │ └── ResetMapping.srv ├── hector_marker_drawing ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── hector_marker_drawing │ │ ├── DrawInterface.h │ │ └── HectorDrawings.h └── package.xml ├── hector_nav_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── srv │ ├── GetDistanceToObstacle.srv │ ├── GetNormal.srv │ ├── GetRecoveryInfo.srv │ ├── GetRobotTrajectory.srv │ └── GetSearchPosition.srv ├── hector_slam ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── hector_slam_launch ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── cityflyer_logfile_processing.launch │ ├── hector_ugv.launch │ ├── height_mapping.launch │ ├── mapping_box.launch │ ├── mpo700_mapping.launch │ ├── postproc_data.launch │ ├── postproc_qut_logs.launch │ ├── pr2os.launch │ └── tutorial.launch ├── package.xml └── rviz_cfg │ └── mapping_demo.rviz └── hector_trajectory_server ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── src └── hector_trajectory_server.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | bin/ 2 | build/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | hector_mapping/src/hector_mapping/ 7 | hector_nav_msgs/src/hector_nav_msgs/ 8 | .idea/ 9 | .vscode/ 10 | cmake-build-*/ 11 | -------------------------------------------------------------------------------- /README.txt: -------------------------------------------------------------------------------- 1 | # hector_slam 2 | 3 | See the ROS Wiki for documentation: http://wiki.ros.org/hector_slam 4 | -------------------------------------------------------------------------------- /hector_compressed_map_transport/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_compressed_map_transport 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | * Fix out of bounds access 8 | Fixes `#52 `_ and `#70 `_ 9 | * Contributors: Stefan Fabian 10 | 11 | 0.5.1 (2021-01-15) 12 | ------------------ 13 | 14 | 0.5.0 (2020-12-17) 15 | ------------------ 16 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 17 | Clean up for noetic release. 18 | * Bump CMake version to avoid CMP0048 warning 19 | * Contributors: Marius Schnaubelt, Stefan Fabian 20 | 21 | 0.4.1 (2020-05-15) 22 | ------------------ 23 | 24 | 0.3.6 (2019-10-31) 25 | ------------------ 26 | 27 | 0.3.5 (2016-06-24) 28 | ------------------ 29 | * Use the FindEigen3.cmake module provided by Eigen 30 | * Contributors: Johannes Meyer 31 | 32 | 0.3.4 (2015-11-07) 33 | ------------------ 34 | 35 | 0.3.3 (2014-06-15) 36 | ------------------ 37 | * fixed cmake find for eigen in indigo 38 | * fixed libopencv-dev rosdep key 39 | * Contributors: Alexander Stumpf, Johannes Meyer 40 | 41 | 0.3.2 (2014-03-30) 42 | ------------------ 43 | 44 | 0.3.1 (2013-10-09) 45 | ------------------ 46 | * added changelogs 47 | 48 | 0.3.0 (2013-08-08) 49 | ------------------ 50 | * catkinized hector_slam 51 | -------------------------------------------------------------------------------- /hector_compressed_map_transport/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_compressed_map_transport) 3 | 4 | find_package(catkin REQUIRED COMPONENTS cv_bridge geometry_msgs hector_map_tools image_transport nav_msgs sensor_msgs) 5 | 6 | find_package(Eigen3 REQUIRED) 7 | find_package(OpenCV REQUIRED) 8 | 9 | catkin_package( 10 | CATKIN_DEPENDS geometry_msgs nav_msgs sensor_msgs 11 | ) 12 | 13 | ########### 14 | ## Build ## 15 | ########### 16 | 17 | include_directories( 18 | ${catkin_INCLUDE_DIRS} 19 | ${EIGEN3_INCLUDE_DIRS} 20 | ${OpenCV_INCLUDE_DIRS} 21 | ) 22 | 23 | add_executable(map_to_image_node src/map_to_image_node.cpp) 24 | target_link_libraries(map_to_image_node 25 | ${catkin_LIBRARIES} 26 | ${OpenCV_LIBRARIES} 27 | ) 28 | 29 | ############# 30 | ## Install ## 31 | ############# 32 | 33 | # all install targets should use catkin DESTINATION variables 34 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 35 | 36 | install(TARGETS map_to_image_node 37 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 40 | ) 41 | -------------------------------------------------------------------------------- /hector_compressed_map_transport/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_compressed_map_transport 4 | 0.5.2 5 | 6 | hector_compressed_map_transport provides means for transporting compressed map data through the use of image_transport. 7 | 8 | 9 | 10 | Johannes Meyer 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | http://ros.org/wiki/hector_compressed_map_transport 23 | 24 | 25 | 26 | 27 | Stefan Kohlbrecher 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | cv_bridge 43 | geometry_msgs 44 | hector_map_tools 45 | image_transport 46 | nav_msgs 47 | sensor_msgs 48 | eigen 49 | cv_bridge 50 | geometry_msgs 51 | hector_map_tools 52 | image_transport 53 | nav_msgs 54 | sensor_msgs 55 | eigen 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /hector_geotiff/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_geotiff 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | * Refactored hector_geotiff dependencies. 8 | * Contributors: Stefan Fabian 9 | 10 | 0.5.1 (2021-01-15) 11 | ------------------ 12 | * Fixed "SEVERE WARNING" by pluginloader when killing geotiff node. 13 | Some minor cleanup. 14 | * Contributors: Stefan Fabian 15 | 16 | 0.5.0 (2020-12-17) 17 | ------------------ 18 | * Added missing dependency for Qt5 cmake. 19 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 20 | Clean up for noetic release. 21 | * Qt5 support for hector geotiff on headless systems. 22 | * Updated platform args. Test on robot. 23 | * Experiments with platform argument. 24 | * Renamed depends for (hopefully soon) rosdep compatibility. 25 | * Moved to Qt5. 26 | * Contributors: Stefan Fabian 27 | 28 | 0.4.1 (2020-05-15) 29 | ------------------ 30 | 31 | 0.3.6 (2019-10-31) 32 | ------------------ 33 | * Update geotiff draw interface to support different shapes 34 | * Contributors: Stefan Kohlbrecher 35 | 36 | 0.3.5 (2016-06-24) 37 | ------------------ 38 | * Use the FindEigen3.cmake module provided by Eigen 39 | * hector_geotiff/hector_geotiff_plugins: added possibility to specify Color of robot path in the geotiff file in order to allow multiple color robot paths 40 | * Contributors: Dorothea Koert, Johannes Meyer 41 | 42 | 0.3.4 (2015-11-07) 43 | ------------------ 44 | * Removes trailing spaces and fixes indents 45 | * Contributors: YuK_Ota 46 | 47 | 0.3.3 (2014-06-15) 48 | ------------------ 49 | * fixed cmake find for eigen in indigo 50 | * added launchfile to restart geotiff node 51 | * Contributors: Alexander Stumpf 52 | 53 | 0.3.2 (2014-03-30) 54 | ------------------ 55 | * Add TrajectoryMapWriter to geotiff_mapper.launch per default 56 | * Add arguments to launch files for specifying geotiff file path 57 | * Contributors: Stefan Kohlbrecher 58 | 59 | 0.3.1 (2013-10-09) 60 | ------------------ 61 | * added missing install rule for launch files 62 | * moved header files from include/geotiff_writer to include/hector_geotiff 63 | * fixed warnings for deprecated pluginlib method/macro calls 64 | * added changelogs 65 | 66 | 0.3.0 (2013-08-08) 67 | ------------------ 68 | * catkinized hector_slam 69 | -------------------------------------------------------------------------------- /hector_geotiff/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_geotiff) 3 | 4 | find_package(catkin REQUIRED COMPONENTS hector_map_tools hector_nav_msgs nav_msgs pluginlib roscpp std_msgs) 5 | 6 | find_package(Qt5 COMPONENTS Widgets REQUIRED) 7 | 8 | find_package(Eigen3 REQUIRED) 9 | 10 | catkin_package( 11 | INCLUDE_DIRS include 12 | LIBRARIES geotiff_writer 13 | CATKIN_DEPENDS hector_map_tools hector_nav_msgs nav_msgs pluginlib roscpp std_msgs 14 | DEPENDS EIGEN3 Qt5Widgets 15 | ) 16 | 17 | ########### 18 | ## Build ## 19 | ########### 20 | 21 | include_directories( 22 | include 23 | ${catkin_INCLUDE_DIRS} 24 | ${EIGEN3_INCLUDE_DIRS} 25 | ${Qt5Widgets_INCLUDE_DIRS} 26 | ) 27 | 28 | add_library(geotiff_writer src/geotiff_writer/geotiff_writer.cpp) 29 | target_link_libraries(geotiff_writer ${catkin_LIBRARIES} Qt5::Widgets stdc++fs) 30 | add_dependencies(geotiff_writer ${catkin_EXPORTED_TARGETS}) 31 | 32 | add_executable(geotiff_saver src/geotiff_saver.cpp) 33 | target_link_libraries(geotiff_saver geotiff_writer) 34 | add_dependencies(geotiff_saver ${catkin_EXPORTED_TARGETS}) 35 | 36 | add_executable(geotiff_node src/geotiff_node.cpp) 37 | target_link_libraries(geotiff_node geotiff_writer) 38 | add_dependencies(geotiff_node ${catkin_EXPORTED_TARGETS}) 39 | 40 | ############# 41 | ## Install ## 42 | ############# 43 | 44 | # all install targets should use catkin DESTINATION variables 45 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 46 | 47 | install(TARGETS geotiff_writer geotiff_saver geotiff_node 48 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 51 | ) 52 | 53 | install(DIRECTORY include/${PROJECT_NAME}/ 54 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 55 | FILES_MATCHING PATTERN "*.h" 56 | PATTERN ".svn" EXCLUDE 57 | ) 58 | 59 | install(DIRECTORY fonts 60 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 61 | ) 62 | 63 | -------------------------------------------------------------------------------- /hector_geotiff/fonts/Roboto-Regular.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tu-darmstadt-ros-pkg/hector_slam/f0f1eb2e28eea00f52434f1f4403a8a57b0e6a6a/hector_geotiff/fonts/Roboto-Regular.ttf -------------------------------------------------------------------------------- /hector_geotiff/include/hector_geotiff/geotiff_writer.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef _GEOTIFFWRITER_H__ 30 | #define _GEOTIFFWRITER_H__ 31 | 32 | #include "map_writer_interface.h" 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | #if __cplusplus < 201703L 46 | #include 47 | namespace fs = std::experimental::filesystem; 48 | #else 49 | #include 50 | namespace fs = std::filesystem; 51 | #endif 52 | 53 | 54 | namespace hector_geotiff{ 55 | 56 | 57 | class GeotiffWriter : public MapWriterInterface 58 | { 59 | public: 60 | explicit GeotiffWriter(bool useCheckerboardCacheIn = false); 61 | virtual ~GeotiffWriter(); 62 | 63 | //setUsePrecalcGrid(bool usePrecalc, const Eigen::Vector2f& size); 64 | 65 | void setMapFileName(const std::string& mapFileName); 66 | void setMapFilePath(const std::string& mapFilePath); 67 | void setUseUtcTimeSuffix(bool useSuffix); 68 | 69 | void setupImageSize(); 70 | bool setupTransforms(const nav_msgs::OccupancyGrid& map); 71 | void drawBackgroundCheckerboard(); 72 | void drawMap(const nav_msgs::OccupancyGrid& map, bool draw_explored_space_grid = true); 73 | void drawObjectOfInterest(const Eigen::Vector2f& coords, const std::string& txt, const Color& color, const Shape& shape); 74 | inline virtual void drawPath(const Eigen::Vector3f& start, const std::vector& points){ 75 | drawPath(start, points, 120,0,240); 76 | } 77 | void drawPath(const Eigen::Vector3f& start, const std::vector& points, int color_r, int color_g, int color_b); 78 | void drawCoords(); 79 | std::string getBasePathAndFileName() const; 80 | void writeGeotiffImage(bool completed); 81 | 82 | 83 | protected: 84 | 85 | void transformPainterToImgCoords(QPainter& painter); 86 | void drawCross(QPainter& painter, const Eigen::Vector2f& coords); 87 | void drawArrow(QPainter& painter); 88 | void drawCoordSystem(QPainter& painter); 89 | 90 | float resolution = std::numeric_limits::quiet_NaN(); 91 | Eigen::Vector2f origin; 92 | 93 | int resolutionFactor = 3; 94 | float resolutionFactorf = std::numeric_limits::quiet_NaN(); 95 | 96 | bool useCheckerboardCache; 97 | bool use_utc_time_suffix_; 98 | 99 | float pixelsPerMapMeter = std::numeric_limits::quiet_NaN(); 100 | float pixelsPerGeoTiffMeter = std::numeric_limits::quiet_NaN(); 101 | 102 | Eigen::Vector2i minCoordsMap; 103 | Eigen::Vector2i maxCoordsMap; 104 | 105 | Eigen::Vector2i sizeMap; 106 | Eigen::Vector2f sizeMapf; 107 | 108 | Eigen::Vector2f rightBottomMarginMeters; 109 | Eigen::Vector2f rightBottomMarginPixelsf; 110 | Eigen::Vector2i rightBottomMarginPixels; 111 | 112 | Eigen::Vector2f leftTopMarginMeters; 113 | 114 | Eigen::Vector2f totalMeters; 115 | 116 | Eigen::Vector2i geoTiffSizePixels; 117 | 118 | Eigen::Vector2f mapOrigInGeotiff; 119 | Eigen::Vector2f mapEndInGeotiff; 120 | 121 | std::string map_file_name_; 122 | std::string map_file_path_; 123 | 124 | QImage image; 125 | QImage checkerboard_cache; 126 | QApplication* app; 127 | QString font_family_; 128 | QFont map_draw_font_; 129 | 130 | HectorMapTools::CoordinateTransformer world_map_transformer_; 131 | HectorMapTools::CoordinateTransformer map_geo_transformer_; 132 | HectorMapTools::CoordinateTransformer world_geo_transformer_; 133 | 134 | nav_msgs::MapMetaData cached_map_meta_data_; 135 | }; 136 | 137 | } 138 | 139 | #endif 140 | -------------------------------------------------------------------------------- /hector_geotiff/include/hector_geotiff/map_writer_interface.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef _MAPWRITERINTERFACE_H__ 30 | #define _MAPWRITERINTERFACE_H__ 31 | 32 | #include 33 | #include 34 | 35 | namespace hector_geotiff{ 36 | 37 | enum Shape { 38 | SHAPE_CIRCLE, 39 | SHAPE_DIAMOND 40 | }; 41 | 42 | class MapWriterInterface{ 43 | public: 44 | struct Color { 45 | Color(unsigned int r, unsigned int g, unsigned int b) : r(r), g(g), b(b) {} 46 | unsigned int r,g,b; 47 | }; 48 | 49 | bool completed_map_ = false; 50 | 51 | virtual std::string getBasePathAndFileName() const = 0; 52 | virtual void drawObjectOfInterest(const Eigen::Vector2f& coords, const std::string& txt, const Color& color, const Shape& shape = SHAPE_CIRCLE) = 0; 53 | //virtual void drawPath(const Eigen::Vector3f& start, const std::vector& points) = 0; 54 | 55 | inline virtual void drawPath(const Eigen::Vector3f& start, const std::vector& points){ 56 | drawPath(start, points, 120,0,240); 57 | } 58 | virtual void drawPath(const Eigen::Vector3f& start, const std::vector& points, int color_r, int color_g, int color_b) = 0; 59 | 60 | }; 61 | 62 | } 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /hector_geotiff/include/hector_geotiff/map_writer_plugin_interface.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef _MAPWRITERPLUGININTERFACE_H__ 30 | #define _MAPWRITERPLUGININTERFACE_H__ 31 | 32 | #include "map_writer_interface.h" 33 | 34 | namespace hector_geotiff{ 35 | 36 | class MapWriterPluginInterface{ 37 | 38 | public: 39 | 40 | virtual void initialize(const std::string& name) = 0; 41 | virtual void draw(MapWriterInterface* map_writer_interface) = 0; 42 | 43 | }; 44 | 45 | } //namespace hector_geotiff 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /hector_geotiff/maps/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | -------------------------------------------------------------------------------- /hector_geotiff/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_geotiff 4 | 0.5.2 5 | 6 | hector_geotiff provides a node that can be used to save occupancy grid map, robot trajectory and object of interest data to RoboCup Rescue compliant GeoTiff images. 7 | 8 | 9 | 10 | Johannes Meyer 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | http://ros.org/wiki/hector_geotiff 22 | 23 | 24 | 25 | 26 | Stefan Kohlbrecher 27 | 28 | 29 | catkin 30 | qtbase5-dev 31 | qtbase5-dev 32 | qt5-image-formats-plugins 33 | hector_map_tools 34 | hector_nav_msgs 35 | libqt5-widgets 36 | nav_msgs 37 | pluginlib 38 | roscpp 39 | std_msgs 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /hector_geotiff/src/geotiff_saver.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #include "hector_geotiff/geotiff_writer.h" 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | #include 38 | 39 | using namespace std; 40 | 41 | namespace hector_geotiff{ 42 | 43 | /** 44 | * @brief Map generation node. 45 | */ 46 | class MapGenerator 47 | { 48 | public: 49 | MapGenerator(const std::string& mapname) : mapname_(mapname) 50 | { 51 | ros::NodeHandle n; 52 | ROS_INFO("Waiting for the map"); 53 | map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this); 54 | } 55 | 56 | void mapCallback(const nav_msgs::OccupancyGridConstPtr& map) 57 | { 58 | ros::Time start_time (ros::Time::now()); 59 | 60 | geotiff_writer.setMapFileName(mapname_); 61 | geotiff_writer.setupTransforms(*map); 62 | geotiff_writer.drawBackgroundCheckerboard(); 63 | geotiff_writer.drawMap(*map); 64 | geotiff_writer.drawCoords(); 65 | 66 | geotiff_writer.writeGeotiffImage(true); 67 | 68 | ros::Duration elapsed_time (ros::Time::now() - start_time); 69 | ROS_INFO("GeoTiff created in %f seconds", elapsed_time.toSec()); 70 | } 71 | 72 | GeotiffWriter geotiff_writer; 73 | 74 | std::string mapname_; 75 | ros::Subscriber map_sub_; 76 | }; 77 | 78 | } 79 | 80 | #define USAGE "Usage: \n" \ 81 | " geotiff_saver -h\n"\ 82 | " geotiff_saver [-f ] [ROS remapping args]" 83 | 84 | int main(int argc, char** argv) 85 | { 86 | ros::init(argc, argv, "map_saver"); 87 | std::string mapname = "map"; 88 | 89 | for(int i=1; i 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 | -------------------------------------------------------------------------------- /hector_geotiff_launch/launch/geotiff_mapper_only.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /hector_geotiff_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_geotiff_launch 4 | 0.5.2 5 | Contains launch files for the hector_geotiff mapper. 6 | 7 | Stefan Fabian 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/hector_geotiff 12 | 13 | Stefan Kohlbrecher 14 | 15 | catkin 16 | hector_geotiff 17 | hector_geotiff_plugins 18 | hector_trajectory_server 19 | 20 | -------------------------------------------------------------------------------- /hector_geotiff_plugins/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_geotiff_plugins 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | 8 | 0.5.1 (2021-01-15) 9 | ------------------ 10 | * Fixed "SEVERE WARNING" by pluginloader when killing geotiff node. 11 | Some minor cleanup. 12 | * Contributors: Stefan Fabian 13 | 14 | 0.5.0 (2020-12-17) 15 | ------------------ 16 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 17 | Clean up for noetic release. 18 | * Bump CMake version to avoid CMP0048 warning 19 | * Contributors: Marius Schnaubelt, Stefan Fabian 20 | 21 | 0.4.1 (2020-05-15) 22 | ------------------ 23 | 24 | 0.3.6 (2019-10-31) 25 | ------------------ 26 | 27 | 0.3.5 (2016-06-24) 28 | ------------------ 29 | * hector_geotiff/hector_geotiff_plugins: added possibility to specify Color of robot path in the geotiff file in order to allow multiple color robot paths 30 | * Contributors: Dorothea Koert 31 | 32 | 0.3.4 (2015-11-07) 33 | ------------------ 34 | 35 | 0.3.3 (2014-06-15) 36 | ------------------ 37 | 38 | 0.3.2 (2014-03-30) 39 | ------------------ 40 | 41 | 0.3.1 (2013-10-09) 42 | ------------------ 43 | * readded PLUGINLIB_DECLARE_CLASS macro for fuerte compatibility 44 | * use hector_geotiff_plugins/TrajectoryMapWriter as legacy lookup name for the trajectory geotiff plugin to not break old launch files 45 | * fixed warnings for deprecated pluginlib method/macro calls 46 | * added changelogs 47 | * added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them 48 | 49 | 0.3.0 (2013-08-08) 50 | ------------------ 51 | * catkinized hector_slam 52 | -------------------------------------------------------------------------------- /hector_geotiff_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_geotiff_plugins) 3 | 4 | find_package(catkin REQUIRED COMPONENTS hector_geotiff hector_nav_msgs) 5 | 6 | 7 | ################################### 8 | ## catkin specific configuration ## 9 | ################################### 10 | catkin_package( 11 | CATKIN_DEPENDS hector_geotiff hector_nav_msgs 12 | ) 13 | 14 | ########### 15 | ## Build ## 16 | ########### 17 | 18 | include_directories( 19 | ${catkin_INCLUDE_DIRS} 20 | ) 21 | 22 | add_library(hector_geotiff_plugins src/trajectory_geotiff_plugin.cpp) 23 | add_dependencies(hector_geotiff_plugins ${catkin_EXPORTED_TARGETS}) 24 | target_link_libraries(hector_geotiff_plugins 25 | ${catkin_LIBRARIES} 26 | ) 27 | 28 | ############# 29 | ## Install ## 30 | ############# 31 | 32 | # all install targets should use catkin DESTINATION variables 33 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 34 | 35 | install(TARGETS hector_geotiff_plugins 36 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 39 | ) 40 | 41 | install(FILES 42 | hector_geotiff_plugins.xml 43 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 44 | ) 45 | -------------------------------------------------------------------------------- /hector_geotiff_plugins/hector_geotiff_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This is a MapWriter plugin that draws the robot's trajectory in the map. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_geotiff_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_geotiff_plugins 4 | 0.5.2 5 | 6 | hector_geotiff_plugins contains plugins that extend geotiff maps generated by hector_geotiff. 7 | 8 | 9 | 10 | Johannes Meyer 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | http://ros.org/wiki/hector_geotiff_plugins 22 | 23 | 24 | 25 | 26 | Stefan Kohlbrecher 27 | Gregor Gebhardt 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | catkin 41 | hector_geotiff 42 | hector_nav_msgs 43 | hector_geotiff 44 | hector_nav_msgs 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /hector_geotiff_plugins/src/trajectory_geotiff_plugin.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Gregor Gebhardt, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | namespace hector_geotiff_plugins 38 | { 39 | 40 | using namespace hector_geotiff; 41 | 42 | class TrajectoryMapWriter : public MapWriterPluginInterface 43 | { 44 | public: 45 | TrajectoryMapWriter(); 46 | virtual ~TrajectoryMapWriter(); 47 | 48 | virtual void initialize(const std::string& name); 49 | virtual void draw(MapWriterInterface *interface); 50 | 51 | protected: 52 | ros::NodeHandle nh_; 53 | ros::ServiceClient service_client_; 54 | 55 | bool initialized_; 56 | std::string name_; 57 | bool draw_all_objects_; 58 | std::string class_id_; 59 | int path_color_r_; 60 | int path_color_g_; 61 | int path_color_b_; 62 | }; 63 | 64 | TrajectoryMapWriter::TrajectoryMapWriter() 65 | : initialized_(false) 66 | {} 67 | 68 | TrajectoryMapWriter::~TrajectoryMapWriter() 69 | {} 70 | 71 | void TrajectoryMapWriter::initialize(const std::string& name) 72 | { 73 | ros::NodeHandle plugin_nh("~/" + name); 74 | std::string service_name_; 75 | 76 | 77 | plugin_nh.param("service_name", service_name_, std::string("trajectory")); 78 | plugin_nh.param("path_color_r", path_color_r_, 120); 79 | plugin_nh.param("path_color_g", path_color_g_, 0); 80 | plugin_nh.param("path_color_b", path_color_b_, 240); 81 | 82 | service_client_ = nh_.serviceClient(service_name_); 83 | 84 | initialized_ = true; 85 | this->name_ = name; 86 | ROS_INFO_NAMED(name_, "Successfully initialized hector_geotiff MapWriter plugin %s.", name_.c_str()); 87 | } 88 | 89 | void TrajectoryMapWriter::draw(MapWriterInterface *interface) 90 | { 91 | if(!initialized_) return; 92 | 93 | hector_nav_msgs::GetRobotTrajectory srv_path; 94 | if (!service_client_.call(srv_path)) { 95 | ROS_ERROR_NAMED(name_, "Cannot draw trajectory, service %s failed", service_client_.getService().c_str()); 96 | return; 97 | } 98 | 99 | std::vector& traj_vector (srv_path.response.trajectory.poses); 100 | 101 | size_t size = traj_vector.size(); 102 | 103 | std::vector pointVec; 104 | pointVec.resize(size); 105 | 106 | for (size_t i = 0; i < size; ++i){ 107 | const geometry_msgs::PoseStamped& pose (traj_vector[i]); 108 | 109 | pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y); 110 | } 111 | 112 | if (size > 0){ 113 | //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z); 114 | Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f); 115 | interface->drawPath(startVec, pointVec, path_color_r_, path_color_g_, path_color_b_); 116 | } 117 | } 118 | 119 | } // namespace 120 | 121 | //register this planner as a MapWriterPluginInterface plugin 122 | #include 123 | PLUGINLIB_EXPORT_CLASS(hector_geotiff_plugins::TrajectoryMapWriter, hector_geotiff::MapWriterPluginInterface) 124 | -------------------------------------------------------------------------------- /hector_imu_attitude_to_tf/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_imu_attitude_to_tf 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | 8 | 0.5.1 (2021-01-15) 9 | ------------------ 10 | 11 | 0.5.0 (2020-12-17) 12 | ------------------ 13 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 14 | Clean up for noetic release. 15 | * Bump CMake version to avoid CMP0048 warning 16 | * Contributors: Marius Schnaubelt, Stefan Fabian 17 | 18 | 0.4.1 (2020-05-15) 19 | ------------------ 20 | 21 | 0.3.6 (2019-10-31) 22 | ------------------ 23 | 24 | 0.3.5 (2016-06-24) 25 | ------------------ 26 | 27 | 0.3.4 (2015-11-07) 28 | ------------------ 29 | * hector_imu_attitude_to_tf: fixed default values of the base_frame and base_stabilized_frame parameters (fix #20) 30 | * Contributors: Johannes Meyer 31 | 32 | 0.3.3 (2014-06-15) 33 | ------------------ 34 | 35 | 0.3.2 (2014-03-30) 36 | ------------------ 37 | 38 | 0.3.1 (2013-10-09) 39 | ------------------ 40 | * added missing install rule for launch files 41 | * added changelogs 42 | 43 | 0.3.0 (2013-08-08) 44 | ------------------ 45 | * catkinized hector_slam 46 | -------------------------------------------------------------------------------- /hector_imu_attitude_to_tf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_imu_attitude_to_tf) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs tf) 5 | 6 | catkin_package( 7 | CATKIN_DEPENDS roscpp sensor_msgs tf 8 | ) 9 | 10 | include_directories( 11 | ${catkin_INCLUDE_DIRS} 12 | ) 13 | 14 | add_executable(imu_attitude_to_tf_node src/imu_attitude_to_tf_node.cpp) 15 | target_link_libraries(imu_attitude_to_tf_node ${catkin_LIBRARIES}) 16 | 17 | ############# 18 | ## Install ## 19 | ############# 20 | 21 | # all install targets should use catkin DESTINATION variables 22 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 23 | 24 | install(TARGETS imu_attitude_to_tf_node 25 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 26 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 27 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 28 | ) 29 | 30 | install(DIRECTORY launch 31 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 32 | ) 33 | -------------------------------------------------------------------------------- /hector_imu_attitude_to_tf/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_imu_attitude_to_tf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_imu_attitude_to_tf 4 | 0.5.2 5 | 6 | hector_imu_attitude_to_tf is a lightweight node that can be used to publish the roll/pitch attitude angles reported via a imu message to tf. 7 | 8 | 9 | 10 | Johannes Meyer 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | http://ros.org/wiki/hector_imu_attitude_to_tf 23 | 24 | 25 | 26 | 27 | Stefan Kohlbrecher 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | roscpp 43 | sensor_msgs 44 | tf 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /hector_imu_attitude_to_tf/src/imu_attitude_to_tf_node.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | 30 | #include "ros/ros.h" 31 | #include "tf/transform_broadcaster.h" 32 | #include "sensor_msgs/Imu.h" 33 | 34 | std::string p_base_stabilized_frame_; 35 | std::string p_base_frame_; 36 | tf::TransformBroadcaster* tfB_; 37 | tf::StampedTransform transform_; 38 | tf::Quaternion tmp_; 39 | 40 | #ifndef TF_MATRIX3x3_H 41 | typedef btScalar tfScalar; 42 | namespace tf { typedef btMatrix3x3 Matrix3x3; } 43 | #endif 44 | 45 | void imuMsgCallback(const sensor_msgs::Imu& imu_msg) 46 | { 47 | tf::quaternionMsgToTF(imu_msg.orientation, tmp_); 48 | 49 | tfScalar yaw, pitch, roll; 50 | tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw); 51 | 52 | tmp_.setRPY(roll, pitch, 0.0); 53 | 54 | transform_.setRotation(tmp_); 55 | 56 | transform_.stamp_ = imu_msg.header.stamp; 57 | 58 | tfB_->sendTransform(transform_); 59 | } 60 | 61 | int main(int argc, char **argv) { 62 | ros::init(argc, argv, ROS_PACKAGE_NAME); 63 | 64 | ros::NodeHandle n; 65 | ros::NodeHandle pn("~"); 66 | 67 | pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized")); 68 | pn.param("base_frame", p_base_frame_, std::string("base_link")); 69 | 70 | tfB_ = new tf::TransformBroadcaster(); 71 | transform_.getOrigin().setX(0.0); 72 | transform_.getOrigin().setY(0.0); 73 | transform_.getOrigin().setZ(0.0); 74 | transform_.frame_id_ = p_base_stabilized_frame_; 75 | transform_.child_frame_id_ = p_base_frame_; 76 | 77 | ros::Subscriber imu_subscriber = n.subscribe("imu_topic", 10, imuMsgCallback); 78 | 79 | ros::spin(); 80 | 81 | delete tfB_; 82 | 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /hector_imu_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_imu_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | 8 | 0.5.1 (2021-01-15) 9 | ------------------ 10 | 11 | 0.5.0 (2020-12-17) 12 | ------------------ 13 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 14 | Clean up for noetic release. 15 | * Bump CMake version to avoid CMP0048 warning 16 | * Contributors: Marius Schnaubelt, Stefan Fabian 17 | 18 | 0.4.1 (2020-05-15) 19 | ------------------ 20 | 21 | 0.3.6 (2019-10-31) 22 | ------------------ 23 | 24 | 0.3.5 (2016-06-24) 25 | ------------------ 26 | 27 | 0.3.4 (2015-11-07) 28 | ------------------ 29 | * Fix sim setup 30 | * remap for bertl setup 31 | * Contributors: Florian Kunz, kohlbrecher 32 | 33 | 0.3.3 (2014-06-15) 34 | ------------------ 35 | * hector_imu_tools: Basics of simple height etimation 36 | * hector_imu_tools: Add tf publishers in hector_imu_tools 37 | * hector_imu_tools: Also write out fake odometry 38 | * hector_imu_tools: Fix typo 39 | * hector_imu_tools: Prevent race conditions in slam, formatting 40 | * hector_imu_tools: Small executable for generating a IMU message out of a (2d) pose and roll/pitch imu message 41 | * Contributors: Stefan Kohlbrecher 42 | -------------------------------------------------------------------------------- /hector_imu_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_imu_tools) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs nav_msgs tf) 5 | 6 | catkin_package( 7 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp sensor_msgs tf 8 | ) 9 | 10 | ########### 11 | ## Build ## 12 | ########### 13 | 14 | include_directories( 15 | ${catkin_INCLUDE_DIRS} 16 | ) 17 | 18 | add_executable(pose_and_orientation_to_imu_node src/pose_and_orientation_to_imu_node.cpp) 19 | 20 | target_link_libraries(pose_and_orientation_to_imu_node 21 | ${catkin_LIBRARIES} 22 | ) 23 | 24 | ############# 25 | ## Install ## 26 | ############# 27 | 28 | # all install targets should use catkin DESTINATION variables 29 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 30 | 31 | install(TARGETS pose_and_orientation_to_imu_node 32 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 35 | ) 36 | 37 | install(DIRECTORY launch/ 38 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/ 39 | ) 40 | -------------------------------------------------------------------------------- /hector_imu_tools/launch/bertl_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /hector_imu_tools/launch/jasmine_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /hector_imu_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_imu_tools 4 | 0.5.2 5 | 6 | hector_imu_tools provides some tools for processing IMU messages 7 | 8 | 9 | 10 | Johannes Meyer 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | http://ros.org/wiki/hector_imu_attitude_to_tf 23 | 24 | 25 | 26 | 27 | Stefan Kohlbrecher 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | roscpp 43 | tf 44 | geometry_msgs 45 | sensor_msgs 46 | nav_msgs 47 | 48 | roscpp 49 | tf 50 | geometry_msgs 51 | sensor_msgs 52 | nav_msgs 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /hector_imu_tools/src/pose_and_orientation_to_imu_node.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | 30 | #include "ros/ros.h" 31 | #include "tf/transform_broadcaster.h" 32 | #include 33 | #include 34 | #include 35 | 36 | std::string p_map_frame_; 37 | std::string p_base_footprint_frame_; 38 | std::string p_base_stabilized_frame_; 39 | std::string p_base_frame_; 40 | tf::TransformBroadcaster* tfB_; 41 | tf::StampedTransform transform_; 42 | 43 | tf::Quaternion robot_pose_quaternion_; 44 | tf::Point robot_pose_position_; 45 | tf::Transform robot_pose_transform_; 46 | 47 | tf::Quaternion tmp_; 48 | tf::Quaternion orientation_quaternion_; 49 | 50 | sensor_msgs::ImuConstPtr last_imu_msg_; 51 | sensor_msgs::Imu fused_imu_msg_; 52 | nav_msgs::Odometry odom_msg_; 53 | geometry_msgs::PoseStampedConstPtr last_pose_msg_; 54 | 55 | ros::Publisher fused_imu_publisher_; 56 | ros::Publisher odometry_publisher_; 57 | 58 | size_t callback_count_; 59 | 60 | #ifndef TF_MATRIX3x3_H 61 | typedef btScalar tfScalar; 62 | namespace tf { typedef btMatrix3x3 Matrix3x3; } 63 | #endif 64 | 65 | void imuMsgCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) 66 | { 67 | callback_count_++; 68 | 69 | tf::quaternionMsgToTF(imu_msg->orientation, tmp_); 70 | 71 | tfScalar imu_yaw, imu_pitch, imu_roll; 72 | tf::Matrix3x3(tmp_).getRPY(imu_roll, imu_pitch, imu_yaw); 73 | 74 | tf::Transform transform; 75 | transform.setIdentity(); 76 | tf::Quaternion quat; 77 | 78 | quat.setRPY(imu_roll, imu_pitch, 0.0); 79 | 80 | if (true){ 81 | transform.setRotation(quat); 82 | tfB_->sendTransform(tf::StampedTransform(transform, imu_msg->header.stamp, p_base_stabilized_frame_, p_base_frame_)); 83 | } 84 | 85 | tfScalar pose_yaw, pose_pitch, pose_roll; 86 | 87 | if (last_pose_msg_ != 0){ 88 | tf::quaternionMsgToTF(last_pose_msg_->pose.orientation, tmp_); 89 | 90 | tf::Matrix3x3(tmp_).getRPY(pose_roll, pose_pitch, pose_yaw); 91 | }else{ 92 | pose_yaw = 0.0; 93 | } 94 | 95 | orientation_quaternion_.setRPY(imu_roll, imu_pitch, pose_yaw); 96 | 97 | fused_imu_msg_.header.stamp = imu_msg->header.stamp; 98 | 99 | fused_imu_msg_.orientation.x = orientation_quaternion_.getX(); 100 | fused_imu_msg_.orientation.y = orientation_quaternion_.getY(); 101 | fused_imu_msg_.orientation.z = orientation_quaternion_.getZ(); 102 | fused_imu_msg_.orientation.w = orientation_quaternion_.getW(); 103 | 104 | fused_imu_publisher_.publish(fused_imu_msg_); 105 | 106 | //If no pose message received, yaw is set to 0. 107 | //@TODO: Check for timestamp of pose and disable sending if too old 108 | if (last_pose_msg_ != 0){ 109 | if ( (callback_count_ % 5) == 0){ 110 | odom_msg_.header.stamp = imu_msg->header.stamp; 111 | odom_msg_.pose.pose.orientation = fused_imu_msg_.orientation; 112 | odom_msg_.pose.pose.position = last_pose_msg_->pose.position; 113 | 114 | odometry_publisher_.publish(odom_msg_); 115 | } 116 | } 117 | 118 | } 119 | 120 | void poseMsgCallback(const geometry_msgs::PoseStamped::ConstPtr& pose_msg) 121 | { 122 | 123 | std::vector transforms; 124 | transforms.resize(2); 125 | 126 | tf::quaternionMsgToTF(pose_msg->pose.orientation, robot_pose_quaternion_); 127 | tf::pointMsgToTF(pose_msg->pose.position, robot_pose_position_); 128 | 129 | robot_pose_transform_.setRotation(robot_pose_quaternion_); 130 | robot_pose_transform_.setOrigin(robot_pose_position_); 131 | 132 | tf::Transform height_transform; 133 | height_transform.setIdentity(); 134 | height_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); 135 | 136 | transforms[0] = tf::StampedTransform(robot_pose_transform_, pose_msg->header.stamp, p_map_frame_, p_base_footprint_frame_); 137 | transforms[1] = tf::StampedTransform(height_transform, pose_msg->header.stamp, p_base_footprint_frame_, p_base_stabilized_frame_); 138 | 139 | tfB_->sendTransform(transforms); 140 | 141 | // Perform simple estimation of vehicle altitude based on orientation 142 | if (last_pose_msg_){ 143 | tf::Vector3 plane_normal = tf::Matrix3x3(orientation_quaternion_) * tf::Vector3(0.0, 0.0, 1.0); 144 | 145 | tf::Vector3 last_position; 146 | tf::pointMsgToTF(last_pose_msg_->pose.position, last_position); 147 | 148 | double height_difference = 149 | (-plane_normal.getX() * (robot_pose_position_.getX() - last_position.getX()) 150 | -plane_normal.getY() * (robot_pose_position_.getY() - last_position.getY()) 151 | +plane_normal.getZ() * last_position.getZ()) / last_position.getZ(); 152 | 153 | } 154 | 155 | 156 | 157 | last_pose_msg_ = pose_msg; 158 | 159 | } 160 | 161 | int main(int argc, char **argv) { 162 | ros::init(argc, argv, ROS_PACKAGE_NAME); 163 | 164 | ros::NodeHandle n; 165 | ros::NodeHandle pn("~"); 166 | 167 | pn.param("map_frame", p_map_frame_, std::string("map")); 168 | pn.param("base_footprint_frame", p_base_footprint_frame_, std::string("base_footprint")); 169 | pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized")); 170 | pn.param("base_frame", p_base_frame_, std::string("base_link")); 171 | 172 | fused_imu_msg_.header.frame_id = p_base_stabilized_frame_; 173 | odom_msg_.header.frame_id = "map"; 174 | 175 | tfB_ = new tf::TransformBroadcaster(); 176 | 177 | fused_imu_publisher_ = n.advertise("/fused_imu",1,false); 178 | odometry_publisher_ = n.advertise("/state", 1, false); 179 | 180 | ros::Subscriber imu_subscriber = n.subscribe("/imu", 10, imuMsgCallback); 181 | ros::Subscriber pose_subscriber = n.subscribe("/pose", 10, poseMsgCallback); 182 | 183 | callback_count_ = 0; 184 | 185 | ros::spin(); 186 | 187 | delete tfB_; 188 | 189 | return 0; 190 | } 191 | -------------------------------------------------------------------------------- /hector_map_server/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_map_server 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | 8 | 0.5.1 (2021-01-15) 9 | ------------------ 10 | 11 | 0.5.0 (2020-12-17) 12 | ------------------ 13 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 14 | Clean up for noetic release. 15 | * Bump CMake version to avoid CMP0048 warning 16 | * Contributors: Marius Schnaubelt, Stefan Fabian 17 | 18 | 0.4.1 (2020-05-15) 19 | ------------------ 20 | 21 | 0.3.6 (2019-10-31) 22 | ------------------ 23 | 24 | 0.3.5 (2016-06-24) 25 | ------------------ 26 | 27 | 0.3.4 (2015-11-07) 28 | ------------------ 29 | 30 | 0.3.3 (2014-06-15) 31 | ------------------ 32 | 33 | 0.3.2 (2014-03-30) 34 | ------------------ 35 | 36 | 0.3.1 (2013-10-09) 37 | ------------------ 38 | * added changelogs 39 | * added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them 40 | 41 | 0.3.0 (2013-08-08) 42 | ------------------ 43 | * catkinized hector_slam 44 | -------------------------------------------------------------------------------- /hector_map_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_map_server) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp hector_map_tools hector_marker_drawing hector_nav_msgs nav_msgs tf) 5 | 6 | catkin_package( 7 | CATKIN_DEPENDS roscpp hector_map_tools hector_marker_drawing hector_nav_msgs nav_msgs tf 8 | ) 9 | 10 | ########### 11 | ## Build ## 12 | ########### 13 | 14 | include_directories( 15 | ${catkin_INCLUDE_DIRS} 16 | ) 17 | 18 | add_executable(hector_map_server src/hector_map_server.cpp) 19 | add_dependencies(hector_map_server ${catkin_EXPORTED_TARGETS}) 20 | target_link_libraries(hector_map_server 21 | ${catkin_LIBRARIES} 22 | ) 23 | 24 | ############# 25 | ## Install ## 26 | ############# 27 | 28 | # all install targets should use catkin DESTINATION variables 29 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 30 | 31 | install(TARGETS hector_map_server 32 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 35 | ) 36 | -------------------------------------------------------------------------------- /hector_map_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_map_server 4 | 0.5.2 5 | 6 | hector_map_server provides a service for retrieving the map, as well as for raycasting based obstacle queries (finds next obstacle in the map, given start and endpoint 7 | in any tf coordinate frame). 8 | 9 | 10 | 11 | Johannes Meyer 12 | 13 | 14 | 15 | 16 | 17 | BSD 18 | 19 | 20 | 21 | 22 | http://ros.org/wiki/hector_map_server 23 | 24 | 25 | 26 | 27 | Stefan Kohlbrecher 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | roscpp 43 | hector_map_tools 44 | hector_marker_drawing 45 | hector_nav_msgs 46 | nav_msgs 47 | tf 48 | roscpp 49 | hector_map_tools 50 | hector_marker_drawing 51 | hector_nav_msgs 52 | nav_msgs 53 | tf 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /hector_map_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_map_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | 8 | 0.5.1 (2021-01-15) 9 | ------------------ 10 | 11 | 0.5.0 (2020-12-17) 12 | ------------------ 13 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 14 | Clean up for noetic release. 15 | * Bump CMake version to avoid CMP0048 warning 16 | * Contributors: Marius Schnaubelt, Stefan Fabian 17 | 18 | 0.4.1 (2020-05-15) 19 | ------------------ 20 | 21 | 0.3.6 (2019-10-31) 22 | ------------------ 23 | * hector_map_tools: Use the FindEigen3.cmake module provided by Eigen 24 | This patch applies the recommendation from http://wiki.ros.org/jade/Migration and removes the 25 | dependency from package cmake_modules (unless your installation of Eigen3 does not provide a 26 | cmake config). 27 | Same as 1251d9dc20854f48da116eed25780c103a5bd003, but package hector_map_tools was not updated 28 | back then. 29 | * Contributors: Johannes Meyer 30 | 31 | 0.3.5 (2016-06-24) 32 | ------------------ 33 | 34 | 0.3.4 (2015-11-07) 35 | ------------------ 36 | * -Fix severe bug when not using square size maps (would results in completely wrong obstacle distances and coordinates) 37 | * Contributors: Stefan Kohlbrecher 38 | 39 | 0.3.3 (2014-06-15) 40 | ------------------ 41 | 42 | 0.3.2 (2014-03-30) 43 | ------------------ 44 | 45 | 0.3.1 (2013-10-09) 46 | ------------------ 47 | * added changelogs 48 | 49 | 0.3.0 (2013-08-08) 50 | ------------------ 51 | * catkinized hector_slam 52 | -------------------------------------------------------------------------------- /hector_map_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_map_tools) 3 | 4 | find_package(catkin REQUIRED COMPONENTS nav_msgs) 5 | 6 | find_package(Eigen3 REQUIRED) 7 | 8 | catkin_package( 9 | INCLUDE_DIRS include 10 | CATKIN_DEPENDS nav_msgs 11 | DEPENDS EIGEN3 12 | ) 13 | 14 | ############# 15 | ## Install ## 16 | ############# 17 | 18 | # all install targets should use catkin DESTINATION variables 19 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 20 | 21 | install(DIRECTORY include/${PROJECT_NAME}/ 22 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 23 | FILES_MATCHING PATTERN "*.h" 24 | PATTERN ".svn" EXCLUDE 25 | ) 26 | -------------------------------------------------------------------------------- /hector_map_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_map_tools 4 | 0.5.2 5 | 6 | hector_map_tools contains some functions related to accessing information from OccupancyGridMap maps. 7 | Currently consists of a single header. 8 | 9 | 10 | 11 | Johannes Meyer 12 | 13 | 14 | 15 | 16 | 17 | BSD 18 | 19 | 20 | 21 | 22 | http://ros.org/wiki/hector_map_tools 23 | 24 | 25 | 26 | 27 | Stefan Kohlbrecher 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | nav_msgs 43 | eigen 44 | nav_msgs 45 | eigen 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /hector_mapping/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_mapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | * Remove tf_conversions as a dependency (`#93 `_) 8 | * Add reset mapping (and pose) service (`#87 `_) 9 | * add service to reset the mapping with a new initial pose 10 | * Reorganize scanCallback and add comments (`#89 `_) 11 | * Refactor, reformat and comment scanCallback 12 | * make rosPointCloudToDataContainer void 13 | * make rosLaserScanToDataContainer void 14 | * Add pause and reset services to hector_mapping (`#86 `_) 15 | * Add pause and reset services to Hector 16 | * Contributors: Marcelino Almeida 17 | 18 | 0.5.1 (2021-01-15) 19 | ------------------ 20 | 21 | 0.5.0 (2020-12-17) 22 | ------------------ 23 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 24 | Clean up for noetic release. 25 | * Bump CMake version to avoid CMP0048 warning 26 | * fixed compilation under noetic 27 | * Contributors: Marius Schnaubelt, Stefan Fabian 28 | 29 | 0.4.1 (2020-05-15) 30 | ------------------ 31 | * Remove unnecessary boost signals find_package 32 | With Boost >1.69 hector_mapping won't build. Furthermore, hector_mapping doesn't use signals anywhere. 33 | * Contributors: Sam Pfeiffer 34 | 35 | 0.3.6 (2019-10-31) 36 | ------------------ 37 | * Merge pull request `#49 `_ from davidbsp/catkin 38 | populate child_frame_id in odometry msg 39 | * Added child_frame_id in hector mapping's odometry msg 40 | * Contributors: David Portugal, Johannes Meyer 41 | 42 | 0.3.5 (2016-06-24) 43 | ------------------ 44 | * Use the FindEigen3.cmake module provided by Eigen 45 | * Contributors: Johannes Meyer 46 | 47 | 0.3.4 (2015-11-07) 48 | ------------------ 49 | 50 | 0.3.3 (2014-06-15) 51 | ------------------ 52 | 53 | 0.3.2 (2014-03-30) 54 | ------------------ 55 | 56 | 0.3.1 (2013-10-09) 57 | ------------------ 58 | * respect ``p_map_frame_`` 59 | * added changelogs 60 | 61 | 0.3.0 (2013-08-08) 62 | ------------------ 63 | * catkinized hector_slam 64 | * hector_mapping: fixed multi-resolution map scan matching index bug in MapRepMultiMap.h 65 | -------------------------------------------------------------------------------- /hector_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_mapping) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | nav_msgs 7 | visualization_msgs 8 | tf 9 | message_filters 10 | laser_geometry 11 | message_generation 12 | std_srvs) 13 | 14 | find_package(Boost REQUIRED COMPONENTS thread) 15 | 16 | find_package(Eigen3 REQUIRED) 17 | 18 | ####################################### 19 | ## Declare ROS messages and services ## 20 | ####################################### 21 | 22 | ## Generate messages in the 'msg' folder 23 | add_message_files( 24 | FILES 25 | HectorDebugInfo.msg 26 | HectorIterData.msg 27 | ) 28 | 29 | ## Generate services in the 'srv' folder 30 | add_service_files( 31 | FILES 32 | ResetMapping.srv 33 | ) 34 | 35 | generate_messages( 36 | DEPENDENCIES 37 | geometry_msgs 38 | ) 39 | 40 | ################################### 41 | ## catkin specific configuration ## 42 | ################################### 43 | catkin_package( 44 | INCLUDE_DIRS include 45 | # LIBRARIES hector_mapping 46 | CATKIN_DEPENDS roscpp nav_msgs visualization_msgs tf message_filters laser_geometry message_runtime 47 | DEPENDS EIGEN3 48 | ) 49 | 50 | ########### 51 | ## Build ## 52 | ########### 53 | 54 | include_directories(include/hector_slam_lib) 55 | include_directories( 56 | ${Boost_INCLUDE_DIRS} 57 | ${catkin_INCLUDE_DIRS} 58 | ${EIGEN3_INCLUDE_DIRS} 59 | ) 60 | 61 | add_executable(hector_mapping 62 | src/HectorDebugInfoProvider.h 63 | src/HectorDrawings.h 64 | src/HectorMappingRos.h 65 | src/HectorMappingRos.cpp 66 | src/main.cpp 67 | src/PoseInfoContainer.cpp 68 | src/PoseInfoContainer.h 69 | ) 70 | 71 | add_dependencies(hector_mapping hector_mapping_generate_messages_cpp) 72 | 73 | target_link_libraries(hector_mapping 74 | ${catkin_LIBRARIES} 75 | ${Boost_LIBRARIES} 76 | ) 77 | 78 | ############# 79 | ## Install ## 80 | ############# 81 | 82 | # all install targets should use catkin DESTINATION variables 83 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 84 | 85 | install(TARGETS hector_mapping 86 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 88 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 89 | ) 90 | 91 | ## Don't ask why it's hector_slam_lib, this was Stefan Kohlbrecher's first ROS package and a wrapper of a pre ROS header only library 92 | install(DIRECTORY include/hector_slam_lib/ 93 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 94 | FILES_MATCHING PATTERN "*.h" 95 | PATTERN ".svn" EXCLUDE 96 | ) 97 | 98 | install(DIRECTORY launch/ 99 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/ 100 | ) 101 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/map/GridMap.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef __GridMap_h_ 30 | #define __GridMap_h_ 31 | 32 | #include "OccGridMapBase.h" 33 | #include "GridMapLogOdds.h" 34 | #include "GridMapReflectanceCount.h" 35 | #include "GridMapSimpleCount.h" 36 | 37 | namespace hectorslam { 38 | 39 | typedef OccGridMapBase GridMap; 40 | //typedef OccGridMapBase GridMap; 41 | //typedef OccGridMapBase GridMap; 42 | 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/map/GridMapCacheArray.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef __GridMapCacheArray_h_ 30 | #define __GridMapCacheArray_h_ 31 | 32 | #include 33 | 34 | class CachedMapElement 35 | { 36 | public: 37 | float val; 38 | int index; 39 | }; 40 | 41 | /** 42 | * Caches filtered grid map accesses in a two dimensional array of the same size as the map. 43 | */ 44 | class GridMapCacheArray 45 | { 46 | public: 47 | 48 | /** 49 | * Constructor 50 | */ 51 | GridMapCacheArray() 52 | : cacheArray(0) 53 | , arrayDimensions(-1,-1) 54 | { 55 | currCacheIndex = 0; 56 | } 57 | 58 | /** 59 | * Destructor 60 | */ 61 | ~GridMapCacheArray() 62 | { 63 | deleteCacheArray(); 64 | } 65 | 66 | /** 67 | * Resets/deletes the cached data 68 | */ 69 | void resetCache() 70 | { 71 | currCacheIndex++; 72 | } 73 | 74 | /** 75 | * Checks wether cached data for coords is available. If this is the case, writes data into val. 76 | * @param coords The coordinates 77 | * @param val Reference to a float the data is written to if available 78 | * @return Indicates if cached data is available 79 | */ 80 | bool containsCachedData(int index, float& val) 81 | { 82 | const CachedMapElement& elem (cacheArray[index]); 83 | 84 | if (elem.index == currCacheIndex) { 85 | val = elem.val; 86 | return true; 87 | } else { 88 | return false; 89 | } 90 | } 91 | 92 | /** 93 | * Caches float value val for coordinates coords. 94 | * @param coords The coordinates 95 | * @param val The value to be cached for coordinates. 96 | */ 97 | void cacheData(int index, float val) 98 | { 99 | CachedMapElement& elem (cacheArray[index]); 100 | elem.index = currCacheIndex; 101 | elem.val = val; 102 | } 103 | 104 | /** 105 | * Sets the map size and resizes the cache array accordingly 106 | * @param sizeIn The map size. 107 | */ 108 | void setMapSize(const Eigen::Vector2i& newDimensions) 109 | { 110 | setArraySize(newDimensions); 111 | } 112 | 113 | protected: 114 | 115 | /** 116 | * Creates a cache array of size sizeIn. 117 | * @param sizeIn The size of the array 118 | */ 119 | void createCacheArray(const Eigen::Vector2i& newDimensions) 120 | { 121 | arrayDimensions = newDimensions; 122 | 123 | int sizeX = arrayDimensions[0]; 124 | int sizeY = arrayDimensions[1]; 125 | 126 | int size = sizeX * sizeY; 127 | 128 | cacheArray = new CachedMapElement [size]; 129 | 130 | for (int x = 0; x < size; ++x) { 131 | cacheArray[x].index = -1; 132 | } 133 | } 134 | 135 | /** 136 | * Deletes the existing cache array. 137 | */ 138 | void deleteCacheArray() 139 | { 140 | delete[] cacheArray; 141 | } 142 | 143 | /** 144 | * Sets a new cache array size 145 | */ 146 | void setArraySize(const Eigen::Vector2i& newDimensions) 147 | { 148 | if (this->arrayDimensions != newDimensions) { 149 | if (cacheArray != 0) { 150 | deleteCacheArray(); 151 | cacheArray = 0; 152 | } 153 | createCacheArray(newDimensions); 154 | } 155 | } 156 | 157 | protected: 158 | 159 | CachedMapElement* cacheArray; ///< Array used for caching data. 160 | int currCacheIndex; ///< The cache iteration index value 161 | 162 | Eigen::Vector2i arrayDimensions; ///< The size of the array 163 | 164 | }; 165 | 166 | 167 | #endif 168 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/map/GridMapLogOdds.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef __GridMapLogOdds_h_ 30 | #define __GridMapLogOdds_h_ 31 | 32 | #include 33 | 34 | /** 35 | * Provides a log odds of occupancy probability representation for cells in a occupancy grid map. 36 | */ 37 | class LogOddsCell 38 | { 39 | public: 40 | 41 | /* 42 | void setOccupied() 43 | { 44 | logOddsVal += 0.7f; 45 | }; 46 | 47 | void setFree() 48 | { 49 | logOddsVal -= 0.4f; 50 | }; 51 | */ 52 | 53 | 54 | /** 55 | * Sets the cell value to val. 56 | * @param val The log odds value. 57 | */ 58 | void set(float val) 59 | { 60 | logOddsVal = val; 61 | } 62 | 63 | /** 64 | * Returns the value of the cell. 65 | * @return The log odds value. 66 | */ 67 | float getValue() const 68 | { 69 | return logOddsVal; 70 | } 71 | 72 | /** 73 | * Returns wether the cell is occupied. 74 | * @return Cell is occupied 75 | */ 76 | bool isOccupied() const 77 | { 78 | return logOddsVal > 0.0f; 79 | } 80 | 81 | bool isFree() const 82 | { 83 | return logOddsVal < 0.0f; 84 | } 85 | 86 | /** 87 | * Reset Cell to prior probability. 88 | */ 89 | void resetGridCell() 90 | { 91 | logOddsVal = 0.0f; 92 | updateIndex = -1; 93 | } 94 | 95 | //protected: 96 | 97 | public: 98 | 99 | float logOddsVal; ///< The log odds representation of occupancy probability. 100 | int updateIndex; 101 | 102 | 103 | }; 104 | 105 | /** 106 | * Provides functions related to a log odds of occupancy probability respresentation for cells in a occupancy grid map. 107 | */ 108 | class GridMapLogOddsFunctions 109 | { 110 | public: 111 | 112 | /** 113 | * Constructor, sets parameters like free and occupied log odds ratios. 114 | */ 115 | GridMapLogOddsFunctions() 116 | { 117 | this->setUpdateFreeFactor(0.4f); 118 | this->setUpdateOccupiedFactor(0.6f); 119 | /* 120 | //float probOccupied = 0.6f; 121 | float probOccupied = 0.9f; 122 | float oddsOccupied = probOccupied / (1.0f - probOccupied); 123 | logOddsOccupied = log(oddsOccupied); 124 | 125 | float probFree = 0.4f; 126 | float oddsFree = probFree / (1.0f - probFree); 127 | logOddsFree = log(oddsFree); 128 | */ 129 | } 130 | 131 | /** 132 | * Update cell as occupied 133 | * @param cell The cell. 134 | */ 135 | void updateSetOccupied(LogOddsCell& cell) const 136 | { 137 | if (cell.logOddsVal < 50.0f){ 138 | cell.logOddsVal += logOddsOccupied; 139 | } 140 | } 141 | 142 | /** 143 | * Update cell as free 144 | * @param cell The cell. 145 | */ 146 | void updateSetFree(LogOddsCell& cell) const 147 | { 148 | 149 | cell.logOddsVal += logOddsFree; 150 | 151 | } 152 | 153 | void updateUnsetFree(LogOddsCell& cell) const 154 | { 155 | cell.logOddsVal -= logOddsFree; 156 | } 157 | 158 | /** 159 | * Get the probability value represented by the grid cell. 160 | * @param cell The cell. 161 | * @return The probability 162 | */ 163 | float getGridProbability(const LogOddsCell& cell) const 164 | { 165 | float odds = exp(cell.logOddsVal); 166 | return odds / (odds + 1.0f); 167 | 168 | /* 169 | float val = cell.logOddsVal; 170 | 171 | //prevent #IND when doing exp(large number). 172 | if (val > 50.0f) { 173 | return 1.0f; 174 | } else { 175 | float odds = exp(val); 176 | return odds / (odds + 1.0f); 177 | } 178 | */ 179 | //return 0.5f; 180 | } 181 | 182 | //void getGridValueMap( const LogOddsCell& cell) const{}; 183 | //void isOccupied(LogOddsCell& cell) {}; 184 | 185 | //void resetGridCell() {}; 186 | 187 | void setUpdateFreeFactor(float factor) 188 | { 189 | logOddsFree = probToLogOdds(factor); 190 | } 191 | 192 | void setUpdateOccupiedFactor(float factor) 193 | { 194 | logOddsOccupied = probToLogOdds(factor); 195 | } 196 | 197 | protected: 198 | 199 | float probToLogOdds(float prob) 200 | { 201 | float odds = prob / (1.0f - prob); 202 | return log(odds); 203 | } 204 | 205 | float logOddsOccupied; /// < The log odds representation of probability used for updating cells as occupied 206 | float logOddsFree; /// < The log odds representation of probability used for updating cells as free 207 | 208 | 209 | }; 210 | 211 | 212 | #endif 213 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/map/GridMapReflectanceCount.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef __GridMapReflectanceCount_h_ 30 | #define __GridMapReflectanceCount_h_ 31 | 32 | /** 33 | * Provides a reflectance count representation for cells in a occupancy grid map. 34 | */ 35 | class ReflectanceCell 36 | { 37 | public: 38 | 39 | void set(float val) 40 | { 41 | probOccupied = val; 42 | } 43 | 44 | float getValue() const 45 | { 46 | return probOccupied; 47 | } 48 | 49 | bool isOccupied() const 50 | { 51 | return probOccupied > 0.5f; 52 | } 53 | 54 | bool isFree() const{ 55 | return probOccupied < 0.5f; 56 | } 57 | 58 | void resetGridCell() 59 | { 60 | probOccupied = 0.5f; 61 | visitedCount = 0.0f; 62 | reflectedCount = 0.0f; 63 | updateIndex = -1; 64 | } 65 | 66 | //protected: 67 | 68 | float visitedCount; 69 | float reflectedCount; 70 | float probOccupied; 71 | int updateIndex; 72 | }; 73 | 74 | 75 | class GridMapReflectanceFunctions 76 | { 77 | public: 78 | 79 | GridMapReflectanceFunctions() 80 | {} 81 | 82 | void updateSetOccupied(ReflectanceCell& cell) const 83 | { 84 | ++cell.reflectedCount; 85 | ++cell.visitedCount; 86 | cell.probOccupied = cell.reflectedCount / cell.visitedCount; 87 | } 88 | 89 | void updateSetFree(ReflectanceCell& cell) const 90 | { 91 | ++cell.visitedCount; 92 | cell.probOccupied = cell.reflectedCount / cell.visitedCount; 93 | } 94 | 95 | void updateUnsetFree(ReflectanceCell& cell) const 96 | { 97 | --cell.visitedCount; 98 | cell.probOccupied = cell.reflectedCount / cell.visitedCount; 99 | } 100 | 101 | float getGridProbability(const ReflectanceCell& cell) const 102 | { 103 | return cell.probOccupied; 104 | } 105 | 106 | protected: 107 | 108 | }; 109 | 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/map/GridMapSimpleCount.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef __GridMapSimpleCount_h_ 30 | #define __GridMapSimpleCount_h_ 31 | 32 | 33 | /** 34 | * Provides a (very) simple count based representation of occupancy 35 | */ 36 | class SimpleCountCell 37 | { 38 | public: 39 | 40 | /** 41 | * Sets the cell value to val. 42 | * @param val The log odds value. 43 | */ 44 | void set(float val) 45 | { 46 | simpleOccVal = val; 47 | } 48 | 49 | /** 50 | * Returns the value of the cell. 51 | * @return The log odds value. 52 | */ 53 | float getValue() const 54 | { 55 | return simpleOccVal; 56 | } 57 | 58 | /** 59 | * Returns wether the cell is occupied. 60 | * @return Cell is occupied 61 | */ 62 | bool isOccupied() const 63 | { 64 | return (simpleOccVal > 0.5f); 65 | } 66 | 67 | bool isFree() const 68 | { 69 | return (simpleOccVal < 0.5f); 70 | } 71 | 72 | /** 73 | * Reset Cell to prior probability. 74 | */ 75 | void resetGridCell() 76 | { 77 | simpleOccVal = 0.5f; 78 | updateIndex = -1; 79 | } 80 | 81 | //protected: 82 | 83 | public: 84 | 85 | float simpleOccVal; ///< The log odds representation of occupancy probability. 86 | int updateIndex; 87 | 88 | 89 | }; 90 | 91 | /** 92 | * Provides functions related to a log odds of occupancy probability respresentation for cells in a occupancy grid map. 93 | */ 94 | class GridMapSimpleCountFunctions 95 | { 96 | public: 97 | 98 | /** 99 | * Constructor, sets parameters like free and occupied log odds ratios. 100 | */ 101 | GridMapSimpleCountFunctions() 102 | { 103 | updateFreeVal = -0.10f; 104 | updateOccVal = 0.15f; 105 | 106 | updateFreeLimit = -updateFreeVal + updateFreeVal/100.0f; 107 | updateOccLimit = 1.0f - (updateOccVal + updateOccVal/100.0f); 108 | } 109 | 110 | /** 111 | * Update cell as occupied 112 | * @param cell The cell. 113 | */ 114 | void updateSetOccupied(SimpleCountCell& cell) const 115 | { 116 | if (cell.simpleOccVal < updateOccLimit){ 117 | cell.simpleOccVal += updateOccVal; 118 | } 119 | } 120 | 121 | /** 122 | * Update cell as free 123 | * @param cell The cell. 124 | */ 125 | void updateSetFree(SimpleCountCell& cell) const 126 | { 127 | if (cell.simpleOccVal > updateFreeLimit){ 128 | cell.simpleOccVal += updateFreeVal; 129 | } 130 | } 131 | 132 | void updateUnsetFree(SimpleCountCell& cell) const 133 | { 134 | cell.simpleOccVal -= updateFreeVal; 135 | } 136 | 137 | /** 138 | * Get the probability value represented by the grid cell. 139 | * @param cell The cell. 140 | * @return The probability 141 | */ 142 | float getGridProbability(const SimpleCountCell& cell) const 143 | { 144 | return cell.simpleOccVal; 145 | } 146 | 147 | protected: 148 | 149 | float updateFreeVal; 150 | float updateOccVal; 151 | 152 | float updateFreeLimit; 153 | float updateOccLimit; 154 | 155 | 156 | }; 157 | 158 | 159 | #endif 160 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/map/MapDimensionProperties.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef __MapDimensionProperties_h_ 30 | #define __MapDimensionProperties_h_ 31 | 32 | class MapDimensionProperties 33 | { 34 | public: 35 | MapDimensionProperties() 36 | : topLeftOffset(-1.0f,-1.0f) 37 | , mapDimensions(-1,-1) 38 | , cellLength(-1.0f) 39 | {} 40 | 41 | 42 | MapDimensionProperties(const Eigen::Vector2f& topLeftOffsetIn, const Eigen::Vector2i& mapDimensionsIn, float cellLengthIn) 43 | : topLeftOffset(topLeftOffsetIn) 44 | , mapDimensions(mapDimensionsIn) 45 | , cellLength(cellLengthIn) 46 | { 47 | mapLimitsf = (mapDimensionsIn.cast()).array() - 1.0f; 48 | } 49 | 50 | bool operator==(const MapDimensionProperties& other) const 51 | { 52 | return (topLeftOffset == other.topLeftOffset) && (mapDimensions == other.mapDimensions) && (cellLength == other.cellLength); 53 | } 54 | 55 | bool hasEqualDimensionProperties(const MapDimensionProperties& other) const 56 | { 57 | return (mapDimensions == other.mapDimensions); 58 | } 59 | 60 | bool hasEqualTransformationProperties(const MapDimensionProperties& other) const 61 | { 62 | return (topLeftOffset == other.topLeftOffset) && (cellLength == other.cellLength); 63 | } 64 | 65 | bool pointOutOfMapBounds(const Eigen::Vector2f& coords) const 66 | { 67 | return ((coords[0] < 0.0f) || (coords[0] > mapLimitsf[0]) || (coords[1] < 0.0f) || (coords[1] > mapLimitsf[1])); 68 | } 69 | 70 | void setMapCellDims(const Eigen::Vector2i& newDims) 71 | { 72 | mapDimensions = newDims; 73 | mapLimitsf = (newDims.cast()).array() - 2.0f; 74 | } 75 | 76 | void setTopLeftOffset(const Eigen::Vector2f& topLeftOffsetIn) 77 | { 78 | topLeftOffset = topLeftOffsetIn; 79 | } 80 | 81 | void setSizeX(int sX) { mapDimensions[0] = sX; }; 82 | void setSizeY(int sY) { mapDimensions[1] = sY; }; 83 | void setCellLength(float cl) { cellLength = cl; }; 84 | 85 | const Eigen::Vector2f& getTopLeftOffset() const { return topLeftOffset; }; 86 | const Eigen::Vector2i& getMapDimensions() const { return mapDimensions; }; 87 | int getSizeX() const { return mapDimensions[0]; }; 88 | int getSizeY() const { return mapDimensions[1]; }; 89 | float getCellLength() const { return cellLength; }; 90 | 91 | protected: 92 | Eigen::Vector2f topLeftOffset; 93 | Eigen::Vector2i mapDimensions; 94 | Eigen::Vector2f mapLimitsf; 95 | float cellLength; 96 | }; 97 | 98 | #endif 99 | 100 | 101 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/map/OccGridMapUtilConfig.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef __OccGridMapUtilConfig_h_ 30 | #define __OccGridMapUtilConfig_h_ 31 | 32 | #include "OccGridMapUtil.h" 33 | 34 | //#define SLAM_USE_HASH_CACHING 35 | #ifdef SLAM_USE_HASH_CACHING 36 | #include "GridMapCacheHash.h" 37 | typedef GridMapCacheHash GridMapCacheMethod; 38 | #else 39 | #include "GridMapCacheArray.h" 40 | typedef GridMapCacheArray GridMapCacheMethod; 41 | #endif 42 | 43 | namespace hectorslam { 44 | 45 | template 46 | class OccGridMapUtilConfig 47 | : public OccGridMapUtil 48 | { 49 | public: 50 | 51 | OccGridMapUtilConfig(ConcreteOccGridMap* gridMap = 0) 52 | : OccGridMapUtil(gridMap) 53 | {} 54 | }; 55 | 56 | } 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/scan/DataPointContainer.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef __DataPointContainer_h_ 30 | #define __DataPointContainer_h_ 31 | 32 | #include 33 | 34 | namespace hectorslam { 35 | 36 | template 37 | class DataPointContainer 38 | { 39 | public: 40 | 41 | DataPointContainer(int size = 1000) 42 | { 43 | dataPoints.reserve(size); 44 | } 45 | 46 | void setFrom(const DataPointContainer& other, float factor) 47 | { 48 | origo = other.getOrigo()*factor; 49 | 50 | dataPoints = other.dataPoints; 51 | 52 | unsigned int size = dataPoints.size(); 53 | 54 | for (unsigned int i = 0; i < size; ++i){ 55 | dataPoints[i] *= factor; 56 | } 57 | 58 | } 59 | 60 | void add(const DataPointType& dataPoint) 61 | { 62 | dataPoints.push_back(dataPoint); 63 | } 64 | 65 | void clear() 66 | { 67 | dataPoints.clear(); 68 | } 69 | 70 | int getSize() const 71 | { 72 | return dataPoints.size(); 73 | } 74 | 75 | const DataPointType& getVecEntry(int index) const 76 | { 77 | return dataPoints[index]; 78 | } 79 | 80 | DataPointType getOrigo() const 81 | { 82 | return origo; 83 | } 84 | 85 | void setOrigo(const DataPointType& origoIn) 86 | { 87 | origo = origoIn; 88 | } 89 | 90 | protected: 91 | 92 | std::vector dataPoints; 93 | DataPointType origo; 94 | }; 95 | 96 | typedef DataPointContainer DataContainer; 97 | 98 | } 99 | 100 | #endif 101 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/slam_main/HectorSlamProcessor.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef _hectorslamprocessor_h__ 30 | #define _hectorslamprocessor_h__ 31 | 32 | #include "../map/GridMap.h" 33 | #include "../map/OccGridMapUtilConfig.h" 34 | #include "../matcher/ScanMatcher.h" 35 | #include "../scan/DataPointContainer.h" 36 | 37 | #include "../util/UtilFunctions.h" 38 | #include "../util/DrawInterface.h" 39 | #include "../util/HectorDebugInfoInterface.h" 40 | #include "../util/MapLockerInterface.h" 41 | 42 | #include "MapRepresentationInterface.h" 43 | #include "MapRepMultiMap.h" 44 | 45 | 46 | #include 47 | 48 | namespace hectorslam{ 49 | 50 | class HectorSlamProcessor 51 | { 52 | public: 53 | 54 | HectorSlamProcessor(float mapResolution, int mapSizeX, int mapSizeY , const Eigen::Vector2f& startCoords, int multi_res_size, DrawInterface* drawInterfaceIn = 0, HectorDebugInfoInterface* debugInterfaceIn = 0) 55 | : drawInterface(drawInterfaceIn) 56 | , debugInterface(debugInterfaceIn) 57 | { 58 | mapRep = new MapRepMultiMap(mapResolution, mapSizeX, mapSizeY, multi_res_size, startCoords, drawInterfaceIn, debugInterfaceIn); 59 | 60 | this->reset(); 61 | 62 | this->setMapUpdateMinDistDiff(0.4f *1.0f); 63 | this->setMapUpdateMinAngleDiff(0.13f * 1.0f); 64 | } 65 | 66 | ~HectorSlamProcessor() 67 | { 68 | delete mapRep; 69 | } 70 | 71 | void update(const DataContainer& dataContainer, const Eigen::Vector3f& poseHintWorld, bool map_without_matching = false) 72 | { 73 | //std::cout << "\nph:\n" << poseHintWorld << "\n"; 74 | 75 | Eigen::Vector3f newPoseEstimateWorld; 76 | 77 | if (!map_without_matching){ 78 | newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov)); 79 | }else{ 80 | newPoseEstimateWorld = poseHintWorld; 81 | } 82 | 83 | lastScanMatchPose = newPoseEstimateWorld; 84 | 85 | //std::cout << "\nt1:\n" << newPoseEstimateWorld << "\n"; 86 | 87 | //std::cout << "\n1"; 88 | //std::cout << "\n" << lastScanMatchPose << "\n"; 89 | if(util::poseDifferenceLargerThan(newPoseEstimateWorld, lastMapUpdatePose, paramMinDistanceDiffForMapUpdate, paramMinAngleDiffForMapUpdate) || map_without_matching){ 90 | 91 | mapRep->updateByScan(dataContainer, newPoseEstimateWorld); 92 | 93 | mapRep->onMapUpdated(); 94 | lastMapUpdatePose = newPoseEstimateWorld; 95 | } 96 | 97 | if(drawInterface){ 98 | const GridMap& gridMapRef (mapRep->getGridMap()); 99 | drawInterface->setColor(1.0, 0.0, 0.0); 100 | drawInterface->setScale(0.15); 101 | 102 | drawInterface->drawPoint(gridMapRef.getWorldCoords(Eigen::Vector2f::Zero())); 103 | drawInterface->drawPoint(gridMapRef.getWorldCoords((gridMapRef.getMapDimensions().array()-1).cast())); 104 | drawInterface->drawPoint(Eigen::Vector2f(1.0f, 1.0f)); 105 | 106 | drawInterface->sendAndResetData(); 107 | } 108 | 109 | if (debugInterface) 110 | { 111 | debugInterface->sendAndResetData(); 112 | } 113 | } 114 | 115 | void reset() 116 | { 117 | lastMapUpdatePose = Eigen::Vector3f(FLT_MAX, FLT_MAX, FLT_MAX); 118 | lastScanMatchPose = Eigen::Vector3f::Zero(); 119 | //lastScanMatchPose.x() = -10.0f; 120 | //lastScanMatchPose.y() = -15.0f; 121 | //lastScanMatchPose.z() = M_PI*0.15f; 122 | 123 | mapRep->reset(); 124 | } 125 | 126 | const Eigen::Vector3f& getLastScanMatchPose() const { return lastScanMatchPose; }; 127 | const Eigen::Matrix3f& getLastScanMatchCovariance() const { return lastScanMatchCov; }; 128 | float getScaleToMap() const { return mapRep->getScaleToMap(); }; 129 | 130 | int getMapLevels() const { return mapRep->getMapLevels(); }; 131 | const GridMap& getGridMap(int mapLevel = 0) const { return mapRep->getGridMap(mapLevel); }; 132 | 133 | void addMapMutex(int i, MapLockerInterface* mapMutex) { mapRep->addMapMutex(i, mapMutex); }; 134 | MapLockerInterface* getMapMutex(int i) { return mapRep->getMapMutex(i); }; 135 | 136 | void setUpdateFactorFree(float free_factor) { mapRep->setUpdateFactorFree(free_factor); }; 137 | void setUpdateFactorOccupied(float occupied_factor) { mapRep->setUpdateFactorOccupied(occupied_factor); }; 138 | void setMapUpdateMinDistDiff(float minDist) { paramMinDistanceDiffForMapUpdate = minDist; }; 139 | void setMapUpdateMinAngleDiff(float angleChange) { paramMinAngleDiffForMapUpdate = angleChange; }; 140 | 141 | protected: 142 | 143 | MapRepresentationInterface* mapRep; 144 | 145 | Eigen::Vector3f lastMapUpdatePose; 146 | Eigen::Vector3f lastScanMatchPose; 147 | Eigen::Matrix3f lastScanMatchCov; 148 | 149 | float paramMinDistanceDiffForMapUpdate; 150 | float paramMinAngleDiffForMapUpdate; 151 | 152 | DrawInterface* drawInterface; 153 | HectorDebugInfoInterface* debugInterface; 154 | }; 155 | 156 | } 157 | 158 | #endif 159 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/slam_main/MapProcContainer.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef _hectormapproccontainer_h__ 30 | #define _hectormapproccontainer_h__ 31 | 32 | #include "../map/GridMap.h" 33 | #include "../map/OccGridMapUtilConfig.h" 34 | #include "../matcher/ScanMatcher.h" 35 | #include "../util/MapLockerInterface.h" 36 | 37 | class GridMap; 38 | class ConcreteOccGridMapUtil; 39 | class DataContainer; 40 | 41 | namespace hectorslam{ 42 | 43 | class MapProcContainer 44 | { 45 | public: 46 | MapProcContainer(GridMap* gridMapIn, OccGridMapUtilConfig* gridMapUtilIn, ScanMatcher >* scanMatcherIn) 47 | : gridMap(gridMapIn) 48 | , gridMapUtil(gridMapUtilIn) 49 | , scanMatcher(scanMatcherIn) 50 | , mapMutex(0) 51 | {} 52 | 53 | virtual ~MapProcContainer() 54 | {} 55 | 56 | void cleanup() 57 | { 58 | delete gridMap; 59 | delete gridMapUtil; 60 | delete scanMatcher; 61 | 62 | if (mapMutex){ 63 | delete mapMutex; 64 | } 65 | } 66 | 67 | void reset() 68 | { 69 | gridMap->reset(); 70 | gridMapUtil->resetCachedData(); 71 | } 72 | 73 | void resetCachedData() 74 | { 75 | gridMapUtil->resetCachedData(); 76 | } 77 | 78 | float getScaleToMap() const { return gridMap->getScaleToMap(); }; 79 | 80 | const GridMap& getGridMap() const { return *gridMap; }; 81 | GridMap& getGridMap() { return *gridMap; }; 82 | 83 | void addMapMutex(MapLockerInterface* mapMutexIn) 84 | { 85 | if (mapMutex) 86 | { 87 | delete mapMutex; 88 | } 89 | 90 | mapMutex = mapMutexIn; 91 | } 92 | 93 | MapLockerInterface* getMapMutex() 94 | { 95 | return mapMutex; 96 | } 97 | 98 | Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix, int maxIterations) 99 | { 100 | return scanMatcher->matchData(beginEstimateWorld, *gridMapUtil, dataContainer, covMatrix, maxIterations); 101 | } 102 | 103 | void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld) 104 | { 105 | if (mapMutex) 106 | { 107 | mapMutex->lockMap(); 108 | } 109 | 110 | gridMap->updateByScan(dataContainer, robotPoseWorld); 111 | 112 | if (mapMutex) 113 | { 114 | mapMutex->unlockMap(); 115 | } 116 | } 117 | 118 | GridMap* gridMap; 119 | OccGridMapUtilConfig* gridMapUtil; 120 | ScanMatcher >* scanMatcher; 121 | MapLockerInterface* mapMutex; 122 | }; 123 | 124 | } 125 | 126 | #endif 127 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/slam_main/MapRepMultiMap.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef _hectormaprepmultimap_h__ 30 | #define _hectormaprepmultimap_h__ 31 | 32 | #include "MapRepresentationInterface.h" 33 | #include "MapProcContainer.h" 34 | 35 | #include "../map/GridMap.h" 36 | #include "../map/OccGridMapUtilConfig.h" 37 | #include "../matcher/ScanMatcher.h" 38 | 39 | #include "../util/DrawInterface.h" 40 | #include "../util/HectorDebugInfoInterface.h" 41 | 42 | namespace hectorslam{ 43 | 44 | class MapRepMultiMap : public MapRepresentationInterface 45 | { 46 | 47 | public: 48 | MapRepMultiMap(float mapResolution, int mapSizeX, int mapSizeY, unsigned int numDepth, const Eigen::Vector2f& startCoords, DrawInterface* drawInterfaceIn, HectorDebugInfoInterface* debugInterfaceIn) 49 | { 50 | //unsigned int numDepth = 3; 51 | Eigen::Vector2i resolution(mapSizeX, mapSizeY); 52 | 53 | float totalMapSizeX = mapResolution * static_cast(mapSizeX); 54 | float mid_offset_x = totalMapSizeX * startCoords.x(); 55 | 56 | float totalMapSizeY = mapResolution * static_cast(mapSizeY); 57 | float mid_offset_y = totalMapSizeY * startCoords.y(); 58 | 59 | for (unsigned int i = 0; i < numDepth; ++i){ 60 | std::cout << "HectorSM map lvl " << i << ": cellLength: " << mapResolution << " res x:" << resolution.x() << " res y: " << resolution.y() << "\n"; 61 | GridMap* gridMap = new hectorslam::GridMap(mapResolution,resolution, Eigen::Vector2f(mid_offset_x, mid_offset_y)); 62 | OccGridMapUtilConfig* gridMapUtil = new OccGridMapUtilConfig(gridMap); 63 | ScanMatcher >* scanMatcher = new hectorslam::ScanMatcher >(drawInterfaceIn, debugInterfaceIn); 64 | 65 | mapContainer.push_back(MapProcContainer(gridMap, gridMapUtil, scanMatcher)); 66 | 67 | resolution /= 2; 68 | mapResolution*=2.0f; 69 | } 70 | 71 | dataContainers.resize(numDepth-1); 72 | } 73 | 74 | virtual ~MapRepMultiMap() 75 | { 76 | unsigned int size = mapContainer.size(); 77 | 78 | for (unsigned int i = 0; i < size; ++i){ 79 | mapContainer[i].cleanup(); 80 | } 81 | } 82 | 83 | virtual void reset() 84 | { 85 | unsigned int size = mapContainer.size(); 86 | 87 | for (unsigned int i = 0; i < size; ++i){ 88 | mapContainer[i].reset(); 89 | } 90 | } 91 | 92 | virtual float getScaleToMap() const { return mapContainer[0].getScaleToMap(); }; 93 | 94 | virtual int getMapLevels() const { return mapContainer.size(); }; 95 | virtual const GridMap& getGridMap(int mapLevel) const { return mapContainer[mapLevel].getGridMap(); }; 96 | 97 | virtual void addMapMutex(int i, MapLockerInterface* mapMutex) 98 | { 99 | mapContainer[i].addMapMutex(mapMutex); 100 | } 101 | 102 | MapLockerInterface* getMapMutex(int i) 103 | { 104 | return mapContainer[i].getMapMutex(); 105 | } 106 | 107 | virtual void onMapUpdated() 108 | { 109 | unsigned int size = mapContainer.size(); 110 | 111 | for (unsigned int i = 0; i < size; ++i){ 112 | mapContainer[i].resetCachedData(); 113 | } 114 | } 115 | 116 | virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix) 117 | { 118 | size_t size = mapContainer.size(); 119 | 120 | Eigen::Vector3f tmp(beginEstimateWorld); 121 | 122 | for (int index = size - 1; index >= 0; --index){ 123 | //std::cout << " m " << i; 124 | if (index == 0){ 125 | tmp = (mapContainer[index].matchData(tmp, dataContainer, covMatrix, 5)); 126 | }else{ 127 | dataContainers[index-1].setFrom(dataContainer, static_cast(1.0 / pow(2.0, static_cast(index)))); 128 | tmp = (mapContainer[index].matchData(tmp, dataContainers[index-1], covMatrix, 3)); 129 | } 130 | } 131 | return tmp; 132 | } 133 | 134 | virtual void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld) 135 | { 136 | unsigned int size = mapContainer.size(); 137 | 138 | for (unsigned int i = 0; i < size; ++i){ 139 | //std::cout << " u " << i; 140 | if (i==0){ 141 | mapContainer[i].updateByScan(dataContainer, robotPoseWorld); 142 | }else{ 143 | mapContainer[i].updateByScan(dataContainers[i-1], robotPoseWorld); 144 | } 145 | } 146 | //std::cout << "\n"; 147 | } 148 | 149 | virtual void setUpdateFactorFree(float free_factor) 150 | { 151 | size_t size = mapContainer.size(); 152 | 153 | for (unsigned int i = 0; i < size; ++i){ 154 | GridMap& map = mapContainer[i].getGridMap(); 155 | map.setUpdateFreeFactor(free_factor); 156 | } 157 | } 158 | 159 | virtual void setUpdateFactorOccupied(float occupied_factor) 160 | { 161 | size_t size = mapContainer.size(); 162 | 163 | for (unsigned int i = 0; i < size; ++i){ 164 | GridMap& map = mapContainer[i].getGridMap(); 165 | map.setUpdateOccupiedFactor(occupied_factor); 166 | } 167 | } 168 | 169 | protected: 170 | std::vector mapContainer; 171 | std::vector dataContainers; 172 | }; 173 | 174 | } 175 | 176 | #endif 177 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/slam_main/MapRepSingleMap.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef _hectormaprepsinglemap_h__ 30 | #define _hectormaprepsinglemap_h__ 31 | 32 | #include "MapRepresentationInterface.h" 33 | 34 | #include "../map/GridMap.h" 35 | #include "../map/OccGridMapUtilConfig.h" 36 | #include "../matcher/ScanMatcher.h" 37 | 38 | #include "../util/DrawInterface.h" 39 | #include "../util/HectorDebugInfoInterface.h" 40 | 41 | namespace hectorslam{ 42 | 43 | class MapRepSingleMap : public MapRepresentationInterface 44 | { 45 | 46 | public: 47 | MapRepSingleMap(float mapResolution, DrawInterface* drawInterfaceIn, HectorDebugInfoInterface* debugInterfaceIn) 48 | { 49 | gridMap = new hectorslam::GridMap(mapResolution,Eigen::Vector2i(1024,1024), Eigen::Vector2f(20.0f, 20.0f)); 50 | gridMapUtil = new OccGridMapUtilConfig(gridMap); 51 | scanMatcher = new hectorslam::ScanMatcher >(drawInterfaceIn, debugInterfaceIn); 52 | } 53 | 54 | virtual ~MapRepSingleMap() 55 | { 56 | delete gridMap; 57 | delete gridMapUtil; 58 | delete scanMatcher; 59 | } 60 | 61 | virtual void reset() 62 | { 63 | gridMap->reset(); 64 | gridMapUtil->resetCachedData(); 65 | } 66 | 67 | virtual float getScaleToMap() const { return gridMap->getScaleToMap(); }; 68 | 69 | virtual int getMapLevels() const { return 1; }; 70 | virtual const GridMap& getGridMap(int mapLevel) const { return *gridMap; }; 71 | 72 | virtual void onMapUpdated() 73 | { 74 | gridMapUtil->resetCachedData(); 75 | } 76 | 77 | virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix) 78 | { 79 | return scanMatcher->matchData(beginEstimateWorld, *gridMapUtil, dataContainer, covMatrix, 20); 80 | } 81 | 82 | virtual void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld) 83 | { 84 | gridMap->updateByScan(dataContainer, robotPoseWorld); 85 | } 86 | 87 | protected: 88 | GridMap* gridMap; 89 | OccGridMapUtilConfig* gridMapUtil; 90 | ScanMatcher >* scanMatcher; 91 | }; 92 | 93 | } 94 | 95 | #endif 96 | 97 | 98 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/slam_main/MapRepresentationInterface.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef _hectormaprepresentationinterface_h__ 30 | #define _hectormaprepresentationinterface_h__ 31 | 32 | class GridMap; 33 | class ConcreteOccGridMapUtil; 34 | class DataContainer; 35 | 36 | namespace hectorslam{ 37 | 38 | class MapRepresentationInterface 39 | { 40 | public: 41 | 42 | virtual ~MapRepresentationInterface() {}; 43 | 44 | virtual void reset() = 0; 45 | 46 | virtual float getScaleToMap() const = 0; 47 | 48 | virtual int getMapLevels() const = 0; 49 | virtual const GridMap& getGridMap(int mapLevel = 0) const = 0; 50 | 51 | virtual void addMapMutex(int i, MapLockerInterface* mapMutex) = 0; 52 | virtual MapLockerInterface* getMapMutex(int i) = 0; 53 | 54 | virtual void onMapUpdated() = 0; 55 | 56 | virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix) = 0; 57 | 58 | virtual void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld) = 0; 59 | 60 | virtual void setUpdateFactorFree(float free_factor) = 0; 61 | virtual void setUpdateFactorOccupied(float occupied_factor) = 0; 62 | }; 63 | 64 | } 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/util/DrawInterface.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef drawinterface_h__ 30 | #define drawinterface_h__ 31 | 32 | class DrawInterface{ 33 | public: 34 | virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame) = 0; 35 | virtual void drawArrow(const Eigen::Vector3f& poseWorld) = 0; 36 | virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& cov) = 0; 37 | 38 | virtual void setScale(double scale) = 0; 39 | virtual void setColor(double r, double g, double b, double a = 1.0) = 0; 40 | 41 | virtual void sendAndResetData() = 0; 42 | }; 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/util/HectorDebugInfoInterface.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef hectordebuginfointerface_h__ 30 | #define hectordebuginfointerface_h__ 31 | 32 | class HectorDebugInfoInterface{ 33 | public: 34 | 35 | virtual void sendAndResetData() = 0; 36 | virtual void addHessianMatrix(const Eigen::Matrix3f& hessian) = 0; 37 | virtual void addPoseLikelihood(float lh) = 0; 38 | }; 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/util/MapLockerInterface.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef maplockerinterface_h__ 30 | #define maplockerinterface_h__ 31 | 32 | class MapLockerInterface 33 | { 34 | public: 35 | virtual void lockMap() = 0; 36 | virtual void unlockMap() = 0; 37 | }; 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /hector_mapping/include/hector_slam_lib/util/UtilFunctions.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef utilfunctions_h__ 30 | #define utilfunctions_h__ 31 | 32 | #include 33 | #include 34 | 35 | namespace util{ 36 | 37 | static inline float normalize_angle_pos(float angle) 38 | { 39 | return fmod(fmod(angle, 2.0f*M_PI) + 2.0f*M_PI, 2.0f*M_PI); 40 | } 41 | 42 | static inline float normalize_angle(float angle) 43 | { 44 | float a = normalize_angle_pos(angle); 45 | if (a > M_PI){ 46 | a -= 2.0f*M_PI; 47 | } 48 | return a; 49 | } 50 | 51 | static inline float sqr(float val) 52 | { 53 | return val*val; 54 | } 55 | 56 | static inline int sign(int x) 57 | { 58 | return x > 0 ? 1 : -1; 59 | } 60 | 61 | template 62 | static T toDeg(const T radVal) 63 | { 64 | return radVal * static_cast(180.0 / M_PI); 65 | } 66 | 67 | template 68 | static T toRad(const T degVal) 69 | { 70 | return degVal * static_cast(M_PI / 180.0); 71 | } 72 | 73 | static bool poseDifferenceLargerThan(const Eigen::Vector3f& pose1, const Eigen::Vector3f& pose2, float distanceDiffThresh, float angleDiffThresh) 74 | { 75 | //check distance 76 | if ( ( (pose1.head<2>() - pose2.head<2>()).norm() ) > distanceDiffThresh){ 77 | return true; 78 | } 79 | 80 | float angleDiff = (pose1.z() - pose2.z()); 81 | 82 | if (angleDiff > M_PI) { 83 | angleDiff -= M_PI * 2.0f; 84 | } else if (angleDiff < -M_PI) { 85 | angleDiff += M_PI * 2.0f; 86 | } 87 | 88 | if (abs(angleDiff) > angleDiffThresh){ 89 | return true; 90 | } 91 | return false; 92 | } 93 | 94 | static double getYawFromQuat(const geometry_msgs::Quaternion &quat) 95 | { 96 | return tf::getYaw(tf::Quaternion(quat.x, quat.y, quat.z, quat.w)); 97 | } 98 | 99 | } 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /hector_mapping/launch/mapping_default.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 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /hector_mapping/msg/HectorDebugInfo.msg: -------------------------------------------------------------------------------- 1 | HectorIterData[] iterData -------------------------------------------------------------------------------- /hector_mapping/msg/HectorIterData.msg: -------------------------------------------------------------------------------- 1 | float64[9] hessian 2 | float64 conditionNum 3 | float64 determinant 4 | float64 conditionNum2d 5 | float64 determinant2d 6 | -------------------------------------------------------------------------------- /hector_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_mapping 4 | 0.5.2 5 | 6 | hector_mapping is a SLAM approach that can be used without odometry as well as on platforms that exhibit roll/pitch motion (of the sensor, the platform or both). 7 | It leverages the high update rate of modern LIDAR systems like the Hokuyo UTM-30LX and provides 2D pose estimates at scan rate of the sensors (40Hz for the UTM-30LX). 8 | While the system does not provide explicit loop closing ability, it is sufficiently accurate for many real world scenarios. The system has successfully been used on 9 | Unmanned Ground Robots, Unmanned Surface Vehicles, Handheld Mapping Devices and logged data from quadrotor UAVs. 10 | 11 | 12 | 13 | Johannes Meyer 14 | 15 | 16 | 17 | 18 | 19 | BSD 20 | 21 | 22 | 23 | 24 | http://ros.org/wiki/hector_mapping 25 | 26 | 27 | 28 | 29 | Stefan Kohlbrecher 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | catkin 44 | roscpp 45 | nav_msgs 46 | visualization_msgs 47 | tf 48 | message_filters 49 | laser_geometry 50 | eigen 51 | boost 52 | message_generation 53 | roscpp 54 | nav_msgs 55 | visualization_msgs 56 | tf 57 | message_filters 58 | laser_geometry 59 | eigen 60 | boost 61 | message_runtime 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /hector_mapping/src/HectorDebugInfoProvider.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef HECTOR_DEBUG_INFO_PROVIDER_H__ 30 | #define HECTOR_DEBUG_INFO_PROVIDER_H__ 31 | 32 | #include "util/HectorDebugInfoInterface.h" 33 | #include "util/UtilFunctions.h" 34 | 35 | #include "ros/ros.h" 36 | 37 | #include "hector_mapping/HectorDebugInfo.h" 38 | 39 | 40 | class HectorDebugInfoProvider : public HectorDebugInfoInterface 41 | { 42 | public: 43 | 44 | HectorDebugInfoProvider() 45 | { 46 | ros::NodeHandle nh_; 47 | 48 | debugInfoPublisher_ = nh_.advertise("hector_debug_info", 50, true); 49 | }; 50 | 51 | virtual void sendAndResetData() 52 | { 53 | debugInfoPublisher_.publish(debugInfo); 54 | debugInfo.iterData.clear(); 55 | } 56 | 57 | 58 | virtual void addHessianMatrix(const Eigen::Matrix3f& hessian) 59 | { 60 | hector_mapping::HectorIterData iterData; 61 | 62 | for (int i=0; i < 9; ++i){ 63 | iterData.hessian[i] = static_cast(hessian.data()[i]); 64 | iterData.determinant = hessian.determinant(); 65 | 66 | Eigen::SelfAdjointEigenSolver eig(hessian); 67 | 68 | const Eigen::Vector3f& eigValues (eig.eigenvalues()); 69 | iterData.conditionNum = eigValues[2] / eigValues[0]; 70 | 71 | 72 | iterData.determinant2d = hessian.block<2,2>(0,0).determinant(); 73 | Eigen::SelfAdjointEigenSolver eig2d(hessian.block<2,2>(0,0)); 74 | 75 | const Eigen::Vector2f& eigValues2d (eig2d.eigenvalues()); 76 | iterData.conditionNum2d = eigValues2d[1] / eigValues2d[0]; 77 | } 78 | 79 | debugInfo.iterData.push_back(iterData); 80 | } 81 | 82 | virtual void addPoseLikelihood(float lh) 83 | { 84 | 85 | } 86 | 87 | 88 | hector_mapping::HectorDebugInfo debugInfo; 89 | 90 | ros::Publisher debugInfoPublisher_; 91 | 92 | }; 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /hector_mapping/src/HectorDrawings.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef HECTOR_DRAWINGS_H__ 30 | #define HECTOR_DRAWINGS_H__ 31 | 32 | #include "util/DrawInterface.h" 33 | #include "util/UtilFunctions.h" 34 | 35 | #include "ros/ros.h" 36 | 37 | #include 38 | 39 | #include 40 | 41 | 42 | class HectorDrawings : public DrawInterface 43 | { 44 | public: 45 | 46 | HectorDrawings() 47 | { 48 | idCounter = 0; 49 | 50 | ros::NodeHandle nh_; 51 | 52 | markerPublisher_ = nh_.advertise("visualization_marker", 1, true); 53 | markerArrayPublisher_ = nh_.advertise("visualization_marker_array", 1, true); 54 | 55 | tempMarker.header.frame_id = "map"; 56 | tempMarker.ns = "slam"; 57 | 58 | this->setScale(1.0); 59 | this->setColor(1.0, 1.0, 1.0); 60 | 61 | tempMarker.action = visualization_msgs::Marker::ADD; 62 | }; 63 | 64 | virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame) 65 | { 66 | tempMarker.id = idCounter++; 67 | 68 | tempMarker.pose.position.x = pointWorldFrame.x(); 69 | tempMarker.pose.position.y = pointWorldFrame.y(); 70 | 71 | tempMarker.pose.orientation.w = 0.0; 72 | tempMarker.pose.orientation.z = 0.0; 73 | tempMarker.type = visualization_msgs::Marker::CUBE; 74 | 75 | //markerPublisher_.publish(tempMarker); 76 | 77 | markerArray.markers.push_back(tempMarker); 78 | } 79 | 80 | virtual void drawArrow(const Eigen::Vector3f& poseWorld) 81 | { 82 | tempMarker.id = idCounter++; 83 | 84 | tempMarker.pose.position.x = poseWorld.x(); 85 | tempMarker.pose.position.y = poseWorld.y(); 86 | 87 | tempMarker.pose.orientation.w = cos(poseWorld.z()*0.5f); 88 | tempMarker.pose.orientation.z = sin(poseWorld.z()*0.5f); 89 | 90 | tempMarker.type = visualization_msgs::Marker::ARROW; 91 | 92 | //markerPublisher_.publish(tempMarker); 93 | 94 | markerArray.markers.push_back(tempMarker); 95 | 96 | } 97 | 98 | virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& covMatrix) 99 | { 100 | tempMarker.pose.position.x = mean[0]; 101 | tempMarker.pose.position.y = mean[1]; 102 | 103 | Eigen::SelfAdjointEigenSolver eig(covMatrix); 104 | 105 | const Eigen::Vector2f& eigValues (eig.eigenvalues()); 106 | const Eigen::Matrix2f& eigVectors (eig.eigenvectors()); 107 | 108 | float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0))); 109 | 110 | tempMarker.type = visualization_msgs::Marker::CYLINDER; 111 | 112 | double lengthMajor = sqrt(eigValues[0]); 113 | double lengthMinor = sqrt(eigValues[1]); 114 | 115 | tempMarker.scale.x = lengthMajor; 116 | tempMarker.scale.y = lengthMinor; 117 | tempMarker.scale.z = 0.001; 118 | 119 | 120 | tempMarker.pose.orientation.w = cos(angle*0.5); 121 | tempMarker.pose.orientation.z = sin(angle*0.5); 122 | 123 | tempMarker.id = idCounter++; 124 | markerArray.markers.push_back(tempMarker); 125 | 126 | //drawLine(Eigen::Vector3f(0,0,0), Eigen::Vector3f(lengthMajor ,0,0)); 127 | //drawLine(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0,lengthMinor,0)); 128 | 129 | //glScalef(lengthMajor, lengthMinor, 0); 130 | //glCallList(dlCircle); 131 | //this->popCS(); 132 | } 133 | 134 | virtual void setScale(double scale) 135 | { 136 | tempMarker.scale.x = scale; 137 | tempMarker.scale.y = scale; 138 | tempMarker.scale.z = scale; 139 | } 140 | 141 | virtual void setColor(double r, double g, double b, double a = 1.0) 142 | { 143 | tempMarker.color.r = r; 144 | tempMarker.color.g = g; 145 | tempMarker.color.b = b; 146 | tempMarker.color.a = a; 147 | } 148 | 149 | virtual void sendAndResetData() 150 | { 151 | markerArrayPublisher_.publish(markerArray); 152 | markerArray.markers.clear(); 153 | idCounter = 0; 154 | } 155 | 156 | void setTime(const ros::Time& time) 157 | { 158 | tempMarker.header.stamp = time; 159 | } 160 | 161 | 162 | ros::Publisher markerPublisher_; 163 | ros::Publisher markerArrayPublisher_; 164 | 165 | visualization_msgs::Marker tempMarker; 166 | visualization_msgs::MarkerArray markerArray; 167 | 168 | int idCounter; 169 | }; 170 | 171 | #endif 172 | -------------------------------------------------------------------------------- /hector_mapping/src/HectorMapMutex.h: -------------------------------------------------------------------------------- 1 | #ifndef hectormapmutex_h__ 2 | #define hectormapmutex_h__ 3 | 4 | #include "util/MapLockerInterface.h" 5 | 6 | #include 7 | 8 | class HectorMapMutex : public MapLockerInterface 9 | { 10 | public: 11 | virtual void lockMap() 12 | { 13 | mapModifyMutex_.lock(); 14 | } 15 | 16 | virtual void unlockMap() 17 | { 18 | mapModifyMutex_.unlock(); 19 | } 20 | 21 | boost::mutex mapModifyMutex_; 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /hector_mapping/src/PoseInfoContainer.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #include "PoseInfoContainer.h" 30 | 31 | void PoseInfoContainer::update(const Eigen::Vector3f& slamPose, const Eigen::Matrix3f& slamCov, const ros::Time& stamp, const std::string& frame_id) 32 | { 33 | //Fill stampedPose 34 | std_msgs::Header& header = stampedPose_.header; 35 | header.stamp = stamp; 36 | header.frame_id = frame_id; 37 | 38 | geometry_msgs::Pose& pose = stampedPose_.pose; 39 | pose.position.x = slamPose.x(); 40 | pose.position.y = slamPose.y(); 41 | 42 | pose.orientation.w = cos(slamPose.z()*0.5f); 43 | pose.orientation.z = sin(slamPose.z()*0.5f); 44 | 45 | //Fill covPose 46 | //geometry_msgs::PoseWithCovarianceStamped covPose; 47 | covPose_.header = header; 48 | covPose_.pose.pose = pose; 49 | 50 | boost::array& cov(covPose_.pose.covariance); 51 | 52 | cov[0] = static_cast(slamCov(0,0)); 53 | cov[7] = static_cast(slamCov(1,1)); 54 | cov[35] = static_cast(slamCov(2,2)); 55 | 56 | double xyC = static_cast(slamCov(0,1)); 57 | cov[1] = xyC; 58 | cov[6] = xyC; 59 | 60 | double xaC = static_cast(slamCov(0,2)); 61 | cov[5] = xaC; 62 | cov[30] = xaC; 63 | 64 | double yaC = static_cast(slamCov(1,2)); 65 | cov[11] = yaC; 66 | cov[31] = yaC; 67 | 68 | //Fill tf tansform 69 | tf::poseMsgToTF(pose, poseTransform_); 70 | } 71 | -------------------------------------------------------------------------------- /hector_mapping/src/PoseInfoContainer.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef POSE_INFO_CONTAINER_H__ 30 | #define POSE_INFO_CONTAINER_H__ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | class PoseInfoContainer{ 39 | public: 40 | 41 | void update(const Eigen::Vector3f& slamPose, const Eigen::Matrix3f& slamCov, const ros::Time& stamp, const std::string& frame_id); 42 | 43 | const geometry_msgs::PoseStamped& getPoseStamped() { return stampedPose_; }; 44 | const geometry_msgs::PoseWithCovarianceStamped& getPoseWithCovarianceStamped() { return covPose_; }; 45 | const tf::Transform& getTfTransform() { return poseTransform_; }; 46 | 47 | protected: 48 | geometry_msgs::PoseStamped stampedPose_; 49 | geometry_msgs::PoseWithCovarianceStamped covPose_; 50 | tf::Transform poseTransform_; 51 | 52 | }; 53 | 54 | #endif 55 | 56 | -------------------------------------------------------------------------------- /hector_mapping/src/main.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | 30 | #include 31 | 32 | #include "HectorMappingRos.h" 33 | 34 | int main(int argc, char** argv) 35 | { 36 | ros::init(argc, argv, "hector_slam"); 37 | 38 | HectorMappingRos sm; 39 | 40 | ros::spin(); 41 | 42 | return(0); 43 | } 44 | 45 | -------------------------------------------------------------------------------- /hector_mapping/src/main_mapper.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | 30 | #include 31 | 32 | #include "HectorMapperRos.h" 33 | 34 | int main(int argc, char** argv) 35 | { 36 | ros::init(argc, argv, "hector_slam"); 37 | 38 | HectorMapperRos sm; 39 | 40 | ros::spin(); 41 | 42 | return(0); 43 | } 44 | 45 | -------------------------------------------------------------------------------- /hector_mapping/srv/ResetMapping.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose initial_pose 2 | --- 3 | -------------------------------------------------------------------------------- /hector_marker_drawing/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_marker_drawing 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | 8 | 0.5.1 (2021-01-15) 9 | ------------------ 10 | 11 | 0.5.0 (2020-12-17) 12 | ------------------ 13 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 14 | Clean up for noetic release. 15 | * Bump CMake version to avoid CMP0048 warning 16 | * Contributors: Marius Schnaubelt, Stefan Fabian 17 | 18 | 0.4.1 (2020-05-15) 19 | ------------------ 20 | 21 | 0.3.6 (2019-10-31) 22 | ------------------ 23 | 24 | 0.3.5 (2016-06-24) 25 | ------------------ 26 | * Use the FindEigen3.cmake module provided by Eigen 27 | * Contributors: Johannes Meyer 28 | 29 | 0.3.4 (2015-11-07) 30 | ------------------ 31 | 32 | 0.3.3 (2014-06-15) 33 | ------------------ 34 | * fixed cmake find for eigen in indigo 35 | * Contributors: Alexander Stumpf 36 | 37 | 0.3.2 (2014-03-30) 38 | ------------------ 39 | 40 | 0.3.1 (2013-10-09) 41 | ------------------ 42 | * added changelogs 43 | 44 | 0.3.0 (2013-08-08) 45 | ------------------ 46 | * catkinized hector_slam 47 | -------------------------------------------------------------------------------- /hector_marker_drawing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_marker_drawing) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp visualization_msgs) 5 | 6 | find_package(Eigen3 REQUIRED) 7 | 8 | catkin_package( 9 | INCLUDE_DIRS include 10 | CATKIN_DEPENDS roscpp visualization_msgs 11 | DEPENDS EIGEN3 12 | ) 13 | 14 | ############# 15 | ## Install ## 16 | ############# 17 | 18 | # all install targets should use catkin DESTINATION variables 19 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 20 | 21 | install(DIRECTORY include/${PROJECT_NAME}/ 22 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 23 | FILES_MATCHING PATTERN "*.h" 24 | PATTERN ".svn" EXCLUDE 25 | ) 26 | -------------------------------------------------------------------------------- /hector_marker_drawing/include/hector_marker_drawing/DrawInterface.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 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 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Simulation, Systems Optimization and Robotics 13 | // group, TU Darmstadt nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef drawinterface_h__ 30 | #define drawinterface_h__ 31 | 32 | #include 33 | #include 34 | 35 | class DrawInterface{ 36 | public: 37 | virtual void setNamespace(const std::string& ns) = 0; 38 | 39 | virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame) = 0; 40 | virtual void drawArrow(const Eigen::Vector3f& poseWorld) = 0; 41 | virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& cov) = 0; 42 | virtual void drawCovariance(const Eigen::Vector3f& mean, const Eigen::Matrix3f& covMatrix) = 0; 43 | 44 | virtual void setScale(double scale) = 0; 45 | virtual void setColor(double r, double g, double b, double a = 1.0) = 0; 46 | 47 | virtual void sendAndResetData() = 0; 48 | }; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /hector_marker_drawing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_marker_drawing 4 | 0.5.2 5 | 6 | hector_marker_drawing provides convenience functions for easier publishing of visualization markers. 7 | 8 | 9 | 10 | Johannes Meyer 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | http://ros.org/wiki/hector_marker_drawing 22 | 23 | 24 | 25 | 26 | Stefan Kohlbrecher 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | catkin 41 | roscpp 42 | visualization_msgs 43 | eigen 44 | roscpp 45 | visualization_msgs 46 | eigen 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /hector_nav_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_nav_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | 8 | 0.5.1 (2021-01-15) 9 | ------------------ 10 | 11 | 0.5.0 (2020-12-17) 12 | ------------------ 13 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 14 | Clean up for noetic release. 15 | * Bump CMake version to avoid CMP0048 warning 16 | * Contributors: Marius Schnaubelt, Stefan Fabian 17 | 18 | 0.4.1 (2020-05-15) 19 | ------------------ 20 | 21 | 0.3.6 (2019-10-31) 22 | ------------------ 23 | 24 | 0.3.5 (2016-06-24) 25 | ------------------ 26 | 27 | 0.3.4 (2015-11-07) 28 | ------------------ 29 | * hector_nav_msgs: removed yaw member from GetNormal response 30 | yaw is implicitly given by the normal vector 31 | * Contributors: Dorothea Koert 32 | 33 | 0.3.3 (2014-06-15) 34 | ------------------ 35 | * added GetNormal service, that returns normal and orientation of an occupied cell 36 | * Contributors: Dorothea Koert 37 | 38 | 0.3.2 (2014-03-30) 39 | ------------------ 40 | 41 | 0.3.1 (2013-10-09) 42 | ------------------ 43 | * added changelogs 44 | 45 | 0.3.0 (2013-08-08) 46 | ------------------ 47 | * catkinized hector_slam 48 | -------------------------------------------------------------------------------- /hector_nav_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_nav_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS nav_msgs geometry_msgs message_generation std_msgs) 5 | 6 | 7 | ####################################### 8 | ## Declare ROS messages and services ## 9 | ####################################### 10 | 11 | ## Generate services in the 'srv' folder 12 | add_service_files( 13 | FILES 14 | GetDistanceToObstacle.srv 15 | GetRecoveryInfo.srv 16 | GetRobotTrajectory.srv 17 | GetSearchPosition.srv 18 | GetNormal.srv 19 | ) 20 | 21 | generate_messages( 22 | DEPENDENCIES 23 | geometry_msgs nav_msgs std_msgs 24 | ) 25 | 26 | catkin_package( 27 | CATKIN_DEPENDS nav_msgs geometry_msgs message_runtime std_msgs 28 | ) 29 | -------------------------------------------------------------------------------- /hector_nav_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_nav_msgs 4 | 0.5.2 5 | 6 | hector_nav_msgs contains messages and services used in the hector_slam stack. 7 | 8 | 9 | 10 | Johannes Meyer 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | http://ros.org/wiki/hector_nav_msgs 22 | 23 | 24 | 25 | 26 | Stefan Kohlbrecher 27 | 28 | 29 | catkin 30 | nav_msgs 31 | geometry_msgs 32 | std_msgs 33 | message_generation 34 | message_runtime 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /hector_nav_msgs/srv/GetDistanceToObstacle.srv: -------------------------------------------------------------------------------- 1 | # Returns the distance to the next obstacle from the origin of frame point.header.frame_id 2 | # in the direction of the point 3 | # 4 | # All units are meters. 5 | 6 | geometry_msgs/PointStamped point 7 | --- 8 | float32 distance 9 | geometry_msgs/PointStamped end_point 10 | 11 | 12 | -------------------------------------------------------------------------------- /hector_nav_msgs/srv/GetNormal.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PointStamped point 2 | --- 3 | geometry_msgs/Vector3 normal 4 | -------------------------------------------------------------------------------- /hector_nav_msgs/srv/GetRecoveryInfo.srv: -------------------------------------------------------------------------------- 1 | # Returns the path travelled to get to req_pose (pose determined by request_time) 2 | # up to request_radius away from req_pose. 3 | # 4 | 5 | time request_time 6 | float64 request_radius 7 | --- 8 | nav_msgs/Path trajectory_radius_entry_pose_to_req_pose 9 | geometry_msgs/PoseStamped radius_entry_pose 10 | geometry_msgs/PoseStamped req_pose 11 | 12 | -------------------------------------------------------------------------------- /hector_nav_msgs/srv/GetRobotTrajectory.srv: -------------------------------------------------------------------------------- 1 | # Returns the distance to the next obstacle from the origin of frame point.header.frame_id 2 | # in the direction of the point 3 | # 4 | # All units are meters. 5 | 6 | --- 7 | nav_msgs/Path trajectory 8 | 9 | -------------------------------------------------------------------------------- /hector_nav_msgs/srv/GetSearchPosition.srv: -------------------------------------------------------------------------------- 1 | #Returns a suggested search/observation position for an object of interest located at ooi_pose 2 | 3 | geometry_msgs/PoseStamped ooi_pose 4 | float32 distance 5 | --- 6 | geometry_msgs/PoseStamped search_pose 7 | 8 | -------------------------------------------------------------------------------- /hector_slam/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_slam 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | 8 | 0.5.1 (2021-01-15) 9 | ------------------ 10 | * Added hector_geotiff_launch to hector_slam metapackge. 11 | * Contributors: Stefan Fabian 12 | 13 | 0.5.0 (2020-12-17) 14 | ------------------ 15 | * Bump CMake version to avoid CMP0048 warning 16 | * Contributors: Marius Schnaubelt, Stefan Fabian 17 | 18 | 0.4.1 (2020-05-15) 19 | ------------------ 20 | 21 | 0.3.6 (2019-10-31) 22 | ------------------ 23 | 24 | 0.3.5 (2016-06-24) 25 | ------------------ 26 | 27 | 0.3.4 (2015-11-07) 28 | ------------------ 29 | 30 | 0.3.3 (2014-06-15) 31 | ------------------ 32 | 33 | 0.3.2 (2014-03-30) 34 | ------------------ 35 | 36 | 0.3.1 (2013-10-09) 37 | ------------------ 38 | * added changelogs 39 | 40 | 0.3.0 (2013-08-08) 41 | ------------------ 42 | * catkinized hector_slam 43 | -------------------------------------------------------------------------------- /hector_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_slam) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /hector_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_slam 4 | 0.5.2 5 | 6 | The hector_slam metapackage that installs hector_mapping and related packages. 7 | 8 | 9 | 10 | Johannes Meyer 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | http://ros.org/wiki/hector_slam 22 | 23 | 24 | 25 | 26 | Stefan Kohlbrecher 27 | Johannes Meyer 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | hector_compressed_map_transport 43 | hector_geotiff 44 | hector_geotiff_launch 45 | hector_geotiff_plugins 46 | hector_imu_attitude_to_tf 47 | hector_map_server 48 | hector_map_tools 49 | hector_mapping 50 | hector_marker_drawing 51 | hector_nav_msgs 52 | hector_slam_launch 53 | hector_trajectory_server 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /hector_slam_launch/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_slam_launch 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | 8 | 0.5.1 (2021-01-15) 9 | ------------------ 10 | * Updated paths to launch files. 11 | * Contributors: Stefan Fabian 12 | 13 | 0.5.0 (2020-12-17) 14 | ------------------ 15 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 16 | Clean up for noetic release. 17 | * Bump CMake version to avoid CMP0048 warning 18 | * Contributors: Marius Schnaubelt, Stefan Fabian 19 | 20 | 0.4.1 (2020-05-15) 21 | ------------------ 22 | 23 | 0.3.6 (2019-10-31) 24 | ------------------ 25 | 26 | 0.3.5 (2016-06-24) 27 | ------------------ 28 | 29 | 0.3.4 (2015-11-07) 30 | ------------------ 31 | * Removes trailing spaces and fixes indents 32 | * "rviz_cfg" directory in the repository version. 33 | * Contributors: YuK_Ota, dani-carbonell 34 | 35 | 0.3.3 (2014-06-15) 36 | ------------------ 37 | 38 | 0.3.2 (2014-03-30) 39 | ------------------ 40 | * Add arguments to launch files for specifying geotiff file path 41 | * Update rviz config to have proper default displays 42 | * deleted quadrotor_uav.launch (moved to hector_quadrotor) 43 | * Contributors: Johannes Meyer, Stefan Kohlbrecher 44 | 45 | 0.3.1 (2013-10-09) 46 | ------------------ 47 | * updated rviz config to the new .rviz format 48 | * added changelogs 49 | 50 | 0.3.0 (2013-08-08) 51 | ------------------ 52 | * catkinized hector_slam 53 | -------------------------------------------------------------------------------- /hector_slam_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_slam_launch) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | 9 | ############# 10 | ## Install ## 11 | ############# 12 | 13 | # all install targets should use catkin DESTINATION variables 14 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 15 | 16 | install(DIRECTORY launch rviz_cfg 17 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 18 | ) 19 | -------------------------------------------------------------------------------- /hector_slam_launch/launch/cityflyer_logfile_processing.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /hector_slam_launch/launch/hector_ugv.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 | -------------------------------------------------------------------------------- /hector_slam_launch/launch/height_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 | -------------------------------------------------------------------------------- /hector_slam_launch/launch/mapping_box.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 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /hector_slam_launch/launch/mpo700_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 | -------------------------------------------------------------------------------- /hector_slam_launch/launch/postproc_data.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /hector_slam_launch/launch/postproc_qut_logs.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /hector_slam_launch/launch/pr2os.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 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /hector_slam_launch/launch/tutorial.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /hector_slam_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_slam_launch 4 | 0.5.2 5 | 6 | hector_slam_launch contains launch files for launching hector_slam with different robot systems/setups/postprocessing scenarios. 7 | 8 | 9 | 10 | Johannes Meyer 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | http://ros.org/wiki/hector_slam_launch 22 | 23 | 24 | 25 | 26 | Stefan Kohlbrecher 27 | 28 | 29 | catkin 30 | hector_mapping 31 | hector_map_server 32 | hector_trajectory_server 33 | hector_geotiff 34 | hector_geotiff_plugins 35 | rviz 36 | tf 37 | topic_tools 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /hector_slam_launch/rviz_cfg/mapping_demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Map1 10 | - /Path1 11 | - /Pose1 12 | Splitter Ratio: 0.5 13 | Tree Height: 540 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.03 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 0.7 55 | Class: rviz/Map 56 | Color Scheme: map 57 | Draw Behind: false 58 | Enabled: true 59 | Name: Map 60 | Topic: /map 61 | Value: true 62 | - Alpha: 1 63 | Buffer Length: 1 64 | Class: rviz/Path 65 | Color: 25; 255; 0 66 | Enabled: true 67 | Name: Path 68 | Topic: /trajectory 69 | Value: true 70 | - Alpha: 1 71 | Axes Length: 1 72 | Axes Radius: 0.1 73 | Class: rviz/Pose 74 | Color: 255; 25; 0 75 | Enabled: true 76 | Head Length: 0.3 77 | Head Radius: 0.1 78 | Name: Pose 79 | Shaft Length: 1 80 | Shaft Radius: 0.05 81 | Shape: Axes 82 | Topic: /slam_out_pose 83 | Value: true 84 | Enabled: true 85 | Global Options: 86 | Background Color: 48; 48; 48 87 | Fixed Frame: map 88 | Frame Rate: 30 89 | Name: root 90 | Tools: 91 | - Class: rviz/Interact 92 | Hide Inactive Objects: true 93 | - Class: rviz/MoveCamera 94 | - Class: rviz/Select 95 | - Class: rviz/FocusCamera 96 | - Class: rviz/Measure 97 | - Class: rviz/SetInitialPose 98 | Topic: /initialpose 99 | - Class: rviz/SetGoal 100 | Topic: /move_base_simple/goal 101 | - Class: rviz/PublishPoint 102 | Single click: true 103 | Topic: /clicked_point 104 | Value: true 105 | Views: 106 | Current: 107 | Class: rviz/XYOrbit 108 | Distance: 44.388 109 | Focal Point: 110 | X: 9.50434 111 | Y: -0.685607 112 | Z: 0 113 | Name: Current View 114 | Near Clip Distance: 0.01 115 | Pitch: 1.56976 116 | Target Frame: 117 | Value: XYOrbit (rviz) 118 | Yaw: -0.7846 119 | Saved: ~ 120 | Window Geometry: 121 | Displays: 122 | collapsed: false 123 | Height: 846 124 | Hide Left Dock: false 125 | Hide Right Dock: false 126 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002ab000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 127 | Selection: 128 | collapsed: false 129 | Time: 130 | collapsed: false 131 | Tool Properties: 132 | collapsed: false 133 | Views: 134 | collapsed: false 135 | Width: 1200 136 | X: 53 137 | Y: 60 138 | -------------------------------------------------------------------------------- /hector_trajectory_server/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_trajectory_server 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.2 (2021-04-08) 6 | ------------------ 7 | 8 | 0.5.1 (2021-01-15) 9 | ------------------ 10 | 11 | 0.5.0 (2020-12-17) 12 | ------------------ 13 | * Moved hector_geotiff launch files to separate package to solve cyclic dependency. 14 | Clean up for noetic release. 15 | * Bump CMake version to avoid CMP0048 warning 16 | * Contributors: Marius Schnaubelt, Stefan Fabian 17 | 18 | 0.4.1 (2020-05-15) 19 | ------------------ 20 | 21 | 0.3.6 (2019-10-31) 22 | ------------------ 23 | 24 | 0.3.5 (2016-06-24) 25 | ------------------ 26 | * Changed from ros::WallTime to ros::Time in trajectory server 27 | * hector_trajectory_server: removed bug leading to potential infinite loop 28 | * Contributors: Andreas Lindahl Flåten, Paul Manns 29 | 30 | 0.3.4 (2015-11-07) 31 | ------------------ 32 | 33 | 0.3.3 (2014-06-15) 34 | ------------------ 35 | 36 | 0.3.2 (2014-03-30) 37 | ------------------ 38 | * wait based on WallTime and check ros::ok() in waitForTf() 39 | * Print out tf availability warning only once. 40 | * Wait for tf become available to suppress warnings on startup 41 | * Create a warning instead of an error for TransformExceptions 42 | * Contributors: Johannes Meyer, Stefan Kohlbrecher, wachaja 43 | 44 | 0.3.1 (2013-10-09) 45 | ------------------ 46 | * added changelogs 47 | * added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them 48 | 49 | 0.3.0 (2013-08-08) 50 | ------------------ 51 | * catkinized hector_slam 52 | -------------------------------------------------------------------------------- /hector_trajectory_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hector_trajectory_server) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp hector_nav_msgs nav_msgs hector_map_tools tf) 5 | 6 | catkin_package( 7 | CATKIN_DEPENDS roscpp hector_nav_msgs nav_msgs hector_map_tools tf 8 | ) 9 | 10 | ########### 11 | ## Build ## 12 | ########### 13 | 14 | include_directories( 15 | ${catkin_INCLUDE_DIRS} 16 | ) 17 | 18 | add_executable(hector_trajectory_server src/hector_trajectory_server.cpp) 19 | add_dependencies(hector_trajectory_server ${catkin_EXPORTED_TARGETS}) 20 | target_link_libraries(hector_trajectory_server 21 | ${catkin_LIBRARIES} 22 | ) 23 | 24 | ############# 25 | ## Install ## 26 | ############# 27 | 28 | # all install targets should use catkin DESTINATION variables 29 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 30 | 31 | install(TARGETS hector_trajectory_server 32 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 35 | ) 36 | -------------------------------------------------------------------------------- /hector_trajectory_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_trajectory_server 4 | 0.5.2 5 | 6 | hector_trajectory_server keeps track of tf trajectories extracted from tf data and makes this data accessible via a service and topic. 7 | 8 | 9 | 10 | Johannes Meyer 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | http://ros.org/wiki/hector_trajectory_server 22 | 23 | 24 | 25 | 26 | Stefan Kohlbrecher 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | catkin 41 | roscpp 42 | hector_nav_msgs 43 | nav_msgs 44 | hector_map_tools 45 | tf 46 | roscpp 47 | hector_nav_msgs 48 | nav_msgs 49 | hector_map_tools 50 | tf 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | --------------------------------------------------------------------------------