Applied agnocast to /map/pointcloud_map topic

Signed-off-by: Koichi Imai <[email protected]>

localization/ndt_scan_matcher/CMakeLists.txt

@@ -30,6 +30,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake_auto REQUIRED)
+
+find_package(agnocastlib REQUIRED)
+
 ament_auto_find_build_dependencies()
 
 ament_auto_add_executable(ndt_scan_matcher
@@ -39,6 +42,11 @@ ament_auto_add_executable(ndt_scan_matcher
   src/util_func.cpp
 )
 
+ament_target_dependencies(${PROJECT_NAME} agnocastlib)
+target_include_directories(${PROJECT_NAME} PRIVATE
+  ${agnocastlib_INCLUDE_DIRS}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -18,6 +18,7 @@
 #define FMT_HEADER_ONLY
 
 #include "ndt_scan_matcher/particle.hpp"
+#include "agnocast.hpp"
 
 #include <ndt/omp.hpp>
 #include <ndt/pcl_generic.hpp>
@@ -100,7 +101,9 @@ class NDTScanMatcher : public rclcpp::Node
     const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
     tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
 
-  void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
+  // void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
+   void callbackMapPoints(agnocast::ipc_shared_ptr<sensor_msgs::msg::PointCloud2> pointcloud2_msg_ptr);
+
   void callbackSensorPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
   void callbackInitialPose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
@@ -127,7 +130,8 @@ class NDTScanMatcher : public rclcpp::Node
   void timerDiagnostic();
 
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
+  //rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
+  agnocast::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_aligned_pose_pub_;

localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml

@@ -12,6 +12,9 @@
   <arg name="node_name" default="ndt_scan_matcher" description="Use a different name for this node"/>
 
   <node pkg="ndt_scan_matcher" exec="ndt_scan_matcher" name="$(var node_name)" output="log">
+     <env name="LD_PRELOAD" value="libagnocast_heaphook.so:$(env LD_PRELOAD '')" />
+    <env name="MEMPOOL_SIZE" value="32212254720" /> <!-- 30GB -->
+
     <remap from="points_raw" to="$(var input/pointcloud)"/>
 
     <remap from="ekf_pose_with_covariance" to="$(var input_initial_pose_topic)"/>

localization/ndt_scan_matcher/package.xml

@@ -28,6 +28,8 @@
   <depend>tier4_localization_msgs</depend>
   <depend>visualization_msgs</depend>
 
+  <depend>agnocastlib</depend>
+
   <test_depend>ament_cmake_cppcheck</test_depend>
   <test_depend>ament_lint_auto</test_depend>
 

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -30,6 +30,7 @@
 #include <functional>
 #include <iomanip>
 #include <thread>
+#include <stdlib.h>
 
 namespace tier4_autoware_utils
 {
@@ -238,13 +239,22 @@ NDTScanMatcher::NDTScanMatcher()
   auto main_sub_opt = rclcpp::SubscriptionOptions();
   main_sub_opt.callback_group = main_callback_group;
 
+  auto agnocast_main_sub_opt = agnocast::SubscriptionOptions();
+  agnocast_main_sub_opt.callback_group = main_callback_group;
+
   initial_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "ekf_pose_with_covariance", 100,
     std::bind(&NDTScanMatcher::callbackInitialPose, this, std::placeholders::_1),
     initial_pose_sub_opt);
-  map_points_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-    "pointcloud_map", rclcpp::QoS{1}.transient_local(),
-    std::bind(&NDTScanMatcher::callbackMapPoints, this, std::placeholders::_1), main_sub_opt);
+  //map_points_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+    //"pointcloud_map", rclcpp::QoS{1}.transient_local(),
+    //std::bind(&NDTScanMatcher::callbackMapPoints, this, std::placeholders::_1), main_sub_opt);
+
+  map_points_sub_ = agnocast::create_subscription<sensor_msgs::msg::PointCloud2>(
+    get_node_base_interface(),
+    "/pointcloud_map_agnocast", rclcpp::QoS{1}.transient_local(),
+    std::bind(&NDTScanMatcher::callbackMapPoints, this, std::placeholders::_1), agnocast_main_sub_opt);
+
   sensor_points_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
     "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size),
     std::bind(&NDTScanMatcher::callbackSensorPoints, this, std::placeholders::_1), main_sub_opt);
