From 992082fa7a59146a5a54e9459547c794704e99b8 Mon Sep 17 00:00:00 2001 From: Koichi Imai Date: Fri, 13 Dec 2024 13:40:40 +0900 Subject: [PATCH] Applied agnocast to /map/pointcloud_map topic Signed-off-by: Koichi Imai --- localization/ndt_scan_matcher/CMakeLists.txt | 8 +++++ .../ndt_scan_matcher_core.hpp | 8 +++-- .../launch/ndt_scan_matcher.launch.xml | 3 ++ localization/ndt_scan_matcher/package.xml | 2 ++ .../src/ndt_scan_matcher_core.cpp | 29 +++++++++++++--- .../src/ndt_scan_matcher_node.cpp | 3 +- localization/pose_initializer/CMakeLists.txt | 9 +++++ .../pose_initializer_core.hpp | 8 +++-- .../launch/pose_initializer.launch.xml | 20 +++++++++++ localization/pose_initializer/package.xml | 2 ++ .../src/pose_initializer_core.cpp | 13 +++++-- .../src/pose_initializer_node.cpp | 3 +- map/map_loader/CMakeLists.txt | 8 +++++ .../map_loader/pointcloud_map_loader_node.hpp | 3 ++ map/map_loader/package.xml | 2 ++ .../pointcloud_map_loader_node.cpp | 4 +++ .../elevation_map_loader/CMakeLists.txt | 7 ++++ .../elevation_map_loader_node.hpp | 8 +++-- perception/elevation_map_loader/package.xml | 2 ++ .../src/elevation_map_loader_node.cpp | 11 ++++-- .../ad_service_state_monitor/CMakeLists.txt | 7 ++++ .../ad_service_state_monitor_node.hpp | 7 ++++ .../ad_service_state_monitor.launch.xml | 3 ++ system/ad_service_state_monitor/package.xml | 2 ++ .../ad_service_state_monitor_node.cpp | 34 +++++++++++++++++-- .../ad_service_state_monitor_node/main.cpp | 10 ++++-- 26 files changed, 192 insertions(+), 24 deletions(-) diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 027d27c0cfd56..8c428704ceb29 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/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() diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 4ef2f88914831..505b33baff8c5 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/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 #include @@ -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 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::SharedPtr initial_pose_sub_; - rclcpp::Subscription::SharedPtr map_points_sub_; + //rclcpp::Subscription::SharedPtr map_points_sub_; + agnocast::Subscription::SharedPtr map_points_sub_; rclcpp::Subscription::SharedPtr sensor_points_sub_; rclcpp::Publisher::SharedPtr sensor_aligned_pose_pub_; diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index 654a4f83b5747..8027cf74a4cae 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -12,6 +12,9 @@ + + + diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index b42a938089931..7a160c8247758 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -28,6 +28,8 @@ tier4_localization_msgs visualization_msgs + agnocastlib + ament_cmake_cppcheck ament_lint_auto diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 4a772ccd4793f..2df14efa58fb1 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -30,6 +30,7 @@ #include #include #include +#include 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( "ekf_pose_with_covariance", 100, std::bind(&NDTScanMatcher::callbackInitialPose, this, std::placeholders::_1), initial_pose_sub_opt); - map_points_sub_ = this->create_subscription( - "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&NDTScanMatcher::callbackMapPoints, this, std::placeholders::_1), main_sub_opt); + //map_points_sub_ = this->create_subscription( + //"pointcloud_map", rclcpp::QoS{1}.transient_local(), + //std::bind(&NDTScanMatcher::callbackMapPoints, this, std::placeholders::_1), main_sub_opt); + + map_points_sub_ = agnocast::create_subscription( + 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( "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 lock(ndt_map_mtx_); + // get TF from pose_frame to map_frame auto TF_pose_to_map_ptr = std::make_shared(); 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 map_points_msg_ptr) { + //std::lock_guard 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 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 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; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp index be6b398d7738e..3b6e2aba3a48b 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp +++ b/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(); - rclcpp::executors::MultiThreadedExecutor exec; + //rclcpp::executors::MultiThreadedExecutor exec; + agnocast::MultiThreadedAgnocastExecutor exec; exec.add_node(ndt_scan_matcher); exec.spin(); rclcpp::shutdown(); diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt index 5b99d1141e1c0..5b4fb7aed3b00 100644 --- a/localization/pose_initializer/CMakeLists.txt +++ b/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) diff --git a/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp index a78c2040dcd77..57c9608815e7f 100644 --- a/localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp +++ b/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 #include @@ -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 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::SharedPtr gnss_pose_sub_; - rclcpp::Subscription::SharedPtr map_points_sub_; + //rclcpp::Subscription::SharedPtr map_points_sub_; + agnocast::Subscription::SharedPtr map_points_sub_; // TODO(Takagi, Isamu): deprecated rclcpp::Subscription::SharedPtr initial_pose_sub_; diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 0645f0cbfca53..667054820dd3e 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -1,7 +1,27 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 1b3ab3d1276e6..0366928f68a7f 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -21,6 +21,8 @@ tier4_external_api_msgs tier4_localization_msgs + agnocastlib + ament_cmake_cppcheck ament_lint_auto autoware_lint_common diff --git a/localization/pose_initializer/src/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer_core.cpp index 3ca67d19d5104..19eb692eaaed5 100644 --- a/localization/pose_initializer/src/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer_core.cpp @@ -75,8 +75,12 @@ PoseInitializer::PoseInitializer() initial_pose_sub_ = this->create_subscription( "initialpose", 10, std::bind(&PoseInitializer::callbackInitialPose, this, std::placeholders::_1)); - map_points_sub_ = this->create_subscription( - "pointcloud_map", rclcpp::QoS{1}.transient_local(), + //map_points_sub_ = this->create_subscription( + //"pointcloud_map", rclcpp::QoS{1}.transient_local(), + //std::bind(&PoseInitializer::callbackMapPoints, this, std::placeholders::_1)); + map_points_sub_ = agnocast::create_subscription( + 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( "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 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::Ptr(new pcl::PointCloud); pcl::fromROSMsg(*map_points_msg_ptr, *map_ptr_); diff --git a/localization/pose_initializer/src/pose_initializer_node.cpp b/localization/pose_initializer/src/pose_initializer_node.cpp index a46969c7711a1..0e912a1b7849d 100644 --- a/localization/pose_initializer/src/pose_initializer_node.cpp +++ b/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(); executor.add_node(node); executor.spin(); diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 11373def78d80..0c00c655e3a85 100644 --- a/map/map_loader/CMakeLists.txt +++ b/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 diff --git a/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp index 6bc55ef642706..b649109edb355 100644 --- a/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp @@ -38,6 +38,8 @@ #include #include +#include "agnocast.hpp" + class PointCloudMapLoaderNode : public rclcpp::Node { public: @@ -45,6 +47,7 @@ class PointCloudMapLoaderNode : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_pointcloud_map_; + agnocast::Publisher::SharedPtr pub_pointcloud_map_agnocast; sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector & pcd_paths); }; diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index d15a2ab1d6b07..553789bd6244b 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -23,6 +23,8 @@ tier4_autoware_utils visualization_msgs + agnocastlib + ament_lint_auto autoware_lint_common diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index d5bd22eeda8ab..a47b3d474e45f 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/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("output/pointcloud_map", durable_qos); + pub_pointcloud_map_agnocast = agnocast::create_publisher("/pointcloud_map_agnocast", durable_qos); const auto pcd_paths_or_directory = declare_parameter("pcd_paths_or_directory", std::vector({})); @@ -89,6 +90,8 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt } const auto pcd = loadPCDFiles(pcd_paths); + agnocast::ipc_shared_ptr 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()); @@ -96,6 +99,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt } pub_pointcloud_map_->publish(pcd); + pub_pointcloud_map_agnocast->publish(std::move(pcd_agnocast)); } sensor_msgs::msg::PointCloud2 PointCloudMapLoaderNode::loadPCDFiles( diff --git a/perception/elevation_map_loader/CMakeLists.txt b/perception/elevation_map_loader/CMakeLists.txt index ab2c1dcc20e1d..6a25c7c27a5a4 100644 --- a/perception/elevation_map_loader/CMakeLists.txt +++ b/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 diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index 57f7a2429c68c..421301f6941ea 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -15,6 +15,8 @@ #ifndef ELEVATION_MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ #define ELEVATION_MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ +#include "agnocast.hpp" + #include #include #include @@ -61,12 +63,14 @@ class ElevationMapLoaderNode : public rclcpp::Node explicit ElevationMapLoaderNode(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_pointcloud_map_; + //rclcpp::Subscription::SharedPtr sub_pointcloud_map_; + agnocast::Subscription::SharedPtr sub_pointcloud_map_; rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_map_hash_; rclcpp::Publisher::SharedPtr pub_elevation_map_; rclcpp::Publisher::SharedPtr pub_elevation_map_cloud_; - void onPointcloudMap(const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map); + //void onPointcloudMap(const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map); + void onPointcloudMap(agnocast::ipc_shared_ptr pointcloud_map); void onMapHash(const tier4_external_api_msgs::msg::MapHash::SharedPtr map_hash); void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr vector_map); diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index 859f4a0cb4ce1..8565269223465 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -25,6 +25,8 @@ tier4_autoware_utils tier4_external_api_msgs + agnocastlib + ament_lint_auto autoware_lint_common diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 7d0f0d2f7a7d3..47e5dbd8243d7 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -79,9 +79,13 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio sub_map_hash_ = create_subscription( "/api/autoware/get/map/info/hash", durable_qos, std::bind(&ElevationMapLoaderNode::onMapHash, this, _1)); - sub_pointcloud_map_ = this->create_subscription( - "input/pointcloud_map", durable_qos, + //sub_pointcloud_map_ = this->create_subscription( + //"input/pointcloud_map", durable_qos, + sub_pointcloud_map_ = agnocast::create_subscription( + get_node_base_interface(), + "/pointcloud_map_agnocast", durable_qos, std::bind(&ElevationMapLoaderNode::onPointcloudMap, this, _1)); + sub_vector_map_ = this->create_subscription( "input/vector_map", durable_qos, std::bind(&ElevationMapLoaderNode::onVectorMap, this, _1)); } @@ -143,7 +147,8 @@ void ElevationMapLoaderNode::onMapHash( } void ElevationMapLoaderNode::onPointcloudMap( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map) + //const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map) + agnocast::ipc_shared_ptr pointcloud_map) { RCLCPP_INFO(this->get_logger(), "subscribe pointcloud_map"); { diff --git a/system/ad_service_state_monitor/CMakeLists.txt b/system/ad_service_state_monitor/CMakeLists.txt index 4ee9674d22dc7..87563462fdd41 100644 --- a/system/ad_service_state_monitor/CMakeLists.txt +++ b/system/ad_service_state_monitor/CMakeLists.txt @@ -13,6 +13,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() +find_package(agnocastlib REQUIRED) + ### Dependencies find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() @@ -31,6 +33,11 @@ ament_auto_add_executable(ad_service_state_monitor ${AD_SERVICE_STATE_MONITOR_SRC} ) +ament_target_dependencies(ad_service_state_monitor agnocastlib) +target_include_directories(ad_service_state_monitor PRIVATE + ${agnocastlib_INCLUDE_DIRS} +) + target_link_libraries(ad_service_state_monitor glog) if(BUILD_TESTING) diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp index f9cb6ff49b3b4..965297d894bec 100644 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp +++ b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp @@ -30,6 +30,8 @@ #include #include #include +#include + #include #include @@ -40,6 +42,8 @@ #include #include +#include "agnocast.hpp" + class AutowareStateMonitorNode : public rclcpp::Node { public: @@ -70,6 +74,7 @@ class AutowareStateMonitorNode : public rclcpp::Node sub_control_mode_; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Subscription::SharedPtr sub_odom_; + agnocast::Subscription::SharedPtr map_points_sub_; void onAutowareEngage(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); void onVehicleControlMode( @@ -80,6 +85,8 @@ class AutowareStateMonitorNode : public rclcpp::Node // Topic Buffer void onTopic( const std::shared_ptr msg, const std::string & topic_name); + void onTopicPointcloudMap( + agnocast::ipc_shared_ptr map_points_msg_ptr, const std::string & topic_name); void registerTopicCallback( const std::string & topic_name, const std::string & topic_type, const bool transient_local, const bool best_effort); diff --git a/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml b/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml index 145d690d49783..9d66d555f6df3 100644 --- a/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml +++ b/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml @@ -26,6 +26,9 @@ + + + diff --git a/system/ad_service_state_monitor/package.xml b/system/ad_service_state_monitor/package.xml index 0ed4ca2dd10a1..4c3517cb679f4 100644 --- a/system/ad_service_state_monitor/package.xml +++ b/system/ad_service_state_monitor/package.xml @@ -26,6 +26,8 @@ tf2_ros tier4_autoware_utils + agnocastlib + sensor_msgs tier4_perception_msgs diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp index 91116658dfd67..114a15a5fde42 100644 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp @@ -293,6 +293,22 @@ void AutowareStateMonitorNode::onTopic( } } +void AutowareStateMonitorNode::onTopicPointcloudMap( + [[maybe_unused]] agnocast::ipc_shared_ptr map_points_msg_ptr, + [[maybe_unused]] const std::string & topic_name) +{ + RCLCPP_INFO(this->get_logger(), "ad_service_state_monitor subscribe pointcloud map"); + const auto now = this->now(); + + auto & buf = topic_received_time_buffer_.at("/map/pointcloud_map"); + buf.push_back(now); + + constexpr size_t topic_received_time_buffer_size = 10; + if (buf.size() > topic_received_time_buffer_size) { + buf.pop_front(); + } +} + void AutowareStateMonitorNode::registerTopicCallback( const std::string & topic_name, const std::string & topic_type, const bool transient_local, const bool best_effort) @@ -462,10 +478,19 @@ AutowareStateMonitorNode::AutowareStateMonitorNode() // Topic Callback for (const auto & topic_config : topic_configs_) { - registerTopicCallback( - topic_config.name, topic_config.type, topic_config.transient_local, topic_config.best_effort); + RCLCPP_INFO(this->get_logger(), "ad_service_state_monitor_node.cpp: %s", topic_config.name.c_str()); + if (topic_config.name == "/map/pointcloud_map") { + // Initialize buffer + topic_received_time_buffer_["/map/pointcloud_map"] = {}; + continue; + + } else{ + registerTopicCallback( + topic_config.name, topic_config.type, topic_config.transient_local, topic_config.best_effort); + } } + // Subscriber sub_autoware_engage_ = this->create_subscription( "input/autoware_engage", 1, std::bind(&AutowareStateMonitorNode::onAutowareEngage, this, _1), @@ -480,6 +505,11 @@ AutowareStateMonitorNode::AutowareStateMonitorNode() "input/odometry", 100, std::bind(&AutowareStateMonitorNode::onOdometry, this, _1), subscriber_option); + auto agnocast_subscriber_option = agnocast::SubscriptionOptions(); + agnocast_subscriber_option.callback_group = callback_group_subscribers_; + map_points_sub_ = agnocast::create_subscription(get_node_base_interface(),"/pointcloud_map_agnocast", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareStateMonitorNode::onTopicPointcloudMap, this, std::placeholders::_1, "/pointcloud_map_agnocast"), agnocast_subscriber_option); + // Service srv_shutdown_ = this->create_service( "service/shutdown", std::bind(&AutowareStateMonitorNode::onShutdownService, this, _1, _2, _3), diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp index 87d31ac8073cf..20a4519ed3b78 100644 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp @@ -19,6 +19,7 @@ #include #include +#include "agnocast.hpp" int main(int argc, char * argv[]) { @@ -27,9 +28,12 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); auto node = std::make_shared(); - auto executor = std::make_shared(); - executor->add_node(node); - executor->spin(); + //auto executor = std::make_shared(); + agnocast::MultiThreadedAgnocastExecutor executor; + executor.add_node(node); + executor.spin(); + //executor->add_node(node); + //executor->spin(); rclcpp::shutdown(); return 0;