Created toid_lidar node

toid_lidar/src/toid_lidar.cpp

@@ -0,0 +1,9 @@
+#include "rclcpp/rclcpp.hpp"
+#include "toid_lidar/toid_lidar_node.hpp"
+
+int main(const int argc,const char** argv) {
+    rclcpp::init(argc,argv);
+
+    rclcpp::shutdown();
+    return 0;
+}

toid_lidar/src/toid_lidar_node.cpp

@@ -0,0 +1,42 @@
+#include "toid_lidar/toid_lidar_node.hpp"
+
+#include "Eigen/Eigen"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "tf2/convert.hpp"
+#include "tf2_eigen/tf2_eigen.hpp"
+
+namespace toid
+{
+
+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));
+
+  tf_buf_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);
+
+  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_);
+}
+
+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");
+  }
+}
+
+}  // namespace toid