91 lines
2.7 KiB
C++
91 lines
2.7 KiB
C++
//
|
|
// Created by bastian on 26.03.22.
|
|
//
|
|
|
|
#include "GenerateXYPose.h"
|
|
|
|
double BT::GenerateXYPose::getArea(const std::shared_ptr<Area> &area, unsigned long id) {
|
|
Position2D a = area->triangles[id];
|
|
Position2D b = area->triangles[id + 1];
|
|
Position2D c = area->triangles[id + 2];
|
|
|
|
return 0.5 * ((b.x - a.x) * (c.y - a.y) - (c.x - a.x) * (b.y - a.y));
|
|
}
|
|
|
|
Position2D BT::GenerateXYPose::getRandomPosInTriangle(const std::shared_ptr<Area> &area, unsigned long id) {
|
|
Position2D a = area->triangles[id];
|
|
Position2D b = area->triangles[id + 1];
|
|
Position2D c = area->triangles[id + 2];
|
|
|
|
Position2D ab = {b.x - a.x, b.y - a.y};
|
|
Position2D ac = {c.x - a.x, c.y - a.y};
|
|
|
|
double u1 = dist(re);
|
|
double u2 = dist(re);
|
|
|
|
if (u1 + u2 > 1) {
|
|
u1 = 1 - u1;
|
|
u2 = 1 - u2;
|
|
}
|
|
|
|
return {a.x + (u1 * ab.x + u2 * ac.x), a.y + (u1 * ab.y + u2 * ac.y)};
|
|
}
|
|
|
|
BT::GenerateXYPose::GenerateXYPose(const std::string &name, const BT::NodeConfiguration &config) :
|
|
SyncActionNode(name, config) {
|
|
}
|
|
|
|
BT::PortsList BT::GenerateXYPose::providedPorts() {
|
|
return {
|
|
InputPort<std::shared_ptr<Area>>("area", "area of pose"),
|
|
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", "generated pose")
|
|
};
|
|
}
|
|
|
|
BT::NodeStatus BT::GenerateXYPose::tick() {
|
|
std::shared_ptr<Area> area;
|
|
getInput("area", area);
|
|
|
|
unsigned long triangleCount = area->triangles.size() - 2;
|
|
|
|
uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
|
|
std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed >> 32)};
|
|
|
|
std::vector<double> weights(triangleCount);
|
|
double sum = 0;
|
|
for (unsigned long i = 0; i < triangleCount; ++i) {
|
|
double currentArea = getArea(area,i);
|
|
weights[i] = currentArea;
|
|
sum += currentArea;
|
|
}
|
|
for (unsigned long i = 0; i < triangleCount; ++i) {
|
|
weights[i] = weights[i] / sum;
|
|
}
|
|
sum = 0;
|
|
for (unsigned long i = 0; i < triangleCount; ++i) {
|
|
weights[i] = weights[i] + sum;
|
|
sum += weights[i];
|
|
}
|
|
|
|
double triangleRandom = dist(re);
|
|
unsigned long i = 0;
|
|
while (triangleRandom > weights[i + 1] && i < weights.size() - 1) {
|
|
i++;
|
|
}
|
|
auto pos = getRandomPosInTriangle(area,i);
|
|
|
|
auto randomPose = std::make_shared<geometry_msgs::msg::Pose>();
|
|
randomPose->position.x = pos.x;
|
|
randomPose->position.y = pos.y;
|
|
randomPose->position.z = 0;
|
|
randomPose->orientation.w = 0;
|
|
randomPose->orientation.x = 0;
|
|
randomPose->orientation.y = 0;
|
|
randomPose->orientation.z = 0;
|
|
|
|
std::cout << "Generated Pose." << std::endl;
|
|
|
|
setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", randomPose);
|
|
return NodeStatus::SUCCESS;
|
|
}
|