diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f8db02..a77f132 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/README.md b/README.md index 3cee501..941aac9 100644 --- a/README.md +++ b/README.md @@ -7,6 +7,7 @@ This package provides the following ROS2 type adaptations: | sensor_msgs/PointCloud2 | StampedPointCloud_PCL | pcl::PointCloud\ |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. diff --git a/include/native_adapters/DigitalElevationMapMsg_CvMat.hpp b/include/native_adapters/DigitalElevationMapMsg_CvMat.hpp new file mode 100644 index 0000000..2b6e445 --- /dev/null +++ b/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 +#include +#include +#include + + +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 +{ + 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_ diff --git a/package.xml b/package.xml index 1537097..9923e78 100644 --- a/package.xml +++ b/package.xml @@ -15,6 +15,7 @@ sensor_msgs pcl_msgs nav_msgs + dem_msgs libopencv-dev libpcl-common pcl_conversions diff --git a/src/DigitalElevationMapMsg_CvMat.cpp b/src/DigitalElevationMapMsg_CvMat.cpp new file mode 100644 index 0000000..56231a6 --- /dev/null +++ b/src/DigitalElevationMapMsg_CvMat.cpp @@ -0,0 +1,69 @@ +/* +Copyright 2023 Patrick Roncagliolo, Antonino Bongiovanni +*/ + +#include +// #include +#include + +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::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(y, x) - 1; + } + } + + destination.header = source.header; + destination.info = source.info; +} + +void rclcpp::TypeAdapter::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::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(y, x) = source.data[y * source.info.width + x] + 1; + } + } + destination.header = source.header; + destination.info = source.info; +}