[ROS2] port grid_map_ros

.circleci/config.yml

@@ -25,20 +25,18 @@ jobs:
               $UNDERLAY_WS/ros2_dependencies.repos
             vcs import src < ros2_dependencies.repos
             rosdep install -y --ignore-src --skip-keys "slam_toolbox" --from-paths src
-            colcon build --symlink-install --packages-up-to nav2_costmap_2d
-            source $UNDERLAY_WS/install/local_setup.bash
+            colcon build --symlink-install --packages-up-to nav2_costmap_2d rcpputils rosbag2_cpp
+            source $UNDERLAY_WS/install/setup.bash
             cd $OVERLAY_WS && rosdep install -y --ignore-src --from-paths src
       - run:
           name: Debug Build
           command: |
-            source `find $ROS_WS -maxdepth 2 -name local_setup.bash | sort | head -1`
-            source $UNDERLAY_WS/install/local_setup.bash
+            source $UNDERLAY_WS/install/setup.bash
             cd $OVERLAY_WS && colcon build --parallel-workers 1
       - run:
           name: Run Tests
           command: |
-            source `find $ROS_WS -maxdepth 2 -name local_setup.bash | sort | head -1`
-            source $UNDERLAY_WS/install/local_setup.bash
+            source $UNDERLAY_WS/install/setup.bash
             cd $OVERLAY_WS
             colcon test --parallel-workers 1
             colcon test-result --verbose

grid_map_ros/CMakeLists.txt

@@ -1,65 +1,50 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(grid_map_ros)
 
