Started working on it

src/world_modeling/occupancy_segmentation/CMakeLists.txt

@@ -13,7 +13,6 @@ endif()
 # Search for dependencies required for building this package
 find_package(ament_cmake REQUIRED) # ROS2 build tool
 find_package(rclcpp REQUIRED)      # ROS2 C++ package
-find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages
 find_package(pcl_ros REQUIRED)
 
 # Compiles source files into a library
@@ -30,7 +29,6 @@ target_include_directories(occupancy_segmentation_lib
 # Add ROS2 dependencies required by package
 ament_target_dependencies(occupancy_segmentation_lib
   rclcpp
-  sample_msgs
   pcl_ros)
 
 # # By default tests are built. This can be overridden by modifying the

src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp

@@ -2,22 +2,47 @@
 #define OCCUPANCY_SEGMENTATION_CORE_HPP_
 
 #include <vector>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/common/centroid.h>
+#include <pcl/io/pcd_io.h>
+
+#include <tbb/parallel_for.h>
+#include <tbb/blocked_range.h>
+
+
+#define NUM_ZONES 4
+#define L_MIN 2.7
+#define L_MAX 80.0
 
 
 /**
  * Implementation for the internal logic for the Transformer ROS2
  * node performing data processing and validation.
  */
 class OccupancySegmentationCore {
- public:
-  // Size of buffer before processed messages are published.
-  static constexpr int BUFFER_CAPACITY = 10;
-
- public:
-  /**
-   * Transformer constructor.
-   */
-  OccupancySegmentationCore();
+  public:
+    typedef std::vector<pcl::PointCloud<pcl::PointXYZ>> Ring;
+    typedef std::vector<Ring> Zone;
+    // Size of buffer before processed messages are published.
+    static constexpr int BUFFER_CAPACITY = 10;
+
+    const int ZONE_RINGS [NUM_ZONES] = {2,4,4,4};
+    const int ZONE_SECTORS [NUM_ZONES] = {16,32,54,32};
+
+
+    std::vector<Zone> czm;
+
+
+    OccupancySegmentationCore();
+
+  private:
+
+    void init_czm();
+
+    void fill_czm(pcl::PointCloud<pcl::PointXYZ> &cloud_in);
+
+
+
 };
 
 

src/world_modeling/occupancy_segmentation/package.xml

@@ -10,7 +10,6 @@
   <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
-  <depend>sample_msgs</depend>
   <depend>pcl_ros</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>

src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp

@@ -4,4 +4,42 @@
 #include "occupancy_segmentation_core.hpp"
 
 
-OccupancySegmentationCore::OccupancySegmentationCore() {}
+OccupancySegmentationCore::OccupancySegmentationCore() {}
+
+void OccupancySegmentationCore::init_czm(){
+    std::vector<Zone> temp_czm;
+    czm = temp_czm;
+    for(int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){
+        Zone zone;
+        for(int r = 0; r < ZONE_RINGS[zone_idx]; r++){
+            Ring ring;
+            for (int s = 0; s < ZONE_SECTORS[zone_idx]; s++){
+                pcl::PointCloud<pcl::PointXYZ> patch;
+                ring.emplace_back(patch);
+            }
+            zone.emplace_back(ring);
+        }
+        czm.emplace_back(zone);
+    }
+}
+
+void OccupancySegmentationCore::fill_czm(pcl::PointCloud<pcl::PointXYZ> &cloud_in){
+    for(pcl::PointXYZ &p : cloud_in.points){
+        double r = sqrt(pow(p.x, 2) + pow(p.y, 2));
+        double theta = atan2(p.y,p.x); // EDITED!
+        if (theta < 0){
+            theta += 2*M_PI;
+        }
+        double lmins[4] = {L_MIN, (7*L_MIN + L_MAX) / 8, (3*L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2};
+        double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX};
+        double deltal = L_MAX - L_MIN;
+
+        for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){
+
+        }
+
+
+
+    }
+
+}

src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp

@@ -12,7 +12,6 @@ void OccupancySegmentationNode::timer_callback(){
 }
 
 int main(int argc, char** argv) {
-  std::cout << "Huh" << std::endl; 
   rclcpp::init(argc, argv);
   rclcpp::spin(std::make_shared<OccupancySegmentationNode>());
   rclcpp::shutdown();