@@ -289,6 +299,7 @@ NDTScanMatcher::NDTScanMatcher()
 
   diagnostic_thread_ = std::thread(&NDTScanMatcher::timerDiagnostic, this);
   diagnostic_thread_.detach();
+
 }
 
 void NDTScanMatcher::timerDiagnostic()
@@ -346,6 +357,8 @@ void NDTScanMatcher::serviceNDTAlign(
   const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
   tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res)
 {
+  //std::lock_guard<std::mutex> lock(ndt_map_mtx_);
+
   // get TF from pose_frame to map_frame
   auto TF_pose_to_map_ptr = std::make_shared<geometry_msgs::msg::TransformStamped>();
   getTransform(map_frame_, req->pose_with_covariance.header.frame_id, TF_pose_to_map_ptr);
@@ -410,8 +423,13 @@ void NDTScanMatcher::callbackInitialPose(
 }
 
 void NDTScanMatcher::callbackMapPoints(
-  sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
+  //sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
+  agnocast::ipc_shared_ptr<sensor_msgs::msg::PointCloud2> map_points_msg_ptr)
 {
+  //std::lock_guard<std::mutex> lock(ndt_map_mtx_);
+
+  RCLCPP_INFO(this->get_logger(), "subscribe pointcloud map ");
+
   const auto trans_epsilon = ndt_ptr_->getTransformationEpsilon();
   const auto step_size = ndt_ptr_->getStepSize();
   const auto resolution = ndt_ptr_->getResolution();
@@ -452,6 +470,8 @@ void NDTScanMatcher::callbackMapPoints(
 void NDTScanMatcher::callbackSensorPoints(
   sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_sensorTF_msg_ptr)
 {
+  //std::lock_guard<std::mutex> lock(ndt_map_mtx_);
+
   if (sensor_points_sensorTF_msg_ptr->data.empty()) {
     RCLCPP_WARN_STREAM_THROTTLE(
       this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
@@ -461,7 +481,6 @@ void NDTScanMatcher::callbackSensorPoints(
   const auto exe_start_time = std::chrono::system_clock::now();
   // mutex Map
   std::lock_guard<std::mutex> lock(ndt_map_mtx_);
-
   const std::string & sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id;
   const rclcpp::Time sensor_ros_time = sensor_points_sensorTF_msg_ptr->header.stamp;
 

localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp

@@ -20,7 +20,8 @@ int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
   auto ndt_scan_matcher = std::make_shared<NDTScanMatcher>();
-  rclcpp::executors::MultiThreadedExecutor exec;
+  //rclcpp::executors::MultiThreadedExecutor exec;
+  agnocast::MultiThreadedAgnocastExecutor exec;
   exec.add_node(ndt_scan_matcher);
   exec.spin();
   rclcpp::shutdown();

localization/pose_initializer/CMakeLists.txt

@@ -14,12 +14,21 @@ endif()
 ### Dependencies
 find_package(ament_cmake_auto REQUIRED)
 find_package(PCL REQUIRED)
+
+find_package(agnocastlib REQUIRED)
+
 ament_auto_find_build_dependencies()
 
 ament_auto_add_executable(pose_initializer
   src/pose_initializer_node.cpp
   src/pose_initializer_core.cpp
 )
+
+ament_target_dependencies(pose_initializer agnocastlib)
+target_include_directories(pose_initializer PRIVATE
+  ${agnocastlib_INCLUDE_DIRS}
+)
+
 target_link_libraries(pose_initializer ${PCL_LIBRARIES})
 
 if(BUILD_TESTING)

localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp

@@ -15,6 +15,8 @@
 #ifndef POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
 #define POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
 
+#include "agnocast.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_api_utils/tier4_api_utils.hpp>
 
@@ -38,7 +40,8 @@ class PoseInitializer : public rclcpp::Node
   PoseInitializer();
 
 private:
-  void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr);
+  //void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr);
+  void callbackMapPoints(agnocast::ipc_shared_ptr<sensor_msgs::msg::PointCloud2> map_points_msg_ptr);
   void serviceInitializePose(
     const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
     tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
@@ -60,7 +63,8 @@ class PoseInitializer : public rclcpp::Node
     const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
 
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr gnss_pose_sub_;
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
+  //rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
+  agnocast::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
 
   // TODO(Takagi, Isamu): deprecated
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;

localization/pose_initializer/launch/pose_initializer.launch.xml

@@ -1,7 +1,27 @@
 <launch>
   <arg name="config_file" default="$(find-pkg-share pose_initializer)/config/pose_initializer.param.yaml"/>
 
+    <!-- <node_container pkg="agnocastlib" exec="agnocast_component_container" name="pose_initializer" namespace="" output="screen"> -->
+    <!-- <env name="LD_PRELOAD" value="libagnocast_heaphook.so:$(env LD_PRELOAD '')" /> -->
+    <!-- <env name="MEMPOOL_SIZE" value="32212254720" /> --> <!-- 30GB-->
+
+    <!-- <composable_node pkg="pose_initializer" plugin="pose_initializer::PoseInitializer" name="pose_initializer" namespace=""> -->
+      <!-- <remap from="initialpose" to="/initialpose"/> -->
+      <!-- <remap from="initialpose3d" to="/initialpose3d"/> -->
+      <!-- <remap from="gnss_pose_cov" to="/sensing/gnss/pose_with_covariance"/> -->
+      <!-- <remap from="pointcloud_map" to="/map/pointcloud_map"/> -->
+      <!-- <remap from="ndt_align_srv" to="/localization/pose_estimator/ndt_align_srv"/> -->
+      <!-- <remap from="service/initialize_pose" to="/localization/util/initialize_pose"/> -->
+      <!-- <remap from="service/initialize_pose_auto" to="/localization/util/initialize_pose_auto"/> -->
+      <!-- <param from="$(var config_file)"/> -->
+      <!-- <param name="enable_gnss_callback" value="true"/> -->
+    <!-- </composable_node> -->
+  <!-- </node_container> -->
+
   <node pkg="pose_initializer" exec="pose_initializer" name="pose_initializer" output="log">
+    <env name="LD_PRELOAD" value="libagnocast_heaphook.so:$(env LD_PRELOAD '')" />
+    <env name="MEMPOOL_SIZE" value="32212254720" /> <!-- 30GB -->
+
     <remap from="initialpose" to="/initialpose"/>
     <remap from="initialpose3d" to="/initialpose3d"/>
     <remap from="gnss_pose_cov" to="/sensing/gnss/pose_with_covariance"/>

localization/pose_initializer/package.xml

@@ -21,6 +21,8 @@
   <depend>tier4_external_api_msgs</depend>
   <depend>tier4_localization_msgs</depend>
 
+  <depend>agnocastlib</depend>
+
   <test_depend>ament_cmake_cppcheck</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

localization/pose_initializer/src/pose_initializer_core.cpp

@@ -75,8 +75,12 @@ PoseInitializer::PoseInitializer()
   initial_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "initialpose", 10,
     std::bind(&PoseInitializer::callbackInitialPose, this, std::placeholders::_1));
-  map_points_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-    "pointcloud_map", rclcpp::QoS{1}.transient_local(),
+  //map_points_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+    //"pointcloud_map", rclcpp::QoS{1}.transient_local(),
+    //std::bind(&PoseInitializer::callbackMapPoints, this, std::placeholders::_1));
+  map_points_sub_ = agnocast::create_subscription<sensor_msgs::msg::PointCloud2>(
+    get_node_base_interface(),
+    "/pointcloud_map_agnocast", rclcpp::QoS{1}.transient_local(),
     std::bind(&PoseInitializer::callbackMapPoints, this, std::placeholders::_1));
   gnss_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "gnss_pose_cov", 1,
@@ -111,8 +115,11 @@ PoseInitializer::PoseInitializer()
 }
 
 void PoseInitializer::callbackMapPoints(
-  sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
+  //sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
+  agnocast::ipc_shared_ptr<sensor_msgs::msg::PointCloud2> map_points_msg_ptr)
 {
+
+  RCLCPP_INFO(this->get_logger(), "pose_initializer subscribe pointcloud map ");
   std::string map_frame_ = map_points_msg_ptr->header.frame_id;
   map_ptr_ = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::fromROSMsg(*map_points_msg_ptr, *map_ptr_);

localization/pose_initializer/src/pose_initializer_node.cpp

@@ -21,7 +21,8 @@
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  rclcpp::executors::MultiThreadedExecutor executor;
+  //rclcpp::executors::MultiThreadedExecutor executor;
+  agnocast::MultiThreadedAgnocastExecutor executor;
   auto node = std::make_shared<PoseInitializer>();
   executor.add_node(node);
   executor.spin();

map/map_loader/CMakeLists.txt

@@ -13,12 +13,20 @@ endif()
 
 find_package(PCL REQUIRED COMPONENTS io)
 
+find_package(agnocastlib REQUIRED)
+
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 ament_auto_add_library(pointcloud_map_loader_node SHARED
   src/pointcloud_map_loader/pointcloud_map_loader_node.cpp
 )
+
+ament_target_dependencies(pointcloud_map_loader_node agnocastlib)
+target_include_directories(pointcloud_map_loader_node PRIVATE
+  ${agnocastlib_INCLUDE_DIRS}
+)
+
 target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES})
 
 target_include_directories(pointcloud_map_loader_node

map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp

@@ -38,13 +38,16 @@
 #include <string>
 #include <vector>
 
+#include "agnocast.hpp"
+
 class PointCloudMapLoaderNode : public rclcpp::Node
 {
 public:
   explicit PointCloudMapLoaderNode(const rclcpp::NodeOptions & options);
 
 private:
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_pointcloud_map_;
+  agnocast::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_pointcloud_map_agnocast;
 
   sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector<std::string> & pcd_paths);
 };

map/map_loader/package.xml

@@ -23,6 +23,8 @@
   <depend>tier4_autoware_utils</depend>
   <depend>visualization_msgs</depend>
 
+  <depend>agnocastlib</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

@@ -63,6 +63,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
   durable_qos.transient_local();
   pub_pointcloud_map_ =
     this->create_publisher<sensor_msgs::msg::PointCloud2>("output/pointcloud_map", durable_qos);
+  pub_pointcloud_map_agnocast = agnocast::create_publisher<sensor_msgs::msg::PointCloud2>("/pointcloud_map_agnocast", durable_qos);
 
   const auto pcd_paths_or_directory =
     declare_parameter("pcd_paths_or_directory", std::vector<std::string>({}));
@@ -89,13 +90,16 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
   }
 
   const auto pcd = loadPCDFiles(pcd_paths);
+  agnocast::ipc_shared_ptr<sensor_msgs::msg::PointCloud2> pcd_agnocast = pub_pointcloud_map_agnocast->borrow_loaned_message();
+  *pcd_agnocast = loadPCDFiles(pcd_paths);
 
   if (pcd.width == 0) {
     RCLCPP_ERROR(get_logger(), "No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size());
     return;
   }
 
   pub_pointcloud_map_->publish(pcd);
+  pub_pointcloud_map_agnocast->publish(std::move(pcd_agnocast));
 }
 
 sensor_msgs::msg::PointCloud2 PointCloudMapLoaderNode::loadPCDFiles(

perception/elevation_map_loader/CMakeLists.txt

@@ -4,6 +4,8 @@ project(elevation_map_loader)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+find_package(agnocastlib REQUIRED)
+
 find_package(PCL REQUIRED COMPONENTS io)
 
 ament_auto_add_library(elevation_map_loader_node SHARED
@@ -16,6 +18,11 @@ rclcpp_components_register_node(elevation_map_loader_node
   EXECUTABLE elevation_map_loader
 )
 
+ament_target_dependencies(elevation_map_loader agnocastlib)
+target_include_directories(elevation_map_loader PRIVATE
+  ${agnocastlib_include_dirs}
+)
+
 ament_auto_package(INSTALL_TO_SHARE
   launch
   config