diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..ccf510a --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,24 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/humble/include/**", + "/root/ws/src/ackermann_kf/go2_driver/include/**", + "/usr/include/**", + "${workspaceFolder}/**", + "/usr/include/opencv4", + "/root/ws/src/cev_msgs/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 03f571e..45999df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,46 +10,112 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(cev_msgs REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(OpenCV REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(visualization_msgs REQUIRED) - rosidl_generate_interfaces(${PROJECT_NAME} "msg/Obstacle.msg" "msg/ObstacleArray.msg" - DEPENDENCIES std_msgs geometry_msgs + DEPENDENCIES std_msgs geometry_msgs cev_msgs nav_msgs ) add_executable(obstacle_node src/obstacle_node.cpp) +add_executable(occupancy_grid src/occupancy_grid.cpp) +add_executable(obstacle_tracker src/obstacle_tracker.cpp) ament_target_dependencies( obstacle_node rclcpp + cev_msgs sensor_msgs std_msgs + pcl_conversions + pcl_ros + geometry_msgs + tf2 + tf2_geometry_msgs + visualization_msgs +) + +ament_target_dependencies( + occupancy_grid + rclcpp + cev_msgs + sensor_msgs + std_msgs + nav_msgs + pcl_conversions + pcl_ros + geometry_msgs + tf2 + tf2_geometry_msgs + visualization_msgs +) + +ament_target_dependencies( + obstacle_tracker + rclcpp + cev_msgs + sensor_msgs + std_msgs + nav_msgs + pcl_conversions + pcl_ros geometry_msgs tf2 tf2_geometry_msgs visualization_msgs ) +set(CEV_ICP_INCLUDE lib/cev_icp/include) +set(CEV_ICP_LIB lib/cev_icp/lib) + +include_directories(${CEV_ICP_INCLUDE}) +link_directories(${CEV_ICP_LIB}) +include_directories( + ${PCL_INCLUDE_DIRS} +) target_link_libraries(obstacle_node ${OpenCV_LIBS} + ${PCL_LIBRARIES} ) -ament_export_dependencies(rosidl_default_runtime) +target_link_libraries(occupancy_grid + ${OpenCV_LIBS} + ${PCL_LIBRARIES} +) +target_link_libraries(obstacle_tracker + ${OpenCV_LIBS} + ${PCL_LIBRARIES} + cevicp +) + +ament_export_dependencies(rosidl_default_runtime) rosidl_target_interfaces(obstacle_node ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_target_interfaces(occupancy_grid + ${PROJECT_NAME} "rosidl_typesupport_cpp") + +rosidl_target_interfaces(obstacle_tracker + ${PROJECT_NAME} "rosidl_typesupport_cpp") + install(TARGETS obstacle_node + occupancy_grid + obstacle_tracker DESTINATION lib/${PROJECT_NAME}) diff --git a/README.md b/README.md index 8e151a5..52197e9 100644 --- a/README.md +++ b/README.md @@ -1 +1,21 @@ # Obstacle_node +Hi all, I'm going to start yapping again! + +## Angle Bins and Ray Tracing Via Bresenham's Line Algo + +![Occupancy Grid 1](demos/occ.png) +![Occupancy Grid 2](demos/occupancygrid.png) + +## Multi-Hypothesis Tracking +``` +cd Obstacle_node +git submodule add https://github.com/cornellev/icp.git lib/cev_icp +git submodule update --init --recursive +cd lib/cev_icp +sudo make install LIB_INSTALL=/usr/local/lib HEADER_INSTALL=/usr/local/include +``` + +# Important Links +[Rasterize a point cloud](https://r-lidar.github.io/lasR/reference/rasterize.html) + +[Dynamic Obstacle Detection and Tracking Based on 3D Lidar](https://www.jstage.jst.go.jp/article/jaciii/22/5/22_602/_pdf) \ No newline at end of file diff --git a/demos/occ.png b/demos/occ.png new file mode 100644 index 0000000..d80a33f Binary files /dev/null and b/demos/occ.png differ diff --git a/demos/occupancygrid.png b/demos/occupancygrid.png new file mode 100644 index 0000000..15fcf97 Binary files /dev/null and b/demos/occupancygrid.png differ diff --git a/lib/cev_icp b/lib/cev_icp new file mode 160000 index 0000000..2e12d9e --- /dev/null +++ b/lib/cev_icp @@ -0,0 +1 @@ +Subproject commit 2e12d9e0fcc0d4611cb60e93030ddf49174a1b6a diff --git a/overlay/icp.h b/overlay/icp.h new file mode 100644 index 0000000..5b79e1e --- /dev/null +++ b/overlay/icp.h @@ -0,0 +1,180 @@ +/** + * @author Ethan Uppal + * @copyright Copyright (C) 2024 Ethan Uppal. + * Copyright (C) 2025 Cornell Electric Vehicles. + * SPDX-License-Identifier: MIT + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "geo.h" +#include "config.h" + +namespace icp { + + /** + * Interface for iterative closest points. + * Generally, you should interact with ICP instances through this interface or `ICPDriver`, + * though interacting with implementations directly isn't explicitly disallowed. + * Read \ref write_icp_instance for additional information. + * + * \par Example + * @code + * // Construct a new vanilla ICP instance. + * std::unique_ptr icp = icp::ICP::from_method("vanilla"); + * @endcode + * + * \par Usage + * Let `a` and `b` be two point clouds of type `std::vector`. + * Then, given an ICP instance `icp` of type `std::unique_ptr`, + * perform the following steps. + * + * 1. Call `icp->begin(a, b, initial_guess)`. + * 2. Repeatedly call `icp->iterate()` until convergence. `ICPDriver` can also be used to + * specify convergence conditions. + * + * If these steps are not followed as described here, the behavior is + * undefined. + * + * At any point in the process, you may query `icp->calculate_cost()` and + * `icp->transform()`. Do note, however, that `icp->calculate_cost()` is not + * a constant-time operation. + */ + template + class ICP { + private: + using MethodConstructor = std::function>(const Config&)>; + static std::unordered_map methods; + + protected: + using Vector = icp::Vector; + using RBTransform = icp::RBTransform; + using PointCloud = icp::PointCloud; + + /** A matching between `point` and `pair` at (arbitrary) cost `cost`. */ + struct Match { + /** An index into the source point cloud. */ + Eigen::Index point; + + /** An index into the destination point cloud. */ + Eigen::Index pair; + + /** The (arbitrary) cost of the pair. */ + double cost; + }; + + /** The current point cloud transformation that is being optimized. */ + RBTransform transform = RBTransform::Identity(); + + /** The source point cloud. */ + PointCloud a; + + /** The destination point cloud. */ + PointCloud b; + + /** The pairing of each point in `a` to its closest in `b`. */ + std::vector matches; + + ICP(): a(Dim, 0), b(Dim, 0) {} + + /** + * @brief Per-method setup code. + * + * @post For implementers: must fill `matches` with match data for the initial point + * clouds. + */ + virtual void setup() = 0; + + public: + static std::optional>> from_method(const std::string& name, + const Config& config) { + if (methods.find(name) == methods.end()) { + return {}; + } + + return methods[name](config); + } + + static bool is_method_registered(const std::string& name) { + return methods.find(name) != methods.end(); + } + + static std::vector registered_methods() { + std::vector keys; + for (auto it = methods.begin(); it != methods.end(); ++it) { + keys.push_back(it->first); + } + return keys; + } + + virtual ~ICP() = default; + + /** Begins the ICP process for point clouds `source` and `target` with an initial + * guess for the transform `guess`.*/ + void begin(const PointCloud& source, const PointCloud& target, const RBTransform& guess) { + // Initial transform guess + this->transform = guess; + + // Copy in point clouds + this->a = source; + this->b = target; + + // Ensure arrays are the right size + matches.resize(this->a.cols()); + + // Per-instance customization routine + setup(); + } + + /** Perform one iteration of ICP for the point clouds `a` and `b` + * provided with ICP::begin. + * + * @pre ICP::begin must have been invoked. + * @post For implementers: must fill `matches` with newest match data. + */ + virtual void iterate() = 0; + + /** + * Computes the cost of the current transformation. + * + * \par Efficiency: + * `O(a.size())` where `a` is the source point cloud. + */ + [[nodiscard]] virtual double calculate_cost() const { + double sum_squares = 0.0; + for (auto& match: matches) { + sum_squares += match.cost; + } + return std::sqrt(sum_squares / a.cols()); + } + + /** The current transform. */ + [[nodiscard]] RBTransform current_transform() const { + return transform; + } + + /** + * @brief Gets the current point matching computed by ICP. + * + * @return A reference to the matching. Invalidates if `begin` or `iterate` are called. + */ + [[nodiscard]] const std::vector& get_matches() const { + return matches; + } + }; + + using ICP2 = ICP; + using ICP3 = ICP; + + template<> + std::unordered_map ICP2::methods; + + template<> + std::unordered_map ICP3::methods; +} diff --git a/overlay/vanilla_3d.cpp b/overlay/vanilla_3d.cpp new file mode 100644 index 0000000..5596412 --- /dev/null +++ b/overlay/vanilla_3d.cpp @@ -0,0 +1,112 @@ +/** + * @copyright Copyright (C) 2025 Cornell Electric Vehicles. + * SPDX-License-Identifier: MIT + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "icp/impl/vanilla_3d.h" +#include "algo/kdtree.h" +#include "icp/geo.h" + + +namespace icp { + Vanilla3d::Vanilla3d([[maybe_unused]] const Config& config) + : c(3, 0), current_cost_(std::numeric_limits::max()) {} + Vanilla3d::Vanilla3d(): c(3, 0), current_cost_(std::numeric_limits::max()) {} + Vanilla3d::~Vanilla3d() = default; + + // Euclidean distance between two points + double Vanilla3d::dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb) { + return (pta - ptb).norm(); + } + + double Vanilla3d::calculate_cost() const { + return current_cost_; + } + + Neighbors Vanilla3d::nearest_neighbor(const PointCloud& src) { + Neighbors neigh; + neigh.distances.resize(src.cols()); + neigh.indices.resize(src.cols()); + + for (Eigen::Index i = 0; i < src.cols(); ++i) { + const Eigen::Vector3d query = src.col(i); + double min_dist = 0.0; + int idx = kdtree_->search(query, &min_dist); + + neigh.indices[i] = idx; + neigh.distances[i] = std::sqrt(min_dist); + } + + return neigh; + } + + Vanilla3d::RBTransform Vanilla3d::best_fit_transform(const PointCloud& A, const PointCloud& B) { + Vector centroid_A = get_centroid(A); + Vector centroid_B = get_centroid(B); + + Eigen::Matrix3d N = (A.colwise() - centroid_A) * (B.colwise() - centroid_B).transpose(); + + Eigen::JacobiSVD svd(N, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose(); + + if (R.determinant() < 0) { + Eigen::Matrix3d V = svd.matrixV(); + V.col(2) *= -1; + R = V * svd.matrixU().transpose(); + } + + Vector t = centroid_B - R * centroid_A; + + RBTransform transform; + transform.linear() = R; + transform.translation() = t; + + return transform; + } + + void Vanilla3d::setup() { + c = a; + current_cost_ = std::numeric_limits::max(); + + std::vector dst_vec(b.cols()); + for (Eigen::Index i = 0; i < b.cols(); ++i) { + dst_vec[i] = b.col(i); + } + + kdtree_ = std::make_unique>(dst_vec, 3); + } + + void Vanilla3d::iterate() { + // Reorder target point set based on nearest neighbor + Neighbors neighbor = nearest_neighbor(c); + PointCloud dst_reordered(3, a.cols()); // Assuming PointCloud is a 3xN matrix + for (Eigen::Index i = 0; i < a.cols(); i++) { + dst_reordered.col(i) = b.col(neighbor.indices[i]); + } + RBTransform T = best_fit_transform(c, dst_reordered); + c = T * c; + + transform = T * transform; + + calculate_cost(neighbor.distances); + } + + void Vanilla3d::calculate_cost(const std::vector& distances) { + if (distances.empty()) { + current_cost_ = std::numeric_limits::max(); + return; + } + + double sum = std::accumulate(distances.begin(), distances.end(), 0.0); + current_cost_ = sum / static_cast(distances.size()); + } +} diff --git a/overlay/vanilla_3d.h b/overlay/vanilla_3d.h new file mode 100644 index 0000000..0f11cb2 --- /dev/null +++ b/overlay/vanilla_3d.h @@ -0,0 +1,38 @@ +/** + * @copyright Copyright (C) 2025 Cornell Electric Vehicles. + * SPDX-License-Identifier: MIT + */ +#pragma once + +#include +#include +#include "icp/icp.h" +#include "icp/config.h" +#include "algo/kdtree.h" + +namespace icp { + + class Vanilla3d final : public ICP3 { + public: + Vanilla3d(const Config& config); + Vanilla3d(); + ~Vanilla3d() override; + + protected: + void setup() override; + void iterate() override; + double calculate_cost() const override; + + private: + PointCloud c; + std::unique_ptr> target_kdtree_; + double current_cost_; + std::unique_ptr> kdtree_; + + Neighbors nearest_neighbor(const PointCloud& src); + static double dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb); + static RBTransform best_fit_transform(const PointCloud& A, const PointCloud& B); + void calculate_cost(const std::vector& distances); + }; + +} // namespace icp \ No newline at end of file diff --git a/package.xml b/package.xml index b4a48b6..23472df 100644 --- a/package.xml +++ b/package.xml @@ -12,7 +12,11 @@ rclcpp sensor_msgs std_msgs + nav_msgs geometry_msgs + cev_msgs + pcl_conversions + pcl_ros rosidl_default_generators rosidl_default_runtime diff --git a/src/obstacle_node.cpp b/src/obstacle_node.cpp index 44bcdeb..bc11305 100644 --- a/src/obstacle_node.cpp +++ b/src/obstacle_node.cpp @@ -2,6 +2,11 @@ #include #include #include +#include +#include +#include +#include +#include "cev_msgs/msg/obstacles.hpp" #include "obstacle/msg/obstacle_array.hpp" #include @@ -27,8 +32,12 @@ class ObstacleNode : public rclcpp::Node { ObstacleNode() : Node("obstacle_node") { + obs_sub_ = this->create_subscription( + "/rslidar_obstacles", 10, + std::bind(&ObstacleNode::obstaclesCallback, this, std::placeholders::_1)); + pc_sub_ = this->create_subscription( - "input_points", 10, + "/rslidar_clusters", 10, std::bind(&ObstacleNode::pcCallback, this, std::placeholders::_1)); obs_pub_ = this->create_publisher("obstacles", 10); @@ -38,6 +47,23 @@ class ObstacleNode : public rclcpp::Node { } private: + void obstaclesCallback(const cev_msgs::msg::Obstacles::SharedPtr msg) + { + RCLCPP_INFO(this->get_logger(), "Received Obstacles message with %zu clouds", msg->obstacles.size()); + + sensor_msgs::msg::PointCloud2 merged_cloud = msg->obstacles[0]; + + for (size_t i = 1; i < msg->obstacles.size(); ++i) + { + const auto &cloud = msg->obstacles[i]; + + pcl::concatenatePointCloud(merged_cloud, cloud, merged_cloud); + } + + auto cloud_ptr = std::make_shared(merged_cloud); + pcCallback(cloud_ptr); + } + void pcCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { // check required fields bool has_x=false, has_y=false, has_z=false, has_id=false; @@ -53,7 +79,6 @@ class ObstacleNode : public rclcpp::Node { return; } - sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); sensor_msgs::PointCloud2ConstIterator iter_z(*msg, "z"); @@ -71,11 +96,9 @@ class ObstacleNode : public rclcpp::Node { ++total_points; } - RCLCPP_INFO(this->get_logger(), - "Received PointCloud2: points=%zu clusters=%zu", - total_points, clusters.size()); - - + // RCLCPP_INFO(this->get_logger(), + // "Received PointCloud2: points=%zu clusters=%zu", + // total_points, clusters.size()); // z-axis filtering const float z_min_allowed = 0.0f; @@ -101,18 +124,19 @@ class ObstacleNode : public rclcpp::Node { if (!kept.empty()) { filtered_clusters[cid] = kept; - RCLCPP_INFO(this->get_logger(), - "Cluster %d kept: %zu points (orig %zu, z_range=[%.2f, %.2f])", - cid, kept.size(), pts.size(), z_min, z_max); - } else { - RCLCPP_INFO(this->get_logger(), - "Cluster %d discarded (all points outside z range [%.2f, %.2f], orig z_range=[%.2f, %.2f])", - cid, z_min_allowed, z_max_allowed, z_min, z_max); - } + // RCLCPP_INFO(this->get_logger(), + // "Cluster %d kept: %zu points (orig %zu, z_range=[%.2f, %.2f])", + // cid, kept.size(), pts.size(), z_min, z_max); + } + // else { + // RCLCPP_INFO(this->get_logger(), + // "Cluster %d discarded (all points outside z range [%.2f, %.2f], orig z_range=[%.2f, %.2f])", + // cid, z_min_allowed, z_max_allowed, z_min, z_max); + // } } - RCLCPP_INFO(this->get_logger(), - "After filtering: %zu clusters remain", filtered_clusters.size()); + // RCLCPP_INFO(this->get_logger(), + // "After filtering: %zu clusters remain", filtered_clusters.size()); // === BEV projection + OBB === ObstacleArray out; @@ -175,9 +199,9 @@ class ObstacleNode : public rclcpp::Node { out.obstacles.push_back(ob); - RCLCPP_INFO(this->get_logger(), - "Cluster %d -> obstacle center=(%.2f,%.2f), L=%.2f W=%.2f yaw=%.2f rad", - cid, cx, cy, length, width, yaw); + // RCLCPP_INFO(this->get_logger(), + // "Cluster %d -> obstacle center=(%.2f,%.2f), L=%.2f W=%.2f yaw=%.2f rad", + // cid, cx, cy, length, width, yaw); } // === publish ObstacleArray === @@ -189,6 +213,7 @@ class ObstacleNode : public rclcpp::Node { for (const auto &ob : out.obstacles) { visualization_msgs::msg::Marker m; m.header = out.header; + m.header.frame_id = "rslidar"; m.ns = "obstacle"; m.id = id_counter++; m.type = visualization_msgs::msg::Marker::CUBE; @@ -205,24 +230,39 @@ class ObstacleNode : public rclcpp::Node { // z at the middle of the box m.pose.position.z = (ob.z_min + ob.z_max) / 2.0; - // color(all red now) - m.color.r = 1.0; - m.color.g = 0.0; - m.color.b = 0.0; - m.color.a = 0.5; + // color(convert cluster id to different color) + m.color = getColorFromId(m.id); markers.markers.push_back(m); } + + RCLCPP_INFO(rclcpp::get_logger("MarkerPrinter"), + "Visualizing %d clouds", (int) markers.markers.size()); + marker_pub_->publish(markers); + } + std_msgs::msg::ColorRGBA getColorFromId(int cluster_id) + { + std_msgs::msg::ColorRGBA color; + uint32_t hash = static_cast(cluster_id * 2654435761 % 4294967296); // Knuth's multiplicative hash + color.r = ((hash & 0xFF0000) >> 16) / 255.0f; + color.g = ((hash & 0x00FF00) >> 8) / 255.0f; + color.b = (hash & 0x0000FF) / 255.0f; + color.a = 0.5f; + + return color; } + rclcpp::Subscription::SharedPtr obs_sub_; rclcpp::Subscription::SharedPtr pc_sub_; rclcpp::Publisher::SharedPtr obs_pub_; rclcpp::Publisher::SharedPtr marker_pub_; }; +#include "visualization_msgs/msg/marker_array.hpp" + int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); diff --git a/src/obstacle_tracker.cpp b/src/obstacle_tracker.cpp new file mode 100644 index 0000000..1f21449 --- /dev/null +++ b/src/obstacle_tracker.cpp @@ -0,0 +1,451 @@ +// src/obstacle_tracker.cpp +#include +#include +#include +#include +#include +#include +#include +#include +#include "cev_msgs/msg/obstacles.hpp" +#include "obstacle/msg/obstacle_array.hpp" +#include "icp/icp.h" +#include "icp/geo.h" +#include "icp/driver.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using obstacle::msg::ObstacleArray; + +struct PointXYZCluster { + float x; + float y; + float z; + int32_t cluster_id; +}; + +struct Edge { + std::tuple edge; + float weight; +}; + +class ObstacleTracker : public rclcpp::Node { +public: + ObstacleTracker() + : Node("obstacle_tracker") + { + match_pub_ = this->create_publisher("rslidar_matches", 10); + prev_pub_ = this->create_publisher("/prev", 10); + curr_pub_ = this->create_publisher("/curr", 10); + // nearest neighbor association method -> MHT + pc_sub_ = this->create_subscription( + "/rslidar_clusters", 10, + std::bind(&ObstacleTracker::pcCallback, this, std::placeholders::_1)); + + obs_sub_ = this->create_subscription( + "/rslidar_obstacles", 10, + std::bind(&ObstacleTracker::obsCallback, this, std::placeholders::_1)); + + // maybe implement joint probability data association (JPDA) also for clustering + + + RCLCPP_INFO(this->get_logger(), "ObstacleTracker started - waiting for PointCloud2 on 'input_points'"); + } + +private: + void pcCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + // check required fields + bool has_x=false, has_y=false, has_z=false, has_id=false; + for (const auto &f : msg->fields) { + if (f.name == "x") has_x = true; + if (f.name == "y") has_y = true; + if (f.name == "z") has_z = true; + if (f.name == "id" || f.name == "cluster_id") has_id = true; + } + if (!(has_x && has_y && has_z && has_id)) { + RCLCPP_ERROR(this->get_logger(), + "PointCloud2 missing required fields (need x,y,z and id/cluster_id). Skipping this message."); + return; + } + + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*msg, "z"); + sensor_msgs::PointCloud2ConstIterator iter_id(*msg, "id"); + + // get transformation matrix from 3d icp between *msg and *msg_prev_; + + std::unordered_map> C; + size_t total_points = 0; + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_id) { + PointXYZCluster c; + c.x = *iter_x; + c.y = *iter_y; + c.z = *iter_z; + c.cluster_id = *iter_id; + C[c.cluster_id].push_back(c); + ++total_points; + } + + RCLCPP_INFO(this->get_logger(), "C_prev count: %d v. C_curr count: %d", C_prev_.size(), C.size()); + // after all the cluster matching and whatever is done, update C_prev + // we can also use this to determine static v. dynamic obstacles + + if (C_prev_.size() * C.size() != 0) { + prev_pub_->publish(*msg_prev_); + curr_pub_->publish(*msg); + } + msg_prev_ = msg; + C_prev_ = C; + } + + icp::PointCloud pc_to_icp_pc(sensor_msgs::msg::PointCloud2 c) { + sensor_msgs::PointCloud2ConstIterator iter_x(c, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(c, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(c, "z"); + + icp::PointCloud eigen_c(3, c.width * c.height); + + for (size_t i = 0; iter_x != iter_x.end(); ++i, ++iter_x, ++iter_y, ++iter_z) { + eigen_c(0, i) = *iter_x; + eigen_c(1, i) = *iter_y; + eigen_c(2, i) = *iter_z; + } + + return eigen_c; + } + + void obsCallback(const cev_msgs::msg::Obstacles::SharedPtr msg) { + // initialize the bipartite graph + std::vector C_PREV = obs_msg_prev_; + std::vector C_CURR = msg->obstacles; + + // or maybe a mapping from (c_prev, c) -> edge weight, we'll see + // hungarian algorithm takes in cost (adjacency) matrix where C_CURR is row + // C_PREV is col + std::vector E; + int max_size = std::max(C_CURR.size(), C_PREV.size()); + std::vector> C(max_size, std::vector(max_size, std::numeric_limits::max())); + // Eigen::MatrixXf cost_matrix(C_CURR.size(), C_PREV.size()); + + // bipartite graph construction: + // C_PREV (t-1), C_CURR (t): sets of clusters at time t-1 and t, C_PREV \intersect C_CURR = \emptyset + // E: set of edges with elements (c_prev, c) s.t. c_prev \in C_PREV and c \in C_CURR + // set cluster_ids of c \in C_CURR to be the bipartite matched cluster_ids of c_prev \in C_PREV: + // i.e. if (c_prev, c) is a match in max bipartite match, then set cluster_id of c_prev to be cluster_id of c + if (max_size == 0) return; + + auto start = std::chrono::high_resolution_clock::now(); + // std::vector> futures; + + // for (int i = 0; i < C_CURR.size(); ++i) { + // sensor_msgs::msg::PointCloud2 c = C_CURR[i]; + // if (c.width * c.height == 0) continue; + + // auto start = std::chrono::high_resolution_clock::now(); + // icp::PointCloud icp_c = pc_to_icp_pc(c); + + // for (int j = 0; j < C_PREV.size(); ++j) + // { + // sensor_msgs::msg::PointCloud2 c_prev = C_PREV[j]; + // icp::PointCloud icp_c_prev = pc_to_icp_pc(c_prev); + + // std::unique_ptr icp = icp::ICP3::from_method("vanilla", icp::Config()).value(); + // icp::ICPDriver driver(std::move(icp)); + + // driver.set_max_iterations(1); + // driver.set_transform_tolerance(0.1 * M_PI / 180, 0.1); + // auto result = driver.converge(icp_c_prev, icp_c, icp::RBTransform3::Identity()); + + // // check whether translation between the two clusters are within max_radius (which we can define dynamically by past cluster's velocity) + // Edge edge; + // edge.edge = std::make_tuple(j, i); + // edge.weight = result.cost; + + // E.push_back(edge); + // C[i][j] = result.cost; + // // cost_matrix(i, j) = result.cost; + // } + // } + + // for (auto &f : futures) f.get(); + + std::mutex mtx; + std::vector> futures; + + for (int i = 0; i < C_CURR.size(); ++i) { + sensor_msgs::msg::PointCloud2 c = C_CURR[i]; + if (c.width * c.height == 0) continue; + + icp::PointCloud icp_c = pc_to_icp_pc(c); + + for (int j = 0; j < C_PREV.size(); ++j) { + futures.push_back(std::async(std::launch::async, [&, i, j, icp_c]() { + sensor_msgs::msg::PointCloud2 c_prev = C_PREV[j]; + + if (c_prev.width * c_prev.height == 0) return; + icp::PointCloud icp_c_prev = pc_to_icp_pc(c_prev); + + std::unique_ptr icp = icp::ICP3::from_method("vanilla", icp::Config()).value(); + icp::ICPDriver driver(std::move(icp)); + + driver.set_max_iterations(1); + driver.set_transform_tolerance(0.1 * M_PI / 180, 0.1); + + auto result = driver.converge(icp_c_prev, icp_c, icp::RBTransform3::Identity()); + + Edge edge; + edge.edge = std::make_tuple(j, i); + edge.weight = result.cost; + + // Need mutex protection for shared data structures + { + std::lock_guard lock(mtx); + E.push_back(edge); + C[i][j] = result.cost; + } + })); + } + } + + // Wait for all tasks to complete + for (auto& f : futures) { + f.get(); + } + + auto end = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end - start); + RCLCPP_INFO(this->get_logger(), "RAN FOR: %ld ms", duration.count()); + + std::vector matchings = hungarian_assignment(C); + auto markers = outputMatching(C_PREV, C_CURR, matchings); + // outputMatching(C_PREV, C_CURR, matchings); + + match_pub_->publish(markers); + + RCLCPP_INFO(this->get_logger(), "done"); + obs_msg_prev_ = msg->obstacles; + } + + visualization_msgs::msg::MarkerArray outputMatching( + const std::vector& C_PREV, + const std::vector& C_CURR, + std::vector matchings) { + + visualization_msgs::msg::MarkerArray marker_array; + + RCLCPP_INFO(this->get_logger(), "C_prev count: %d v. C_curr count: %d", C_PREV.size(), C_CURR.size()); + for (int i = 0; i < matchings.size(); ++i) { + int prev_idx = i; + int curr_idx = matchings[i]; + + // if prev_idx or curr_idx is -1, then this is an invalid match + if (curr_idx == -1 || curr_idx >= C_CURR.size() || prev_idx == -1 || prev_idx >= C_PREV.size()) { + continue; + } + + RCLCPP_INFO(this->get_logger(), "C_PREV [%d] -- C_CURR [%d]\n", prev_idx, curr_idx); + + Eigen::Vector4f centroid_prev, centroid_curr; + + // Convert to PCL and compute centroids + pcl::PointCloud::Ptr cloud_prev(new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_curr(new pcl::PointCloud); + + pcl::fromROSMsg(C_PREV[prev_idx], *cloud_prev); + pcl::fromROSMsg(C_CURR[curr_idx], *cloud_curr); + + pcl::compute3DCentroid(*cloud_prev, centroid_prev); + pcl::compute3DCentroid(*cloud_curr, centroid_curr); + + RCLCPP_INFO(this->get_logger(), "C_PREV: (%f, %f, %f) -- C_CURR: (%f, %f, %f)\n", + centroid_prev[0], centroid_prev[1], centroid_prev[2], centroid_curr[0], centroid_curr[1], centroid_curr[2]); + + // Create line marker + visualization_msgs::msg::Marker line; + line.header.frame_id = "rslidar"; + line.header.stamp = this->now(); + line.ns = "cluster_matches"; + line.id = i; + line.type = visualization_msgs::msg::Marker::ARROW; + line.action = visualization_msgs::msg::Marker::ADD; + + // Start point (previous cluster) + geometry_msgs::msg::Point p1; + p1.x = centroid_prev[0]; + p1.y = centroid_prev[1]; + p1.z = centroid_prev[2]; + + // End point (current cluster) + geometry_msgs::msg::Point p2; + p2.x = centroid_curr[0]; + p2.y = centroid_curr[1]; + p2.z = centroid_curr[2]; + + line.points.push_back(p1); + line.points.push_back(p2); + + // Style + line.scale.x = 0.05; + line.scale.y = 0.1; + line.scale.z = 0.1; + + // Color (green for matched) + line.color.r = 0.0; + line.color.g = 1.0; + line.color.b = 0.0; + line.color.a = 0.8; + + line.lifetime = rclcpp::Duration::from_seconds(2.0); + + marker_array.markers.push_back(line); + } + + return marker_array; + } + + // void sanityCheckHungarian() { + // RCLCPP_INFO(this->get_logger(), "STARTING SANITY CHECK\n"); + // std::vector> costs{{8.0, 5.0, 9.0}, {4.0, 2.0, 4.0}, {7.0, 3.0, 8.0}}; + // // A B C + // // clean 8.0 5.0 9.0 + // // sweep 4.0 2.0 4.0 + // // wash 7.0 3.0 8.0 + // assert((hungarian_assignment(costs) == std::vector{5.0, 9.0, 15.0})); + // RCLCPP_INFO(this->get_logger(), "Sanity check passed\n"); + // } + + void multi_hypothesis_tracking(std::unordered_map> C_prev, + std::unordered_map> C) { + // takes in ??? vector or the entire point cloud idrk + // tbh for both MHT and max bipartite matching, might need to consider all + // possible matches + // or like icp_cost_bipartite_matching is a way to generate all possible + // association hypothesis -> run SHT -> reduce number of hypotheses + + // cases for matching clusters: + // 1) old obstacle being tracked gradually / suddenly disappear + // -> there are no obstacles associated w/ the current frame + // -> no cluster in C that matches old obstacle in C_prev_: {T_jN} + // 2) new obstacle suddenly / gradually enters the LiDAR range + // -> previous frame is not associated with the obstacle + // -> no cluster in C_prev_ that matches new obstacle in C: {Z_jN} + // 3) obstacle in current frame is assocaited with previously tracked obstacle + // -> denote as {Y(T_j, Z_j)} + } + + // void hungarian_assignment(Eigen::MatrixXf mat, float max_cost) { + std::vector hungarian_assignment(std::vector> mat) { + // i is the cluster id of C_PREV, job[i] is the cluster id of C_CURR + + // find perfect matching from C_PREV to C_CURR that minimizes total assignment cost + const int J = mat.size(); // cost_matrix.rows() is C_CURR idx + assert(J > 0); + const int W = mat[0].size(); // cost_matrix.cols() is C_PREV idx + + std::vector job(W + 1, -1); + std::vector ys(J, 0.0f); + std::vector yt(W + 1, 0.0f); + std::vector answers; + const float inf = std::numeric_limits::max(); + const float eps = static_cast(1e-9); + + for (int jCur = 0; jCur < J; ++jCur) { // assign jCur-th job + int wCur = W; + job[wCur] = jCur; + + std::vector minTo(W + 1, inf); + std::vector prev(W + 1, -1); // previous worker on alternating path + std::vector inZ(W + 1, false); // whether worker is in Z + + while (job[wCur] != -1) { // runs at most jCur + 1 times + inZ[wCur] = true; + const int j = job[wCur]; + float delta = inf; + int wNext = -1; + + for (int w = 0; w < W; ++w) { + if (!inZ[w]) { + float new_cost = mat[j][w] - ys[j] - yt[w]; + if (new_cost < minTo[w] - eps) { + minTo[w] = new_cost; + prev[w] = wCur; + } + if (minTo[w] < delta - eps) { + delta = minTo[w]; + wNext = w; + } + } + } + + if (wNext == -1) { + RCLCPP_ERROR(this->get_logger(), "No valid worker found!"); + break; + } + // delta will always be nonnegative, + // except possibly during the first time this loop runs + // if any entries of C[jCur] are negative + for (int w = 0; w <= W; ++w) { + if (inZ[w]) { + if (job[w] != -1) ys[job[w]] += delta; + yt[w] -= delta; + } else { + minTo[w] -= delta; + } + } + wCur = wNext; + } + // update assignments along alternating path + while (wCur != W) { + int w = prev[wCur]; + job[wCur] = job[w]; + wCur = w; + } + + answers.push_back(-yt[W]); + } + + job.pop_back(); + return job; + } + + void bertsekas_auction() { + // for parallelization approximate max bipartite matching + // problems: we have non-integral bipartite graph. + // multiplicative auction algo for (1-e)-approximation of max bipartite: + // one-sided vertex deletion (delete c_prev given corr obstacle left the frame) + // other-sided vertex insertion (insert c given corr new obstacle appear in frame) + // could scale up the edge weights by 10^6 to get integer weights -> run the algo + + // allow unmatched bidders: can assign matches s.t. (c_prev, null): c_prev's + // obstacle disappeared + // or (null, c): c's obstacle newly appeared in frame + // WLOG: if there are no c_prev's remaining or if all remaining c_prev's have cost > threshold + // then (null, c) + } + + rclcpp::Publisher::SharedPtr prev_pub_; + rclcpp::Publisher::SharedPtr curr_pub_; + rclcpp::Publisher::SharedPtr match_pub_; + sensor_msgs::msg::PointCloud2::SharedPtr msg_prev_; + std::vector obs_msg_prev_; + std::unordered_map> C_prev_; + rclcpp::Subscription::SharedPtr obs_sub_; + rclcpp::Subscription::SharedPtr pc_sub_; +}; + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/occupancy_grid.cpp b/src/occupancy_grid.cpp new file mode 100644 index 0000000..c51110e --- /dev/null +++ b/src/occupancy_grid.cpp @@ -0,0 +1,441 @@ +// src/occupancy_grid.cpp +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using obstacle::msg::ObstacleArray; + +constexpr double deg2rad(double deg) { + return deg * M_PI / 180.0; +}; + +struct PointXYZCluster { + float x; + float y; + float z; + int32_t cluster_id; +}; + + struct BinInfo { + BinInfo() = default; + BinInfo(const double _range, const double _wx, const double _wy) + : range(_range), wx(_wx), wy(_wy) + { + } + double range; + double wx; + double wy; +}; + +enum class CellState { + OCCUPIED, + UNKNOWN, + FREE +}; + +class OccupancyGridNode : public rclcpp::Node { +public: + OccupancyGridNode() + : Node("occupancy_grid") + { + obs_sub_ = this->create_subscription( + "/rslidar_obstacles", 10, + std::bind(&OccupancyGridNode::obstaclesCallback, this, std::placeholders::_1)); + + pc_sub_ = this->create_subscription( + "/rslidar_clusters", 10, + std::bind(&OccupancyGridNode::pcCallback, this, std::placeholders::_1)); + + bev_pub_ = this->create_publisher("/bev_obstacles", 10); + + grid_pub_ = this->create_publisher("/occupancy_grid", 10); + + RCLCPP_INFO(this->get_logger(), "OccupancyGridNode started - waiting for PointCloud2 on 'input_points'"); + } + +private: + void obstaclesCallback(const cev_msgs::msg::Obstacles::SharedPtr msg) + { + sensor_msgs::msg::PointCloud2 merged_cloud = msg->obstacles[0]; + + for (size_t i = 1; i < msg->obstacles.size(); ++i) + { + const auto &cloud = msg->obstacles[i]; + + pcl::concatenatePointCloud(merged_cloud, cloud, merged_cloud); + } + + auto cloud_ptr = std::make_shared(merged_cloud); + + pcCallback(cloud_ptr); + } + + void pcCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + // check required fields + bool has_x=false, has_y=false, has_z=false, has_id=false; + for (const auto &f : msg->fields) { + if (f.name == "x") has_x = true; + if (f.name == "y") has_y = true; + if (f.name == "z") has_z = true; + if (f.name == "id" || f.name == "cluster_id") has_id = true; + } + if (!(has_x && has_y && has_z && has_id)) { + RCLCPP_ERROR(this->get_logger(), + "PointCloud2 missing required fields (need x,y,z and id/cluster_id). Skipping this message."); + return; + } + + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*msg, "z"); + sensor_msgs::PointCloud2ConstIterator iter_id(*msg, "id"); + + float min_x = std::numeric_limits::max(); + float min_y = std::numeric_limits::max(); + float max_x = std::numeric_limits::lowest(); + float max_y = std::numeric_limits::lowest(); + + std::unordered_map> clusters; + size_t total_points = 0; + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_id) { + PointXYZCluster p; + p.x = *iter_x; + p.y = *iter_y; + p.z = *iter_z; + p.cluster_id = *iter_id; + clusters[p.cluster_id].push_back(p); + + min_x = std::min(min_x, p.x); + min_y = std::min(min_y, p.y); + max_x = std::max(max_x, p.x); + max_y = std::max(max_y, p.y); + + ++total_points; + } + + const float padding = 1.0f; + min_x -= padding; + min_y -= padding; + max_x += padding; + max_y += padding; + + float cell_size = 0.05f; + float angle_increment = deg2rad(0.4); + // 26 fov has horizontal angle: 0.4° + int width = static_cast((max_x - min_x) / cell_size); + int height = static_cast((max_y - min_y) / cell_size); + + grid_msg_.header.frame_id = "rslidar"; + grid_msg_.info.resolution = cell_size; + grid_msg_.info.width = width; + grid_msg_.info.height = height; + grid_msg_.info.origin.position.x = min_x; + grid_msg_.info.origin.position.y = min_y; + grid_msg_.info.origin.position.z = 0.0; + grid_msg_.info.origin.orientation.x = 0.0; + grid_msg_.info.origin.orientation.y = 0.0; + grid_msg_.info.origin.orientation.z = 0.0; + grid_msg_.info.origin.orientation.w = 1.0; + + grid_msg_.data.assign(width * height, static_cast(CellState::UNKNOWN)); + + // z-axis filtering + const float z_min_allowed = 0.0f; + const float z_max_allowed = 2.5f; + + std::unordered_map> filtered_clusters; + size_t total_kept_points = 0; + + for (const auto &kv : clusters) { + int cid = kv.first; + const auto &pts = kv.second; + + std::vector kept; + float z_min = std::numeric_limits::max(); + float z_max = std::numeric_limits::lowest(); + + for (const auto &p : pts) { + z_min = std::min(z_min, p.z); + z_max = std::max(z_max, p.z); + if (p.z >= z_min_allowed && p.z <= z_max_allowed) { + kept.push_back(p); + } + } + + if (!kept.empty()) { + filtered_clusters[cid] = kept; + total_kept_points += filtered_clusters[cid].size(); + } + } + + // === BEV projection + OBB === + ObstacleArray out; + out.header = msg->header; + + sensor_msgs::msg::PointCloud2 bev_points; + bev_points.header = out.header; + bev_points.header.frame_id = "rslidar"; + bev_points.height = 1; + + sensor_msgs::PointCloud2Modifier modifier(bev_points); + modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + modifier.resize(total_kept_points); + + sensor_msgs::PointCloud2Iterator out_x(bev_points, "x"); + sensor_msgs::PointCloud2Iterator out_y(bev_points, "y"); + sensor_msgs::PointCloud2Iterator out_z(bev_points, "z"); + sensor_msgs::PointCloud2Iterator out_r(bev_points, "r"); + sensor_msgs::PointCloud2Iterator out_g(bev_points, "g"); + sensor_msgs::PointCloud2Iterator out_b(bev_points, "b"); + + for (const auto &kv : filtered_clusters) { + int cid = kv.first; + const auto &pts = kv.second; + + if (pts.size() < 3) { + RCLCPP_WARN(this->get_logger(), + "Cluster %d has only %zu points, skipping OBB", + cid, pts.size()); + continue; + } + + std::vector cv_points; + cv_points.reserve(pts.size()); + float z_min = std::numeric_limits::max(); + float z_max = std::numeric_limits::lowest(); + for (const auto &p : pts) { + cv_points.emplace_back(p.x, p.y); + z_min = std::min(z_min, p.z); + z_max = std::max(z_max, p.z); + std_msgs::msg::ColorRGBA color = getColorFromId(cid); + + *out_x = p.x; + *out_y = p.y; + *out_z = 0.0f; + *out_r = color.r * 255; + *out_g = color.g * 255; + *out_b = color.b * 255; + ++out_x; ++out_y; ++out_z; + ++out_r; ++out_g; ++out_b; + } + } + + bev_points.width = static_cast(bev_points.data.size() / bev_points.point_step); + bev_points.row_step = bev_points.point_step * bev_points.width; + + // bev_points done -> get occupancy grid: + sensor_msgs::msg::PointCloud2 map_scan; + map_scan.header = out.header; + map_scan.header.frame_id = "rslidar"; + map_scan.height = 1; + + sensor_msgs::PointCloud2Modifier map_modifier(map_scan); + map_modifier.setPointCloud2Fields( + 4, + "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "id", 1, sensor_msgs::msg::PointField::UINT32 + ); + + map_modifier.resize(total_kept_points); + + worldToMap(filtered_clusters, map_scan); + Obstacle2OccupancyGrid(bev_points, map_scan, angle_increment); + + bev_pub_->publish(bev_points); + grid_pub_->publish(grid_msg_); + } + + // need ground segmentor before obstacle seg + void Obstacle2OccupancyGrid( + sensor_msgs::msg::PointCloud2 &world_scan, + sensor_msgs::msg::PointCloud2 &map_scan, + float angle_increment + ) { + // add step that transforms point cloud + std::vector> obstacle_angle_bins; + std::vector grid_points_(grid_msg_.info.width * grid_msg_.info.height, 50); + // 360 degrees + constexpr double min_angle = deg2rad(-180.0); + constexpr double max_angle = deg2rad(180.0); + const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1); + obstacle_angle_bins.resize(angle_bin_size); + + // add obstacle points to their corresponding angle bins -> 1 bin per ray + RCLCPP_INFO(this->get_logger(), "world_scan %zu and map_scan %zu", world_scan.data.size(), map_scan.data.size()); + + for (sensor_msgs::PointCloud2ConstIterator iter_x(world_scan, "x"), iter_y(world_scan, "y"), + iter_wx(map_scan, "x"), iter_wy(map_scan, "y"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) + { + const double angle = atan2(*iter_y, *iter_x); + int angle_bin_idx = (angle - min_angle) / angle_increment; + // BinInfo(l2-dist of obstacle point from origin, x, y) + obstacle_angle_bins.at(angle_bin_idx) + .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy)); + } + + double ox = std::floor(grid_msg_.info.origin.position.x / grid_msg_.info.resolution); + double oy = std::floor(grid_msg_.info.origin.position.y / grid_msg_.info.resolution); + // sort by distance + for (auto & obstacle_angle_bin : obstacle_angle_bins) { + std::sort(obstacle_angle_bin.begin(), obstacle_angle_bin.end(), + [](auto a, auto b) { return a.range < b.range; }); + } + + // initialize cells to the final point with freespace + for (size_t bin_idx = 0; bin_idx < obstacle_angle_bins.size(); ++bin_idx) { + // iterate through all angle_bins, find the farthest point of each bin -> last element + auto & obstacle_angle_bin = obstacle_angle_bins.at(bin_idx); + + BinInfo end_distance; + if (obstacle_angle_bin.empty()) { + continue; + } else { + end_distance = obstacle_angle_bin.back(); // furthest away point + } + // from origin to farthest point are all FREE initially + rayTrace(ox, oy, end_distance.wx, end_distance.wy, CellState::FREE); + + // later implement method that takes into account blindspot behind obstacles -> UNKNOWN + + // fill in obstacle points as OCCUPIED + fillOccupied(obstacle_angle_bins, 0.0); + } + } + + void setCellValue(double x, double y, CellState state) { + int xi = static_cast(x); + int yi = static_cast(y); + if (xi < 0 || yi < 0 || + xi >= static_cast(grid_msg_.info.width) || + yi >= static_cast(grid_msg_.info.height)) + return; + + size_t index = yi * grid_msg_.info.width + xi; + switch (state) { + case CellState::OCCUPIED: grid_msg_.data[index] = 97; break; + case CellState::UNKNOWN: grid_msg_.data[index] = 50; break; + case CellState::FREE: grid_msg_.data[index] = 3; break; + } + } + + void fillOccupied(std::vector> obstacle_angle_bins, double distance_margin) { + for (size_t bin_idx = 0; bin_idx < obstacle_angle_bins.size(); ++bin_idx) { + auto & obstacle_angle_bin = obstacle_angle_bins.at(bin_idx); + for (size_t dist_idx = 0; dist_idx < obstacle_angle_bin.size(); ++dist_idx) { + const auto & source = obstacle_angle_bin.at(dist_idx); + setCellValue(source.wx, source.wy, CellState::OCCUPIED); + + if (dist_idx + 1 == obstacle_angle_bin.size()) { + continue; + } + + auto next_dist = std::abs( + obstacle_angle_bin.at(dist_idx + 1).range - + obstacle_angle_bin.at(dist_idx).range); + // the distance_margin should be + // obstacle height |\ + // | \ -> angle is vertical fov + // ---- + //obstacle dist from og intersect w/ ground + if (next_dist <= distance_margin) { + const auto & source = obstacle_angle_bin.at(dist_idx); + const auto & target = obstacle_angle_bin.at(dist_idx + 1); + rayTrace(source.wx, source.wy, target.wx, target.wy, CellState::OCCUPIED); + continue; + } + } + } + } + + void worldToMap(std::unordered_map> &world_scan, sensor_msgs::msg::PointCloud2 &map_scan) { + sensor_msgs::PointCloud2Iterator mx(map_scan, "x"); + sensor_msgs::PointCloud2Iterator my(map_scan, "y"); + sensor_msgs::PointCloud2Iterator mz(map_scan, "z"); + sensor_msgs::PointCloud2Iterator mid(map_scan, "id"); + + for (const auto &kv : world_scan) { + int cid = kv.first; + const auto &pts = kv.second; + + for (const auto &p : pts) { + *mx = std::floor(fabs((p.x - (grid_msg_.info.origin.position.x)) / grid_msg_.info.resolution)); + *my = std::floor(fabs((p.y - (grid_msg_.info.origin.position.y)) / grid_msg_.info.resolution)); + *mz = 0.0; + *mid = cid; + + ++mx; ++my; ++mz; ++mid; + } + } + + map_scan.width = static_cast(map_scan.data.size() / map_scan.point_step); + map_scan.row_step = map_scan.point_step * map_scan.width; + + } + + // implement Bresenham's line algo for ray tracing + void rayTrace(double ox, double oy, double tx, double ty, CellState state) { + // ray defined by (ox, oy) + (tx - ox, ty - oy) * t; + int x = static_cast(ox); + int y = static_cast(oy); + int x_end = static_cast(tx); + int y_end = static_cast(ty); + + int dx = std::abs(x_end - x); + int dy = std::abs(y_end - y); + int sx = (x < x_end) ? 1 : -1; + int sy = (y < y_end) ? 1 : -1; + int err = dx - dy; + + while (true) { + setCellValue(x, y, state); + if (x == x_end && y == y_end) break; + int e2 = 2 * err; + if (e2 > -dy) { err -= dy; x += sx; } + if (e2 < dx) { err += dx; y += sy; } + } + } + + std_msgs::msg::ColorRGBA getColorFromId(int cluster_id) + { + std_msgs::msg::ColorRGBA color; + uint32_t hash = static_cast(cluster_id * 2654435761 % 4294967296); // Knuth's multiplicative hash + color.r = ((hash & 0xFF0000) >> 16) / 255.0f; + color.g = ((hash & 0x00FF00) >> 8) / 255.0f; + color.b = (hash & 0x0000FF) / 255.0f; + color.a = 0.5f; + + return color; + } + + rclcpp::Subscription::SharedPtr obs_sub_; + rclcpp::Subscription::SharedPtr pc_sub_; + rclcpp::Publisher::SharedPtr bev_pub_; + rclcpp::Publisher::SharedPtr grid_pub_; + nav_msgs::msg::OccupancyGrid grid_msg_; +}; + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}