diff --git a/grid_map_demos/CMakeLists.txt b/grid_map_demos/CMakeLists.txt index 244109947..1a9a6c8e7 100644 --- a/grid_map_demos/CMakeLists.txt +++ b/grid_map_demos/CMakeLists.txt @@ -1,26 +1,26 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5.0) project(grid_map_demos) -set(CMAKE_CXX_STANDARD 11) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp - grid_map_core - grid_map_ros - grid_map_cv - grid_map_filters - grid_map_loader - grid_map_msgs - grid_map_octomap - grid_map_rviz_plugin - grid_map_visualization - geometry_msgs - sensor_msgs - cv_bridge - octomap_msgs - filters -) +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(filters REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(grid_map_cv REQUIRED) +# find_package(grid_map_filters REQUIRED) +# find_package(grid_map_loader REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(grid_map_octomap REQUIRED) +find_package(grid_map_ros REQUIRED) +# find_package(grid_map_rviz_plugin REQUIRED) +# find_package(grid_map_visualization REQUIRED) +find_package(OCTOMAP REQUIRED) +find_package(octomap_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(OpenCV REQUIRED COMPONENTS @@ -28,22 +28,12 @@ find_package(OpenCV REQUIRED CONFIG ) -find_package(octomap REQUIRED) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS -# DEPENDS system_lib +grid_map_package() + +set(dependencies + rclcpp + grid_map_ros + grid_map_msgs ) ########### @@ -54,9 +44,8 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( include - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} - ${OCTOMAP_INCLUDE_DIR} + # ${OCTOMAP_INCLUDE_DIR} ) ## Declare a cpp executable @@ -82,143 +71,184 @@ add_executable( src/ImageToGridmapDemo.cpp ) -add_executable( - octomap_to_gridmap_demo - src/octomap_to_gridmap_demo_node.cpp - src/OctomapToGridmapDemo.cpp -) - -add_executable( - move_demo - src/move_demo_node.cpp -) - -add_executable( - iterator_benchmark - src/iterator_benchmark.cpp -) - -add_executable( - opencv_demo - src/opencv_demo_node.cpp -) - -add_executable( - resolution_change_demo - src/resolution_change_demo_node.cpp -) - -add_executable( - filters_demo - src/filters_demo_node.cpp - src/FiltersDemo.cpp -) - -add_executable( - normal_filter_comparison_demo - src/normal_filter_comparison_node.cpp -) - -add_executable( - interpolation_demo - src/interpolation_demo_node.cpp - src/InterpolationDemo.cpp -) +# add_executable( +# octomap_to_gridmap_demo +# src/octomap_to_gridmap_demo_node.cpp +# src/OctomapToGridmapDemo.cpp +# ) + +# add_executable( +# move_demo +# src/move_demo_node.cpp +# ) + +# add_executable( +# iterator_benchmark +# src/iterator_benchmark.cpp +# ) + +# add_executable( +# opencv_demo +# src/opencv_demo_node.cpp +# ) + +# add_executable( +# resolution_change_demo +# src/resolution_change_demo_node.cpp +# ) + +# add_executable( +# filters_demo +# src/filters_demo_node.cpp +# src/FiltersDemo.cpp +# ) + +# add_executable( +# normal_filter_comparison_demo +# src/normal_filter_comparison_node.cpp +# ) + +# add_executable( +# interpolation_demo +# src/interpolation_demo_node.cpp +# src/InterpolationDemo.cpp +# ) ## Specify libraries to link a library or executable target against -target_link_libraries( +ament_target_dependencies( simple_demo - ${catkin_LIBRARIES} + "rclcpp" + "grid_map_ros" + "grid_map_msgs" ) -target_link_libraries( +ament_target_dependencies( tutorial_demo - ${catkin_LIBRARIES} + "rclcpp" + "grid_map_ros" + "grid_map_msgs" ) -target_link_libraries( +ament_target_dependencies( iterators_demo - ${catkin_LIBRARIES} -) - -target_link_libraries( - image_to_gridmap_demo - ${catkin_LIBRARIES} -) - -target_link_libraries( - octomap_to_gridmap_demo - ${catkin_LIBRARIES} - ${OCTOMAP_LIBRARIES} -) - -target_link_libraries( - move_demo - ${catkin_LIBRARIES} -) - -target_link_libraries( - iterator_benchmark - ${catkin_LIBRARIES} -) - -target_link_libraries( - opencv_demo - ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} -) - -target_link_libraries( - resolution_change_demo - ${catkin_LIBRARIES} -) - -target_link_libraries( - filters_demo - ${catkin_LIBRARIES} -) - -target_link_libraries( - normal_filter_comparison_demo - ${catkin_LIBRARIES} -) - -target_link_libraries( - interpolation_demo - ${catkin_LIBRARIES} -) + "rclcpp" + "grid_map_ros" + "grid_map_msgs" +) + +ament_target_dependencies(image_to_gridmap_demo + "rclcpp" + "grid_map_ros" + "grid_map_msgs" +) + +# ament_target_dependencies( +# octomap_to_gridmap_demo +# ${dependencies} +# ${OCTOMAP_LIBRARIES} +# ) + +# ament_target_dependencies( +# move_demo +# ) + +# ament_target_dependencies( +# iterator_benchmark +# ${dependencies} +# ) + +# ament_target_dependencies( +# opencv_demo +# ${dependencies} +# ${OpenCV_LIBRARIES} +# ) + +# ament_target_dependencies( +# resolution_change_demo +# ${dependencies} +# ) + +# ament_target_dependencies( +# filters_demo +# ${dependencies} +# ) + +# ament_target_dependencies( +# normal_filter_comparison_demo +# ${dependencies} +# ) + +# ament_target_dependencies( +# interpolation_demo +# ${dependencies} +# ) ############# ## Install ## ############# -catkin_install_python( - PROGRAMS scripts/image_publisher.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - # Mark executables and/or libraries for installation install( - TARGETS - filters_demo + TARGETS + # filters_demo image_to_gridmap_demo - interpolation_demo - iterator_benchmark + # interpolation_demo + # iterator_benchmark iterators_demo - move_demo - normal_filter_comparison_demo - octomap_to_gridmap_demo - opencv_demo - resolution_change_demo + # move_demo + # normal_filter_comparison_demo + # octomap_to_gridmap_demo + # opencv_demo + # resolution_change_demo simple_demo tutorial_demo - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Mark other files for installation install( - DIRECTORY config data doc launch rviz scripts - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DIRECTORY config data doc launch rviz + DESTINATION share/${PROJECT_NAME} ) + +# Install python scripts +install( + PROGRAMS scripts/image_publisher.py + DESTINATION lib/${PROJECT_NAME} +) + +############# +## Testing ## +############# + +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright + ) + endif() + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include ${OCTOMAP_INCLUDE_DIRS}) +# ament_export_libraries(${OCTOMAP_LIBRARIES}) +ament_package() diff --git a/grid_map_demos/COLCON_IGNORE b/grid_map_demos/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/grid_map_demos/config/image_to_gridmap_demo.yaml b/grid_map_demos/config/image_to_gridmap_demo.yaml index c9a8684b6..c020fb88e 100644 --- a/grid_map_demos/config/image_to_gridmap_demo.yaml +++ b/grid_map_demos/config/image_to_gridmap_demo.yaml @@ -1,24 +1,29 @@ -image_to_gridmap_demo: - image_topic: "/image_publisher/image" - resolution: 0.03 - map_frame_id: "map" - min_height: -0.1 - max_height: 0.1 +image_publisher: + ros__parameters: + image_topic: "/image" + map_frame_id: "map" + +image_to_gridmap: + ros__parameters: + resolution: 0.03 + min_height: -0.1 + max_height: 0.1 grid_map_visualization: - grid_map_topic: /image_to_gridmap_demo/grid_map - grid_map_visualizations: - - name: elevation_points + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [elevation_points, elevation_grid, flat_grid] + elevation_points: type: point_cloud params: layer: elevation - - name: elevation_grid + elevation_grid: type: occupancy_grid params: layer: elevation data_min: -0.2 data_max: 0.2 - - name: flat_grid + flat_grid: type: flat_point_cloud params: height: 0.0 diff --git a/grid_map_demos/config/iterators_demo.yaml b/grid_map_demos/config/iterators_demo.yaml index 5398979f5..9cf441d94 100644 --- a/grid_map_demos/config/iterators_demo.yaml +++ b/grid_map_demos/config/iterators_demo.yaml @@ -1,8 +1,10 @@ -grid_map_topic: /grid_map_iterators_demo/grid_map -grid_map_visualizations: - - name: occupancy_grid - type: occupancy_grid - params: - layer: type - data_min: 0.0 - data_max: 1.0 +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [occupancy_grid] + occupancy_grid: + type: occupancy_grid + params: + layer: type + data_min: 0.0 + data_max: 1.0 diff --git a/grid_map_demos/config/simple_demo.yaml b/grid_map_demos/config/simple_demo.yaml index 895ecb8f9..15ec03613 100644 --- a/grid_map_demos/config/simple_demo.yaml +++ b/grid_map_demos/config/simple_demo.yaml @@ -1,12 +1,14 @@ -grid_map_topic: /grid_map_simple_demo/grid_map -grid_map_visualizations: - - name: elevation_points - type: point_cloud - params: - layer: elevation - - name: elevation_grid - type: occupancy_grid - params: - layer: elevation - data_min: 0.08 - data_max: -0.16 +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [elevation_points, elevation_grid] + elevation_points: + type: point_cloud + params: + layer: elevation + elevation_grid: + type: occupancy_grid + params: + layer: elevation + data_min: 0.08 + data_max: -0.16 diff --git a/grid_map_demos/config/tutorial_demo.yaml b/grid_map_demos/config/tutorial_demo.yaml index 715004f9e..070f71312 100644 --- a/grid_map_demos/config/tutorial_demo.yaml +++ b/grid_map_demos/config/tutorial_demo.yaml @@ -1,54 +1,56 @@ -grid_map_topic: /grid_map_tutorial_demo/grid_map - -grid_map_visualizations: - - - name: elevation_points - type: point_cloud - params: - layer: elevation - - - name: noisy_points - type: point_cloud - params: - layer: elevation_noisy - - - name: filtered_points - type: point_cloud - params: - layer: elevation_filtered - - - name: map_region - type: map_region - params: - color: 3289650 - line_width: 0.003 - - - name: elevation_grid - type: occupancy_grid - params: - layer: elevation - data_min: 0.08 - data_max: -0.16 - - - name: error_grid - type: occupancy_grid - params: - layer: error - data_min: -0.15 - data_max: 0.15 - - - name: surface_normals - type: vectors - params: - layer_prefix: normal_ - position_layer: elevation - scale: 0.06 - line_width: 0.005 - color: 15600153 # red - - - name: elevation_cells - type: grid_cells - params: - layer: elevation - lower_threshold: -0.08 - upper_threshold: 0.0 +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map + + grid_map_visualizations: [elevation_points, noisy_points, filtered_points, map_region, elevation_grid, error_grid, surface_normals, elevation_cells] + + elevation_points: + type: point_cloud + params: + layer: elevation + + noisy_points: + type: point_cloud + params: + layer: elevation_noisy + + filtered_points: + type: point_cloud + params: + layer: elevation_filtered + + map_region: + type: map_region + params: + color: 3289650 + line_width: 0.003 + + elevation_grid: + type: occupancy_grid + params: + layer: elevation + data_min: 0.08 + data_max: -0.16 + + error_grid: + type: occupancy_grid + params: + layer: error + data_min: -0.15 + data_max: 0.15 + + surface_normals: + type: vectors + params: + layer_prefix: normal_ + position_layer: elevation + scale: 0.06 + line_width: 0.005 + color: 15600153 # red + + elevation_cells: + type: grid_cells + params: + layer: elevation + lower_threshold: -0.08 + upper_threshold: 0.0 diff --git a/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp b/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp index 2a81c25a9..5e361a3c0 100644 --- a/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp +++ b/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp @@ -7,7 +7,8 @@ * */ -#pragma once +#ifndef GRID_MAP_DEMOS__FILTERSDEMO_HPP_ +#define GRID_MAP_DEMOS__FILTERSDEMO_HPP_ #include @@ -15,7 +16,8 @@ #include #include -namespace grid_map_demos { +namespace grid_map_demos +{ /*! * Applies a chain of grid map filters to a topic and @@ -23,14 +25,13 @@ namespace grid_map_demos { */ class FiltersDemo { - public: - +public: /*! * Constructor. * @param nodeHandle the ROS node handle. * @param success signalizes if filter is configured ok or not. */ - FiltersDemo(ros::NodeHandle& nodeHandle, bool& success); + FiltersDemo(ros::NodeHandle & nodeHandle, bool & success); /*! * Destructor. @@ -47,12 +48,11 @@ class FiltersDemo * Callback method for the incoming grid map message. * @param message the incoming message. */ - void callback(const grid_map_msgs::GridMap& message); - - private: + void callback(const grid_map_msgs::GridMap & message); +private: //! ROS nodehandle. - ros::NodeHandle& nodeHandle_; + ros::NodeHandle & nodeHandle_; //! Name of the input grid map topic. std::string inputTopic_; @@ -73,4 +73,5 @@ class FiltersDemo std::string filterChainParametersName_; }; -} /* namespace */ \ No newline at end of file +} // namespace grid_map_demos +#endif // GRID_MAP_DEMOS__FILTERSDEMO_HPP_ diff --git a/grid_map_demos/include/grid_map_demos/ImageToGridmapDemo.hpp b/grid_map_demos/include/grid_map_demos/ImageToGridmapDemo.hpp index 5b25d06ca..0ed82e843 100644 --- a/grid_map_demos/include/grid_map_demos/ImageToGridmapDemo.hpp +++ b/grid_map_demos/include/grid_map_demos/ImageToGridmapDemo.hpp @@ -7,31 +7,32 @@ * */ -#pragma once +#ifndef GRID_MAP_DEMOS__IMAGETOGRIDMAPDEMO_HPP_ +#define GRID_MAP_DEMOS__IMAGETOGRIDMAPDEMO_HPP_ // ROS -#include -#include +#include +#include #include #include -namespace grid_map_demos { +namespace grid_map_demos +{ /*! * Loads an image and saves it as layer 'elevation' of a grid map. * The grid map is published and can be viewed in Rviz. */ -class ImageToGridmapDemo +class ImageToGridmapDemo : public rclcpp::Node { - public: - +public: /*! * Constructor. * @param nodeHandle the ROS node handle. */ - ImageToGridmapDemo(ros::NodeHandle& nodeHandle); + ImageToGridmapDemo(); /*! * Destructor. @@ -44,21 +45,17 @@ class ImageToGridmapDemo */ bool readParameters(); - void imageCallback(const sensor_msgs::Image& msg); - - private: - - //! ROS nodehandle. - ros::NodeHandle& nodeHandle_; + void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg); +private: //! Grid map publisher. - ros::Publisher gridMapPublisher_; + rclcpp::Publisher::SharedPtr gridMapPublisher_; //! Grid map data. grid_map::GridMap map_; //! Image subscriber - ros::Subscriber imageSubscriber_; + rclcpp::Subscription::SharedPtr imageSubscriber_; //! Name of the grid map topic. std::string imageTopic_; @@ -79,4 +76,5 @@ class ImageToGridmapDemo bool mapInitialized_; }; -} /* namespace */ +} // namespace grid_map_demos +#endif // GRID_MAP_DEMOS__IMAGETOGRIDMAPDEMO_HPP_ diff --git a/grid_map_demos/include/grid_map_demos/InterpolationDemo.hpp b/grid_map_demos/include/grid_map_demos/InterpolationDemo.hpp index b94746c0f..d69a77b09 100644 --- a/grid_map_demos/include/grid_map_demos/InterpolationDemo.hpp +++ b/grid_map_demos/include/grid_map_demos/InterpolationDemo.hpp @@ -6,18 +6,23 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#pragma once +#ifndef GRID_MAP_DEMOS__INTERPOLATIONDEMO_HPP_ +#define GRID_MAP_DEMOS__INTERPOLATIONDEMO_HPP_ -#include "grid_map_core/GridMap.hpp" -#include "grid_map_core/TypeDefs.hpp" +#include #include #include #include #include -#include +#include + +#include "grid_map_core/GridMap.hpp" +#include "grid_map_core/TypeDefs.hpp" -namespace grid_map_demos { + +namespace grid_map_demos +{ /* * This demo visualizes the quality of function approximation @@ -50,7 +55,8 @@ namespace grid_map_demos { * Different analytical functions used * to evaluate the quality of interpolation approximation. */ -enum class Worlds: int { +enum class Worlds: int +{ Poly, GaussMixture, Tanh, @@ -58,14 +64,15 @@ enum class Worlds: int { NUM_WORLDS }; -static const std::string demoLayer = "demo"; +static const char * demoLayer = "demo"; struct AnalyticalFunctions { std::function f_; }; -struct Error { +struct Error +{ double meanSquare_ = 0.0; double meanAbs_ = 0.0; double max_ = 0.0; @@ -74,21 +81,22 @@ struct Error { /* * Create map geometry and allocate memory */ -grid_map::GridMap createMap(const grid_map::Length &length, double resolution, - const grid_map::Position &pos); +grid_map::GridMap createMap( + const grid_map::Length & length, double resolution, + const grid_map::Position & pos); /* * Fill grid map with values computed from analytical function */ -void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions); +void fillGridMap(grid_map::GridMap * map, const AnalyticalFunctions & functions); /* * Create various analytical functions */ -AnalyticalFunctions createPolyWorld(grid_map::GridMap *map); -AnalyticalFunctions createSineWorld(grid_map::GridMap *map); -AnalyticalFunctions createTanhWorld(grid_map::GridMap *map); -AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map); +AnalyticalFunctions createPolyWorld(grid_map::GridMap * map); +AnalyticalFunctions createSineWorld(grid_map::GridMap * map); +AnalyticalFunctions createTanhWorld(grid_map::GridMap * map); +AnalyticalFunctions createGaussianWorld(grid_map::GridMap * map); /* @@ -96,39 +104,44 @@ AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map); * and create sparse grid map with data points that are used for the * interpolation. */ -AnalyticalFunctions createWorld(Worlds world, double highResolution, double lowResolution, - double length, double width, - grid_map::GridMap *groundTruthHighRes, - grid_map::GridMap *groundTruthLowRes); +AnalyticalFunctions createWorld( + Worlds world, double highResolution, double lowResolution, + double length, double width, + grid_map::GridMap * groundTruthHighRes, + grid_map::GridMap * groundTruthLowRes); /* * Allocate memory and set geometry for the interpolated grid map */ -grid_map::GridMap createInterpolatedMapFromDataMap(const grid_map::GridMap &dataMap, double desiredResolution); +grid_map::GridMap createInterpolatedMapFromDataMap( + const grid_map::GridMap & dataMap, + double desiredResolution); /* * Compute the interpolated values */ -void interpolateInputMap(const grid_map::GridMap &dataMap, - grid_map::InterpolationMethods interpolationMethod, grid_map::GridMap *inteprolatedMap); +void interpolateInputMap( + const grid_map::GridMap & dataMap, + grid_map::InterpolationMethods interpolationMethod, grid_map::GridMap * inteprolatedMap); /* * Compute error measures between the ground truth and the interpolated map */ -Error computeInterpolationError(const AnalyticalFunctions &groundTruth, - const grid_map::GridMap &map); +Error computeInterpolationError( + const AnalyticalFunctions & groundTruth, + const grid_map::GridMap & map); static const std::map worlds = { - { "Sine", Worlds::Sine }, - { "Poly", Worlds::Poly }, - { "Gauss", Worlds::GaussMixture }, - { "Tanh", Worlds::Tanh } }; + {"Sine", Worlds::Sine}, + {"Poly", Worlds::Poly}, + {"Gauss", Worlds::GaussMixture}, + {"Tanh", Worlds::Tanh}}; static const std::map interpolationMethods = { - { "Nearest", grid_map::InterpolationMethods::INTER_NEAREST}, - { "Linear", grid_map::InterpolationMethods::INTER_LINEAR }, - { "Cubic_convolution", grid_map::InterpolationMethods::INTER_CUBIC_CONVOLUTION }, - { "Cubic", grid_map::InterpolationMethods::INTER_CUBIC } }; + {"Nearest", grid_map::InterpolationMethods::INTER_NEAREST}, + {"Linear", grid_map::InterpolationMethods::INTER_LINEAR}, + {"Cubic_convolution", grid_map::InterpolationMethods::INTER_CUBIC_CONVOLUTION}, + {"Cubic", grid_map::InterpolationMethods::INTER_CUBIC}}; /* * Class that actually runs the demo @@ -138,25 +151,27 @@ static const std::map interpolation */ class InterpolationDemo { +public: + InterpolationDemo(); - public: - InterpolationDemo(ros::NodeHandle *nh); - - private: +private: using clk = std::chrono::steady_clock; using ErrorAndDuration = std::pair; - struct Statistic { + struct Statistic + { Error error_; std::string world_; std::string interpolationMethod_; double duration_ = 0.0; }; - using Statistics = std::map >; + using Statistics = std::map>; void runDemo(); - ErrorAndDuration interpolateAndComputeError(const std::string world, const std::string &method) const; + ErrorAndDuration interpolateAndComputeError( + const std::string world, + const std::string & method) const; Statistics computeStatistics() const; - void printStatistics(const Statistics &stats) const; + void printStatistics(const Statistics & stats) const; void publishGridMaps() const; ros::Publisher groundTruthMapPub_; @@ -169,13 +184,13 @@ class InterpolationDemo std::string world_; std::string interpolationMethod_; - double groundTruthResolution_ = 0.02; // resolution in which ground truth is displayed in rviz - double dataResolution_ = 0.1; // resolution of the data points for the interpolating algorithm - double interpolatedResolution_ = 0.02; // resolution requested for the interpolated map + double groundTruthResolution_ = 0.02; // resolution in which ground truth is displayed in rviz + double dataResolution_ = 0.1; // resolution of the data points for the interpolating algorithm + double interpolatedResolution_ = 0.02; // resolution requested for the interpolated map AnalyticalFunctions groundTruth_; double worldWidth_ = 4.0; double worldLength_ = 4.0; - }; -} /* namespace grid_map_demos */ +} // namespace grid_map_demos +#endif // GRID_MAP_DEMOS__INTERPOLATIONDEMO_HPP_ diff --git a/grid_map_demos/include/grid_map_demos/IteratorsDemo.hpp b/grid_map_demos/include/grid_map_demos/IteratorsDemo.hpp index ceded1195..4abe7a2e4 100644 --- a/grid_map_demos/include/grid_map_demos/IteratorsDemo.hpp +++ b/grid_map_demos/include/grid_map_demos/IteratorsDemo.hpp @@ -7,27 +7,28 @@ * */ -#pragma once +#ifndef GRID_MAP_DEMOS__ITERATORSDEMO_HPP_ +#define GRID_MAP_DEMOS__ITERATORSDEMO_HPP_ #include // ROS -#include +#include +#include -namespace grid_map_demos { +namespace grid_map_demos +{ /*! * Visualizes a grid map by publishing different topics that can be viewed in Rviz. */ -class IteratorsDemo +class IteratorsDemo : public rclcpp::Node { - public: - +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. */ - IteratorsDemo(ros::NodeHandle& nodeHandle); + IteratorsDemo(); /*! * Destructor. @@ -51,19 +52,16 @@ class IteratorsDemo */ void publish(); - private: - - //! ROS nodehandle. - ros::NodeHandle& nodeHandle_; - +private: //! Grid map publisher. - ros::Publisher gridMapPublisher_; + rclcpp::Publisher::SharedPtr gridMapPublisher_; //! Polygon publisher. - ros::Publisher polygonPublisher_; + rclcpp::Publisher::SharedPtr polygonPublisher_; //! Grid map data. grid_map::GridMap map_; }; -} /* namespace */ +} // namespace grid_map_demos +#endif // GRID_MAP_DEMOS__ITERATORSDEMO_HPP_ diff --git a/grid_map_demos/include/grid_map_demos/OctomapToGridmapDemo.hpp b/grid_map_demos/include/grid_map_demos/OctomapToGridmapDemo.hpp index 2a4f7a628..258954f5a 100644 --- a/grid_map_demos/include/grid_map_demos/OctomapToGridmapDemo.hpp +++ b/grid_map_demos/include/grid_map_demos/OctomapToGridmapDemo.hpp @@ -6,7 +6,8 @@ * Institute: University of Zürich, Robotics and Perception Group */ -#pragma once +#ifndef GRID_MAP_DEMOS__OCTOMAPTOGRIDMAPDEMO_HPP_ +#define GRID_MAP_DEMOS__OCTOMAPTOGRIDMAPDEMO_HPP_ // ROS #include @@ -15,7 +16,8 @@ #include -namespace grid_map_demos { +namespace grid_map_demos +{ /*! * Receives a volumetric OctoMap and converts it to a grid map with an elevation layer. @@ -23,13 +25,12 @@ namespace grid_map_demos { */ class OctomapToGridmapDemo { - public: - +public: /*! * Constructor. * @param nodeHandle the ROS node handle. */ - OctomapToGridmapDemo(ros::NodeHandle& nodeHandle); + OctomapToGridmapDemo(); /*! * Destructor. @@ -44,10 +45,9 @@ class OctomapToGridmapDemo void convertAndPublishMap(); - private: - +private: //! ROS nodehandle. - ros::NodeHandle& nodeHandle_; + ros::NodeHandle & nodeHandle_; //! Grid map publisher. ros::Publisher gridMapPublisher_; @@ -73,4 +73,5 @@ class OctomapToGridmapDemo float maxZ_; }; -} /* namespace */ +} // namespace grid_map_demos +#endif // GRID_MAP_DEMOS__OCTOMAPTOGRIDMAPDEMO_HPP_ diff --git a/grid_map_demos/launch/image_to_gridmap_demo.launch b/grid_map_demos/launch/image_to_gridmap_demo.launch deleted file mode 100644 index a4f0ad2e5..000000000 --- a/grid_map_demos/launch/image_to_gridmap_demo.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/grid_map_demos/launch/image_to_gridmap_demo_launch.py b/grid_map_demos/launch/image_to_gridmap_demo_launch.py new file mode 100644 index 000000000..028c805f5 --- /dev/null +++ b/grid_map_demos/launch/image_to_gridmap_demo_launch.py @@ -0,0 +1,75 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'image_to_gridmap_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_demo.rviz'), + description='Full path to the RVIZ config file to use') + + image_publisher_node = Node( + package='grid_map_demos', + executable='image_publisher.py', + name='image_publisher', + output='screen', + parameters=[ + { + 'image_path': os.path.join(grid_map_demos_dir, 'data', 'eth_logo.png'), + 'topic': 'image' + } + ] + ) + + image_to_gridmap_demo_node = Node( + package='grid_map_demos', + executable='image_to_gridmap_demo', + name='image_to_gridmap', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + ld = LaunchDescription() + + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + ld.add_action(image_publisher_node) + ld.add_action(image_to_gridmap_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/iterators_demo.launch b/grid_map_demos/launch/iterators_demo.launch deleted file mode 100644 index 747cd4e67..000000000 --- a/grid_map_demos/launch/iterators_demo.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_demos/launch/iterators_demo_launch.py b/grid_map_demos/launch/iterators_demo_launch.py new file mode 100644 index 000000000..7e54c9dfc --- /dev/null +++ b/grid_map_demos/launch/iterators_demo_launch.py @@ -0,0 +1,61 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'iterators_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_iterators_demo.rviz'), + description='Full path to the RVIZ config file to use') + + iterators_demo_node = Node( + package='grid_map_demos', + executable='iterators_demo', + name='grid_map_iterators_demo', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + ld = LaunchDescription() + + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + ld.add_action(iterators_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/simple_demo.launch b/grid_map_demos/launch/simple_demo.launch deleted file mode 100644 index 065311feb..000000000 --- a/grid_map_demos/launch/simple_demo.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_demos/launch/simple_demo_launch.py b/grid_map_demos/launch/simple_demo_launch.py new file mode 100644 index 000000000..77e7a6eaf --- /dev/null +++ b/grid_map_demos/launch/simple_demo_launch.py @@ -0,0 +1,62 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'simple_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_demo.rviz'), + description='Full path to the RVIZ config file to use') + + simple_demo_node = Node( + package='grid_map_demos', + executable='simple_demo', + name='grid_map_simple_demo', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + ld.add_action(simple_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/tutorial_demo.launch b/grid_map_demos/launch/tutorial_demo.launch deleted file mode 100644 index 616649c34..000000000 --- a/grid_map_demos/launch/tutorial_demo.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/grid_map_demos/launch/tutorial_demo_launch.py b/grid_map_demos/launch/tutorial_demo_launch.py new file mode 100644 index 000000000..081de367d --- /dev/null +++ b/grid_map_demos/launch/tutorial_demo_launch.py @@ -0,0 +1,61 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'tutorial_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_demo.rviz'), + description='Full path to the RVIZ config file to use') + + tutorial_demo_node = Node( + package='grid_map_demos', + executable='tutorial_demo', + name='grid_map_tutorial_demo', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + ld = LaunchDescription() + + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + ld.add_action(tutorial_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/package.xml b/grid_map_demos/package.xml index a12734261..9df12fcd0 100644 --- a/grid_map_demos/package.xml +++ b/grid_map_demos/package.xml @@ -1,7 +1,8 @@ - + + grid_map_demos - 1.6.2 + 2.0.0 Demo nodes to demonstrate the usage of the grid map library. Maximilian Wulf Yoshua Nava @@ -9,8 +10,11 @@ http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser - catkin - roscpp + + ament_cmake + grid_map_cmake_helpers + + rclcpp grid_map_core grid_map_ros grid_map_cv @@ -24,4 +28,14 @@ sensor_msgs cv_bridge octomap_msgs + + rclpy + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + + + ament_cmake + diff --git a/grid_map_demos/rviz/grid_map_demo.rviz b/grid_map_demos/rviz/grid_map_demo.rviz index d784c85ff..ab1fa879c 100644 --- a/grid_map_demos/rviz/grid_map_demo.rviz +++ b/grid_map_demos/rviz/grid_map_demo.rviz @@ -1,38 +1,32 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: ~ - Splitter Ratio: 0.611465 - Tree Height: 375 - - Class: rviz/Selection + Splitter Ratio: 0.6114649772644043 + Tree Height: 864 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Elevation Visualization Manager: Class: "" Displays: - - Class: rviz/Axes + - Class: rviz_default_plugins/Axes Enabled: false - Length: 0.2 + Length: 0.20000000298023224 Name: Axes - Radius: 0.01 + Radius: 0.009999999776482582 Reference Frame: Value: false - Alpha: 1 @@ -45,51 +39,66 @@ Visualization Manager: Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 + Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 10 Min Color: 0; 0; 0 Min Intensity: 0 Name: GridMap Show Grid Lines: true - Topic: /grid_map_simple_demo/grid_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /grid_map Use Rainbow: true Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /grid_map_visualization/map_region + - Class: rviz_default_plugins/Marker + Enabled: false Name: Map Region Namespaces: {} - Queue Size: 100 - Value: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_region + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.0849175 - Min Value: -0.165153 + Max Value: 0.08491750061511993 + Min Value: -0.16515299677848816 Value: false Axis: Z Channel Name: z - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.0768171 + Max Intensity: 0.07679121941328049 Min Color: 0; 85; 127 - Min Intensity: -0.156817 + Min Intensity: -0.15679122507572174 Name: Elevation Position Transformer: XYZ - Queue Size: 10 Selectable: false Size (Pixels): 3 - Size (m): 0.03 + Size (m): 0.029999999329447746 Style: Boxes - Topic: /grid_map_visualization/elevation_points - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_points Use Fixed Frame: true Use rainbow: false Value: true @@ -101,60 +110,99 @@ Visualization Manager: Value: true Axis: Z Channel Name: error - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.02 + Max Intensity: 0.019999999552965164 Min Color: 0; 85; 127 Min Intensity: 0 Name: Elevation Filtered Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.03 + Size (m): 0.029999999329447746 Style: Boxes - Topic: /grid_map_visualization/filtered_points - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /filtered_points Use Fixed Frame: true Use rainbow: false Value: false - Alpha: 1 - Class: rviz/Map + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: false Name: Elevation - Topic: /grid_map_visualization/elevation_grid - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_grid_updates + Use Timestamp: false Value: false - - Alpha: 0.7 - Class: rviz/Map + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: false Name: Error - Topic: /grid_map_visualization/error_grid - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /error_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /error_grid_updates + Use Timestamp: false Value: false - Alpha: 1 - Class: rviz/GridCells + Class: rviz_default_plugins/GridCells Color: 25; 255; 0 Enabled: false Name: GridCells - Topic: /grid_map_visualization/elevation_cells + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_cells Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: false - Marker Topic: /grid_map_visualization/surface_normals Name: Surface Normals Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /surface_normals Value: false Enabled: true Global Options: @@ -163,51 +211,78 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 2.43931 + Class: rviz_default_plugins/Orbit + Distance: 2.439310073852539 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.00701846 - Y: 0.113553 - Z: -0.107872 + X: 0.007018459960818291 + Y: 0.1135530024766922 + Z: -0.10787200182676315 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.474798 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4747979938983917 Target Frame: Value: Orbit (rviz) - Yaw: 2.39541 + Yaw: 2.3954100608825684 Saved: ~ Window Geometry: Displays: collapsed: false + Height: 1015 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001b8000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004120000003efc0100000002fb0000000800540069006d0065010000000000000412000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002a2000001b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000039dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004120000003efc0100000002fb0000000800540069006d00650100000000000004120000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005c80000039d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false Views: collapsed: true + Width: 1848 + X: 72 + Y: 28 diff --git a/grid_map_demos/rviz/grid_map_iterators_demo.rviz b/grid_map_demos/rviz/grid_map_iterators_demo.rviz index d870cec21..3d55298a1 100644 --- a/grid_map_demos/rviz/grid_map_iterators_demo.rviz +++ b/grid_map_demos/rviz/grid_map_iterators_demo.rviz @@ -1,26 +1,25 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: ~ - Splitter Ratio: 0.642659 - Tree Height: 297 - - Class: rviz/Selection + Splitter Ratio: 0.6426590085029602 + Tree Height: 670 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -29,12 +28,12 @@ Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 0.05 - Class: rviz/Grid + Cell Size: 0.05000000074505806 + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -46,27 +45,46 @@ Visualization Manager: Plane Cell Count: 20 Reference Frame: Value: true - - Class: rviz/Axes + - Class: rviz_default_plugins/Axes Enabled: true - Length: 0.1 + Length: 0.10000000149011612 Name: Axes - Radius: 0.01 + Radius: 0.009999999776482582 Reference Frame: Value: true - - Alpha: 0.7 - Class: rviz/Map + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map - Topic: /grid_map_visualization/occupancy_grid + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /occupancy_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /occupancy_grid_updates + Use Timestamp: false Value: true - Alpha: 0.5 - Class: rviz/Polygon + Class: rviz_default_plugins/Polygon Color: 255; 255; 255 Enabled: true Name: Polygon - Topic: /grid_map_iterators_demo/polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /polygon Value: true Enabled: true Global Options: @@ -75,32 +93,55 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Angle: -1.57079 - Class: rviz/TopDownOrtho + Angle: -1.5707900524139404 + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Scale: 420.916 + Near Clip Distance: 0.009999999776482582 + Scale: 420.9159851074219 Target Frame: Value: TopDownOrtho (rviz) X: 0 @@ -109,9 +150,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false + Height: 1015 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016b000001b8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001b8000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002a0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004120000003efc0100000002fb0000000800540069006d0065010000000000000412000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002a1000001b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016b00000329fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000329000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002a0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000006efc0100000002fb0000000800540069006d00650100000000000007380000005800fffffffb0000000800540069006d00650100000000000004500000000000000000000005c70000032900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -120,3 +162,6 @@ Window Geometry: collapsed: false Views: collapsed: true + Width: 1848 + X: 72 + Y: 28 diff --git a/grid_map_demos/scripts/image_publisher.py b/grid_map_demos/scripts/image_publisher.py index 9c1745db3..94fa71216 100755 --- a/grid_map_demos/scripts/image_publisher.py +++ b/grid_map_demos/scripts/image_publisher.py @@ -1,64 +1,74 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # simple script to publish a image from a file. -import rospy -import rospkg -import time import cv2 -import sensor_msgs.msg - -def callback(self): - """ Convert a image to a ROS compatible message - (sensor_msgs.Image). - """ - global publisher, imagePath - - img = cv2.imread(imagePath, cv2.IMREAD_UNCHANGED) - -# print img.shape -# print img.size -# print img.dtype.itemsize - - rosimage = sensor_msgs.msg.Image() - if img.dtype.itemsize == 2: - if len(img.shape) == 3: - if img.shape[2] == 3: - rosimage.encoding = 'bgr16' - if img.shape[2] == 4: - rosimage.encoding = 'bgra16' - else: - rosimage.encoding = 'mono16' - if img.dtype.itemsize == 1: - if len(img.shape) == 3: - if img.shape[2] == 3: - rosimage.encoding = 'bgr8' - if img.shape[2] == 4: - rosimage.encoding = 'bgra8' - else: - rosimage.encoding = 'mono8' -# print "Encoding: ", rosimage.encoding - - rosimage.width = img.shape[1] - rosimage.height = img.shape[0] - rosimage.step = img.strides[0] - rosimage.data = img.tostring() - rosimage.header.stamp = rospy.Time.now() - rosimage.header.frame_id = 'map' - - publisher.publish(rosimage) - - -#Main function initializes node and subscribers and starts the ROS loop -def main_program(): - global publisher, imagePath - rospack = rospkg.RosPack() - rospy.init_node('image_publisher') - imagePath = rospy.get_param('~image_path') - topicName = rospy.get_param('~topic') - publisher = rospy.Publisher(topicName, sensor_msgs.msg.Image, queue_size=10) - rospy.Timer(rospy.Duration(2), callback) - rospy.spin() +import rclpy + +from rclpy.node import Node +from sensor_msgs.msg import Image + + +class ImagePublisher(Node): + + def __init__(self): + super().__init__('image_publisher') + + self.declare_parameter('image_path') + self.declare_parameter('topic') + + self.imagePath_ = self.get_parameter( + 'image_path').get_parameter_value().string_value + topicName = self.get_parameter( + 'topic').get_parameter_value().string_value + + self.publisher_ = self.create_publisher(Image, topicName, 10) + timer_period = 2 # seconds + self.timer = self.create_timer(timer_period, self.callback) + + def callback(self): + """Convert a image to a ROS compatible message (sensor_msgs.Image).""" + img = cv2.imread(self.imagePath_, cv2.IMREAD_UNCHANGED) + + # print img.shape + # print img.size + # print img.dtype.itemsize + + rosimage = Image() + + if img.dtype.itemsize == 2: + if len(img.shape) == 3: + if img.shape[2] == 3: + rosimage.encoding = 'bgr16' + if img.shape[2] == 4: + rosimage.encoding = 'bgra16' + else: + rosimage.encoding = 'mono16' + if img.dtype.itemsize == 1: + if len(img.shape) == 3: + if img.shape[2] == 3: + rosimage.encoding = 'bgr8' + if img.shape[2] == 4: + rosimage.encoding = 'bgra8' + else: + rosimage.encoding = 'mono8' + # print('Encoding: ', rosimage.encoding) + + rosimage.width = img.shape[1] + rosimage.height = img.shape[0] + rosimage.step = img.strides[0] + rosimage.data = img.tostring() + rosimage.header.stamp = self.get_clock().now().to_msg() + rosimage.header.frame_id = 'map' + + self.publisher_.publish(rosimage) + + +# Main function initializes node and subscribers and starts the ROS loop +def main_program(args=None): + rclpy.init(args=args) + image_publisher = ImagePublisher() + rclpy.spin(image_publisher) + rclpy.shutdown() + if __name__ == '__main__': - try: - main_program() - except rospy.ROSInterruptException: pass + main_program() diff --git a/grid_map_demos/src/FiltersDemo.cpp b/grid_map_demos/src/FiltersDemo.cpp index 666580952..7205eb27d 100644 --- a/grid_map_demos/src/FiltersDemo.cpp +++ b/grid_map_demos/src/FiltersDemo.cpp @@ -7,15 +7,16 @@ * */ -#include "grid_map_demos/FiltersDemo.hpp" +#include -using namespace grid_map; +#include "grid_map_demos/FiltersDemo.hpp" -namespace grid_map_demos { +namespace grid_map_demos +{ -FiltersDemo::FiltersDemo(ros::NodeHandle& nodeHandle, bool& success) - : nodeHandle_(nodeHandle), - filterChain_("grid_map::GridMap") +FiltersDemo::FiltersDemo(ros::NodeHandle & nodeHandle, bool & success) +: nodeHandle_(nodeHandle), + filterChain_("grid_map::GridMap") { if (!readParameters()) { success = false; @@ -46,11 +47,13 @@ bool FiltersDemo::readParameters() return false; } nodeHandle_.param("output_topic", outputTopic_, std::string("output")); - nodeHandle_.param("filter_chain_parameter_name", filterChainParametersName_, std::string("grid_map_filters")); + nodeHandle_.param( + "filter_chain_parameter_name", filterChainParametersName_, + std::string("grid_map_filters")); return true; } -void FiltersDemo::callback(const grid_map_msgs::GridMap& message) +void FiltersDemo::callback(const grid_map_msgs::GridMap & message) { // Convert message to map. GridMap inputMap; @@ -70,4 +73,4 @@ void FiltersDemo::callback(const grid_map_msgs::GridMap& message) publisher_.publish(outputMessage); } -} /* namespace */ +} // namespace grid_map_demos diff --git a/grid_map_demos/src/ImageToGridmapDemo.cpp b/grid_map_demos/src/ImageToGridmapDemo.cpp index 295b08023..a0dcb35ee 100644 --- a/grid_map_demos/src/ImageToGridmapDemo.cpp +++ b/grid_map_demos/src/ImageToGridmapDemo.cpp @@ -6,19 +6,28 @@ * Institute: ETH Zurich, ANYbotics */ +#include +#include + #include "grid_map_demos/ImageToGridmapDemo.hpp" -namespace grid_map_demos { +namespace grid_map_demos +{ -ImageToGridmapDemo::ImageToGridmapDemo(ros::NodeHandle& nodeHandle) - : nodeHandle_(nodeHandle), - map_(grid_map::GridMap({"elevation"})), - mapInitialized_(false) +ImageToGridmapDemo::ImageToGridmapDemo() +: Node("image_to_gridmap_demo"), + map_(grid_map::GridMap({"elevation"})), + mapInitialized_(false) { readParameters(); map_.setBasicLayers({"elevation"}); - imageSubscriber_ = nodeHandle_.subscribe(imageTopic_, 1, &ImageToGridmapDemo::imageCallback, this); - gridMapPublisher_ = nodeHandle_.advertise("grid_map", 1, true); + imageSubscriber_ = + this->create_subscription( + imageTopic_, 1, + std::bind(&ImageToGridmapDemo::imageCallback, this, std::placeholders::_1)); + + gridMapPublisher_ = this->create_publisher( + "grid_map", rclcpp::QoS(1).transient_local()); } ImageToGridmapDemo::~ImageToGridmapDemo() @@ -27,28 +36,34 @@ ImageToGridmapDemo::~ImageToGridmapDemo() bool ImageToGridmapDemo::readParameters() { - nodeHandle_.param("image_topic", imageTopic_, std::string("/image")); - nodeHandle_.param("resolution", resolution_, 0.03); - nodeHandle_.param("min_height", minHeight_, 0.0); - nodeHandle_.param("max_height", maxHeight_, 1.0); + this->declare_parameter("image_topic", std::string("/image")); + this->declare_parameter("resolution", rclcpp::ParameterValue(0.03)); + this->declare_parameter("min_height", rclcpp::ParameterValue(0.0)); + this->declare_parameter("max_height", rclcpp::ParameterValue(1.0)); + + this->get_parameter("image_topic", imageTopic_); + this->get_parameter("resolution", resolution_); + this->get_parameter("min_height", minHeight_); + this->get_parameter("max_height", maxHeight_); return true; } -void ImageToGridmapDemo::imageCallback(const sensor_msgs::Image& msg) +void ImageToGridmapDemo::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) { if (!mapInitialized_) { - grid_map::GridMapRosConverter::initializeFromImage(msg, resolution_, map_); - ROS_INFO("Initialized map with size %f x %f m (%i x %i cells).", map_.getLength().x(), - map_.getLength().y(), map_.getSize()(0), map_.getSize()(1)); + grid_map::GridMapRosConverter::initializeFromImage(*msg, resolution_, map_); + RCLCPP_INFO( + this->get_logger(), + "Initialized map with size %f x %f m (%i x %i cells).", map_.getLength().x(), + map_.getLength().y(), map_.getSize()(0), map_.getSize()(1)); mapInitialized_ = true; } - grid_map::GridMapRosConverter::addLayerFromImage(msg, "elevation", map_, minHeight_, maxHeight_); - grid_map::GridMapRosConverter::addColorLayerFromImage(msg, "color", map_); + grid_map::GridMapRosConverter::addLayerFromImage(*msg, "elevation", map_, minHeight_, maxHeight_); + grid_map::GridMapRosConverter::addColorLayerFromImage(*msg, "color", map_); // Publish as grid map. - grid_map_msgs::GridMap mapMessage; - grid_map::GridMapRosConverter::toMessage(map_, mapMessage); - gridMapPublisher_.publish(mapMessage); + auto message = grid_map::GridMapRosConverter::toMessage(map_); + gridMapPublisher_->publish(std::move(message)); } -} /* namespace */ +} // namespace grid_map_demos diff --git a/grid_map_demos/src/InterpolationDemo.cpp b/grid_map_demos/src/InterpolationDemo.cpp index 5f4d3678b..ce371f6f0 100644 --- a/grid_map_demos/src/InterpolationDemo.cpp +++ b/grid_map_demos/src/InterpolationDemo.cpp @@ -8,44 +8,48 @@ #include "grid_map_demos/InterpolationDemo.hpp" +#include +#include +#include + #include "grid_map_core/iterators/GridMapIterator.hpp" #include "grid_map_msgs/GridMap.h" #include "grid_map_ros/GridMapRosConverter.hpp" -namespace grid_map_demos { - -AnalyticalFunctions createWorld(Worlds world, double highResolution, double lowResolution, - double length, double width, grid_map::GridMap *groundTruthHighRes, - grid_map::GridMap *groundTruthLowRes) +namespace grid_map_demos { +AnalyticalFunctions createWorld( + Worlds world, double highResolution, double lowResolution, + double length, double width, grid_map::GridMap * groundTruthHighRes, + grid_map::GridMap * groundTruthLowRes) +{ const grid_map::Length mapLength(length, width); const grid_map::Position mapPosition(0.0, 0.0); *groundTruthHighRes = createMap(mapLength, highResolution, mapPosition); *groundTruthLowRes = createMap(mapLength, lowResolution, mapPosition); AnalyticalFunctions groundTruth; switch (world) { - case Worlds::Sine: { - groundTruth = createSineWorld(groundTruthHighRes); - createSineWorld(groundTruthLowRes); - break; - } + groundTruth = createSineWorld(groundTruthHighRes); + createSineWorld(groundTruthLowRes); + break; + } case Worlds::Tanh: { - groundTruth = createTanhWorld(groundTruthHighRes); - createTanhWorld(groundTruthLowRes); - break; - } + groundTruth = createTanhWorld(groundTruthHighRes); + createTanhWorld(groundTruthLowRes); + break; + } case Worlds::GaussMixture: { - groundTruth = createGaussianWorld(groundTruthHighRes); - createGaussianWorld(groundTruthLowRes); - break; - } + groundTruth = createGaussianWorld(groundTruthHighRes); + createGaussianWorld(groundTruthLowRes); + break; + } case Worlds::Poly: { - groundTruth = createPolyWorld(groundTruthHighRes); - createPolyWorld(groundTruthLowRes); - break; - } + groundTruth = createPolyWorld(groundTruthHighRes); + createPolyWorld(groundTruthLowRes); + break; + } default: throw std::runtime_error("Interpolation demo: Unknown world requested."); @@ -54,24 +58,21 @@ AnalyticalFunctions createWorld(Worlds world, double highResolution, double lowR return groundTruth; } -AnalyticalFunctions createPolyWorld(grid_map::GridMap *map) +AnalyticalFunctions createPolyWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; - func.f_ = [](double x,double y) { - return (-x*x -y*y +2.0*x*y +x*x*y*y); - }; + func.f_ = [](double x, double y) { + return -x * x - y * y + 2.0 * x * y + x * x * y * y; + }; fillGridMap(map, func); return func; - } -AnalyticalFunctions createSineWorld(grid_map::GridMap *map) +AnalyticalFunctions createSineWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; const double w1 = 1.0; @@ -79,38 +80,35 @@ AnalyticalFunctions createSineWorld(grid_map::GridMap *map) const double w3 = 3.0; const double w4 = 4.0; - func.f_ = [w1,w2,w3,w4](double x,double y) { - return std::cos(w1*x) + std::sin(w2*y) + std::cos(w3*x) + std::sin(w4*y); - }; + func.f_ = [w1, w2, w3, w4](double x, double y) { + return std::cos(w1 * x) + std::sin(w2 * y) + std::cos(w3 * x) + std::sin(w4 * y); + }; fillGridMap(map, func); return func; - } -AnalyticalFunctions createTanhWorld(grid_map::GridMap *map) +AnalyticalFunctions createTanhWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; const double s = 5.0; - func.f_ = [s](double x,double y) { - const double expZx = std::exp(2 *s* x); - const double tanX = (expZx - 1) / (expZx + 1); - const double expZy = std::exp(2 *s* y); - const double tanY = (expZy - 1) / (expZy + 1); - return tanX + tanY; - }; + func.f_ = [s](double x, double y) { + const double expZx = std::exp(2 * s * x); + const double tanX = (expZx - 1) / (expZx + 1); + const double expZy = std::exp(2 * s * y); + const double tanY = (expZy - 1) / (expZy + 1); + return tanX + tanY; + }; fillGridMap(map, func); return func; } -AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map) +AnalyticalFunctions createGaussianWorld(grid_map::GridMap * map) { - struct Gaussian { double x0, y0; @@ -120,11 +118,11 @@ AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map) AnalyticalFunctions func; constexpr int numGaussians = 7; - std::array, numGaussians> vars = { { { 1.0, 0.3 }, { 0.25, 0.25 }, { - 0.1, 0.1 }, { 0.1, 0.1 }, { 0.1, 0.1 }, { 0.1, 0.05 }, { 0.05, 0.05 } } }; - std::array, numGaussians> means = { { { 1, -1 }, { 1, 1.7 }, - { -1, 1.6 }, { -1.8, -1.8 }, { -1, 1.8 }, { 0, 0 }, { -1.2, 0 } } }; - std::array scales = { -2.0, -1.0, 2.0, 1.0, 3.0, 4.0, 1.0 }; + std::array, numGaussians> vars = {{{1.0, 0.3}, {0.25, 0.25}, { + 0.1, 0.1}, {0.1, 0.1}, {0.1, 0.1}, {0.1, 0.05}, {0.05, 0.05}}}; + std::array, numGaussians> means = {{{1, -1}, {1, 1.7}, + {-1, 1.6}, {-1.8, -1.8}, {-1, 1.8}, {0, 0}, {-1.2, 0}}}; + std::array scales = {-2.0, -1.0, 2.0, 1.0, 3.0, 4.0, 1.0}; std::array g; @@ -136,28 +134,29 @@ AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map) g.at(i).s = scales.at(i); } - func.f_ = [g](double x,double y) { - double value = 0.0; - for (int i = 0; i < g.size(); ++i) { - const double x0 = g.at(i).x0; - const double y0 = g.at(i).y0; - const double varX = g.at(i).varX; - const double varY = g.at(i).varY; - const double s = g.at(i).s; - value += s * std::exp(-(x-x0)*(x-x0) / (2.0*varX) - (y-y0)*(y-y0) / (2.0 * varY)); - } - - return value; - }; + func.f_ = [g](double x, double y) { + double value = 0.0; + for (int i = 0; i < g.size(); ++i) { + const double x0 = g.at(i).x0; + const double y0 = g.at(i).y0; + const double varX = g.at(i).varX; + const double varY = g.at(i).varY; + const double s = g.at(i).s; + value += s * + std::exp(-(x - x0) * (x - x0) / (2.0 * varX) - (y - y0) * (y - y0) / (2.0 * varY)); + } + + return value; + }; fillGridMap(map, func); return func; } -void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions) +void fillGridMap(grid_map::GridMap * map, const AnalyticalFunctions & functions) { - grid_map::Matrix& data = (*map)[demoLayer]; + grid_map::Matrix & data = (*map)[demoLayer]; for (grid_map::GridMapIterator iterator(*map); !iterator.isPastEnd(); ++iterator) { const grid_map::Index index(*iterator); grid_map::Position pos; @@ -166,8 +165,9 @@ void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions) } } -grid_map::GridMap createMap(const grid_map::Length &length, double resolution, - const grid_map::Position &pos) +grid_map::GridMap createMap( + const grid_map::Length & length, double resolution, + const grid_map::Position & pos) { grid_map::GridMap map; @@ -178,21 +178,23 @@ grid_map::GridMap createMap(const grid_map::Length &length, double resolution, return map; } -grid_map::GridMap createInterpolatedMapFromDataMap(const grid_map::GridMap &dataMap, - double desiredResolution) +grid_map::GridMap createInterpolatedMapFromDataMap( + const grid_map::GridMap & dataMap, + double desiredResolution) { grid_map::GridMap interpolatedMap; interpolatedMap.setGeometry(dataMap.getLength(), desiredResolution, dataMap.getPosition()); - const std::string &layer = demoLayer; + const std::string & layer = demoLayer; interpolatedMap.add(layer, 0.0); interpolatedMap.setFrameId(dataMap.getFrameId()); return interpolatedMap; } -void interpolateInputMap(const grid_map::GridMap &dataMap, - grid_map::InterpolationMethods interpolationMethod, - grid_map::GridMap *interpolatedMap) +void interpolateInputMap( + const grid_map::GridMap & dataMap, + grid_map::InterpolationMethods interpolationMethod, + grid_map::GridMap * interpolatedMap) { for (grid_map::GridMapIterator iterator(*interpolatedMap); !iterator.isPastEnd(); ++iterator) { const grid_map::Index index(*iterator); @@ -203,8 +205,9 @@ void interpolateInputMap(const grid_map::GridMap &dataMap, } } -Error computeInterpolationError(const AnalyticalFunctions &groundTruth, - const grid_map::GridMap &map) +Error computeInterpolationError( + const AnalyticalFunctions & groundTruth, + const grid_map::GridMap & map) { const double r = map.getResolution(); const double dimX = map.getLength().x() / 2.0 - 3.0 * r; @@ -217,7 +220,7 @@ Error computeInterpolationError(const AnalyticalFunctions &groundTruth, for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { const auto row = (*iterator).x(); const auto col = (*iterator).y(); - const bool skipEvaluation = row < 2 || col < 2 || col > (nCol - 3) || row > (nRow - 3); + const bool skipEvaluation = row<2 || col<2 || col>(nCol - 3) || row>(nRow - 3); if (skipEvaluation) { continue; } @@ -236,12 +239,10 @@ Error computeInterpolationError(const AnalyticalFunctions &groundTruth, error.meanSquare_ /= count; error.meanAbs_ /= count; return error; - } -InterpolationDemo::InterpolationDemo(ros::NodeHandle *nh) +InterpolationDemo::InterpolationDemo(ros::NodeHandle * nh) { - nh->param("interpolation_type", interpolationMethod_, "Nearest"); nh->param("world", world_, "Sine"); nh->param("groundtruth_resolution", groundTruthResolution_, 0.02); @@ -259,14 +260,15 @@ InterpolationDemo::InterpolationDemo(ros::NodeHandle *nh) void InterpolationDemo::runDemo() { - // visualize stuff - groundTruth_ = createWorld(worlds.at(world_), groundTruthResolution_, dataResolution_, - worldLength_, worldWidth_, &groundTruthMap_, &dataSparseMap_); + groundTruth_ = createWorld( + worlds.at(world_), groundTruthResolution_, dataResolution_, + worldLength_, worldWidth_, &groundTruthMap_, &dataSparseMap_); interpolatedMap_ = createInterpolatedMapFromDataMap(dataSparseMap_, interpolatedResolution_); - interpolateInputMap(dataSparseMap_, interpolationMethods.at(interpolationMethod_), - &interpolatedMap_); + interpolateInputMap( + dataSparseMap_, interpolationMethods.at(interpolationMethod_), + &interpolatedMap_); publishGridMaps(); std::cout << "\n \n visualized the world: " << world_ << std::endl; @@ -274,7 +276,6 @@ void InterpolationDemo::runDemo() // print some info const auto statistics = computeStatistics(); printStatistics(statistics); - } InterpolationDemo::Statistics InterpolationDemo::computeStatistics() const @@ -283,27 +284,29 @@ InterpolationDemo::Statistics InterpolationDemo::computeStatistics() const for (auto world = worlds.cbegin(); world != worlds.cend(); ++world) { std::map methodsStats; for (auto method = interpolationMethods.cbegin(); method != interpolationMethods.cend(); - ++method) { + ++method) + { const auto errorAndDuration = interpolateAndComputeError(world->first, method->first); Statistic statistic; statistic.duration_ = errorAndDuration.second; statistic.error_ = errorAndDuration.first; statistic.interpolationMethod_ = method->first; statistic.world_ = world->first; - methodsStats.insert( { method->first, statistic }); + methodsStats.insert({method->first, statistic}); } - stats.insert( { world->first, methodsStats }); + stats.insert({world->first, methodsStats}); } return std::move(stats); } InterpolationDemo::ErrorAndDuration InterpolationDemo::interpolateAndComputeError( - const std::string world, const std::string &method) const + const std::string world, const std::string & method) const { grid_map::GridMap highResMap, dataMap; - auto groundTruth = createWorld(worlds.at(world), groundTruthResolution_, dataResolution_, - worldLength_, worldWidth_, &highResMap, &dataMap); + auto groundTruth = createWorld( + worlds.at(world), groundTruthResolution_, dataResolution_, + worldLength_, worldWidth_, &highResMap, &dataMap); auto interpolatedMap = createInterpolatedMapFromDataMap(dataMap, interpolatedResolution_); const auto start = clk::now(); @@ -318,22 +321,26 @@ InterpolationDemo::ErrorAndDuration InterpolationDemo::interpolateAndComputeErro return errorAndDuration; } -void InterpolationDemo::printStatistics(const Statistics &stats) const +void InterpolationDemo::printStatistics(const Statistics & stats) const { std::cout << " \n \n ================================== \n"; - printf("Interpolated map of size: %f x %f \n", interpolatedMap_.getLength().x(), - interpolatedMap_.getLength().y()); - printf("Resolution of the data: %f, resolution of the interpolated map: %f, \n \n", - dataSparseMap_.getResolution(), interpolatedMap_.getResolution()); + printf( + "Interpolated map of size: %f x %f \n", interpolatedMap_.getLength().x(), + interpolatedMap_.getLength().y()); + printf( + "Resolution of the data: %f, resolution of the interpolated map: %f, \n \n", + dataSparseMap_.getResolution(), interpolatedMap_.getResolution()); for (auto world = worlds.cbegin(); world != worlds.cend(); ++world) { std::cout << "Stats for the world: " << world->first << std::endl; - for (auto method = interpolationMethods.cbegin(); method != interpolationMethods.cend(); - ++method) { + for (auto method = interpolationMethods.cbegin(); + method != interpolationMethods.cend(); ++method) + { Statistic s = stats.at(world->first).at(method->first); printf( - "Method: %-20s Mean absolute error: %-10f max error: %-10f avg query duration: %-10f microsec \n", - method->first.c_str(), s.error_.meanAbs_, s.error_.max_, s.duration_); + "Method: %-20s Mean absolute error: %-10f " + "max error: %-10f avg query duration: %-10f microsec \n", + method->first.c_str(), s.error_.meanAbs_, s.error_.max_, s.duration_); } std::cout << std::endl; } @@ -350,5 +357,4 @@ void InterpolationDemo::publishGridMaps() const interpolatedMapPub_.publish(interpolatedMsg); } -} /* namespace grid_map_demos */ - +} // namespace grid_map_demos diff --git a/grid_map_demos/src/IteratorsDemo.cpp b/grid_map_demos/src/IteratorsDemo.cpp index abc0b2e7b..dea065a46 100644 --- a/grid_map_demos/src/IteratorsDemo.cpp +++ b/grid_map_demos/src/IteratorsDemo.cpp @@ -8,30 +8,30 @@ #include "grid_map_demos/IteratorsDemo.hpp" -// ROS -#include +#include +#include +#include +#include -using namespace std; -using namespace ros; -using namespace grid_map; - -namespace grid_map_demos { +namespace grid_map_demos +{ -IteratorsDemo::IteratorsDemo(ros::NodeHandle& nodeHandle) - : nodeHandle_(nodeHandle), - map_(vector({"type"})) +IteratorsDemo::IteratorsDemo() +: Node("grid_map_iterators_demo"), + map_(std::vector({"type"})) { - ROS_INFO("Grid map iterators demo node started."); - gridMapPublisher_ = nodeHandle_.advertise("grid_map", 1, true); - polygonPublisher_ = nodeHandle_.advertise("polygon", 1, true); + RCLCPP_INFO(this->get_logger(), "Grid map iterators demo node started."); + gridMapPublisher_ = this->create_publisher( + "grid_map", rclcpp::QoS(1).transient_local()); + polygonPublisher_ = this->create_publisher( + "polygon", rclcpp::QoS(1).transient_local()); // Setting up map. - map_.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0)); + map_.setGeometry(grid_map::Length(1.0, 1.0), 0.05, grid_map::Position(0.0, 0.0)); map_.setFrameId("map"); publish(); - ros::Duration duration(2.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(2)); demoGridMapIterator(); demoSubmapIterator(); @@ -47,212 +47,208 @@ IteratorsDemo::~IteratorsDemo() {} void IteratorsDemo::demoGridMapIterator() { - ROS_INFO("Running grid map iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running grid map iterator demo."); map_.clearAll(); publish(); // Note: In this example we locally store a reference to the map data // for improved performance. See `iterator_benchmark.cpp` and the // README.md for a comparison and more information. - grid_map::Matrix& data = map_["type"]; + grid_map::Matrix & data = map_["type"]; for (grid_map::GridMapIterator iterator(map_); !iterator.isPastEnd(); ++iterator) { const int i = iterator.getLinearIndex(); data(i) = 1.0; publish(); - ros::Duration duration(0.01); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(10)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoSubmapIterator() { - ROS_INFO("Running submap iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running submap iterator demo."); map_.clearAll(); publish(); - Index submapStartIndex(3, 5); - Index submapBufferSize(12, 7); + grid_map::Index submapStartIndex(3, 5); + grid_map::Index submapBufferSize(12, 7); for (grid_map::SubmapIterator iterator(map_, submapStartIndex, submapBufferSize); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoCircleIterator() { - ROS_INFO("Running circle iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running circle iterator demo."); map_.clearAll(); publish(); - Position center(0.0, -0.15); + grid_map::Position center(0.0, -0.15); double radius = 0.4; for (grid_map::CircleIterator iterator(map_, center, radius); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoEllipseIterator() { - ROS_INFO("Running ellipse iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running ellipse iterator demo."); map_.clearAll(); publish(); - Position center(0.0, -0.15); - Length length(0.45, 0.9); + grid_map::Position center(0.0, -0.15); + grid_map::Length length(0.45, 0.9); for (grid_map::EllipseIterator iterator(map_, center, length, M_PI_4); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoSpiralIterator() { - ROS_INFO("Running spiral iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running spiral iterator demo."); map_.clearAll(); publish(); - Position center(0.0, -0.15); + grid_map::Position center(0.0, -0.15); double radius = 0.4; for (grid_map::SpiralIterator iterator(map_, center, radius); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoLineIterator() { - ROS_INFO("Running line iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running line iterator demo."); map_.clearAll(); publish(); - Index start(18, 2); - Index end(2, 13); + grid_map::Index start(18, 2); + grid_map::Index end(2, 13); for (grid_map::LineIterator iterator(map_, start, end); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoPolygonIterator(const bool prepareForOtherDemos) { - ROS_INFO("Running polygon iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running polygon iterator demo."); map_.clearAll(); - if (prepareForOtherDemos) map_["type"].setZero(); + if (prepareForOtherDemos) {map_["type"].setZero();} publish(); grid_map::Polygon polygon; polygon.setFrameId(map_.getFrameId()); - polygon.addVertex(Position( 0.480, 0.000)); - polygon.addVertex(Position( 0.164, 0.155)); - polygon.addVertex(Position( 0.116, 0.500)); - polygon.addVertex(Position(-0.133, 0.250)); - polygon.addVertex(Position(-0.480, 0.399)); - polygon.addVertex(Position(-0.316, 0.000)); - polygon.addVertex(Position(-0.480, -0.399)); - polygon.addVertex(Position(-0.133, -0.250)); - polygon.addVertex(Position( 0.116, -0.500)); - polygon.addVertex(Position( 0.164, -0.155)); - polygon.addVertex(Position( 0.480, 0.000)); - - geometry_msgs::PolygonStamped message; + polygon.addVertex(grid_map::Position(0.480, 0.000)); + polygon.addVertex(grid_map::Position(0.164, 0.155)); + polygon.addVertex(grid_map::Position(0.116, 0.500)); + polygon.addVertex(grid_map::Position(-0.133, 0.250)); + polygon.addVertex(grid_map::Position(-0.480, 0.399)); + polygon.addVertex(grid_map::Position(-0.316, 0.000)); + polygon.addVertex(grid_map::Position(-0.480, -0.399)); + polygon.addVertex(grid_map::Position(-0.133, -0.250)); + polygon.addVertex(grid_map::Position(0.116, -0.500)); + polygon.addVertex(grid_map::Position(0.164, -0.155)); + polygon.addVertex(grid_map::Position(0.480, 0.000)); + + geometry_msgs::msg::PolygonStamped message; grid_map::PolygonRosConverter::toMessage(polygon, message); - polygonPublisher_.publish(message); + polygonPublisher_->publish(message); for (grid_map::PolygonIterator iterator(map_, polygon); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; if (!prepareForOtherDemos) { publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } } if (!prepareForOtherDemos) { - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } } void IteratorsDemo::demoSlidingWindowIterator() { - ROS_INFO("Running sliding window iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running sliding window iterator demo."); demoPolygonIterator(true); publish(); const size_t windowSize = 3; - const grid_map::SlidingWindowIterator::EdgeHandling edgeHandling = grid_map::SlidingWindowIterator::EdgeHandling::CROP; + const grid_map::SlidingWindowIterator::EdgeHandling edgeHandling = + grid_map::SlidingWindowIterator::EdgeHandling::CROP; map_.add("copy", map_["type"]); for (grid_map::SlidingWindowIterator iterator(map_, "copy", edgeHandling, windowSize); - !iterator.isPastEnd(); ++iterator) { - map_.at("type", *iterator) = iterator.getData().meanOfFinites(); // Blurring. + !iterator.isPastEnd(); ++iterator) + { + map_.at("type", *iterator) = iterator.getData().meanOfFinites(); // Blurring. publish(); // Visualize sliding window as polygon. grid_map::Polygon polygon; - Position center; + grid_map::Position center; map_.getPosition(*iterator, center); - const Length windowHalfLength(Length::Constant(0.5 * (double) windowSize * map_.getResolution())); - polygon.addVertex(center + (Eigen::Array2d(-1.0,-1.0) * windowHalfLength).matrix()); + const grid_map::Length windowHalfLength(grid_map::Length::Constant( + 0.5 * static_cast(windowSize) * map_.getResolution())); + polygon.addVertex(center + (Eigen::Array2d(-1.0, -1.0) * windowHalfLength).matrix()); polygon.addVertex(center + (Eigen::Array2d(-1.0, 1.0) * windowHalfLength).matrix()); - polygon.addVertex(center + (Eigen::Array2d( 1.0, 1.0) * windowHalfLength).matrix()); - polygon.addVertex(center + (Eigen::Array2d( 1.0,-1.0) * windowHalfLength).matrix()); + polygon.addVertex(center + (Eigen::Array2d(1.0, 1.0) * windowHalfLength).matrix()); + polygon.addVertex(center + (Eigen::Array2d(1.0, -1.0) * windowHalfLength).matrix()); polygon.setFrameId(map_.getFrameId()); - geometry_msgs::PolygonStamped message; + geometry_msgs::msg::PolygonStamped message; grid_map::PolygonRosConverter::toMessage(polygon, message); - polygonPublisher_.publish(message); + polygonPublisher_->publish(message); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } } void IteratorsDemo::publish() { - map_.setTimestamp(ros::Time::now().toNSec()); - grid_map_msgs::GridMap message; - grid_map::GridMapRosConverter::toMessage(map_, message); - gridMapPublisher_.publish(message); - ROS_DEBUG("Grid map (timestamp %f) published.", message.info.header.stamp.toSec()); + map_.setTimestamp(this->now().nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map_); + RCLCPP_DEBUG( + this->get_logger(), "Publishing grid map (timestamp %f).", + rclcpp::Time(message->header.stamp).nanoseconds() * 1e-9); + gridMapPublisher_->publish(std::move(message)); } -} /* namespace */ +} // namespace grid_map_demos diff --git a/grid_map_demos/src/OctomapToGridmapDemo.cpp b/grid_map_demos/src/OctomapToGridmapDemo.cpp index 4f614a385..a48155ce9 100644 --- a/grid_map_demos/src/OctomapToGridmapDemo.cpp +++ b/grid_map_demos/src/OctomapToGridmapDemo.cpp @@ -15,11 +15,14 @@ #include #include -namespace grid_map_demos { +#include -OctomapToGridmapDemo::OctomapToGridmapDemo(ros::NodeHandle& nodeHandle) - : nodeHandle_(nodeHandle), - map_(grid_map::GridMap({"elevation"})) +namespace grid_map_demos +{ + +OctomapToGridmapDemo::OctomapToGridmapDemo(ros::NodeHandle & nodeHandle) +: nodeHandle_(nodeHandle), + map_(grid_map::GridMap({"elevation"})) { readParameters(); client_ = nodeHandle_.serviceClient(octomapServiceTopic_); @@ -53,10 +56,10 @@ void OctomapToGridmapDemo::convertAndPublishMap() } // creating octree - octomap::OcTree* octomap = nullptr; - octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(srv.response.map); + octomap::OcTree * octomap = nullptr; + octomap::AbstractOcTree * tree = octomap_msgs::msgToMap(srv.response.map); if (tree) { - octomap = dynamic_cast(tree); + octomap = dynamic_cast(tree); } else { ROS_ERROR("Failed to call convert Octomap."); return; @@ -66,19 +69,27 @@ void OctomapToGridmapDemo::convertAndPublishMap() grid_map::Position3 max_bound; octomap->getMetricMin(min_bound(0), min_bound(1), min_bound(2)); octomap->getMetricMax(max_bound(0), max_bound(1), max_bound(2)); - if(!std::isnan(minX_)) + if (!std::isnan(minX_)) { min_bound(0) = minX_; - if(!std::isnan(maxX_)) + } + if (!std::isnan(maxX_)) { max_bound(0) = maxX_; - if(!std::isnan(minY_)) + } + if (!std::isnan(minY_)) { min_bound(1) = minY_; - if(!std::isnan(maxY_)) + } + if (!std::isnan(maxY_)) { max_bound(1) = maxY_; - if(!std::isnan(minZ_)) + } + if (!std::isnan(minZ_)) { min_bound(2) = minZ_; - if(!std::isnan(maxZ_)) + } + if (!std::isnan(maxZ_)) { max_bound(2) = maxZ_; - bool res = grid_map::GridMapOctomapConverter::fromOctomap(*octomap, "elevation", map_, &min_bound, &max_bound); + } + bool res = grid_map::GridMapOctomapConverter::fromOctomap( + *octomap, "elevation", map_, &min_bound, + &max_bound); if (!res) { ROS_ERROR("Failed to call convert Octomap."); return; @@ -97,4 +108,4 @@ void OctomapToGridmapDemo::convertAndPublishMap() octomapPublisher_.publish(octomapMessage); } -} /* namespace */ +} // namespace grid_map_demos diff --git a/grid_map_demos/src/filters_demo_node.cpp b/grid_map_demos/src/filters_demo_node.cpp index b8b142a8c..6dca95492 100644 --- a/grid_map_demos/src/filters_demo_node.cpp +++ b/grid_map_demos/src/filters_demo_node.cpp @@ -8,98 +8,12 @@ #include "grid_map_demos/FiltersDemo.hpp" -#include - -int main(int argc, char** argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "grid_map_filters_demo"); ros::NodeHandle nodeHandle("~"); bool success; grid_map_demos::FiltersDemo filtersDemo(nodeHandle, success); - if (success) ros::spin(); + if (success) {ros::spin();} return 0; } - - - - - -///* -// * filters_demo_node.cpp -// * -// * Created on: Aug 14, 2017 -// * Author: Peter Fankhauser -// * Institute: ETH Zurich, ANYbotics -// */ -// -//#include -//#include -//#include -//#include -//#include -// -//using namespace grid_map; -// -//class GridMapFilterChain -// -// -//(const grid_map_msgs::GridMapPtr& message) -//{ -// GridMap inputMap; -// GridMapRosConverter::fromMessage(message, inputMap); -// ROS_INFO("I heard: [%s]", msg->data.c_str()); -//} -// -//int main(int argc, char** argv) -//{ -// ros::init(argc, argv, "grid_map_filters_demo"); -// ros::NodeHandle nodeHandle("~"); -// ros::Subscriber subscriber = nodeHandle.subscribe("grid_map_in", 1000, chatterCallback); -// ros::Publisher publisher = nodeHandle.advertise("grid_map_out", 1, true); -// -// // Setup filter chain. -// filters::FilterChain filterChain("grid_map::GridMap"); -// if (!filterChain.configure("grid_map_filters", nodeHandle)) { -// ROS_ERROR("Could not configure the filter chain!"); -// return false; -// } -// -// // Create input grid map. -// GridMap inputMap({"elevation"}); -// inputMap.setFrameId("map"); -// inputMap.setGeometry(Length(0.7, 0.7), 0.01, Position(0.0, 0.0)); -// ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.", -// inputMap.getLength().x(), inputMap.getLength().y(), -// inputMap.getSize()(0), inputMap.getSize()(1), -// inputMap.getPosition().x(), inputMap.getPosition().y(), inputMap.getFrameId().c_str()); -// inputMap["elevation"].setRandom(); -// inputMap["elevation"] *= 0.01; -// for (grid_map::CircleIterator iterator(inputMap, Position(0.0, 0.0), 0.15); !iterator.isPastEnd(); ++iterator) { -// inputMap.at("elevation", *iterator) = 0.1; -// } -// -// while (nodeHandle.ok()) { -// -// // Publish original grid map. -// inputMap.setTimestamp(ros::Time::now().toNSec()); -// grid_map_msgs::GridMap message; -// GridMapRosConverter::toMessage(inputMap, message); -// publisher.publish(message); -// ros::Duration(1.0).sleep(); -// -// // Apply filter chain. -// GridMap outputMap; -// if (!filterChain.update(inputMap, outputMap)) { -// ROS_ERROR("Could not update the grid map filter chain!"); -// break; -// } -// -// // Publish filtered output grid map. -// outputMap.setTimestamp(ros::Time::now().toNSec()); -// GridMapRosConverter::toMessage(outputMap, message); -// publisher.publish(message); -// ros::Duration(1.0).sleep(); -// } -// -// return 0; -//} diff --git a/grid_map_demos/src/image_to_gridmap_demo_node.cpp b/grid_map_demos/src/image_to_gridmap_demo_node.cpp index 0b7349592..80a6697c0 100644 --- a/grid_map_demos/src/image_to_gridmap_demo_node.cpp +++ b/grid_map_demos/src/image_to_gridmap_demo_node.cpp @@ -6,16 +6,14 @@ * Institute: ETH Zurich, ANYbotics */ -#include +#include +#include #include "grid_map_demos/ImageToGridmapDemo.hpp" -int main(int argc, char** argv) +int main(int argc, char ** argv) { - // Initialize node and publisher. - ros::init(argc, argv, "image_to_gridmap_demo"); - ros::NodeHandle nh("~"); - grid_map_demos::ImageToGridmapDemo imageToGridmapDemo(nh); - - ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/grid_map_demos/src/interpolation_demo_node.cpp b/grid_map_demos/src/interpolation_demo_node.cpp index 873e8761f..d36c6e267 100644 --- a/grid_map_demos/src/interpolation_demo_node.cpp +++ b/grid_map_demos/src/interpolation_demo_node.cpp @@ -5,10 +5,10 @@ * Author: jelavice */ -#include "grid_map_demos/InterpolationDemo.hpp" #include +#include "grid_map_demos/InterpolationDemo.hpp" -int main(int argc, char** argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "grid_map_interpolation_demo"); ros::NodeHandle nodeHandle("~"); diff --git a/grid_map_demos/src/iterator_benchmark.cpp b/grid_map_demos/src/iterator_benchmark.cpp index ec09714e4..ee5a899e6 100644 --- a/grid_map_demos/src/iterator_benchmark.cpp +++ b/grid_map_demos/src/iterator_benchmark.cpp @@ -8,10 +8,7 @@ #include #include #include - -using namespace std; -using namespace std::chrono; -using namespace grid_map; +#include #define duration(a) duration_cast(a).count() typedef high_resolution_clock clk; @@ -19,11 +16,11 @@ typedef high_resolution_clock clk; /*! * Convenient use of iterator. */ -void runGridMapIteratorVersion1(GridMap& map, const string& layer_from, const string& layer_to) +void runGridMapIteratorVersion1(GridMap & map, const string & layer_from, const string & layer_to) { for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { const float value_from = map.at(layer_from, *iterator); - float& value_to = map.at(layer_to, *iterator); + float & value_to = map.at(layer_to, *iterator); value_to = value_to > value_from ? value_to : value_from; } } @@ -31,14 +28,14 @@ void runGridMapIteratorVersion1(GridMap& map, const string& layer_from, const st /*! * Improved efficiency by storing direct access to data layers. */ -void runGridMapIteratorVersion2(GridMap& map, const string& layer_from, const string& layer_to) +void runGridMapIteratorVersion2(GridMap & map, const string & layer_from, const string & layer_to) { - const auto& data_from = map[layer_from]; - auto& data_to = map[layer_to]; + const auto & data_from = map[layer_from]; + auto & data_to = map[layer_to]; for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { const Index index(*iterator); const float value_from = data_from(index(0), index(1)); - float& value_to = data_to(index(0), index(1)); + float & value_to = data_to(index(0), index(1)); value_to = value_to > value_from ? value_to : value_from; } } @@ -46,14 +43,14 @@ void runGridMapIteratorVersion2(GridMap& map, const string& layer_from, const st /*! * Improved efficiency by using linear index. */ -void runGridMapIteratorVersion3(GridMap& map, const string& layer_from, const string& layer_to) +void runGridMapIteratorVersion3(GridMap & map, const string & layer_from, const string & layer_to) { - const auto& data_from = map[layer_from]; - auto& data_to = map[layer_to]; + const auto & data_from = map[layer_from]; + auto & data_to = map[layer_to]; for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { const size_t i = iterator.getLinearIndex(); const float value_from = data_from(i); - float& value_to = data_to(i); + float & value_to = data_to(i); value_to = value_to > value_from ? value_to : value_from; } } @@ -62,7 +59,7 @@ void runGridMapIteratorVersion3(GridMap& map, const string& layer_from, const st * Whenever possible, make use of the Eigen methods for maximum efficiency * and readability. */ -void runEigenFunction(GridMap& map, const string& layer_from, const string& layer_to) +void runEigenFunction(GridMap & map, const string & layer_from, const string & layer_to) { map[layer_to] = map[layer_to].cwiseMax(map[layer_from]); } @@ -70,14 +67,14 @@ void runEigenFunction(GridMap& map, const string& layer_from, const string& laye /*! * For comparison. */ -void runCustomIndexIteration(GridMap& map, const string& layer_from, const string& layer_to) +void runCustomIndexIteration(GridMap & map, const string & layer_from, const string & layer_to) { - const auto& data_from = map[layer_from]; - auto& data_to = map[layer_to]; + const auto & data_from = map[layer_from]; + auto & data_to = map[layer_to]; for (size_t j = 0; j < data_to.cols(); ++j) { for (size_t i = 0; i < data_to.rows(); ++i) { const float value_from = data_from(i, j); - float& value_to = data_to(i, j); + float & value_to = data_to(i, j); value_to = value_to > value_from ? value_to : value_from; } } @@ -86,16 +83,18 @@ void runCustomIndexIteration(GridMap& map, const string& layer_from, const strin /*! * For comparison. */ -void runCustomLinearIndexIteration(GridMap& map, const string& layer_from, const string& layer_to) +void runCustomLinearIndexIteration( + GridMap & map, const string & layer_from, + const string & layer_to) { - const auto& data_from = map[layer_from]; - auto& data_to = map[layer_to]; + const auto & data_from = map[layer_from]; + auto & data_to = map[layer_to]; for (size_t i = 0; i < data_to.size(); ++i) { data_to(i) = data_to(i) > data_from(i) ? data_to(i) : data_from(i); } } -int main(int argc, char* argv[]) +int main(int argc, char * argv[]) { GridMap map; map.setGeometry(Length(20.0, 20.0), 0.004, Position(0.0, 0.0)); @@ -108,7 +107,8 @@ int main(int argc, char* argv[]) map.add("layer5", 0.0); map.add("layer6", 0.0); - cout << "Results for iteration over " << map.getSize()(0) << " x " << map.getSize()(1) << " (" << map.getSize().prod() << ") grid cells." << endl; + cout << "Results for iteration over " << map.getSize()(0) << " x " << map.getSize()(1) << " (" << + map.getSize().prod() << ") grid cells." << endl; cout << "=========================================" << endl; clk::time_point t1 = clk::now(); @@ -119,7 +119,8 @@ int main(int argc, char* argv[]) t1 = clk::now(); runGridMapIteratorVersion2(map, "random", "layer2"); t2 = clk::now(); - cout << "Duration grid map iterator (direct access to data layers): " << duration(t2 - t1) << " ms" << endl; + cout << "Duration grid map iterator (direct access to data layers): " << duration(t2 - t1) << + " ms" << endl; t1 = clk::now(); runGridMapIteratorVersion3(map, "random", "layer3"); @@ -129,17 +130,17 @@ int main(int argc, char* argv[]) t1 = clk::now(); runEigenFunction(map, "random", "layer4"); t2 = clk::now(); - cout << "Duration Eigen function: " << duration(t2 - t1) << " ms" << endl; + cout << "Duration Eigen function: " << duration(t2 - t1) << " ms" << endl; t1 = clk::now(); runCustomIndexIteration(map, "random", "layer5"); t2 = clk::now(); - cout << "Duration custom index iteration: " << duration(t2 - t1) << " ms" << endl; + cout << "Duration custom index iteration: " << duration(t2 - t1) << " ms" << endl; t1 = clk::now(); runCustomLinearIndexIteration(map, "random", "layer6"); t2 = clk::now(); - cout << "Duration custom linear index iteration: " << duration(t2 - t1) << " ms" << endl; + cout << "Duration custom linear index iteration: " << duration(t2 - t1) << " ms" << endl; return 0; } diff --git a/grid_map_demos/src/iterators_demo_node.cpp b/grid_map_demos/src/iterators_demo_node.cpp index dc4c50096..42b721cb0 100644 --- a/grid_map_demos/src/iterators_demo_node.cpp +++ b/grid_map_demos/src/iterators_demo_node.cpp @@ -6,16 +6,14 @@ * Institute: ETH Zurich, ANYbotics */ -#include +#include +#include #include "grid_map_demos/IteratorsDemo.hpp" -int main(int argc, char** argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "grid_map_iterators_demo"); - - ros::NodeHandle nodeHandle("~"); - grid_map_demos::IteratorsDemo iteratorsDemo(nodeHandle); - - ros::requestShutdown(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/grid_map_demos/src/move_demo_node.cpp b/grid_map_demos/src/move_demo_node.cpp index 5f379eac5..dba9c9e5e 100644 --- a/grid_map_demos/src/move_demo_node.cpp +++ b/grid_map_demos/src/move_demo_node.cpp @@ -1,10 +1,7 @@ #include #include - -using namespace grid_map; -using namespace ros; - -int main(int argc, char** argv) +[] +int main(int argc, char ** argv) { // Initialize node and publisher. init(argc, argv, "move_demo"); @@ -15,7 +12,9 @@ int main(int argc, char** argv) GridMap map({"layer"}); map.setFrameId("map"); map.setGeometry(Length(0.7, 0.7), 0.01, Position(0.0, 0.0)); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.", + ROS_INFO( + "Created map with size %f x %f m (%i x %i cells).\n" + " The center of the map is located at (%f, %f) in the %s frame.", map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1), map.getPosition().x(), map.getPosition().y(), map.getFrameId().c_str()); @@ -23,7 +22,6 @@ int main(int argc, char** argv) bool useMoveMethod = true; while (nh.ok()) { - if (useMoveMethod) { ROS_INFO("Using the `move(...)` method."); } else { @@ -55,8 +53,9 @@ int main(int argc, char** argv) grid_map_msgs::GridMap message; GridMapRosConverter::toMessage(tempMap, message); publisher.publish(message); - ROS_DEBUG("Grid map (duration %f) published with new position [%f, %f].", - duration.toSec(), tempMap.getPosition().x(), tempMap.getPosition().y()); + ROS_DEBUG( + "Grid map (duration %f) published with new position [%f, %f].", + duration.toSec(), tempMap.getPosition().x(), tempMap.getPosition().y()); rate.sleep(); } diff --git a/grid_map_demos/src/normal_filter_comparison_node.cpp b/grid_map_demos/src/normal_filter_comparison_node.cpp index bb78e1ca8..23369a7b4 100644 --- a/grid_map_demos/src/normal_filter_comparison_node.cpp +++ b/grid_map_demos/src/normal_filter_comparison_node.cpp @@ -6,15 +6,20 @@ #include #include -#include -#include -#include #include #include +#include +#include + +#include +#include +#include + // Function headers -namespace grid_map { +namespace grid_map +{ /*! * Function to calculate the normal vector computation error.Error is defined as sum over the map of absolute cosines * between analytical and computed normals. The cosines are calculated using the dot product between two unitary vectors. @@ -25,8 +30,9 @@ namespace grid_map { * @param directionalErrorAreaSum: Average of the summed error over the map for area method. * @param directionalErrorRasterSum: Average of the summed error over the map for raster method. */ -void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridMapSize, double& directionalErrorAreaSum, - double& directionalErrorRasterSum); +void normalsErrorCalculation( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, double & directionalErrorAreaSum, + double & directionalErrorRasterSum); /*! * Function to add noise to the elevation map. Noise added is white noise from a uniform distribution [-1:1] multiplied by @@ -36,7 +42,9 @@ void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridM * @param gridMapSize: Dimensions of grid map, passed as parameter to not being calculated every time. * @param noise_on_map: Amount of noise wanted in meters, can be set as an argument in the roslaunch phase. */ -void mapAddNoise(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double& noise_on_map); +void mapAddNoise( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double & noise_on_map); /*! * Function to add outliers to the elevation map. Outliers are point where the elevation value is set to infinity. @@ -46,7 +54,9 @@ void mapAddNoise(grid_map::GridMap& map, const grid_map::Size& gridMapSize, cons * @param gridMapSize: Dimensions of grid map, passed as parameter to not being calculated every time. * @param outlierPercentage: Amount of outliers wanted percentage, can be set as an argument in the roslaunch phase. */ -void mapAddOutliers(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double outlierPercentage); +void mapAddOutliers( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double outlierPercentage); /*! * Function to add outliers to the elevation map. Outliers are point where the elevation value is set to infinity. @@ -56,10 +66,13 @@ void mapAddOutliers(grid_map::GridMap& map, const grid_map::Size& gridMapSize, c * @param gridMapSize: Dimensions of grid map, passed as parameter to not being calculated every time. * @param filterRadius: Radius of the wanted shifting average filter. */ -void mapAverageFiltering(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double filterRadius); +void mapAverageFiltering( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double filterRadius); } // namespace grid_map -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ // Initialize node and publisher. ros::init(argc, argv, "normal_filter_comparison_demo"); ros::NodeHandle nh("~"); @@ -71,7 +84,9 @@ int main(int argc, char** argv) { // Load filter chain, defined in grid_map_demos/config/normal_filter_comparison.yaml. filters::FilterChain filterChain_("grid_map::GridMap"); std::string filterChainParametersName_; - nh.param("filter_chain_parameter_name", filterChainParametersName_, std::string("grid_map_filters")); + nh.param( + "filter_chain_parameter_name", filterChainParametersName_, std::string( + "grid_map_filters")); // Read noise amount, in meters, from parameters server. double noise_on_map; @@ -79,7 +94,6 @@ int main(int argc, char** argv) { ROS_INFO("noise_on_map = %f", noise_on_map); double outliers_percentage; - ; nh.param("outliers_percentage", outliers_percentage, 0.0); ROS_INFO("outliers_percentage = %f", outliers_percentage); @@ -94,13 +108,19 @@ int main(int argc, char** argv) { const double mapResolution = 0.02; // Grid map object creation. - grid_map::GridMap map({"elevation", "normal_analytic_x", "normal_analytic_y", "normal_analytic_z"}); + grid_map::GridMap map({"elevation", "normal_analytic_x", "normal_analytic_y", + "normal_analytic_z"}); map.setFrameId("map"); - map.setGeometry(grid_map::Length(mapLength, mapWidth), mapResolution, grid_map::Position(0.0, 0.0)); + map.setGeometry( + grid_map::Length(mapLength, mapWidth), mapResolution, + grid_map::Position(0.0, 0.0)); const grid_map::Size gridMapSize = map.getSize(); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.", - map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1), map.getPosition().x(), map.getPosition().y(), - map.getFrameId().c_str()); + ROS_INFO( + "Created map with size %f x %f m (%i x %i cells).\n" + " The center of the map is located at (%f, %f) in the %s frame.", + map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1), + map.getPosition().x(), map.getPosition().y(), + map.getFrameId().c_str()); // Initialize variables for normal quality comparison. double directionalErrorAreaSum = 0; @@ -112,7 +132,8 @@ int main(int argc, char** argv) { const double surfaceBias = -0.04; const double wavePeriod = 5.0; - // Work with grid map in a loop. Grid map and analytic normals are continuously generated using exact functions. + // Work with grid map in a loop. + // Grid map and analytic normals are continuously generated using exact functions. ros::Rate rate(10.0); while (nh.ok()) { ros::Time time = ros::Time::now(); @@ -122,18 +143,21 @@ int main(int argc, char** argv) { grid_map::Position position; map.getPosition(*it, position); map.at("elevation", *it) = - surfaceBias + surfaceSlope * std::sin(surfaceSpeed * time.toSec() + wavePeriod * position.y()) * position.x(); + surfaceBias + surfaceSlope * std::sin( + surfaceSpeed * time.toSec() + wavePeriod * position.y()) * position.x(); // Analytic normals computation. - grid_map::Vector3 normalAnalytic(-surfaceSlope * std::sin(surfaceSpeed * time.toSec() + wavePeriod * position.y()), - -position.x() * std::cos(surfaceSpeed * time.toSec() + wavePeriod * position.y()), 1.0); + grid_map::Vector3 normalAnalytic(-surfaceSlope * std::sin( + surfaceSpeed * time.toSec() + wavePeriod * position.y()), + -position.x() * std::cos(surfaceSpeed * time.toSec() + wavePeriod * position.y()), 1.0); normalAnalytic.normalize(); map.at("normal_analytic_x", *it) = normalAnalytic.x(); map.at("normal_analytic_y", *it) = normalAnalytic.y(); map.at("normal_analytic_z", *it) = normalAnalytic.z(); } - // elevation_filtered layer is used for visualization, initialize it here and if there is noise and filtering it will be updated. + // elevation_filtered layer is used for visualization, + // initialize it here and if there is noise and filtering it will be updated. map.add("elevation_filtered", map.get("elevation")); // Perturb and then filter map only if noise != 0. @@ -153,7 +177,9 @@ int main(int argc, char** argv) { } // Normals error computation - grid_map::normalsErrorCalculation(map, gridMapSize, directionalErrorAreaSum, directionalErrorRasterSum); + grid_map::normalsErrorCalculation( + map, gridMapSize, directionalErrorAreaSum, + directionalErrorRasterSum); ROS_INFO_THROTTLE(2.0, "directionalErrorArea = %f", directionalErrorAreaSum); ROS_INFO_THROTTLE(2.0, "directionalErrorRaster = %f", directionalErrorRasterSum); @@ -173,9 +199,12 @@ int main(int argc, char** argv) { return 0; } -namespace grid_map { -void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridMapSize, double& directionalErrorAreaSum, - double& directionalErrorRasterSum) { +namespace grid_map +{ +void normalsErrorCalculation( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, double & directionalErrorAreaSum, + double & directionalErrorRasterSum) +{ // If layers saved as matrices accessing values is faster. const grid_map::Matrix mapNormalAnalyticX = map["normal_analytic_x"]; const grid_map::Matrix mapNormalAnalyticY = map["normal_analytic_y"]; @@ -199,7 +228,9 @@ void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridM // Raster normals not defined for outermost layer of cells. const grid_map::Index submapStartIndex(1, 1); const grid_map::Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2); - for (grid_map::SubmapIterator iterator(map, submapStartIndex, submapBufferSize); !iterator.isPastEnd(); ++iterator) { + for (grid_map::SubmapIterator iterator(map, submapStartIndex, submapBufferSize); + !iterator.isPastEnd(); ++iterator) + { const grid_map::Index index(*iterator); normalVectorAnalytic(0) = mapNormalAnalyticX(index(0), index(1)); @@ -214,13 +245,19 @@ void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridM normalVectorRaster(1) = mapNormalRasterY(index(0), index(1)); normalVectorRaster(2) = mapNormalRasterZ(index(0), index(1)); - // Error(alpha) = 1.0 - abs(cos(alpha)), where alpha = angle(normalVectorAnalytic, normalVectorArea) + // Error(alpha) = 1.0 - abs(cos(alpha)), + // where alpha = angle(normalVectorAnalytic, normalVectorArea) // Error of perfect normals will be 0.0. - map.at("directionalErrorArea", *iterator) = 1.0 - std::abs(normalVectorAnalytic.dot(normalVectorArea)); - map.at("directionalErrorRaster", *iterator) = 1.0 - std::abs(normalVectorAnalytic.dot(normalVectorRaster)); + map.at( + "directionalErrorArea", + *iterator) = 1.0 - std::abs(normalVectorAnalytic.dot(normalVectorArea)); + map.at( + "directionalErrorRaster", + *iterator) = 1.0 - std::abs(normalVectorAnalytic.dot(normalVectorRaster)); } - // Directional error defined as sum of absolute cosines of normal calculated with different methods + // Directional error defined as sum of + // absolute cosines of normal calculated with different methods const double directionalErrorArea = map["directionalErrorArea"].array().sum(); const double directionalErrorRaster = map["directionalErrorRaster"].array().sum(); @@ -229,13 +266,19 @@ void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridM directionalErrorRasterSum = (directionalErrorRasterSum * 19.0 + directionalErrorRaster) / 20.0; } -void mapAddNoise(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double& noise_on_map) { +void mapAddNoise( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double & noise_on_map) +{ // Add noise (using Eigen operators). map.add("noise", noise_on_map * Matrix::Random(gridMapSize(0), gridMapSize(1))); map.add("elevation_noisy", map.get("elevation") + map["noise"]); } -void mapAddOutliers(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double outlierPercentage) { +void mapAddOutliers( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double outlierPercentage) +{ // Adding outliers at infinite height (accessing cell by position). const double numberInfPoints = outlierPercentage * gridMapSize(0) * gridMapSize(1); @@ -247,7 +290,10 @@ void mapAddOutliers(grid_map::GridMap& map, const grid_map::Size& gridMapSize, c } } -void mapAverageFiltering(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double filterRadius) { +void mapAverageFiltering( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double filterRadius) +{ grid_map::Index startIndex(0, 0); grid_map::SubmapIterator it(map, startIndex, gridMapSize); // Iterate over whole map. @@ -258,7 +304,9 @@ void mapAverageFiltering(grid_map::GridMap& map, const grid_map::Size& gridMapSi double sumOfWeights = 0.0; // Compute weighted mean. - for (grid_map::CircleIterator circleIt(map, currentPosition, filterRadius); !circleIt.isPastEnd(); ++circleIt) { + for (grid_map::CircleIterator circleIt(map, currentPosition, filterRadius); + !circleIt.isPastEnd(); ++circleIt) + { if (!map.isValid(*circleIt, "elevation_noisy")) { continue; } @@ -271,11 +319,11 @@ void mapAverageFiltering(grid_map::GridMap& map, const grid_map::Size& gridMapSi mean += weight * map.at("elevation_noisy", *circleIt); sumOfWeights += weight; } - if (sumOfWeights!=0) { + if (sumOfWeights != 0) { map.at("elevation_filtered", *it) = mean / sumOfWeights; } else { map.at("elevation_filtered", *it) = std::numeric_limits::infinity(); } } } -} // namespace grid_map \ No newline at end of file +} // namespace grid_map diff --git a/grid_map_demos/src/octomap_to_gridmap_demo_node.cpp b/grid_map_demos/src/octomap_to_gridmap_demo_node.cpp index c976320f8..f9cf4d8dc 100644 --- a/grid_map_demos/src/octomap_to_gridmap_demo_node.cpp +++ b/grid_map_demos/src/octomap_to_gridmap_demo_node.cpp @@ -9,7 +9,7 @@ #include #include "grid_map_demos/OctomapToGridmapDemo.hpp" -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. ros::init(argc, argv, "octomap_to_gridmap_demo"); @@ -17,9 +17,8 @@ int main(int argc, char** argv) grid_map_demos::OctomapToGridmapDemo octomapToGridmapDemo(nh); ros::Duration(2.0).sleep(); - ros::Rate r(0.1); // 1 hz - while (ros::ok()) - { + ros::Rate r(0.1); // 1 hz + while (ros::ok()) { octomapToGridmapDemo.convertAndPublishMap(); ros::spinOnce(); r.sleep(); diff --git a/grid_map_demos/src/opencv_demo_node.cpp b/grid_map_demos/src/opencv_demo_node.cpp index bfdc4a58c..246ca70da 100644 --- a/grid_map_demos/src/opencv_demo_node.cpp +++ b/grid_map_demos/src/opencv_demo_node.cpp @@ -4,10 +4,7 @@ #include #include -using namespace grid_map; -using namespace ros; - -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. init(argc, argv, "opencv_demo"); @@ -19,25 +16,26 @@ int main(int argc, char** argv) GridMap map({"original", "elevation"}); map.setFrameId("map"); map.setGeometry(Length(1.2, 2.0), 0.01); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).", + ROS_INFO( + "Created map with size %f x %f m (%i x %i cells).", map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1)); // Add data. - if (!useTransparency) map["original"].setZero(); + if (!useTransparency) {map["original"].setZero();} grid_map::Polygon polygon; polygon.setFrameId(map.getFrameId()); - polygon.addVertex(Position( 0.480, 0.000)); - polygon.addVertex(Position( 0.164, 0.155)); - polygon.addVertex(Position( 0.116, 0.500)); - polygon.addVertex(Position(-0.133, 0.250)); - polygon.addVertex(Position(-0.480, 0.399)); - polygon.addVertex(Position(-0.316, 0.000)); + polygon.addVertex(Position(0.480, 0.000)); + polygon.addVertex(Position(0.164, 0.155)); + polygon.addVertex(Position(0.116, 0.500)); + polygon.addVertex(Position(-0.133, 0.250)); + polygon.addVertex(Position(-0.480, 0.399)); + polygon.addVertex(Position(-0.316, 0.000)); polygon.addVertex(Position(-0.480, -0.399)); polygon.addVertex(Position(-0.133, -0.250)); - polygon.addVertex(Position( 0.116, -0.500)); - polygon.addVertex(Position( 0.164, -0.155)); - polygon.addVertex(Position( 0.480, 0.000)); + polygon.addVertex(Position(0.116, -0.500)); + polygon.addVertex(Position(0.164, -0.155)); + polygon.addVertex(Position(0.480, 0.000)); for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) { map.at("original", *iterator) = 0.3; @@ -48,9 +46,13 @@ int main(int argc, char** argv) if (useTransparency) { // Note: The template parameters have to be set based on your encoding // of the image. For 8-bit images use `unsigned char`. - GridMapCvConverter::toImage(map, "original", CV_16UC4, 0.0, 0.3, originalImage); + GridMapCvConverter::toImage( + map, "original", CV_16UC4, 0.0, 0.3, + originalImage); } else { - GridMapCvConverter::toImage(map, "original", CV_16UC1, 0.0, 0.3, originalImage); + GridMapCvConverter::toImage( + map, "original", CV_16UC1, 0.0, 0.3, + originalImage); } // Create OpenCV window. @@ -58,12 +60,11 @@ int main(int argc, char** argv) // Work with copy of image in a loop. while (nodeHandle.ok()) { - // Initialize. ros::Time time = ros::Time::now(); map.setTimestamp(time.toNSec()); cv::Mat modifiedImage; - int blurRadius = 200 - abs((int)(200.0 * sin(time.toSec()))); + int blurRadius = 200 - abs(static_cast(200.0 * sin(time.toSec()))); blurRadius = blurRadius - (blurRadius % 2) + 1; // Apply Gaussian blur. @@ -75,9 +76,13 @@ int main(int argc, char** argv) // Convert resulting image to a grid map. if (useTransparency) { - GridMapCvConverter::addLayerFromImage(modifiedImage, "elevation", map, 0.0, 0.3, 0.3); + GridMapCvConverter::addLayerFromImage( + modifiedImage, "elevation", map, 0.0, + 0.3, 0.3); } else { - GridMapCvConverter::addLayerFromImage(modifiedImage, "elevation", map, 0.0, 0.3); + GridMapCvConverter::addLayerFromImage( + modifiedImage, "elevation", map, 0.0, + 0.3); } // Publish grid map. diff --git a/grid_map_demos/src/resolution_change_demo_node.cpp b/grid_map_demos/src/resolution_change_demo_node.cpp index ea0084ebb..ea1fcb21b 100644 --- a/grid_map_demos/src/resolution_change_demo_node.cpp +++ b/grid_map_demos/src/resolution_change_demo_node.cpp @@ -2,10 +2,7 @@ #include #include -using namespace grid_map; -using namespace ros; - -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. init(argc, argv, "resolution_change_demo"); @@ -16,25 +13,26 @@ int main(int argc, char** argv) GridMap map({"elevation"}); map.setFrameId("map"); map.setGeometry(Length(1.2, 2.0), 0.03); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).", + ROS_INFO( + "Created map with size %f x %f m (%i x %i cells).", map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1)); // Add data. - map["elevation"].setZero(); // Un/comment this line to try with and without transparency. + map["elevation"].setZero(); // Un/comment this line to try with and without transparency. grid_map::Polygon polygon; polygon.setFrameId(map.getFrameId()); - polygon.addVertex(Position( 0.480, 0.000)); - polygon.addVertex(Position( 0.164, 0.155)); - polygon.addVertex(Position( 0.116, 0.500)); - polygon.addVertex(Position(-0.133, 0.250)); - polygon.addVertex(Position(-0.480, 0.399)); - polygon.addVertex(Position(-0.316, 0.000)); + polygon.addVertex(Position(0.480, 0.000)); + polygon.addVertex(Position(0.164, 0.155)); + polygon.addVertex(Position(0.116, 0.500)); + polygon.addVertex(Position(-0.133, 0.250)); + polygon.addVertex(Position(-0.480, 0.399)); + polygon.addVertex(Position(-0.316, 0.000)); polygon.addVertex(Position(-0.480, -0.399)); polygon.addVertex(Position(-0.133, -0.250)); - polygon.addVertex(Position( 0.116, -0.500)); - polygon.addVertex(Position( 0.164, -0.155)); - polygon.addVertex(Position( 0.480, 0.000)); + polygon.addVertex(Position(0.116, -0.500)); + polygon.addVertex(Position(0.164, -0.155)); + polygon.addVertex(Position(0.480, 0.000)); for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) { map.at("elevation", *iterator) = 0.3; @@ -44,7 +42,6 @@ int main(int argc, char** argv) // Work in a loop. while (nodeHandle.ok()) { - // Initialize. ros::Time time = ros::Time::now(); const double resolution = 0.05 + 0.04 * sin(time.toSec()); diff --git a/grid_map_demos/src/simple_demo_node.cpp b/grid_map_demos/src/simple_demo_node.cpp index 253e05760..449a5eb0b 100644 --- a/grid_map_demos/src/simple_demo_node.cpp +++ b/grid_map_demos/src/simple_demo_node.cpp @@ -1,43 +1,48 @@ -#include +#include #include -#include +#include #include +#include +#include -using namespace grid_map; - -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. - ros::init(argc, argv, "grid_map_simple_demo"); - ros::NodeHandle nh("~"); - ros::Publisher publisher = nh.advertise("grid_map", 1, true); + rclcpp::init(argc, argv); + rclcpp::Node node("grid_map_simple_demo"); + auto publisher = node.create_publisher( + "grid_map", rclcpp::QoS(1).transient_local()); // Create grid map. - GridMap map({"elevation"}); + grid_map::GridMap map({"elevation"}); map.setFrameId("map"); - map.setGeometry(Length(1.2, 2.0), 0.03); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).", + map.setGeometry(grid_map::Length(1.2, 2.0), 0.03); + RCLCPP_INFO( + node.get_logger(), + "Created map with size %f x %f m (%i x %i cells).", map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1)); // Work with grid map in a loop. - ros::Rate rate(30.0); - while (nh.ok()) { - + rclcpp::Rate rate(30.0); + rclcpp::Clock clock; + while (rclcpp::ok()) { // Add data to grid map. - ros::Time time = ros::Time::now(); - for (GridMapIterator it(map); !it.isPastEnd(); ++it) { - Position position; + rclcpp::Time time = node.now(); + for (grid_map::GridMapIterator it(map); !it.isPastEnd(); ++it) { + grid_map::Position position; map.getPosition(*it, position); - map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()) * position.x(); + map.at( + "elevation", + *it) = -0.04 + 0.2 * std::sin(3.0 * time.seconds() + 5.0 * position.y()) * position.x(); } // Publish grid map. - map.setTimestamp(time.toNSec()); - grid_map_msgs::GridMap message; - GridMapRosConverter::toMessage(map, message); - publisher.publish(message); - ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec()); + map.setTimestamp(time.nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map); + publisher->publish(std::move(message)); + RCLCPP_INFO_THROTTLE(node.get_logger(), clock, 1000, "Grid map published."); // Wait for next cycle. rate.sleep(); diff --git a/grid_map_demos/src/tutorial_demo_node.cpp b/grid_map_demos/src/tutorial_demo_node.cpp index 343dea3d5..32768d040 100644 --- a/grid_map_demos/src/tutorial_demo_node.cpp +++ b/grid_map_demos/src/tutorial_demo_node.cpp @@ -1,40 +1,47 @@ -#include +#include #include -#include -#include #include #include +#include +#include +#include +#include -using namespace grid_map; - -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. - ros::init(argc, argv, "grid_map_tutorial_demo"); - ros::NodeHandle nh("~"); - ros::Publisher publisher = nh.advertise("grid_map", 1, true); + rclcpp::init(argc, argv); + rclcpp::Node node("grid_map_tutorial_demo"); + auto publisher = node.create_publisher( + "grid_map", rclcpp::QoS(1).transient_local()); // Create grid map. - GridMap map({"elevation", "normal_x", "normal_y", "normal_z"}); + grid_map::GridMap map({"elevation", "normal_x", "normal_y", "normal_z"}); map.setFrameId("map"); - map.setGeometry(Length(1.2, 2.0), 0.03, Position(0.0, -0.1)); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.", + map.setGeometry(grid_map::Length(1.2, 2.0), 0.03, grid_map::Position(0.0, -0.1)); + RCLCPP_INFO( + node.get_logger(), + "Created map with size %f x %f m (%i x %i cells).\n" + " The center of the map is located at (%f, %f) in the %s frame.", map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1), map.getPosition().x(), map.getPosition().y(), map.getFrameId().c_str()); // Work with grid map in a loop. - ros::Rate rate(30.0); - while (nh.ok()) { - ros::Time time = ros::Time::now(); + rclcpp::Rate rate(30.0); + rclcpp::Clock clock; + while (rclcpp::ok()) { + rclcpp::Time time = node.now(); // Add elevation and surface normal (iterating through grid map and adding data). - for (GridMapIterator it(map); !it.isPastEnd(); ++it) { - Position position; + for (grid_map::GridMapIterator it(map); !it.isPastEnd(); ++it) { + grid_map::Position position; map.getPosition(*it, position); - map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()) * position.x(); - Eigen::Vector3d normal(-0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()), - -position.x() * std::cos(3.0 * time.toSec() + 5.0 * position.y()), 1.0); + map.at( + "elevation", + *it) = -0.04 + 0.2 * std::sin(3.0 * time.seconds() + 5.0 * position.y()) * position.x(); + Eigen::Vector3d normal(-0.2 * std::sin(3.0 * time.seconds() + 5.0 * position.y()), + -position.x() * std::cos(3.0 * time.seconds() + 5.0 * position.y()), 1.0); normal.normalize(); map.at("normal_x", *it) = normal.x(); map.at("normal_y", *it) = normal.y(); @@ -42,38 +49,43 @@ int main(int argc, char** argv) } // Add noise (using Eigen operators). - map.add("noise", 0.015 * Matrix::Random(map.getSize()(0), map.getSize()(1))); + map.add("noise", 0.015 * grid_map::Matrix::Random(map.getSize()(0), map.getSize()(1))); map.add("elevation_noisy", map.get("elevation") + map["noise"]); // Adding outliers (accessing cell by position). for (unsigned int i = 0; i < 500; ++i) { - Position randomPosition = Position::Random(); - if (map.isInside(randomPosition)) + grid_map::Position randomPosition = grid_map::Position::Random(); + if (map.isInside(randomPosition)) { map.atPosition("elevation_noisy", randomPosition) = std::numeric_limits::infinity(); + } } // Filter values for submap (iterators). map.add("elevation_filtered", map.get("elevation_noisy")); - Position topLeftCorner(1.0, 0.4); - boundPositionToRange(topLeftCorner, map.getLength(), map.getPosition()); - Index startIndex; + grid_map::Position topLeftCorner(1.0, 0.4); + grid_map::boundPositionToRange(topLeftCorner, map.getLength(), map.getPosition()); + grid_map::Index startIndex; map.getIndex(topLeftCorner, startIndex); - ROS_INFO_ONCE("Top left corner was limited from (1.0, 0.2) to (%f, %f) and corresponds to index (%i, %i).", + RCLCPP_INFO_ONCE( + node.get_logger(), + "Top left corner was limited from (1.0, 0.2) to (%f, %f) and corresponds to index (%i, %i).", topLeftCorner.x(), topLeftCorner.y(), startIndex(0), startIndex(1)); - Size size = (Length(1.2, 0.8) / map.getResolution()).cast(); - SubmapIterator it(map, startIndex, size); + grid_map::Size size = (grid_map::Length(1.2, 0.8) / map.getResolution()).cast(); + grid_map::SubmapIterator it(map, startIndex, size); for (; !it.isPastEnd(); ++it) { - Position currentPosition; + grid_map::Position currentPosition; map.getPosition(*it, currentPosition); double radius = 0.1; double mean = 0.0; double sumOfWeights = 0.0; // Compute weighted mean. - for (CircleIterator circleIt(map, currentPosition, radius); !circleIt.isPastEnd(); ++circleIt) { - if (!map.isValid(*circleIt, "elevation_noisy")) continue; - Position currentPositionInCircle; + for (grid_map::CircleIterator circleIt(map, currentPosition, radius); !circleIt.isPastEnd(); + ++circleIt) + { + if (!map.isValid(*circleIt, "elevation_noisy")) {continue;} + grid_map::Position currentPositionInCircle; map.getPosition(*circleIt, currentPositionInCircle); // Computed weighted mean based on Euclidian distance. @@ -88,19 +100,17 @@ int main(int argc, char** argv) // Show absolute difference and compute mean squared error. map.add("error", (map.get("elevation_filtered") - map.get("elevation")).cwiseAbs()); - unsigned int nCells = map.getSize().prod(); - // cppcheck-suppress unreadVariable - double rootMeanSquaredError = sqrt((map["error"].array().pow(2).sum()) / nCells); // Publish grid map. - map.setTimestamp(time.toNSec()); - grid_map_msgs::GridMap message; - GridMapRosConverter::toMessage(map, message); - publisher.publish(message); - ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec()); + map.setTimestamp(time.nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map); + publisher->publish(std::move(message)); + RCLCPP_INFO_THROTTLE(node.get_logger(), clock, 1000, "Grid map published."); + rclcpp::spin_some(node.get_node_base_interface()); rate.sleep(); } return 0; -} \ No newline at end of file +} diff --git a/grid_map_ros/CMakeLists.txt b/grid_map_ros/CMakeLists.txt index 2aca3528a..5403cb793 100644 --- a/grid_map_ros/CMakeLists.txt +++ b/grid_map_ros/CMakeLists.txt @@ -49,7 +49,7 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/GridMapRosConverter.cpp src/GridMapMsgHelpers.cpp src/PolygonRosConverter.cpp diff --git a/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp index 46cd2bcf3..5e07efe86 100644 --- a/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp @@ -31,7 +31,7 @@ namespace grid_map_visualization /*! * Visualizes a grid map by publishing different topics that can be viewed in Rviz. */ -class GridMapVisualization : public rclcpp::Node +class GridMapVisualization { public: /*! @@ -51,6 +51,9 @@ class GridMapVisualization : public rclcpp::Node */ void callback(const grid_map_msgs::msg::GridMap::SharedPtr message); + //! ROS node shared pointer + rclcpp::Node::SharedPtr nodePtr; + private: /*! * Read parameters from ROS. diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp index ddb804553..2440bc6df 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp @@ -31,7 +31,7 @@ class FlatPointCloudVisualization : public VisualizationBase * Constructor. * @param name the name of the visualization. */ - explicit FlatPointCloudVisualization(const std::string & name); + explicit FlatPointCloudVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/GridCellsVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/GridCellsVisualization.hpp index 29dee3820..4ca0a9b3c 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/GridCellsVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/GridCellsVisualization.hpp @@ -30,7 +30,7 @@ class GridCellsVisualization : public VisualizationBase * Constructor. * @param name the name of the visualization. */ - explicit GridCellsVisualization(const std::string & name); + explicit GridCellsVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/MapRegionVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/MapRegionVisualization.hpp index 37ecc56d7..1f48ea8cd 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/MapRegionVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/MapRegionVisualization.hpp @@ -33,7 +33,7 @@ class MapRegionVisualization : public VisualizationBase * Constructor. * @param name the name of the visualization. */ - explicit MapRegionVisualization(const std::string & name); + explicit MapRegionVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/OccupancyGridVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/OccupancyGridVisualization.hpp index 29aeeb684..94552bb53 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/OccupancyGridVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/OccupancyGridVisualization.hpp @@ -26,7 +26,7 @@ class OccupancyGridVisualization : public VisualizationBase * Constructor. * @param name the name of the visualization. */ - explicit OccupancyGridVisualization(const std::string & name); + explicit OccupancyGridVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/PointCloudVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/PointCloudVisualization.hpp index d6bc3e5e6..27ee6fa9e 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/PointCloudVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/PointCloudVisualization.hpp @@ -32,7 +32,7 @@ class PointCloudVisualization : public VisualizationBase * Constructor. * @param name the name of the visualization. */ - explicit PointCloudVisualization(const std::string & name); + explicit PointCloudVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/VectorVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/VectorVisualization.hpp index fdcf910dc..f0f84a43a 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/VectorVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/VectorVisualization.hpp @@ -35,7 +35,7 @@ class VectorVisualization : public VisualizationBase * Constructor. * @param name the name of the visualization. */ - explicit VectorVisualization(const std::string & name); + explicit VectorVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationBase.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationBase.hpp index b00daea59..1f729890a 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationBase.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationBase.hpp @@ -21,14 +21,14 @@ namespace grid_map_visualization { -class VisualizationBase : public rclcpp::Node +class VisualizationBase { public: /*! * Constructor. * @param name the name of the visualization. */ - explicit VisualizationBase(const std::string & name); + explicit VisualizationBase(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. @@ -58,6 +58,13 @@ class VisualizationBase : public rclcpp::Node * @return true if active, false otherwise. */ bool isActive() const; + +protected: + //! visualization name + std::string name_; + + //! ROS node shared pointer + rclcpp::Node::SharedPtr nodePtr_; }; } // namespace grid_map_visualization diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationFactory.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationFactory.hpp index 28d811565..f31c165bb 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationFactory.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationFactory.hpp @@ -21,7 +21,7 @@ namespace grid_map_visualization class VisualizationFactory { public: - VisualizationFactory(); + explicit VisualizationFactory(rclcpp::Node::SharedPtr nodePtr); virtual ~VisualizationFactory(); bool isValidType(const std::string & type); @@ -32,6 +32,7 @@ class VisualizationFactory private: std::vector types_; + rclcpp::Node::SharedPtr nodePtr_; }; } // namespace grid_map_visualization diff --git a/grid_map_visualization/src/GridMapVisualization.cpp b/grid_map_visualization/src/GridMapVisualization.cpp index 06c939627..bf2a42ec5 100644 --- a/grid_map_visualization/src/GridMapVisualization.cpp +++ b/grid_map_visualization/src/GridMapVisualization.cpp @@ -21,16 +21,16 @@ namespace grid_map_visualization { GridMapVisualization::GridMapVisualization(const std::string & parameterName) -: Node("grid_map_visualization"), - visualizationsParameter_(parameterName), +: visualizationsParameter_(parameterName), isSubscribed_(false) { - factory_ = std::make_shared(); + nodePtr = std::make_shared("grid_map_visualization"); + factory_ = std::make_shared(nodePtr); - RCLCPP_INFO(this->get_logger(), "Grid map visualization node started."); + RCLCPP_INFO(nodePtr->get_logger(), "Grid map visualization nodePtr started."); readParameters(); - activityCheckTimer_ = this->create_wall_timer( + activityCheckTimer_ = nodePtr->create_wall_timer( std::chrono::duration(1.0 / activityCheckRate_), std::bind(&GridMapVisualization::updateSubscriptionCallback, this)); initialize(); @@ -42,20 +42,20 @@ GridMapVisualization::~GridMapVisualization() bool GridMapVisualization::readParameters() { - this->declare_parameter("grid_map_topic", std::string("/grid_map")); - this->declare_parameter("activity_check_rate", 2.0); - this->declare_parameter(visualizationsParameter_, std::vector()); + nodePtr->declare_parameter("grid_map_topic", std::string("/grid_map")); + nodePtr->declare_parameter("activity_check_rate", 2.0); + nodePtr->declare_parameter(visualizationsParameter_, std::vector()); - this->get_parameter("grid_map_topic", mapTopic_); - this->get_parameter("activity_check_rate", activityCheckRate_); + nodePtr->get_parameter("grid_map_topic", mapTopic_); + nodePtr->get_parameter("activity_check_rate", activityCheckRate_); assert(activityCheckRate_); // Configure the visualizations from a configuration stored on the parameter server. std::vector config; - if (!this->get_parameter(visualizationsParameter_, config)) { + if (!nodePtr->get_parameter(visualizationsParameter_, config)) { RCLCPP_WARN( - this->get_logger(), + nodePtr->get_logger(), "Could not load the visualizations configuration from parameter %s,are you sure it" "was pushed to the parameter server? Assuming that you meant to leave it empty.", visualizationsParameter_.c_str()); @@ -73,24 +73,24 @@ bool GridMapVisualization::readParameters() config_check.insert(name); } else { RCLCPP_ERROR( - this->get_logger(), + nodePtr->get_logger(), "%s: A visualization with the name '%s' already exists.", visualizationsParameter_.c_str(), name.c_str()); return false; } - this->declare_parameter(name + ".type"); + nodePtr->declare_parameter(name + ".type"); try { - if (!this->get_parameter(name + ".type", type)) { + if (!nodePtr->get_parameter(name + ".type", type)) { RCLCPP_ERROR( - this->get_logger(), + nodePtr->get_logger(), "%s: Could not add a visualization because no type was given", name.c_str()); return false; } } catch (const rclcpp::ParameterTypeException & e) { RCLCPP_ERROR( - this->get_logger(), + nodePtr->get_logger(), "Could not add %s visualization, because the %s.type parameter is not a string.", name.c_str(), name.c_str()); return false; @@ -99,7 +99,7 @@ bool GridMapVisualization::readParameters() // Make sure the visualization has a valid type. if (!factory_->isValidType(type)) { RCLCPP_ERROR( - this->get_logger(), + nodePtr->get_logger(), "Could not add %s visualization, no visualization of type '%s' found.", name.c_str(), type.c_str()); return false; @@ -109,7 +109,7 @@ bool GridMapVisualization::readParameters() visualization->readParameters(); visualizations_.push_back(visualization); RCLCPP_INFO( - this->get_logger(), "%s: Configured visualization of type '%s' with name '%s'.", + nodePtr->get_logger(), "%s: Configured visualization of type '%s' with name '%s'.", visualizationsParameter_.c_str(), type.c_str(), name.c_str()); } return true; @@ -122,7 +122,7 @@ bool GridMapVisualization::initialize() } updateSubscriptionCallback(); - RCLCPP_INFO(this->get_logger(), "Grid map visualization initialized."); + RCLCPP_INFO(nodePtr->get_logger(), "Grid map visualization initialized."); return true; } @@ -138,24 +138,24 @@ void GridMapVisualization::updateSubscriptionCallback() } if (!isSubscribed_ && isActive) { - mapSubscriber_ = this->create_subscription( + mapSubscriber_ = nodePtr->create_subscription( mapTopic_, rclcpp::SystemDefaultsQoS(), std::bind(&GridMapVisualization::callback, this, std::placeholders::_1)); isSubscribed_ = true; - RCLCPP_DEBUG(this->get_logger(), "Subscribed to grid map at '%s'.", mapTopic_.c_str()); + RCLCPP_DEBUG(nodePtr->get_logger(), "Subscribed to grid map at '%s'.", mapTopic_.c_str()); } if (isSubscribed_ && !isActive) { mapSubscriber_.reset(); isSubscribed_ = false; - RCLCPP_DEBUG(this->get_logger(), "Cancelled subscription to grid map."); + RCLCPP_DEBUG(nodePtr->get_logger(), "Cancelled subscription to grid map."); } } void GridMapVisualization::callback(const grid_map_msgs::msg::GridMap::SharedPtr message) { RCLCPP_DEBUG( - this->get_logger(), + nodePtr->get_logger(), "Grid map visualization received a map (timestamp %f) for visualization.", rclcpp::Time(message->header.stamp).seconds()); grid_map::GridMap map; diff --git a/grid_map_visualization/src/grid_map_visualization_node.cpp b/grid_map_visualization/src/grid_map_visualization_node.cpp index 5255f9266..743b8026e 100644 --- a/grid_map_visualization/src/grid_map_visualization_node.cpp +++ b/grid_map_visualization/src/grid_map_visualization_node.cpp @@ -16,7 +16,7 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto node = std::make_shared( "grid_map_visualizations"); - rclcpp::spin(node->get_node_base_interface()); + rclcpp::spin(node->nodePtr); rclcpp::shutdown(); return 0; } diff --git a/grid_map_visualization/src/visualizations/FlatPointCloudVisualization.cpp b/grid_map_visualization/src/visualizations/FlatPointCloudVisualization.cpp index ba6528462..a84aec867 100644 --- a/grid_map_visualization/src/visualizations/FlatPointCloudVisualization.cpp +++ b/grid_map_visualization/src/visualizations/FlatPointCloudVisualization.cpp @@ -17,8 +17,10 @@ namespace grid_map_visualization { -FlatPointCloudVisualization::FlatPointCloudVisualization(const std::string & name) -: VisualizationBase(name), +FlatPointCloudVisualization::FlatPointCloudVisualization( + const std::string & name, + rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr), height_(0.0) { } @@ -30,13 +32,12 @@ FlatPointCloudVisualization::~FlatPointCloudVisualization() bool FlatPointCloudVisualization::readParameters() { height_ = 0.0; - this->declare_parameter(std::string(this->get_name()) + ".params.height", 0.0); - if (!this->get_parameter(std::string(this->get_name()) + ".params.height", height_)) { + nodePtr_->declare_parameter(name_ + ".params.height", 0.0); + if (!nodePtr_->get_parameter(name_ + ".params.height", height_)) { RCLCPP_INFO( - this->get_logger(), + nodePtr_->get_logger(), "FlatPointCloudVisualization with name '%s' " - "did not find a 'height' parameter. Using default.", - this->get_name()); + "did not find a 'height' parameter. Using default.", name_); } return true; @@ -44,8 +45,8 @@ bool FlatPointCloudVisualization::readParameters() bool FlatPointCloudVisualization::initialize() { - publisher_ = this->create_publisher( - this->get_name(), + publisher_ = nodePtr_->create_publisher( + name_, rclcpp::QoS(1).transient_local()); return true; } diff --git a/grid_map_visualization/src/visualizations/GridCellsVisualization.cpp b/grid_map_visualization/src/visualizations/GridCellsVisualization.cpp index e60561584..cde97e07f 100644 --- a/grid_map_visualization/src/visualizations/GridCellsVisualization.cpp +++ b/grid_map_visualization/src/visualizations/GridCellsVisualization.cpp @@ -17,8 +17,10 @@ namespace grid_map_visualization { -GridCellsVisualization::GridCellsVisualization(const std::string & name) -: VisualizationBase(name), +GridCellsVisualization::GridCellsVisualization( + const std::string & name, + rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr), lowerThreshold_(-std::numeric_limits::infinity()), upperThreshold_(std::numeric_limits::infinity()) { @@ -30,40 +32,33 @@ GridCellsVisualization::~GridCellsVisualization() bool GridCellsVisualization::readParameters() { - this->declare_parameter( - std::string(this->get_name()) + ".params.layer", + nodePtr_->declare_parameter( + name_ + ".params.layer", std::string("elevation")); - this->declare_parameter(std::string(this->get_name()) + ".params.lower_threshold", 5.0); - this->declare_parameter(std::string(this->get_name()) + ".params.upper_threshold", -5.0); + nodePtr_->declare_parameter(name_ + ".params.lower_threshold", 5.0); + nodePtr_->declare_parameter(name_ + ".params.upper_threshold", -5.0); - if (!this->get_parameter(std::string(this->get_name()) + ".params.layer", layer_)) { + if (!nodePtr_->get_parameter(name_ + ".params.layer", layer_)) { RCLCPP_ERROR( - this->get_logger(), + nodePtr_->get_logger(), "GridCellsVisualization with name '%s' did not find a 'layer' parameter.", - this->get_name()); + name_); return false; } - if (!this->get_parameter( - std::string(this->get_name()) + ".params.lower_threshold", - lowerThreshold_)) - { + if (!nodePtr_->get_parameter(name_ + ".params.lower_threshold", lowerThreshold_)) { RCLCPP_INFO( - this->get_logger(), + nodePtr_->get_logger(), "GridCellsVisualization with name '%s' " "did not find a 'lower_threshold' parameter. Using negative infinity.", - this->get_name()); + name_); } - if (!this->get_parameter( - std::string(this->get_name()) + ".params.upper_threshold", - upperThreshold_)) - { + if (!nodePtr_->get_parameter(name_ + ".params.upper_threshold", upperThreshold_)) { RCLCPP_INFO( - this->get_logger(), + nodePtr_->get_logger(), "GridCellsVisualization with name '%s' " - "did not find a 'upper_threshold' parameter. Using infinity.", - this->get_name()); + "did not find a 'upper_threshold' parameter. Using infinity.", name_); } return true; @@ -71,8 +66,8 @@ bool GridCellsVisualization::readParameters() bool GridCellsVisualization::initialize() { - publisher_ = this->create_publisher( - this->get_name(), + publisher_ = nodePtr_->create_publisher( + name_, rclcpp::QoS(1).transient_local()); return true; } @@ -82,7 +77,7 @@ bool GridCellsVisualization::visualize(const grid_map::GridMap & map) if (!isActive()) {return false;} if (!map.exists(layer_)) { RCLCPP_WARN_STREAM( - this->get_logger(), + nodePtr_->get_logger(), "GridCellsVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); return false; } diff --git a/grid_map_visualization/src/visualizations/MapRegionVisualization.cpp b/grid_map_visualization/src/visualizations/MapRegionVisualization.cpp index e541b2516..1f0a4ea2e 100644 --- a/grid_map_visualization/src/visualizations/MapRegionVisualization.cpp +++ b/grid_map_visualization/src/visualizations/MapRegionVisualization.cpp @@ -17,8 +17,10 @@ namespace grid_map_visualization { -MapRegionVisualization::MapRegionVisualization(const std::string & name) -: VisualizationBase(name), +MapRegionVisualization::MapRegionVisualization( + const std::string & name, + rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr), nVertices_(5), lineWidth_(0.01) { @@ -30,23 +32,23 @@ MapRegionVisualization::~MapRegionVisualization() bool MapRegionVisualization::readParameters() { - this->declare_parameter(std::string(this->get_name()) + ".params.line_width", 0.003); - this->declare_parameter(std::string(this->get_name()) + ".params.color", 16777215); + nodePtr_->declare_parameter(name_ + ".params.line_width", 0.003); + nodePtr_->declare_parameter(name_ + ".params.color", 16777215); lineWidth_ = 0.003; - if (!this->get_parameter(std::string(this->get_name()) + ".params.line_width", lineWidth_)) { + if (!nodePtr_->get_parameter(name_ + ".params.line_width", lineWidth_)) { RCLCPP_INFO( - this->get_logger(), + nodePtr_->get_logger(), "MapRegionVisualization with name '%s' did not find a 'line_width' parameter. Using default.", - this->get_name()); + name_); return false; } int colorValue = 16777215; // white, http://www.wolframalpha.com/input/?i=BitOr%5BBitShiftLeft%5Br%2C16%5D%2C+BitShiftLeft%5Bg%2C8%5D%2C+b%5D+where+%7Br%3D20%2C+g%3D50%2C+b%3D230%7D // NOLINT - if (!this->get_parameter(std::string(this->get_name()) + ".params.color", colorValue)) { + if (!nodePtr_->get_parameter(name_ + ".params.color", colorValue)) { RCLCPP_INFO( - this->get_logger(), + nodePtr_->get_logger(), "MapRegionVisualization with name '%s' did not find a 'color' parameter. Using default.", - this->get_name()); + name_); } setColorFromColorValue(color_, colorValue, true); @@ -62,8 +64,8 @@ bool MapRegionVisualization::initialize() marker_.scale.x = lineWidth_; marker_.points.resize(nVertices_); // Initialized to [0.0, 0.0, 0.0] marker_.colors.resize(nVertices_, color_); - publisher_ = this->create_publisher( - this->get_name(), + publisher_ = nodePtr_->create_publisher( + name_, rclcpp::QoS(1).transient_local()); return true; } diff --git a/grid_map_visualization/src/visualizations/OccupancyGridVisualization.cpp b/grid_map_visualization/src/visualizations/OccupancyGridVisualization.cpp index ba71452bd..2afcdc7bd 100644 --- a/grid_map_visualization/src/visualizations/OccupancyGridVisualization.cpp +++ b/grid_map_visualization/src/visualizations/OccupancyGridVisualization.cpp @@ -15,8 +15,10 @@ namespace grid_map_visualization { -OccupancyGridVisualization::OccupancyGridVisualization(const std::string & name) -: VisualizationBase(name), +OccupancyGridVisualization::OccupancyGridVisualization( + const std::string & name, + rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr), dataMin_(0.0), dataMax_(1.0) { @@ -28,33 +30,31 @@ OccupancyGridVisualization::~OccupancyGridVisualization() bool OccupancyGridVisualization::readParameters() { - this->declare_parameter( - std::string(this->get_name()) + ".params.layer", - std::string("elevation")); - this->declare_parameter(std::string(this->get_name()) + ".params.data_min", 0.0); - this->declare_parameter(std::string(this->get_name()) + ".params.data_max", 1.0); + nodePtr_->declare_parameter(name_ + ".params.layer", std::string("elevation")); + nodePtr_->declare_parameter(name_ + ".params.data_min", 0.0); + nodePtr_->declare_parameter(name_ + ".params.data_max", 1.0); - if (!this->get_parameter(std::string(this->get_name()) + ".params.layer", layer_)) { + if (!nodePtr_->get_parameter(name_ + ".params.layer", layer_)) { RCLCPP_ERROR( - this->get_logger(), + nodePtr_->get_logger(), "OccupancyGridVisualization with name '%s' did not find a 'layer' parameter.", - this->get_name()); + name_); return false; } - if (!this->get_parameter(std::string(this->get_name()) + ".params.data_min", dataMin_)) { + if (!nodePtr_->get_parameter(name_ + ".params.data_min", dataMin_)) { RCLCPP_ERROR( - this->get_logger(), + nodePtr_->get_logger(), "OccupancyGridVisualization with name '%s' did not find a 'data_min' parameter.", - this->get_name()); + name_); return false; } - if (!this->get_parameter(std::string(this->get_name()) + ".params.data_max", dataMax_)) { + if (!nodePtr_->get_parameter(name_ + ".params.data_max", dataMax_)) { RCLCPP_ERROR( - this->get_logger(), + nodePtr_->get_logger(), "OccupancyGridVisualization with name '%s' did not find a 'data_max' parameter.", - this->get_name()); + name_); return false; } @@ -63,8 +63,8 @@ bool OccupancyGridVisualization::readParameters() bool OccupancyGridVisualization::initialize() { - publisher_ = this->create_publisher( - this->get_name(), + publisher_ = nodePtr_->create_publisher( + name_, rclcpp::QoS(1).transient_local()); return true; } @@ -74,7 +74,7 @@ bool OccupancyGridVisualization::visualize(const grid_map::GridMap & map) if (!isActive()) {return false;} if (!map.exists(layer_)) { RCLCPP_WARN_STREAM( - this->get_logger(), + nodePtr_->get_logger(), "OccupancyGridVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); return false; diff --git a/grid_map_visualization/src/visualizations/PointCloudVisualization.cpp b/grid_map_visualization/src/visualizations/PointCloudVisualization.cpp index 3e7e7f187..121f3274b 100644 --- a/grid_map_visualization/src/visualizations/PointCloudVisualization.cpp +++ b/grid_map_visualization/src/visualizations/PointCloudVisualization.cpp @@ -14,8 +14,10 @@ namespace grid_map_visualization { -PointCloudVisualization::PointCloudVisualization(const std::string & name) -: VisualizationBase(name) +PointCloudVisualization::PointCloudVisualization( + const std::string & name, + rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr) { } @@ -25,14 +27,12 @@ PointCloudVisualization::~PointCloudVisualization() bool PointCloudVisualization::readParameters() { - this->declare_parameter( - std::string(this->get_name()) + ".params.layer", - std::string("elevation")); - if (!this->get_parameter(std::string(this->get_name()) + ".params.layer", layer_)) { + nodePtr_->declare_parameter(name_ + ".params.layer", std::string("elevation")); + if (!nodePtr_->get_parameter(name_ + ".params.layer", layer_)) { RCLCPP_ERROR( - this->get_logger(), + nodePtr_->get_logger(), "PointCloudVisualization with name '%s' did not find a 'layer' parameter.", - this->get_name()); + name_); return false; } return true; @@ -40,8 +40,8 @@ bool PointCloudVisualization::readParameters() bool PointCloudVisualization::initialize() { - publisher_ = this->create_publisher( - this->get_name(), + publisher_ = nodePtr_->create_publisher( + name_, rclcpp::QoS(1).transient_local()); return true; } @@ -51,7 +51,7 @@ bool PointCloudVisualization::visualize(const grid_map::GridMap & map) if (!isActive()) {return false;} if (!map.exists(layer_)) { RCLCPP_WARN_STREAM( - this->get_logger(), + nodePtr_->get_logger(), "PointCloudVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); return false; diff --git a/grid_map_visualization/src/visualizations/VectorVisualization.cpp b/grid_map_visualization/src/visualizations/VectorVisualization.cpp index 82cc21f3e..11c050309 100644 --- a/grid_map_visualization/src/visualizations/VectorVisualization.cpp +++ b/grid_map_visualization/src/visualizations/VectorVisualization.cpp @@ -23,8 +23,8 @@ namespace grid_map_visualization { -VectorVisualization::VectorVisualization(const std::string & name) -: VisualizationBase(name) +VectorVisualization::VectorVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr) { } @@ -34,59 +34,57 @@ VectorVisualization::~VectorVisualization() bool VectorVisualization::readParameters() { - this->declare_parameter(std::string(this->get_name()) + ".params.layer_prefix", std::string("")); - this->declare_parameter( - std::string(this->get_name()) + ".params.position_layer", - std::string("")); - this->declare_parameter(std::string(this->get_name()) + ".params.scale", 1.0); - this->declare_parameter(std::string(this->get_name()) + ".params.line_width", 0.003); - this->declare_parameter(std::string(this->get_name()) + ".params.color", 65280); + nodePtr_->declare_parameter(name_ + ".params.layer_prefix", std::string("")); + nodePtr_->declare_parameter(name_ + ".params.position_layer", std::string("")); + nodePtr_->declare_parameter(name_ + ".params.scale", 1.0); + nodePtr_->declare_parameter(name_ + ".params.line_width", 0.003); + nodePtr_->declare_parameter(name_ + ".params.color", 65280); std::string typePrefix; - if (!this->get_parameter(std::string(this->get_name()) + ".params.layer_prefix", typePrefix)) { + if (!nodePtr_->get_parameter(name_ + ".params.layer_prefix", typePrefix)) { RCLCPP_ERROR( - this->get_logger(), + nodePtr_->get_logger(), "VectorVisualization with name '%s' did not find a 'layer_prefix' parameter.", - this->get_name()); + name_); return false; } types_.push_back(typePrefix + "x"); types_.push_back(typePrefix + "y"); types_.push_back(typePrefix + "z"); - if (!this->get_parameter( - std::string(this->get_name()) + ".params.position_layer", + if (!nodePtr_->get_parameter( + name_ + ".params.position_layer", positionLayer_)) { RCLCPP_ERROR( - this->get_logger(), + nodePtr_->get_logger(), "VectorVisualization with name '%s' did not find a 'position_layer' parameter.", - this->get_name()); + name_); return false; } scale_ = 1.0; - if (!this->get_parameter(std::string(this->get_name()) + ".params.scale", scale_)) { + if (!nodePtr_->get_parameter(name_ + ".params.scale", scale_)) { RCLCPP_INFO( - this->get_logger(), + nodePtr_->get_logger(), "VectorVisualization with name '%s' did not find a 'scale' parameter. Using default.", - this->get_name()); + name_); } lineWidth_ = 0.003; - if (!this->get_parameter(std::string(this->get_name()) + ".params.line_width", lineWidth_)) { + if (!nodePtr_->get_parameter(name_ + ".params.line_width", lineWidth_)) { RCLCPP_INFO( - this->get_logger(), + nodePtr_->get_logger(), "VectorVisualization with name '%s' did not find a 'line_width' parameter. Using default.", - this->get_name()); + name_); } int colorValue = 65280; // green - if (!this->get_parameter(std::string(this->get_name()) + ".params.color", colorValue)) { + if (!nodePtr_->get_parameter(name_ + ".params.color", colorValue)) { RCLCPP_INFO( - this->get_logger(), + nodePtr_->get_logger(), "VectorVisualization with name '%s' did not find a 'color' parameter. Using default.", - this->get_name()); + name_); } setColorFromColorValue(color_, colorValue, true); @@ -100,8 +98,8 @@ bool VectorVisualization::initialize() marker_.action = visualization_msgs::msg::Marker::ADD; marker_.type = visualization_msgs::msg::Marker::LINE_LIST; marker_.scale.x = lineWidth_; - publisher_ = this->create_publisher( - this->get_name(), + publisher_ = nodePtr_->create_publisher( + name_, rclcpp::QoS(1).transient_local()); return true; } @@ -113,7 +111,7 @@ bool VectorVisualization::visualize(const grid_map::GridMap & map) for (const auto & type : types_) { if (!map.exists(type)) { RCLCPP_WARN_STREAM( - this->get_logger(), + nodePtr_->get_logger(), "VectorVisualization::visualize: No grid map layer with name '" << type << "' found."); return false; } diff --git a/grid_map_visualization/src/visualizations/VisualizationBase.cpp b/grid_map_visualization/src/visualizations/VisualizationBase.cpp index 7dd119404..cb09765a9 100644 --- a/grid_map_visualization/src/visualizations/VisualizationBase.cpp +++ b/grid_map_visualization/src/visualizations/VisualizationBase.cpp @@ -13,8 +13,9 @@ namespace grid_map_visualization { -VisualizationBase::VisualizationBase(const std::string & name) -: Node(name) +VisualizationBase::VisualizationBase(const std::string & name, rclcpp::Node::SharedPtr nodePtr) +: name_(name), + nodePtr_(nodePtr) { } @@ -24,6 +25,6 @@ VisualizationBase::~VisualizationBase() bool VisualizationBase::isActive() const { - return static_cast(this->count_subscribers(this->get_name())); + return static_cast(nodePtr_->count_subscribers(name_)); } } // namespace grid_map_visualization diff --git a/grid_map_visualization/src/visualizations/VisualizationFactory.cpp b/grid_map_visualization/src/visualizations/VisualizationFactory.cpp index 3ccb49eab..3736ad10b 100644 --- a/grid_map_visualization/src/visualizations/VisualizationFactory.cpp +++ b/grid_map_visualization/src/visualizations/VisualizationFactory.cpp @@ -22,7 +22,8 @@ namespace grid_map_visualization { -VisualizationFactory::VisualizationFactory() +VisualizationFactory::VisualizationFactory(rclcpp::Node::SharedPtr nodePtr) +: nodePtr_(nodePtr) { types_.push_back("point_cloud"); types_.push_back("flat_point_cloud"); @@ -48,22 +49,22 @@ std::shared_ptr VisualizationFactory::getInstance( // TODO(needs_assignment): // Make this nicer: http://stackoverflow.com/questions/9975672/c-automatic-factory-registration-of-derived-types if (type == "point_cloud") { - return std::shared_ptr(new PointCloudVisualization(name)); + return std::shared_ptr(new PointCloudVisualization(name, nodePtr_)); } if (type == "flat_point_cloud") { - return std::shared_ptr(new FlatPointCloudVisualization(name)); + return std::shared_ptr(new FlatPointCloudVisualization(name, nodePtr_)); } if (type == "vectors") { - return std::shared_ptr(new VectorVisualization(name)); + return std::shared_ptr(new VectorVisualization(name, nodePtr_)); } if (type == "occupancy_grid") { - return std::shared_ptr(new OccupancyGridVisualization(name)); + return std::shared_ptr(new OccupancyGridVisualization(name, nodePtr_)); } if (type == "grid_cells") { - return std::shared_ptr(new GridCellsVisualization(name)); + return std::shared_ptr(new GridCellsVisualization(name, nodePtr_)); } if (type == "map_region") { - return std::shared_ptr(new MapRegionVisualization(name)); + return std::shared_ptr(new MapRegionVisualization(name, nodePtr_)); } return std::shared_ptr(); }