-set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
-
 ## Find catkin macros and libraries
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
+find_package(ament_cmake REQUIRED)
+find_package(grid_map_cmake_helpers REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(grid_map_core REQUIRED)
+find_package(grid_map_msgs REQUIRED)
+find_package(grid_map_cv REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(rosbag2_cpp REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+grid_map_package()
+
+set(dependencies
+  rclcpp
+  rcutils
   grid_map_core
   grid_map_msgs
   grid_map_cv
   sensor_msgs
   nav_msgs
+  nav2_msgs
   std_msgs
   geometry_msgs
   cv_bridge
-  rosbag
-  tf
+  rosbag2_cpp
+  tf2
   visualization_msgs
 )
 
-## System dependencies are found with CMake's conventions
-#find_package(Eigen3 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 you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-  INCLUDE_DIRS
-    include
-  LIBRARIES
-    ${PROJECT_NAME}
-  CATKIN_DEPENDS
-    roscpp
-    grid_map_core
-    grid_map_msgs
-    grid_map_cv
-    sensor_msgs
-    nav_msgs
-    std_msgs
-    geometry_msgs
-    cv_bridge
-    rosbag
-    tf
-    visualization_msgs
-  DEPENDS
-)
-
 ###########
 ## Build ##
 ###########
 
 ## Specify additional locations of header files
 include_directories(
   include
-  ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
 )
 
@@ -70,12 +55,8 @@ add_library(${PROJECT_NAME}
   src/PolygonRosConverter.cpp
 )
 
-target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-)
-
-add_dependencies(${PROJECT_NAME}
-  ${catkin_EXPORTED_TARGETS}
+ament_target_dependencies(${PROJECT_NAME}
+  ${dependencies}
 )
 
 #############
@@ -85,31 +66,61 @@ add_dependencies(${PROJECT_NAME}
 # Mark executables and/or libraries for installation
 install(
   TARGETS ${PROJECT_NAME}
-  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 cpp header files for installation
 install(
   DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  DESTINATION include/${PROJECT_NAME}
   FILES_MATCHING PATTERN "*.hpp"
 )
 
 #############
 ## Testing ##
 #############
 
-if(CATKIN_ENABLE_TESTING)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
-## Add gtest based cpp test target and link libraries
-catkin_add_gtest(${PROJECT_NAME}-test
-  test/test_grid_map_ros.cpp
-  test/GridMapRosTest.cpp
-)
+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()
+
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ## Add gtest based cpp test target and link libraries
+  ament_add_gtest(${PROJECT_NAME}-test
+    test/test_grid_map_ros.cpp
+    test/GridMapRosTest.cpp
+  )
 endif()
 
 if(TARGET ${PROJECT_NAME}-test)
   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 endif()
+
+ament_export_include_directories(include)
+ament_export_dependencies(${dependencies})
+ament_export_libraries(${PROJECT_NAME})
+ament_package()

grid_map_ros/include/grid_map_ros/GridMapMsgHelpers.hpp

@@ -6,31 +6,37 @@
  *   Institute: ETH Zurich, ANYbotics
  */
 
-#pragma once
+#ifndef GRID_MAP_ROS__GRIDMAPMSGHELPERS_HPP_
+#define GRID_MAP_ROS__GRIDMAPMSGHELPERS_HPP_
 
 // ROS
-#include <ros/ros.h>
-#include <std_msgs/UInt32MultiArray.h>
-#include <std_msgs/Float32MultiArray.h>
-#include <std_msgs/Float64MultiArray.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/u_int32_multi_array.hpp>
+#include <std_msgs/msg/float32_multi_array.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
 
 // Eigen
 #include <Eigen/Core>
 
-namespace grid_map {
+#include <map>
+#include <string>
+
+namespace grid_map
+{
 
 /*!
  * Returns the number of dimensions of the grid map.
  * @return number of dimensions.
  */
 int nDimensions();
 
-enum class StorageIndices {
-    Column,
-    Row
+enum class StorageIndices
+{
+  Column,
+  Row
 };
 
-//! Holds the names of the storage indeces.
+//! Holds the names of the storage indices.
 extern std::map<StorageIndices, std::string> storageIndexNames;
 
 /*!
@@ -40,11 +46,17 @@ extern std::map<StorageIndices, std::string> storageIndexNames;
  * @return true if is in row-major format, false if is in column-major format.
  */
 template<typename MultiArrayMessageType_>
-bool isRowMajor(const MultiArrayMessageType_& message)
+bool isRowMajor(const MultiArrayMessageType_ & message)
 {
-  if (message.layout.dim[0].label == grid_map::storageIndexNames[grid_map::StorageIndices::Column]) return false;
-  else if (message.layout.dim[0].label == grid_map::storageIndexNames[grid_map::StorageIndices::Row]) return true;
-  ROS_ERROR("isRowMajor() failed because layout label is not set correctly.");
+  if (message.layout.dim[0].label ==
+    grid_map::storageIndexNames[grid_map::StorageIndices::Column])
+  {
+    return false;
+  } else if (message.layout.dim[0].label ==
+    grid_map::storageIndexNames[grid_map::StorageIndices::Row]) {return true;}
+  RCLCPP_ERROR(
+    rclcpp::get_logger(
+      "isRowMajor"), "isRowMajor() failed because layout label is not set correctly.");
   return false;
 }
 
@@ -55,9 +67,9 @@ bool isRowMajor(const MultiArrayMessageType_& message)
  * @return the number of columns.
  */
 template<typename MultiArrayMessageType_>
-unsigned int getCols(const MultiArrayMessageType_& message)
+unsigned int getCols(const MultiArrayMessageType_ & message)
 {
-  if (isRowMajor(message)) return message.layout.dim.at(1).size;
+  if (isRowMajor(message)) {return message.layout.dim.at(1).size;}
   return message.layout.dim.at(0).size;
 }
 
@@ -68,9 +80,9 @@ unsigned int getCols(const MultiArrayMessageType_& message)
  * @return the number of rows.
  */
 template<typename MultiArrayMessageType_>
-unsigned int getRows(const MultiArrayMessageType_& message)
+unsigned int getRows(const MultiArrayMessageType_ & message)
 {
-  if (isRowMajor(message)) return message.layout.dim.at(0).size;
+  if (isRowMajor(message)) {return message.layout.dim.at(0).size;}
   return message.layout.dim.at(1).size;
 }
 
@@ -85,7 +97,7 @@ unsigned int getRows(const MultiArrayMessageType_& message)
  * @return true if successful.
  */
 template<typename EigenType_, typename MultiArrayMessageType_>
-bool matrixEigenCopyToMultiArrayMessage(const EigenType_& e, MultiArrayMessageType_& m)
+bool matrixEigenCopyToMultiArrayMessage(const EigenType_ & e, MultiArrayMessageType_ & m)
 {
   m.layout.dim.resize(nDimensions());
   m.layout.dim[0].stride = e.size();
@@ -114,10 +126,12 @@ bool matrixEigenCopyToMultiArrayMessage(const EigenType_& e, MultiArrayMessageTy
  * @return true if successful.
  */
 template<typename EigenType_, typename MultiArrayMessageType_>
-bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_& m, EigenType_& e)
+bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_ & m, EigenType_ & e)
 {
   if (e.IsRowMajor != isRowMajor(m)) {
-    ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible.");
+    RCLCPP_ERROR(
+      rclcpp::get_logger("multiArrayMessageCopyToMatrixEigen"), "multiArrayMessageToMatrixEigen() "
+      "failed because the storage order is not compatible.");
     return false;
   }
 
@@ -135,10 +149,12 @@ bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_& m, EigenTy
  * @return true if successful.
  */
 template<typename EigenType_, typename MultiArrayMessageType_>
-bool multiArrayMessageMapToMatrixEigen(MultiArrayMessageType_& m, EigenType_& e)
+bool multiArrayMessageMapToMatrixEigen(MultiArrayMessageType_ & m, EigenType_ & e)
 {
   if (e.IsRowMajor != isRowMajor(m)) {
-    ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible.");
+    RCLCPP_ERROR(
+      rclcpp::get_logger("multiArrayMessageMapToMatrixEigen"), "multiArrayMessageToMatrixEigen() "
+      "failed because the storage order is not compatible.");
     return false;
   }
 
@@ -147,4 +163,5 @@ bool multiArrayMessageMapToMatrixEigen(MultiArrayMessageType_& m, EigenType_& e)
   return true;
 }
 
-} /* namespace */
+}  // namespace grid_map
+#endif  // GRID_MAP_ROS__GRIDMAPMSGHELPERS_HPP_