diff --git a/mg_lidar/CMakeLists.txt b/mg_lidar/CMakeLists.txt new file mode 100644 index 0000000..da9e8a2 --- /dev/null +++ b/mg_lidar/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.12) +project(mg_lidar) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(PACKAGE_DEPS + rclcpp + tf2 + tf2_ros + tf2_geometry_msgs + mg_msgs + sensor_msgs + ) + +find_package(ament_cmake REQUIRED) + +include(FindPkgConfig) +pkg_search_module(LIBGLM REQUIRED glm) + +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +set(SOURCES + src/scanner.cpp +) + +add_executable(mg_scanner ${SOURCES}) + +target_include_directories( + mg_scanner + PRIVATE + ${LIBGLM_INCLUDE_DIRS} + include +) + +ament_target_dependencies(mg_scanner ${PACKAGE_DEPS}) + +install( TARGETS + mg_scanner + DESTINATION lib/${PROJECT_NAME} +) + +# install(DIRECTORY +# mg_scanner +# config +# launch +# DESTINATION share/${PROJECT_NAME}/ +# ) + +target_compile_features(mg_scanner PUBLIC + c_std_99 + cxx_std_17 +) # Require C99 and C++17 + +ament_package() \ No newline at end of file diff --git a/mg_lidar/package.xml b/mg_lidar/package.xml new file mode 100644 index 0000000..ba393e9 --- /dev/null +++ b/mg_lidar/package.xml @@ -0,0 +1,27 @@ + + + + mg_lidar + 0.0.0 + Lidar based opponent tracking + Pimpest + Apache 2.0 + + ament_cmake + + rclcpp + mg_msgs + std_msgs + tf2 + tf2_ros + tf2_geometry_msgs + visualization_msgs + sensor_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/mg_lidar/src/scanner.cpp b/mg_lidar/src/scanner.cpp new file mode 100644 index 0000000..5c8d3a5 --- /dev/null +++ b/mg_lidar/src/scanner.cpp @@ -0,0 +1,134 @@ +#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 + +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( + "scan", rclcpp::QoS(1), [this](LaserScan::ConstSharedPtr msg) { process_scan(msg); }); + enemy_pub_ = create_publisher("/dynamicObstacle", rclcpp::QoS(1)); + + tf_buf_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buf_); + } + + private: + rclcpp::Subscription::SharedPtr scan_sup_; + rclcpp::Publisher::SharedPtr enemy_pub_; + std::shared_ptr tf_buf_; + std::shared_ptr tf_listener_; + + std::vector 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()); + rclcpp::shutdown(); +} \ No newline at end of file diff --git a/mg_obstacles/src/mg_obstacles.cpp b/mg_obstacles/src/mg_obstacles.cpp index 9e564e8..9bce3f1 100644 --- a/mg_obstacles/src/mg_obstacles.cpp +++ b/mg_obstacles/src/mg_obstacles.cpp @@ -153,7 +153,7 @@ namespace mg { double ObstacleManager::dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, double rot) { // Find me - const glm::dmat2 rot_mat{ { glm::cos(rot), -glm::sin(rot) }, { glm::sin(rot), glm::cos(rot) } }; + const glm::dmat2 rot_mat{ { glm::cos(rot), glm::sin(rot) }, { -glm::sin(rot), glm::cos(rot) } }; return ObstacleManager::dist_to_bounds(pos, size, rot_mat); }