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..117f822 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,46 +10,115 @@ 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(Open3D 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} + Open3D::Open3D ) -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 + Open3D::Open3D +) + +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..7e78ed4 100644 --- a/src/obstacle_node.cpp +++ b/src/obstacle_node.cpp @@ -2,12 +2,18 @@ #include #include #include +#include +#include +#include +#include +#include "cev_msgs/msg/obstacles.hpp" #include "obstacle/msg/obstacle_array.hpp" #include #include #include #include +#include #include #include #include @@ -27,8 +33,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 +48,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 +80,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 +97,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; @@ -92,31 +116,24 @@ class ObstacleNode : public rclcpp::Node { 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); - } + // 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); + // } + kept.push_back(p); } 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(), - "After filtering: %zu clusters remain", filtered_clusters.size()); // === BEV projection + OBB === ObstacleArray out; out.header = msg->header; + visualization_msgs::msg::MarkerArray obb_markers; for (const auto &kv : filtered_clusters) { int cid = kv.first; @@ -130,15 +147,62 @@ class ObstacleNode : public rclcpp::Node { } std::vector cv_points; + auto cloud = std::make_shared(); + 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); + cloud->points_.push_back(Eigen::Vector3d(p.x, p.y, p.z)); z_min = std::min(z_min, p.z); z_max = std::max(z_max, p.z); } + if (cloud && cloud->points_.size() > 3) { + cloud->RemoveNonFinitePoints(); + if (cloud->points_.size() > 3) { + RCLCPP_INFO(this->get_logger(), "cloud size %d", cloud->points_.size()); + auto obb = cloud->GetOrientedBoundingBox(); + Eigen::Vector3d center = obb.center_; + Eigen::Matrix3d R = obb.R_; + Eigen::Quaterniond q(R); + q.normalize(); + Eigen::Vector3d extent = obb.extent_; + + visualization_msgs::msg::Marker m; + m.header = msg->header; + m.header.frame_id = "rslidar"; + m.ns = "obstacle"; + m.id = cid; + m.type = visualization_msgs::msg::Marker::CUBE; + m.action = visualization_msgs::msg::Marker::ADD; + + // centroid + m.pose.position.x = center.x(); + m.pose.position.y = center.y(); + m.pose.position.z = center.z(); + m.pose.orientation.x = q.x(); + m.pose.orientation.y = q.y(); + m.pose.orientation.z = q.z(); + m.pose.orientation.w = q.w(); + + // size + m.scale.x = extent.y(); + m.scale.y = extent.x(); + m.scale.z = extent.z(); + + // m.scale.x = extent.x(); + // m.scale.y = extent.z(); + // m.scale.z = extent.y(); + + // xyz xzy yxz yzx zyx zxy + m.color = getColorFromId(m.id); + + obb_markers.markers.push_back(m); + } + } + cv::RotatedRect rect = cv::minAreaRect(cv_points); float cx = rect.center.x; @@ -170,59 +234,76 @@ class ObstacleNode : public rclcpp::Node { ob.z_min = z_min; ob.z_max = z_max; - // if height <0.1m then not blocking ob.blocking = (z_max - z_min > 0.1); 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); } + marker_pub_->publish(obb_markers); + // === publish ObstacleArray === obs_pub_->publish(out); - visualization_msgs::msg::MarkerArray markers; - int id_counter = 0; + // visualization_msgs::msg::MarkerArray markers; + // int id_counter = 0; - for (const auto &ob : out.obstacles) { - visualization_msgs::msg::Marker m; - m.header = out.header; - m.ns = "obstacle"; - m.id = id_counter++; - m.type = visualization_msgs::msg::Marker::CUBE; - m.action = visualization_msgs::msg::Marker::ADD; + // 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 = ob.id; + // m.type = visualization_msgs::msg::Marker::CUBE; + // m.action = visualization_msgs::msg::Marker::ADD; - // centroid - m.pose = ob.pose; + // // centroid + // m.pose = ob.pose; - // size - m.scale.x = ob.length; - m.scale.y = ob.width; - m.scale.z = ob.z_max - ob.z_min; + // // size + // m.scale.x = ob.length; + // m.scale.y = ob.width; + // m.scale.z = ob.z_max - ob.z_min; - // z at the middle of the box - m.pose.position.z = (ob.z_min + ob.z_max) / 2.0; + // // 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); - } - marker_pub_->publish(markers); + // 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..ef5eedf --- /dev/null +++ b/src/obstacle_tracker.cpp @@ -0,0 +1,566 @@ +// 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 +#include + +using obstacle::msg::ObstacleArray; + +struct PointXYZCluster { + float x; + float y; + float z; + int32_t cluster_id; +}; + +struct Edge { + std::tuple edge; + float weight; +}; + +struct Box { + float cx, cy; + float vx, vy; + float length, width; + float yaw, yaw_rate; + int cid; +}; + +struct Transform { + std::tuple vel; + float yaw_rate; +}; + + +class ObstacleTracker : public rclcpp::Node { +public: + ObstacleTracker() + : Node("obstacle_tracker") + { + bev_pub_ = this->create_publisher("/bev_obstacles", 10); + 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)); + + RCLCPP_INFO(this->get_logger(), "ObstacleTracker started - waiting for PointCloud2 on 'input_points'"); + } + +private: + void pcCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + // rclcpp::Time curr_stamp(msg->header.stamp); + // rclcpp::Time prev_stamp(msg_prev_->header.stamp); + + // rclcpp::Duration dt_sec = curr_stamp - prev_stamp; + // dt = dt_sec.seconds(); + + // 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; + } + + inline float wrapAngle(float angle) { + while (angle > M_PI) angle -= 2.0f * M_PI; + while (angle < -M_PI) angle += 2.0f * M_PI; + return angle; + } + + std::tuple computeVelocity(const Box& a, const Box& b) { + float dx = a.cx - b.cx; + float dy = a.cy - b.cy; + + return std::make_tuple(dx/0.1f, dy/0.1f); + } + + float computeYawRate(const Box& a, const Box& b) { + float d = a.yaw - b.yaw; + while (d > M_PI) d -= 2.0 * M_PI; + while (d < -M_PI) d += 2.0 * M_PI; + return d / 0.1f; + } + + // some version of iou, which is sometimes bad. + float boxLoss(const Box& a, const Box& b) { + // Tunable normalization constants + const float d_max = 10.0f; // meters + const float l_max = 5.0f; // meters + const float w_max = 5.0f; // meters + + // Weights + const float w_d = 0.5f; + const float w_s = 0.3f; + const float w_y = 0.2f; + + // Position distance + float dx = a.cx - b.cx; + float dy = a.cy - b.cy; + float D_pos = std::sqrt(dx*dx + dy*dy) / d_max; + + // Size difference + float D_size = std::abs(a.length - b.length) / l_max + + std::abs(a.width - b.width) / w_max; + + // Yaw difference + float d_yaw = std::abs(wrapAngle(a.yaw - b.yaw)); + float D_yaw = d_yaw / static_cast(M_PI); + + return w_d * D_pos + w_s * D_size + w_y * D_yaw; + } + + Box predictBox(const Box& prev, Transform transform) { + Box pred = prev; + float dt = 0.5f; + pred.cx += std::get<0>(transform.vel) * dt; + pred.cy += std::get<1>(transform.vel) * dt; + pred.yaw += transform.yaw_rate * dt; + return pred; + } + + 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; + cev_msgs::msg::Obstacles obstacles_msg; + obstacles_msg.obstacles.reserve(C_CURR.size()); + + // 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())); + std::vector> T( + max_size, + std::vector(max_size, Transform{std::make_tuple(0.0f, 0.0f), 0.0f}) + ); + + + // 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::mutex mtx; + std::vector> futures; + + for (int i = 0; i < C_CURR.size(); ++i) { + int j = 0; + sensor_msgs::msg::PointCloud2 c = C_CURR[i]; + if (c.width * c.height == 0) continue; + + // icp::PointCloud icp_c = pc_to_icp_pc(c); + + std::vector cv_points_curr; + // auto cloud = std::make_shared(); + cv_points_curr.reserve(c.width * c.height); + + sensor_msgs::PointCloud2ConstIterator in_x(c, "x"); + sensor_msgs::PointCloud2ConstIterator in_y(c, "y"); + sensor_msgs::PointCloud2ConstIterator in_z(c, "z"); + + for (; in_x != in_x.end(); ++in_x, ++in_y, ++in_z) { + // cloud->points_.push_back(Eigen::Vector3d(*in_x, *in_y, *in_z)); + cv_points_curr.emplace_back(*in_x, *in_y); + } + cv::RotatedRect rect = cv::minAreaRect(cv_points_curr); + + // if (cloud && cloud->points_.size() > 3) { + // cloud->RemoveNonFinitePoints(); + // if (cloud->points_.size() > 3) { + // RCLCPP_INFO(this->get_logger(), "cloud size %d", cloud->points_.size()); + // auto obb = cloud->GetOrientedBoundingBox(); + // } + // } + + Box box_curr{rect.center.x, rect.center.y, 0.0f, 0.0f, rect.size.width, rect.size.height, rect.angle * M_PI / 180.0f, 0.0f, getClusterId(C_CURR[i])}; + + for (int j = 0; j < C_PREV.size(); ++j) { + sensor_msgs::msg::PointCloud2 c_prev = C_PREV[j]; + + std::vector cv_points_prev; + cv_points_prev.reserve(c.width * c.height); + + sensor_msgs::PointCloud2ConstIterator in_x(c, "x"); + sensor_msgs::PointCloud2ConstIterator in_y(c, "y"); + sensor_msgs::PointCloud2ConstIterator in_z(c, "z"); + + for (; in_x != in_x.end(); ++in_x, ++in_y, ++in_z) { + cv_points_prev.emplace_back(*in_x, *in_y); + } + cv::RotatedRect rect = cv::minAreaRect(cv_points_prev); + + // instead of the actual previous box, we use the predicted new orientation and shape of the box given + // past information from the same cluster_id over time, and use that prediction for the loss computation + // vs. current boxes + // could probably also use this to maybe merge clusters: if satisfying the cases that these clusters + // are within the predicted new box, not in the vicinity of any other previous matched clusters. + Box box_prev{rect.center.x, rect.center.y, 0.0f, 0.0f, rect.size.width, rect.size.height, rect.angle * M_PI / 180.0f, 0.0f, getClusterId(c_prev)}; + if (transform.count(getClusterId(c_prev)) > 0) { + box_prev = predictBox(box_prev, transform[getClusterId(c_prev)]); + } + + Edge edge; + edge.edge = std::make_tuple(j, i); + // edge.weight = result.cost; + edge.weight = boxLoss(box_curr, box_prev); + + E.push_back(edge); + C[i][j] = edge.weight; + T[i][j] = Transform{computeVelocity(box_curr, box_prev), computeYawRate(box_curr, box_prev)}; + } + } + + 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); + + std::vector inactive_ids{}; + + for (int i = 0; i < matchings.size(); ++i) { + // prev_idx may not be the previous cluster's cluster_id + int prev_idx = i; + int curr_idx = matchings[i]; + + if (curr_idx != -1 && curr_idx < C_CURR.size() && prev_idx != -1 && prev_idx < C_PREV.size()) { + // this is a valid matching + if (prev_idx > max_active_id) { + max_active_id = prev_idx; + } + } + + if ((curr_idx == -1 || curr_idx >= C_CURR.size()) && prev_idx != -1 && prev_idx < C_PREV.size()) { + uint8_t cid = getClusterId(C_PREV[prev_idx]); + // if c_prev is not matched to some c_curr: + // then c_prev --- invalid c_curr + // add prev_idx to inactive ids, s.t. we can reuse this id if necessary + inactive_ids.push_back(cid); + } + } + + // auto markers = outputMatching(C_PREV, C_CURR, matchings); + writeClusterIds(C_PREV, C_CURR, matchings, inactive_ids, T); + + // given cluster_id of past C_PREV, output C_CURR s.t. the cluster_id is consistent + obstacles_msg.obstacles = C_CURR; + match_pub_->publish(obstacles_msg); + + RCLCPP_INFO(this->get_logger(), "done"); + + // instead of saving C_CURR, we make a prediction as to where the next matched cluster will be + // and save that predicition as obs_msg_prev_. + + // naive bayes filter for prediction: + // via markov assumption, we only need xt-1 for xt + // i should define a global parameter that maps + // cluster_id : prev pc, prev box, prev state (xt-1) + // compute velocity via dx/dt + obs_msg_prev_ = C_CURR; + } + + 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; + } + + void writeClusterIds( + const std::vector& C_PREV, + std::vector& C_CURR, + std::vector matchings, std::vector inactive_ids, + std::vector> T) { + + sensor_msgs::msg::PointCloud2 bev_points; + bev_points.header.frame_id = "rslidar"; + bev_points.height = 1; + + size_t total_points = 0; + for (const auto &pts : C_CURR) { + total_points += pts.width * pts.height; + } + + sensor_msgs::PointCloud2Modifier modifier(bev_points); + modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + modifier.resize(total_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"); + + 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 (curr_idx == -1 || curr_idx >= C_CURR.size()) { + // if c_prev --- invalid c_curr: then this c_prev is just not matched to anything: skip + continue; + } + + // == LOGIC FOR SETTING CLUSTER_ID == + uint32_t cluster_id; + if (prev_idx == -1 || prev_idx >= C_PREV.size()) { + // if c_curr --- invalid c_prev: then this c_curr is not matched to anything: assign new unique cluster_id + // this new unique cluster_id is either an inactive id in inactive_ids, or max_active_id + if (!inactive_ids.empty()) { + cluster_id = inactive_ids.back(); + inactive_ids.pop_back(); + RCLCPP_INFO(this->get_logger(), "Inactive_ids: %d\n", inactive_ids.size()); + } else { + cluster_id = max_active_id; + max_active_id++; + } + } else { + cluster_id = getClusterId(C_PREV[prev_idx]); + transform[cluster_id] = T[curr_idx][prev_idx]; + } + + setClusterId(C_CURR[curr_idx], cluster_id); + + std::vector cv_points; + cv_points.reserve(C_CURR[curr_idx].width * C_CURR[curr_idx].height); + + sensor_msgs::PointCloud2ConstIterator in_x(C_CURR[curr_idx], "x"); + sensor_msgs::PointCloud2ConstIterator in_y(C_CURR[curr_idx], "y"); + sensor_msgs::PointCloud2ConstIterator in_z(C_CURR[curr_idx], "z"); + + for (; in_x != in_x.end(); ++in_x, ++in_y, ++in_z) { + cv_points.emplace_back(*in_x, *in_y); + + std_msgs::msg::ColorRGBA color = getColorFromId(cluster_id); + + *out_x = *in_x; + *out_y = *in_y; + *out_z = *in_z; + *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_pub_->publish(bev_points); + } + + uint32_t getClusterId(const sensor_msgs::msg::PointCloud2& cloud) + { + for (const auto& field : cloud.fields) { + if (field.name == "id") { + const uint8_t* data_ptr = &cloud.data[field.offset]; + return *reinterpret_cast(data_ptr); + } + } + throw std::runtime_error("id field not found"); + } + + void setClusterId(sensor_msgs::msg::PointCloud2& cloud, uint32_t cluster_id) + { + for (auto& field : cloud.fields) { + if (field.name == "id") { + for (size_t i = 0; i < cloud.width * cloud.height; ++i) { + uint8_t* data_ptr = &cloud.data[i * cloud.point_step + field.offset]; + *reinterpret_cast(data_ptr) = cluster_id; + } + return; + } + } + throw std::runtime_error("id field not found"); + } + + 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; + } + + 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_; + rclcpp::Publisher::SharedPtr bev_pub_; + int max_active_id = 0; + // maps cluster_id to a state change + std::map transform; + // float dt; +}; + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file 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; +}