toid_lidar/CMakeLists.txt

@@ -19,6 +19,7 @@ set(PACKAGE_DEPS
   tf2_eigen
   tf2_geometry_msgs
   toid_msgs
+  visualization_msgs
 )
 
 foreach(PACKAGE ${PACKAGE_DEPS})

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<Eigen::Matrix4d> &) {}
-
   ToidRivalDetect();
-
-  static std::vector<Eigen::Matrix3d> rotations;
 private:
 
   void process_scan(LaserScan::ConstSharedPtr msg);
 
   rclcpp::Subscription<LaserScan>::SharedPtr scan_sub_;
   rclcpp::Publisher<Rival>::SharedPtr rival_pub_;
-  rclcpp::Publisher<PointStamped>::SharedPtr point_pub_;
+  rclcpp::Publisher<Marker>::SharedPtr marker_pub_;
 
   std::shared_ptr<tf2_ros::Buffer> tf_buf_;
   std::shared_ptr<tf2_ros::TransformListener> 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_;

toid_lidar/package.xml

@@ -18,6 +18,8 @@
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>toid_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>rplidar</depend>
 
 
 

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

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()

toid_msgs/msg/Rival.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+geometry_msgs/Point[] point

toid_msgs/package.xml

@@ -10,6 +10,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>