From 9bbe14629450ad595a6a3451c143b57720a0f6da Mon Sep 17 00:00:00 2001
From: Yashas Ambati <yashas.amb@gmail.com>
Date: Mon, 18 Nov 2024 10:46:02 -0500
Subject: [PATCH] Initial structure for traversability mapping node

---
 urc_perception/CMakeLists.txt                 | 13 ++++++
 .../include/traversability_mapping.hpp        | 42 +++++++++++++++++++
 urc_perception/src/traversability_mapping.cpp | 42 +++++++++++++++++++
 3 files changed, 97 insertions(+)
 create mode 100644 urc_perception/include/traversability_mapping.hpp
 create mode 100644 urc_perception/src/traversability_mapping.cpp

diff --git a/urc_perception/CMakeLists.txt b/urc_perception/CMakeLists.txt
index b84a7d19..b9f37ce2 100644
--- a/urc_perception/CMakeLists.txt
+++ b/urc_perception/CMakeLists.txt
@@ -15,6 +15,10 @@ find_package(tf2_sensor_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(pcl_conversions REQUIRED)
 find_package(PCL 1.10 REQUIRED)
+find_package(grid_map_core)
+find_package(grid_map_ros)
+find_package(grid_map_msgs)
+find_package(Eigen3 REQUIRED)
 
 include_directories(
   include
@@ -43,6 +47,9 @@ set(dependencies
   tf2_geometry_msgs
   nav_msgs
   pcl_conversions
+  grid_map_core
+  grid_map_ros
+  grid_map_msgs
 )
 
 ament_target_dependencies(${PROJECT_NAME}
@@ -55,6 +62,12 @@ rclcpp_components_register_node(
   EXECUTABLE ${PROJECT_NAME}_ElevationMapping
 )
 
+rclcpp_components_register_node(
+  ${PROJECT_NAME}
+  PLUGIN "urc_perception::TraversabilityMapping"
+  EXECUTABLE ${PROJECT_NAME}_TraversabilityMapping
+)
+
 # Install launch files.
 install(
   DIRECTORY
diff --git a/urc_perception/include/traversability_mapping.hpp b/urc_perception/include/traversability_mapping.hpp
new file mode 100644
index 00000000..b518f100
--- /dev/null
+++ b/urc_perception/include/traversability_mapping.hpp
@@ -0,0 +1,42 @@
+#ifndef TRAVERSABILITY_MAPPING_HPP_
+#define TRAVERSABILITY_MAPPING_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <grid_map_ros/grid_map_ros.hpp>
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
+#include <grid_map_ros/grid_map_ros.hpp>
+
+namespace urc_perception
+{
+
+    class TraversabilityMapping : public rclcpp::Node
+    {
+    public:
+        explicit TraversabilityMapping(const rclcpp::NodeOptions &options);
+        ~TraversabilityMapping();
+
+    private:
+        void handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
+        void initializeMap();
+
+        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_subscriber_;
+        rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr grid_map_publisher_;
+
+        std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+        std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+        grid_map::GridMap map_;
+
+        double resolution_;
+        double min_z_;
+        double max_z_;
+        unsigned int width_;
+
+        std::string map_frame_;
+    };
+
+} // namespace urc_perception
+
+#endif // TRAVERSABILITY_MAPPING_HPP_
diff --git a/urc_perception/src/traversability_mapping.cpp b/urc_perception/src/traversability_mapping.cpp
new file mode 100644
index 00000000..bd0010de
--- /dev/null
+++ b/urc_perception/src/traversability_mapping.cpp
@@ -0,0 +1,42 @@
+#include "traversability_mapping.hpp"
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/filter.h>
+#include <pcl/PCLPointCloud2.h>
+
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+namespace urc_perception
+{
+
+    TraversabilityMapping::TraversabilityMapping(const rclcpp::NodeOptions &options)
+        : Node("traversability_mapping", options)
+    {
+        RCLCPP_INFO(this->get_logger(), "Traversability mapping node has been started.");
+
+        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+    }
+
+    TraversabilityMapping::~TraversabilityMapping() = default;
+
+    void TraversabilityMapping::handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
+    {
+        // This function should be called whenever a new point cloud message is received.
+        // The point cloud message is transformed to the map frame and then processed.
+        // The resulting traversability map should be published.
+    }
+
+    void TraversabilityMapping::initializeMap()
+    {
+        // Initialize the map_ object.
+    }
+
+} // namespace urc_perception
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(urc_perception::TraversabilityMapping)