update pt1
This commit is contained in:
BogBogdan
24
CMakeLists.txt
Normal file
24
CMakeLists.txt
Normal 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
71
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
|
||||
|
||||
|
||||
|
||||
# Klasterovanje podataka
|
||||
|
||||
Pristup klasterovanja već korišćen od strane ostalih
|
||||
|
||||
|
||||
|
||||
|
||||
```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
18
package.xml
Normal 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
BIN
source/a.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 482 KiB |
BIN
source/b.png
Normal file
BIN
source/b.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 18 KiB |
360
src/lidar_points.txt
Normal file
360
src/lidar_points.txt
Normal 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
18
src/my_node.cpp
Normal 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
90
src/publisher_node.cpp
Normal 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
339
src/subscriber_node.cpp
Normal 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;
|
||||
}
|
||||
Reference in New Issue
Block a user