diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..b2899a6 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.5) +project(lidarProject) + +# Find ROS 2 dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + +# Add executable files +add_executable(publisher_node src/publisher_node.cpp) +add_executable(subscriber_node src/subscriber_node.cpp) + +# Link executables with ROS 2 libraries +ament_target_dependencies(publisher_node rclcpp std_msgs) +ament_target_dependencies(subscriber_node rclcpp std_msgs visualization_msgs) + +# Install executables +install(TARGETS + publisher_node + subscriber_node + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/README.md b/README.md index c679671..c338a0e 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,74 @@ # Lidar-Scanner +# Obrada Lidar podataka +Obradu koordinata sam radio po ovom principu. Za kooridnate od lidara sam koristio simulator, taj deo možeš i da preskočiš. + +1. Učitavanje lidar podataka +2. Klasterovanje podataka +3. Proveravanje dimenzija klastera +4. Računanje brzine i ubrzanja + + +# Učitavanje lidar podataka +Simulator predstavlja jenostavnu puthon skriptu koja ima centralnu tačku i prati njene projekcije na svaku oblast u njenoj okolini. +Fomrat učitavanja podataka je (x, y) što bi trebalo da se izmeni da odgovara pravim lidar podacima. +Pretpostavke za lidar tačke + - Tačke se ciklično prosledjuju + - Dve tačke ne mogu imati isti ugao od centra do sebe tj. ne može jedna tačka biti iza druge + +![Logo](source/b.png) + +# Klasterovanje podataka + +Pristup klasterovanja već korišćen od strane ostalih + +![Logo](source/a.jpg) + + +```c++ +bool arePointsInSameCluster(const Point& p1, const Point& p2, double thresholdAngle, double radiusMultiplier) +{ + double angleDifference = std::fabs(p1.angle - p2.angle); + if (angleDifference > M_PI) + angleDifference = 2 * M_PI - angleDifference; + + double adaptiveThreshold = std::min(p1.r, p2.r) * std::sin(angleDifference) / std::sin(thresholdAngle - angleDifference); + double distance = std::sqrt(std::pow(p2.x - p1.x, 2) + std::pow(p2.y - p1.y, 2)); + + return distance <= adaptiveThreshold* radiusMultiplier; +} +``` + +Moj pristup samo ne koristimo ugao već razdaljinu + +```c++ +bool arePointsInSameCluster2(const Point& p1, const Point& p2, double thresholdAngle) +{ + + double distance = std::sqrt(std::pow(p2.x - p1.x, 2) + std::pow(p2.y - p1.y, 2)); + + return distance <= thresholdAngle; +} +``` + + + +# Proveravanje dimenzija klastera + +Posle toga odredjujemo da li selektovana erija pripada odredjenom području. +U suštini proveravano najdalj tačke u sva četiri smera da li se nalaze u odredjenom pravugaoniku kako bi bili sigurni da se radi o protivniku. + +# Računanje brzine i ubrzanja + +Za računanje ubrzanja i brzine prvo pronalazimo centar kruga i čuvamo ga sa trenutnim vremenom. +Treba proveriti na pravom primeru kako radi. +Čuvamo trenutnu i 2 prošle tačke koje su nam dovoljne da izračunamo ubrzanje i vektor kretanja. + + + +# Pokretanje + + colcon build + source install/setup.bash + ros2 run lidarProject subscriber_node \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..4ad36e6 --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + + + lidarProject + 0.0.0 + TODO: Package description + bogda + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/source/a.jpg b/source/a.jpg new file mode 100644 index 0000000..6ff91a3 Binary files /dev/null and b/source/a.jpg differ diff --git a/source/b.png b/source/b.png new file mode 100644 index 0000000..c3cc882 Binary files /dev/null and b/source/b.png differ diff --git a/src/lidar_points.txt b/src/lidar_points.txt new file mode 100644 index 0000000..f898312 --- /dev/null +++ b/src/lidar_points.txt @@ -0,0 +1,360 @@ +(800, 300) +(800, 306) +(800, 313) +(800, 320) +(800, 327) +(800, 335) +(800, 342) +(800, 349) +(800, 356) +(800, 363) +(800, 370) +(800, 377) +(800, 385) +(800, 392) +(800, 399) +(800, 407) +(800, 414) +(800, 422) +(800, 430) +(800, 438) +(800, 445) +(800, 453) +(800, 461) +(800, 469) +(800, 478) +(800, 486) +(800, 495) +(800, 503) +(800, 513) +(800, 522) +(800, 531) +(800, 540) +(800, 550) +(800, 559) +(800, 570) +(800, 580) +(800, 590) +(798, 600) +(784, 600) +(770, 600) +(757, 600) +(745, 600) +(733, 600) +(721, 600) +(710, 600) +(700, 600) +(690, 600) +(680, 600) +(670, 600) +(661, 600) +(651, 600) +(643, 600) +(634, 600) +(626, 600) +(618, 600) +(610, 600) +(602, 600) +(594, 600) +(587, 600) +(580, 600) +(573, 600) +(566, 600) +(559, 600) +(552, 600) +(546, 600) +(540, 600) +(533, 600) +(527, 600) +(521, 600) +(515, 600) +(509, 600) +(503, 600) +(497, 600) +(491, 600) +(486, 600) +(480, 600) +(474, 600) +(469, 600) +(463, 600) +(458, 600) +(452, 600) +(447, 600) +(442, 600) +(436, 600) +(431, 600) +(426, 600) +(420, 600) +(415, 600) +(410, 600) +(405, 600) +(400, 600) +(394, 600) +(389, 600) +(384, 600) +(379, 600) +(373, 600) +(368, 600) +(363, 600) +(357, 600) +(352, 600) +(347, 600) +(341, 600) +(336, 600) +(330, 600) +(325, 600) +(319, 600) +(313, 600) +(308, 600) +(302, 600) +(296, 600) +(290, 600) +(284, 600) +(278, 600) +(272, 600) +(266, 600) +(259, 600) +(253, 600) +(247, 600) +(240, 600) +(233, 600) +(226, 600) +(219, 600) +(212, 600) +(205, 600) +(197, 600) +(189, 600) +(181, 600) +(173, 600) +(165, 600) +(156, 600) +(148, 600) +(138, 600) +(129, 600) +(119, 600) +(109, 600) +(99, 600) +(89, 600) +(78, 600) +(66, 600) +(54, 600) +(42, 600) +(29, 600) +(15, 600) +(1, 600) +(0, 590) +(0, 580) +(0, 570) +(0, 559) +(0, 550) +(0, 540) +(0, 531) +(0, 522) +(0, 513) +(0, 503) +(0, 495) +(322, 335) +(328, 331) +(330, 329) +(333, 326) +(334, 325) +(335, 323) +(336, 321) +(337, 320) +(337, 319) +(338, 317) +(339, 316) +(338, 315) +(339, 313) +(340, 312) +(340, 311) +(340, 310) +(340, 309) +(340, 308) +(341, 307) +(341, 306) +(341, 305) +(341, 304) +(341, 303) +(341, 302) +(341, 301) +(341, 300) +(341, 298) +(341, 297) +(341, 296) +(341, 295) +(341, 294) +(340, 293) +(340, 292) +(340, 291) +(339, 290) +(339, 289) +(339, 288) +(338, 286) +(337, 285) +(337, 284) +(337, 283) +(335, 281) +(334, 280) +(334, 278) +(332, 276) +(331, 275) +(329, 272) +(325, 270) +(0, 130) +(0, 121) +(0, 113) +(0, 104) +(0, 96) +(0, 86) +(0, 77) +(0, 68) +(0, 59) +(0, 49) +(0, 40) +(0, 29) +(0, 19) +(0, 9) +(1, 0) +(15, 0) +(29, 0) +(42, 0) +(54, 0) +(66, 0) +(78, 0) +(89, 0) +(99, 0) +(109, 0) +(119, 0) +(129, 0) +(138, 0) +(148, 0) +(156, 0) +(165, 0) +(173, 0) +(181, 0) +(189, 0) +(197, 0) +(205, 0) +(212, 0) +(219, 0) +(226, 0) +(233, 0) +(240, 0) +(247, 0) +(253, 0) +(259, 0) +(266, 0) +(272, 0) +(278, 0) +(284, 0) +(290, 0) +(296, 0) +(302, 0) +(308, 0) +(313, 0) +(319, 0) +(325, 0) +(330, 0) +(336, 0) +(341, 0) +(347, 0) +(352, 0) +(357, 0) +(363, 0) +(368, 0) +(373, 0) +(379, 0) +(384, 0) +(389, 0) +(394, 0) +(399, 0) +(405, 0) +(410, 0) +(415, 0) +(420, 0) +(426, 0) +(431, 0) +(436, 0) +(442, 0) +(447, 0) +(452, 0) +(458, 0) +(463, 0) +(469, 0) +(474, 0) +(480, 0) +(486, 0) +(491, 0) +(497, 0) +(503, 0) +(509, 0) +(515, 0) +(521, 0) +(527, 0) +(533, 0) +(540, 0) +(546, 0) +(552, 0) +(559, 0) +(566, 0) +(573, 0) +(580, 0) +(587, 0) +(469, 192) +(468, 198) +(468, 202) +(468, 205) +(469, 208) +(469, 210) +(470, 212) +(471, 214) +(472, 216) +(473, 218) +(474, 220) +(475, 221) +(477, 222) +(478, 224) +(479, 225) +(481, 226) +(483, 227) +(485, 228) +(487, 229) +(489, 230) +(491, 230) +(494, 231) +(497, 231) +(501, 231) +(507, 230) +(800, 49) +(800, 59) +(800, 68) +(800, 77) +(800, 86) +(800, 96) +(800, 104) +(800, 113) +(800, 121) +(800, 130) +(800, 138) +(800, 146) +(800, 154) +(800, 161) +(800, 169) +(800, 177) +(800, 185) +(800, 192) +(800, 200) +(800, 207) +(800, 214) +(800, 222) +(800, 229) +(800, 236) +(800, 243) +(800, 250) +(800, 257) +(800, 264) +(800, 272) +(800, 279) +(800, 286) +(800, 293) diff --git a/src/my_node.cpp b/src/my_node.cpp new file mode 100644 index 0000000..4605201 --- /dev/null +++ b/src/my_node.cpp @@ -0,0 +1,18 @@ +#include "rclcpp/rclcpp.hpp" + +class MyNode : public rclcpp::Node +{ +public: + MyNode() : Node("my_node") + { + RCLCPP_INFO(this->get_logger(), "Hello, ROS 2!"); + } +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/publisher_node.cpp b/src/publisher_node.cpp new file mode 100644 index 0000000..ba95f45 --- /dev/null +++ b/src/publisher_node.cpp @@ -0,0 +1,90 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include +#include +#include +#include + +class PublisherNode : public rclcpp::Node +{ +public: + PublisherNode() : Node("publisher_node"), current_section_index_(0) + { + publisher_ = this->create_publisher("coordinates", 10); + + // Učitavanje sekcija iz fajla + load_sections_from_file("/mnt/c/Users/bogda/OneDrive/Desktop/lidarProject/src/lidar_points.txt"); + + // Postavljanje tajmera za slanje jedne sekcije po sekundi + timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&PublisherNode::publish_next_section, this) + ); + } + +private: + void load_sections_from_file(const std::string &filename) + { + std::ifstream infile(filename); + + if (!infile.is_open()) { + RCLCPP_ERROR(this->get_logger(), "Ne može se otvoriti fajl %s", filename.c_str()); + return; + } + + std::string line; + std::ostringstream current_section; + + while (std::getline(infile, line)) { + if (line == "NEW SECTION") { + // Sačuvaj trenutnu sekciju i započni novu + if (!current_section.str().empty()) { + sections_.push_back(current_section.str()); + current_section.str(""); // Resetuj sekciju + } + } else if (!line.empty()) { + // Dodaj liniju (koordinate) u trenutnu sekciju + current_section << line << "\n"; + } + } + + // Dodaj poslednju sekciju ako postoji + if (!current_section.str().empty()) { + sections_.push_back(current_section.str()); + } + + infile.close(); + } + + void publish_next_section() + { + if (sections_.empty()) { + RCLCPP_WARN(this->get_logger(), "Nema sekcija za slanje."); + return; + } + + // Proveri indeks i objavi trenutnu sekciju + std_msgs::msg::String message; + message.data = sections_[current_section_index_]; + publisher_->publish(message); + + RCLCPP_INFO(this->get_logger(), "Objavljena sekcija %zu:\n%s", current_section_index_ + 1, message.data.c_str()); + + + // Pređi na sledeću sekciju (kružni indeks) + current_section_index_ = (current_section_index_ + 1) % sections_.size(); + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + std::vector sections_; + size_t current_section_index_; +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/subscriber_node.cpp b/src/subscriber_node.cpp new file mode 100644 index 0000000..6954451 --- /dev/null +++ b/src/subscriber_node.cpp @@ -0,0 +1,339 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include +#include +#include +#include +#include + +struct Point +{ + double x; + double y; + double r; + double angle; +}; + +struct PointT +{ + double x; + double y; + double t; +}; + +struct Cluster { + + std::vector points; + Point southernmost_y = {0, std::numeric_limits::max()}; + Point northernmost_y = {0, std::numeric_limits::lowest()}; + Point westernmost_x = {std::numeric_limits::max(), 0}; + Point easternmost_x = {std::numeric_limits::lowest(), 0}; +}; + + +std::vector parseCoordinates(const std::string& data) +{ + std::vector points; + std::istringstream stream(data); + double x, y; + + std::string line; + while (std::getline(stream, line)) + { + + + if(line =="NEW SECTION") + { + continue; + } + + size_t xPos = line.find("("); + size_t yPos = line.find(","); + size_t yEnd = line.find(")"); + std::cout << line+"\n"; + + if (xPos != std::string::npos && yPos != std::string::npos) + { + + x = std::stod(line.substr(xPos + 1, yPos - (xPos + 1))); + y = std::stod(line.substr(yPos + 2, yEnd - (yPos + 2))); + + // Adjust for the new center at (400, 300) + double newX = x + 400; // Shift X by 400 to center at (400, 300) + double newY = y + 300; // Shift Y by 300 to center at (400, 300) + + // Output the adjusted coordinates + //std::cout << "Adjusted coordinates: " << std::to_string(newX) + " " + std::to_string(newY) + "\n"; + + // Calculate the distance from the new center (400, 300) + double r = std::sqrt(newX * newX + newY * newY); + //std::cout << "Distance (r): " << r << "\n"; + + // Calculate the angle from the new center (400, 300) + double angle = std::atan2(newY, newX); + //std::cout << "Angle: " << angle << "\n"; + + // Store the results: the adjusted coordinates, distance, and angle + points.push_back({newX, newY, r, angle}); + } + + std::cout << line+"\n"; + } + + return points; +} + +bool arePointsInSameCluster(const Point& p1, const Point& p2, double thresholdAngle, double radiusMultiplier) +{ + double angleDifference = std::fabs(p1.angle - p2.angle); + if (angleDifference > M_PI) + angleDifference = 2 * M_PI - angleDifference; + + double adaptiveThreshold = std::min(p1.r, p2.r) * std::sin(angleDifference) / std::sin(thresholdAngle - angleDifference); + double distance = std::sqrt(std::pow(p2.x - p1.x, 2) + std::pow(p2.y - p1.y, 2)); + + return distance <= adaptiveThreshold* radiusMultiplier; +} + +bool arePointsInSameCluster2(const Point& p1, const Point& p2, double thresholdAngle) +{ + + double distance = std::sqrt(std::pow(p2.x - p1.x, 2) + std::pow(p2.y - p1.y, 2)); + + return distance <= thresholdAngle; +} + +Point findCentroid(std::vector& points) { + double sumX = 0.0; + double sumY = 0.0; + + // Jedan prolaz kroz sve tačke da bismo sabrali koordinate + + for (const auto& point : points) { + sumX += point.x; + sumY += point.y; + } + + // Prosečne koordinate (centroid) + size_t n = points.size(); + return {sumX / n, sumY / n,0,0}; +} + +void findLineEquation(Point p1, Point p2, double& m, double& b) { + if (p1.x == p2.x) { + m = 0; + b = p1.x; + } else { + m = (p2.y - p1.y) / (p2.x - p1.x); + b = p1.y - m * p1.x; + } +} + +Point findIntersection(Point p1, Point p2, Point p3, Point p4) { + double m1, b1, m2, b2; + + findLineEquation(p1, p2, m1, b1); + findLineEquation(p3, p4, m2, b2); + + Point intersection; + + if (m1 == m2) { + std::cerr << "Prave su paralelne i nemaju presek." << std::endl; + return {0, 0}; + } + + // m1 * x + b1 = m2 * x + b2 + // (m1 - m2) * x = b2 - b1 + intersection.x = (b2 - b1) / (m1 - m2); + intersection.y = m1 * intersection.x + b1; + + return intersection; +} + +Point findCentroid2(Point p1, Point p2, Point p3, Point p4) { + double sumX = 0.0; + double sumY = 0.0; + + //Nadji najblizu tacku centru u ovom slucaju + double dist1 = std::sqrt(p1.x * p1.x + p1.y * p1.y); // Rastojanje od (0, 0) + double dist2 = std::sqrt(p2.x * p2.x + p2.y * p2.y); // Rastojanje od (0, 0) + double dist3 = std::sqrt(p3.x * p3.x + p3.y * p3.y); // Rastojanje od (0, 0) + double dist4 = std::sqrt(p4.x * p4.x + p4.y * p4.y); // Rastojanje od (0, 0) + + Point p = {0,0,0,0}; + + if (dist1 <= dist2 && dist1 <= dist3 && dist1 <= dist4) { + + // pronadji kvadrat p1, p3, p4 + return findIntersection(p1, p, p3, p4); + + } else if (dist2 <= dist3 && dist2 <= dist4) { + + // pronadji kvadrat p2, p3, p4 + return findIntersection(p2, p, p3, p4); + + } else if (dist3 <= dist4) { + + // pronadji kvadrat p3, p1, p2 + return findIntersection(p3, p, p1, p2); + + } else { + + // pronadji kvadrat p4, p1, p2 + return findIntersection(p4, p, p1, p2); + + } + +} + + +PointT calculateVelocity(const PointT& p1, const PointT& p2) { + return {p2.x - p1.x, p2.y - p1.y, p2.t - p1.t}; +} + +// Funkcija za izračunavanje ubrzanja između dva vektora kretanja +PointT calculateAcceleration(const PointT& p1, const PointT& p2, const PointT& p3) { + PointT velocity1 = calculateVelocity(p1, p2); + PointT velocity2 = calculateVelocity(p2, p3); + + return {velocity2.x - velocity1.x, velocity2.y - velocity1.y, velocity2.t - velocity1.t}; +} + +// Predikcija sledeće tačke na osnovu trenutne tačke, prethodne dve tačke i ubrzanja +PointT predictNextPoint(const PointT& p1, const PointT& p2, const PointT& p3) { + // Izračunavamo ubrzanje + PointT acceleration = calculateAcceleration(p1, p2, p3); + + // Predikcija nove tačke (x, y) uzimajući u obzir ubrzanje + double predictedX = p3.x + (p3.x - p2.x) + acceleration.x; + double predictedY = p3.y + (p3.y - p2.y) + acceleration.y; + double predictedT = p3.t + (p3.t - p2.t) + acceleration.t; + + return {predictedX, predictedY, predictedT}; +} + +PointT prev1 = {0.0, 0.0, 0.0}; // Prethodna tačka 1 + +PointT prev2 = {0.0, 0.0, 0.0}; // Prethodna tačka 2 + + +class SubscriberNode : public rclcpp::Node +{ +public: + SubscriberNode() : Node("subscriber_node") + { + + subscription_ = this->create_subscription( + "coordinates", 10, std::bind(&SubscriberNode::message_callback, this, std::placeholders::_1) + ); + } + +private: + void message_callback(const std_msgs::msg::String::SharedPtr msg) + { + std::vector points = parseCoordinates(msg->data); + + std::vector clusters; + Cluster current_cluster; + + double thresholdAngle = M_PI / 6; // Prag od 30 stepeni + double radiusMultiplier = 50; // Povećajte radius faktor + + double boxdimensions = 100; + + for (size_t i = 0; i < points.size(); ++i) + { + if (current_cluster.points.empty() || arePointsInSameCluster2(current_cluster.points.back(), points[i], radiusMultiplier)) + { + current_cluster.points.push_back(points[i]); + + // Ažuriraj ekstremne tačke + if (points[i].y < current_cluster.southernmost_y.y) { + current_cluster.southernmost_y = points[i]; + } + if (points[i].y > current_cluster.northernmost_y.y) { + current_cluster.northernmost_y = points[i]; + } + if (points[i].x < current_cluster.westernmost_x.x) { + current_cluster.westernmost_x = points[i]; + } + if (points[i].x > current_cluster.easternmost_x.x) { + current_cluster.easternmost_x = points[i]; + } + + + } + else + { + clusters.push_back(current_cluster); + current_cluster = Cluster{}; + current_cluster.points.push_back(points[i]); + + if (points[i].y < current_cluster.southernmost_y.y) { + current_cluster.southernmost_y = points[i]; + } + if (points[i].y > current_cluster.northernmost_y.y) { + current_cluster.northernmost_y = points[i]; + } + if (points[i].x < current_cluster.westernmost_x.x) { + current_cluster.westernmost_x = points[i]; + } + if (points[i].x > current_cluster.easternmost_x.x) { + current_cluster.easternmost_x = points[i]; + } + } + } + + if (!current_cluster.points.empty()) + { + clusters.push_back(current_cluster); + } + + // Ispis za svaki klaster + for (size_t i = 0; i < clusters.size(); ++i) + { + double vertical_distance = std::fabs(clusters[i].northernmost_y.y - clusters[i].southernmost_y.y); + double horizontal_distance = std::fabs(clusters[i].easternmost_x.x - clusters[i].westernmost_x.x); + + if(boxdimensions > vertical_distance && boxdimensions > horizontal_distance) + { + std::cout << "Klaster moze da bude protivnik " << i + 1 << ":\n"; + for (const auto& point : clusters[i].points) + { + std::cout << " Tacka (x=" << point.x << ", y=" << point.y << ")\n"; + } + + const auto& finalpoint = findCentroid2(clusters[i].easternmost_x,clusters[i].westernmost_x,clusters[i].northernmost_y,clusters[i].southernmost_y); + std::cout << " FINALNA (x=" << finalpoint.x << ", y=" << finalpoint.y << ")\n"; + + //Pretpostavka da ce biti samo jedan ovakav objekat + + PointT current; + current.x = finalpoint.x; + current.y = finalpoint.y; + current.t = 0.0; + + PointT predicted = predictNextPoint(prev1, prev2, current); + + std::cout << "Predvidjena sledeca tacka: (" + << predicted.x << ", " << predicted.y << ", " << predicted.t << ")" << std::endl; + + prev1 = prev2; + prev2 = current; + + break; + } + + } + } + + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}