From 97f08804d80f174f210b5f1f96dab6fc788510c5 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 25 Mar 2026 14:02:10 +0100 Subject: [PATCH] Finised lidar node --- toid_lidar/CMakeLists.txt | 1 + .../include/toid_lidar/toid_lidar_node.hpp | 13 +- toid_lidar/package.xml | 2 + toid_lidar/src/toid_lidar.cpp | 2 +- toid_lidar/src/toid_lidar_node.cpp | 121 +++++++++++++++++- toid_msgs/CMakeLists.txt | 4 + toid_msgs/msg/Rival.msg | 2 + toid_msgs/package.xml | 2 + 8 files changed, 133 insertions(+), 14 deletions(-) create mode 100644 toid_msgs/msg/Rival.msg diff --git a/toid_lidar/CMakeLists.txt b/toid_lidar/CMakeLists.txt index 61ae729..41afecf 100644 --- a/toid_lidar/CMakeLists.txt +++ b/toid_lidar/CMakeLists.txt @@ -19,6 +19,7 @@ set(PACKAGE_DEPS tf2_eigen tf2_geometry_msgs toid_msgs + visualization_msgs ) foreach(PACKAGE ${PACKAGE_DEPS}) diff --git a/toid_lidar/include/toid_lidar/toid_lidar_node.hpp b/toid_lidar/include/toid_lidar/toid_lidar_node.hpp index 084b63a..c86f0aa 100644 --- a/toid_lidar/include/toid_lidar/toid_lidar_node.hpp +++ b/toid_lidar/include/toid_lidar/toid_lidar_node.hpp @@ -7,6 +7,7 @@ #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" #include "toid_msgs/msg/rival.hpp" +#include "visualization_msgs/msg/marker.hpp" #include "vector" namespace toid @@ -17,27 +18,27 @@ class ToidRivalDetect : public rclcpp::Node using LaserScan = sensor_msgs::msg::LaserScan; using Rival = toid_msgs::msg::Rival; using PointStamped = geometry_msgs::msg::PointStamped; + using Marker = visualization_msgs::msg::Marker; public: - static void generate_rotation_matricies(std::vector &) {} - ToidRivalDetect(); - - static std::vector rotations; private: void process_scan(LaserScan::ConstSharedPtr msg); rclcpp::Subscription::SharedPtr scan_sub_; rclcpp::Publisher::SharedPtr rival_pub_; - rclcpp::Publisher::SharedPtr point_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; std::shared_ptr tf_buf_; std::shared_ptr tf_listener_; - std::string lidar_frame_ = "lidar"; std::string base_frame_ = "base_footprint"; std::string global_frame_ = "map"; + double map_width_ = 3.0; + double map_height_ = 2.0; + bool visualize_ = false; + bool closest_ = false; rclcpp::Logger logger_ = get_logger(); rclcpp::Clock::SharedPtr clock_; diff --git a/toid_lidar/package.xml b/toid_lidar/package.xml index b5004ec..0b47a33 100644 --- a/toid_lidar/package.xml +++ b/toid_lidar/package.xml @@ -18,6 +18,8 @@ tf2_ros tf2_geometry_msgs toid_msgs + visualization_msgs + rplidar diff --git a/toid_lidar/src/toid_lidar.cpp b/toid_lidar/src/toid_lidar.cpp index 450a812..5cf2fe2 100644 --- a/toid_lidar/src/toid_lidar.cpp +++ b/toid_lidar/src/toid_lidar.cpp @@ -3,7 +3,7 @@ int main(const int argc,const char** argv) { rclcpp::init(argc,argv); - + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/toid_lidar/src/toid_lidar_node.cpp b/toid_lidar/src/toid_lidar_node.cpp index 901dc6a..52062cf 100644 --- a/toid_lidar/src/toid_lidar_node.cpp +++ b/toid_lidar/src/toid_lidar_node.cpp @@ -1,6 +1,7 @@ #include "toid_lidar/toid_lidar_node.hpp" #include "Eigen/Eigen" +#include "Eigen/Geometry" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -15,6 +16,7 @@ ToidRivalDetect::ToidRivalDetect() : rclcpp::Node("RivalDetect") scan_sub_ = create_subscription( "scan", rclcpp::QoS(1), [this](LaserScan::ConstSharedPtr msg) { this->process_scan(msg); }); rival_pub_ = create_publisher("/dynamicObstacle", rclcpp::QoS(1)); + marker_pub_ = create_publisher("/rivalMaker", rclcpp::QoS(1)); tf_buf_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buf_); @@ -22,21 +24,126 @@ ToidRivalDetect::ToidRivalDetect() : rclcpp::Node("RivalDetect") clock_ = this->get_clock(); logger_ = this->get_logger(); - nav2_util::declare_parameter_if_not_declared( - this, "lidar_frame", rclcpp::ParameterValue("lidar")); - this->get_parameter("lidar_frame", lidar_frame_); - nav2_util::declare_parameter_if_not_declared(this, "global_frame", rclcpp::ParameterValue("map")); this->get_parameter("global_frame", global_frame_); + + nav2_util::declare_parameter_if_not_declared(this, "map_width", rclcpp::ParameterValue(3.0)); + this->get_parameter("map_width", map_width_); + + nav2_util::declare_parameter_if_not_declared(this, "map_height", rclcpp::ParameterValue(2.0)); + this->get_parameter("map_height", map_height_); + + nav2_util::declare_parameter_if_not_declared(this, "draw_markers", rclcpp::ParameterValue(false)); + this->get_parameter("draw_markers", visualize_); + + nav2_util::declare_parameter_if_not_declared(this, "closest", rclcpp::ParameterValue(false)); + this->get_parameter("closest", closest_); } void ToidRivalDetect::process_scan(LaserScan::ConstSharedPtr msg) { - geometry_msgs::msg::PoseStamped curr_pose; Eigen::Isometry3d pose; - if (!nav2_util::getCurrentPose(curr_pose, *tf_buf_, global_frame_, lidar_frame_)) { - RCLCPP_ERROR(this->logger_, "Could not get current transform"); + + geometry_msgs::msg::TransformStamped transform; + + try { + transform = tf_buf_->lookupTransform(global_frame_, msg->header.frame_id, msg->header.stamp); + } catch (const tf2::TransformException & e) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "TF Link missing: %s", e.what()); + return; } + + Eigen::Isometry3d iso = tf2::transformToEigen(transform); + + double min_dist = INFINITY; + + toid_msgs::msg::Rival rival; + rival.header.frame_id = global_frame_; + rival.header.stamp = msg->header.stamp; + + auto marker_loan = marker_pub_->borrow_loaned_message(); + Marker & marker = marker_loan.get(); + if(visualize_) { + marker.header = rival.header; + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + marker.type = Marker::SPHERE_LIST; + marker.frame_locked = true; + marker.lifetime.sec = 1; + marker.color.a = 1.0; + marker.color.g = 1.0; + marker.id = 1; + } + + Eigen::Vector3d accum(0, 0, 0); + Eigen::Vector3d start_point(0, 0, 0); + + int points = 0; + + for (size_t i = 0; i < msg->ranges.size(); ++i) { + double range = msg->ranges[i]; + double intensity = msg->intensities[i]; + + if (range < msg->range_min || range > msg->range_max) continue; + if (intensity < 35.0) continue; + + double angle = msg->angle_min + (i * msg->angle_increment); + + Eigen::Vector3d local_point(range * std::cos(angle), range * std::sin(angle), 0.0); + Eigen::Vector3d map_point = iso * local_point; + + if ( + std::fabs(map_point.x()) < map_width_ / 2.0 && std::fabs(map_point.y()) < map_height_ / 2.0) { + if ((start_point - map_point).dot(start_point - map_point) < 0.025) { + accum += map_point; + points++; + } else { + if (points != 0 && !closest_) { + geometry_msgs::msg::Point p; + accum /= points; + tf2::convert(accum, p); + if(visualize_){ + marker.points.push_back(p); + } + rival.point.push_back(p); + } + points = 1; + accum = map_point; + start_point = map_point; + } + + if(range < min_dist && closest_) { + min_dist = range; + if(rival.point.empty()) { + if(visualize_){ + marker.points.emplace_back(); + } + rival.point.emplace_back(); + } + tf2::convert(map_point, rival.point.front()); + if(visualize_) { + tf2::convert(map_point, marker.points.front()); + } + } + } + } + + if (points != 0 && !closest_) { + geometry_msgs::msg::Point p; + accum /= points; + tf2::convert(accum, p); + if(visualize_){ + marker.points.push_back(p); + } + rival.point.push_back(p); + } + + if(visualize_) { + marker_pub_->publish(std::move(marker_loan)); + } + rival_pub_->publish(rival); } } // namespace toid \ No newline at end of file diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index a0a4761..a505d52 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -9,11 +9,15 @@ endif() find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) + rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Rival.msg" "srv/SendDouble.srv" "action/SimpleMoveCoords.action" "action/SimpleRotate.action" "action/SimpleTranslateX.action" + DEPENDENCIES geometry_msgs ) ament_package() \ No newline at end of file diff --git a/toid_msgs/msg/Rival.msg b/toid_msgs/msg/Rival.msg new file mode 100644 index 0000000..06f6a29 --- /dev/null +++ b/toid_msgs/msg/Rival.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +geometry_msgs/Point[] point \ No newline at end of file diff --git a/toid_msgs/package.xml b/toid_msgs/package.xml index dc19523..2f16062 100644 --- a/toid_msgs/package.xml +++ b/toid_msgs/package.xml @@ -10,6 +10,8 @@ ament_cmake rosidl_default_generators + geometry_msgs + geometry_msgs rosidl_default_runtime rosidl_interface_packages