Finised lidar node

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<toid::ToidRivalDetect>());
     rclcpp::shutdown();
     return 0;
 }

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<LaserScan>(
     "scan", rclcpp::QoS(1), [this](LaserScan::ConstSharedPtr msg) { this->process_scan(msg); });
   rival_pub_ = create_publisher<Rival>("/dynamicObstacle", rclcpp::QoS(1));
+  marker_pub_ = create_publisher<Marker>("/rivalMaker", rclcpp::QoS(1));
 
   tf_buf_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*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