Magnezijum-Robotics/Lidar-Scanner
2
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update pt1

Browse Source
This commit is contained in:
BogBogdan
2024-11-13 17:08:31 +01:00
parent 47787c5279
commit aa633e9020
9 changed files with 920 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
CMakeLists.txt Normal file
View File

@ -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()

71
README.md
View File

@ -1,3 +1,74 @@
# Lidar-Scanner # 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

18
package.xml Normal file
View File

@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>lidarProject</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="bogda@todo.todo">bogda</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

BIN
source/a.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 482 KiB

BIN
source/b.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 18 KiB

360
src/lidar_points.txt Normal file
View File

@ -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)

18
src/my_node.cpp Normal file
View File

@ -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<MyNode>());
rclcpp::shutdown();
return 0;
}

90
src/publisher_node.cpp Normal file
View File

@ -0,0 +1,90 @@
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
class PublisherNode : public rclcpp::Node
{
public:
PublisherNode() : Node("publisher_node"), current_section_index_(0)
{
publisher_ = this->create_publisher<std_msgs::msg::String>("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<std_msgs::msg::String>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
std::vector<std::string> sections_;
size_t current_section_index_;
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<PublisherNode>());
rclcpp::shutdown();
return 0;
}

339
src/subscriber_node.cpp Normal file
View File

@ -0,0 +1,339 @@
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <sstream>
#include <iostream>
#include <vector>
#include <cmath>
#include <limits>
struct Point
{
double x;
double y;
double r;
double angle;
};
struct PointT
{
double x;
double y;
double t;
};
struct Cluster {
std::vector<Point> points;
Point southernmost_y = {0, std::numeric_limits<double>::max()};
Point northernmost_y = {0, std::numeric_limits<double>::lowest()};
Point westernmost_x = {std::numeric_limits<double>::max(), 0};
Point easternmost_x = {std::numeric_limits<double>::lowest(), 0};
};
std::vector<Point> parseCoordinates(const std::string& data)
{
std::vector<Point> 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<Point>& 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<std_msgs::msg::String>(
"coordinates", 10, std::bind(&SubscriberNode::message_callback, this, std::placeholders::_1)
);
}
private:
void message_callback(const std_msgs::msg::String::SharedPtr msg)
{
std::vector<Point> points = parseCoordinates(msg->data);
std::vector<Cluster> 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<std_msgs::msg::String>::SharedPtr subscription_;
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<SubscriberNode>());
rclcpp::shutdown();
return 0;
}