134 lines
4.6 KiB
C++
134 lines
4.6 KiB
C++
#include "rclcpp/rclcpp.hpp"
|
|
|
|
#include "sensor_msgs/msg/laser_scan.hpp"
|
|
#include "geometry_msgs/msg/point_stamped.hpp"
|
|
#include "glm/glm.hpp"
|
|
|
|
#include "tf2_ros/buffer.h"
|
|
#include "tf2_ros/transform_listener.h"
|
|
#include "tf2/convert.hpp"
|
|
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
|
#include <glm/gtx/norm.hpp>
|
|
|
|
class MgScanner : public rclcpp::Node {
|
|
using LaserScan = sensor_msgs::msg::LaserScan;
|
|
using PointStamped = geometry_msgs::msg::PointStamped;
|
|
|
|
public:
|
|
MgScanner() : rclcpp::Node("EnemyScanner") {
|
|
gen_rotations();
|
|
scan_sup_ = create_subscription<LaserScan>(
|
|
"scan", rclcpp::QoS(1), [this](LaserScan::ConstSharedPtr msg) { process_scan(msg); });
|
|
enemy_pub_ = create_publisher<PointStamped>("/dynamicObstacle", rclcpp::QoS(1));
|
|
|
|
tf_buf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
|
|
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);
|
|
}
|
|
|
|
private:
|
|
rclcpp::Subscription<LaserScan>::SharedPtr scan_sup_;
|
|
rclcpp::Publisher<PointStamped>::SharedPtr enemy_pub_;
|
|
std::shared_ptr<tf2_ros::Buffer> tf_buf_;
|
|
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
|
|
|
std::vector<glm::dmat2> rotations;
|
|
|
|
void gen_rotations() {
|
|
constexpr double min_angle = -3.1241393089294434;
|
|
constexpr double max_angle = 3.1415927410125732;
|
|
constexpr double angle_increment = 0.008714509196579456;
|
|
|
|
double curr = min_angle;
|
|
while (curr <= max_angle) {
|
|
rotations.emplace_back(glm::cos(curr), glm::sin(curr), -glm::sin(curr), glm::cos(curr));
|
|
curr += angle_increment;
|
|
}
|
|
|
|
rotations.emplace_back(glm::cos(curr), glm::sin(curr), -glm::sin(curr), glm::cos(curr));
|
|
}
|
|
|
|
glm::dvec3 pos_query() {
|
|
RCLCPP_ERROR(get_logger(), "Works to here");
|
|
try {
|
|
double x = NAN;
|
|
double y = NAN;
|
|
double rot = NAN;
|
|
|
|
auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero);
|
|
|
|
tf2::Transform t;
|
|
tf2::convert(tmsg.transform, t);
|
|
t.getBasis().getRPY(x, y, rot);
|
|
auto vec3 = tmsg.transform.translation;
|
|
|
|
return { vec3.x, vec3.y, rot };
|
|
|
|
} catch (const tf2::TransformException& e) {
|
|
RCLCPP_ERROR(get_logger(), "Got following error when looking up transform:\n\t%s", e.what());
|
|
}
|
|
return { 0, 0, 0 };
|
|
}
|
|
|
|
static bool part_of_clump(const glm::dvec2 prev, glm::dvec2 curr) {
|
|
return glm::length2(prev - curr) < 0.05 * 0.05;
|
|
}
|
|
|
|
void process_scan(LaserScan::ConstSharedPtr msg) {
|
|
// TODO: finish processing
|
|
const glm::dvec3 v = pos_query();
|
|
const glm::dvec2 pos = { v.x, v.y };
|
|
const glm::dmat2 rot = { glm::cos(v.z), glm::sin(v.z), -glm::sin(v.z), glm::cos(v.z) };
|
|
|
|
double mini = INFINITY;
|
|
|
|
glm::dvec2 mini_pos = { 0, 0 };
|
|
glm::dvec2 prev = { -1, -1 };
|
|
|
|
glm::dvec2 clump = { -1, -1 };
|
|
int clump_size = 0;
|
|
|
|
bool good_clump = false;
|
|
|
|
if (msg->ranges.size() != rotations.size()) {
|
|
RCLCPP_ERROR(get_logger(),
|
|
"The amount of rotations differs from amount of ranges(%lu != %lu)",
|
|
msg->ranges.size(),
|
|
rotations.size());
|
|
}
|
|
|
|
for (uint i = 0; i < msg->ranges.size(); i++) {
|
|
if (msg->intensities.at(i) < 35.0) {
|
|
continue;
|
|
}
|
|
const glm::dvec2 potential_pos = rot * rotations.at(i) * glm::dvec2{ msg->ranges.at(i), 0 } + pos;
|
|
if (0.1 < potential_pos.x && potential_pos.x < 2.9 && 0.1 < potential_pos.y && potential_pos.y < 1.9) {
|
|
if (!part_of_clump(prev, potential_pos)) {
|
|
clump_size = 0;
|
|
clump = { 0, 0 };
|
|
}
|
|
good_clump |= mini > msg->ranges.at(i);
|
|
clump += potential_pos;
|
|
clump_size++;
|
|
if (good_clump) {
|
|
mini = msg->ranges.at(i);
|
|
mini_pos = clump / (double)clump_size;
|
|
}
|
|
}
|
|
prev = potential_pos;
|
|
}
|
|
if (mini < INFINITY) {
|
|
PointStamped m;
|
|
m.header.frame_id = "odom";
|
|
m.point.x = mini_pos.x;
|
|
m.point.y = mini_pos.y;
|
|
|
|
enemy_pub_->publish(m);
|
|
}
|
|
}
|
|
};
|
|
|
|
int main(const int argc, const char** argv) {
|
|
rclcpp::init(argc, argv);
|
|
rclcpp::spin(std::make_shared<MgScanner>());
|
|
rclcpp::shutdown();
|
|
} |