feature: added DigitalElevationMapMs_CVMat adapter.

CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(PCL 1.12 REQUIRED COMPONENTS common)
 find_package(pcl_conversions REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(pcl_msgs REQUIRED)
+find_package(dem_msgs REQUIRED)
 
 set(PUB_HEADERS
   include/native_adapters/NavMsgOccupancyGrid_CvMat.hpp
@@ -21,12 +22,14 @@ set(PUB_HEADERS
   include/native_adapters/SensorMsgPointCloud2_PCLPointCloud.hpp
   include/native_adapters/PCL.hpp
   include/native_adapters/PointerDefinesMacro.hpp
+  include/native_adapters/DigitalElevationMapMsg_CvMat.hpp
 )
 
 add_library(NativeAdapters SHARED
   src/NavMsgOccupancyGrid_CvMat.cpp
   src/SensorMsgPointCloud2_PCLPointCloud.cpp
-  src/SensorMsgImage_CvMat.cpp)
+  src/SensorMsgImage_CvMat.cpp
+  src/DigitalElevationMapMsg_CvMat.cpp)
 set_target_properties(NativeAdapters PROPERTIES PUBLIC_HEADER "${PUB_HEADERS}")
 target_include_directories(NativeAdapters PUBLIC include)
 
@@ -40,6 +43,7 @@ target_link_libraries(NativeAdapters
   ${std_msgs_TARGETS}
   ${sensor_msgs_TARGETS}
   ${nav_msgs_TARGETS}
+  ${dem_msgs_TARGETS}
   ${cv_bridge_TARGETS}
   ${OpenCV_LIBS}
   ${PCL_LIBRARIES}
@@ -57,7 +61,7 @@ install(
 #ament_export_targets(NativeAdapters)
 ament_export_include_directories(include ${PCL_INCLUDE_DIRS})
 ament_export_libraries(NativeAdapters)
-ament_export_dependencies(rclcpp rclcpp_components std_msgs sensor_msgs nav_msgs OpenCV PCL pcl_conversions pcl_msgs)
+ament_export_dependencies(rclcpp rclcpp_components std_msgs sensor_msgs nav_msgs OpenCV PCL pcl_conversions pcl_msgs dem_msgs)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

README.md

@@ -7,6 +7,7 @@ This package provides the following ROS2 type adaptations:
 | sensor_msgs/PointCloud2 | StampedPointCloud_PCL | pcl::PointCloud\<PointT\> |PCL.hpp|
 | sensor_msgs/Image | StampedImage_CV | cv::Mat |CV.hpp|
 | nav_msgs/OccupancyGrid | StampedOccupancyGrid_CV | cv::Mat |CV.hpp|
+| dem_msgs/DigitalElevationMap | StampedImage_CV | cv::Mat |CV.hpp|
 
 ## Wrapper philosophy
 Each wrapper class includes at least an std_msgs::Header attribute and a native handle.

include/native_adapters/DigitalElevationMapMsg_CvMat.hpp

@@ -0,0 +1,38 @@
+/*
+Copyright 2023 Patrick Roncagliolo, Antonino Bongiovanni
+*/
+
+#ifndef NATIVE_ADAPTERS__SENSORMSGIMAGE_CVMAT_HPP_
+#define NATIVE_ADAPTERS__SENSORMSGIMAGE_CVMAT_HPP_
+#include <rclcpp/type_adapter.hpp>
+#include <dem_msgs/msg/digital_elevation_map.hpp>
+#include <opencv2/core/mat.hpp>
+#include <native_adapters/PointerDefinesMacro.hpp>
+
+
+struct StampedDigitalElevationMap_CV
+{
+  std_msgs::msg::Header header;
+  nav_msgs::msg::MapMetaData info;
+  cv::Mat mat;
+
+  StampedDigitalElevationMap_CV() = default;
+  StampedDigitalElevationMap_CV(const StampedDigitalElevationMap_CV & other);
+  StampedDigitalElevationMap_CV(StampedDigitalElevationMap_CV && other);
+  StampedDigitalElevationMap_CV & operator=(const StampedDigitalElevationMap_CV & other);
+
+  DEFINE_MSG_POINTERS(StampedDigitalElevationMap_CV)
+};
+
+template<>
+struct rclcpp::TypeAdapter<StampedDigitalElevationMap_CV, dem_msgs::msg::DigitalElevationMap>
+{
+  using is_specialized = std::true_type;
+  using custom_type = StampedDigitalElevationMap_CV;
+  using ros_message_type = dem_msgs::msg::DigitalElevationMap;
+  static void convert_to_ros_message(const custom_type & source, ros_message_type & destination);
+  static void convert_to_custom(const ros_message_type & source, custom_type & destination);
+};
+
+
+#endif  // NATIVE_ADAPTERS__DIGITALELEVATIONMAP_CVMAT_HPP_

package.xml

@@ -15,6 +15,7 @@
   <depend>sensor_msgs</depend>
   <depend>pcl_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>dem_msgs</depend>
   <depend>libopencv-dev</depend>
   <depend>libpcl-common</depend>
   <depend>pcl_conversions</depend>

src/DigitalElevationMapMsg_CvMat.cpp

@@ -0,0 +1,69 @@
+/*
+Copyright 2023 Patrick Roncagliolo, Antonino Bongiovanni
+*/
+
+#include <native_adapters/DigitalElevationMapMsg_CvMat.hpp>
+// #include <sensor_msgs/image_encodings.hpp>
+#include <rclcpp/logging.hpp>
+
+StampedDigitalElevationMap_CV::StampedDigitalElevationMap_CV(
+  const StampedDigitalElevationMap_CV & other)
+{
+  //RCLCPP_WARN(rclcpp::get_logger("DEM Adapter"), "Copy constructor called");
+  this->header = other.header;
+  this->info = other.info;
+  this->mat = other.mat.clone();
+}
+
+StampedDigitalElevationMap_CV::StampedDigitalElevationMap_CV(StampedDigitalElevationMap_CV && other)
+{
+  //RCLCPP_WARN(rclcpp::get_logger("DEM Adapter"), "Move constructor called");
+  this->header = std::move(other.header);
+  this->info = std::move(other.info);
+  this->mat = std::move(other.mat);
+}
+
+StampedDigitalElevationMap_CV & StampedDigitalElevationMap_CV::operator=(
+  const StampedDigitalElevationMap_CV & other)
+{
+  //RCLCPP_WARN(rclcpp::get_logger("DEM Adapter"), "Assignment operator called");
+  if (this == &other) {return *this;}
+  this->header = other.header;
+  this->info = other.info;
+  this->mat = other.mat.clone();
+  return *this;
+}
+
+void rclcpp::TypeAdapter<StampedDigitalElevationMap_CV,
+  dem_msgs::msg::DigitalElevationMap>::convert_to_ros_message(
+  const custom_type & source, ros_message_type & destination)
+{
+  //RCLCPP_WARN(rclcpp::get_logger("DEM Adapter"), "Conversion to message");
+
+  destination.data.resize(source.mat.rows * source.mat.cols);
+  for (int y = 0; y < source.mat.rows; ++y) {
+    for (int x = 0; x < source.mat.cols; ++x) {
+      destination.data[y * source.mat.cols + x] = source.mat.at<float>(y, x) - 1;
+    }
+  }
+
+  destination.header = source.header;
+  destination.info = source.info;
+}
+
+void rclcpp::TypeAdapter<StampedDigitalElevationMap_CV,
+  dem_msgs::msg::DigitalElevationMap>::convert_to_custom(
+  const ros_message_type & source, custom_type & destination)
+{
+  //RCLCPP_WARN(rclcpp::get_logger("DEM Adapter"), "Conversion from message");
+
+  destination.mat =
+    cv::Mat(source.info.height, source.info.width, CV_MAKETYPE(cv::DataType<float>::type, 1));
+  for (unsigned int y = 0; y < source.info.height; ++y) {
+    for (unsigned int x = 0; x < source.info.width; ++x) {
+      destination.mat.at<float>(y, x) = source.data[y * source.info.width + x] + 1;
+    }
+  }
+  destination.header = source.header;
+  destination.info = source.info;
+}