From 8d804241dc3b1980b8762fb40e7c91c950842d16 Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Wed, 30 Aug 2023 12:00:28 +0900 Subject: [PATCH] Deleted files remained in localization folder since it is completely moved to pointcloud_preprocessor. Changed enable_pointcloud_switch default to false Signed-off-by: TaikiYamada4 --- .../launch/localization.launch.xml | 4 +- .../pointcloud_switcher/CMakeLists.txt | 33 ---------- .../config/pointcloud_switcher.param.yaml | 6 -- .../pointcloud_switcher.hpp | 39 ------------ .../launch/pointcloud_switcher.launch.xml | 8 --- localization/pointcloud_switcher/package.xml | 23 ------- .../src/pointcloud_switcher.cpp | 60 ------------------- .../config/pointcloud_switcher.param.yaml | 10 ++++ .../pointcloud_switcher.hpp | 4 +- .../pointcloud_switcher.cpp | 27 ++++++--- 10 files changed, 35 insertions(+), 179 deletions(-) delete mode 100644 localization/pointcloud_switcher/CMakeLists.txt delete mode 100644 localization/pointcloud_switcher/config/pointcloud_switcher.param.yaml delete mode 100644 localization/pointcloud_switcher/include/pointcloud_switcher/pointcloud_switcher.hpp delete mode 100644 localization/pointcloud_switcher/launch/pointcloud_switcher.launch.xml delete mode 100644 localization/pointcloud_switcher/package.xml delete mode 100644 localization/pointcloud_switcher/src/pointcloud_switcher.cpp create mode 100644 sensing/pointcloud_preprocessor/config/pointcloud_switcher.param.yaml diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index a49f70a7cce95..f6154b0d9032e 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -15,8 +15,8 @@ - - + + diff --git a/localization/pointcloud_switcher/CMakeLists.txt b/localization/pointcloud_switcher/CMakeLists.txt deleted file mode 100644 index aa8f8f7bb7673..0000000000000 --- a/localization/pointcloud_switcher/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(pointcloud_switcher) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -ament_auto_add_executable(pointcloud_switcher src/pointcloud_switcher.cpp) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/localization/pointcloud_switcher/config/pointcloud_switcher.param.yaml b/localization/pointcloud_switcher/config/pointcloud_switcher.param.yaml deleted file mode 100644 index b9126ae2ff1cd..0000000000000 --- a/localization/pointcloud_switcher/config/pointcloud_switcher.param.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - pointcloud_topic_name: - - "/sensing/lidar/concatenated/pointcloud" - - "/sensing/lidar/left_upper/outlier_filtered/pointcloud" - - "/sensing/lidar/right_upper/outlier_filtered/pointcloud" \ No newline at end of file diff --git a/localization/pointcloud_switcher/include/pointcloud_switcher/pointcloud_switcher.hpp b/localization/pointcloud_switcher/include/pointcloud_switcher/pointcloud_switcher.hpp deleted file mode 100644 index 9fcfb929eb518..0000000000000 --- a/localization/pointcloud_switcher/include/pointcloud_switcher/pointcloud_switcher.hpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POINTCLOUD_SWITCHER__POINTCLOUD_SWITCHER_HPP_ -#define POINTCLOUD_SWITCHER__POINTCLOUD_SWITCHER_HPP_ - -#include -#include - -#include -#include - -using namespace std; - -class PointCloudSwitcher : public rclcpp::Node -{ - public: - PointCloudSwitcher(); - - private: - void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg, const string topic_name); - vector pointcloud_candidates_; - string selected_pointcloud_topic_name_; - vector::SharedPtr> subscribers_; - rclcpp::Publisher::SharedPtr selected_pointcloud_publisher_; -}; - -#endif // POINTCLOUD_SWITCHER__POINTCLOUD_SWITCHER_HPP_ \ No newline at end of file diff --git a/localization/pointcloud_switcher/launch/pointcloud_switcher.launch.xml b/localization/pointcloud_switcher/launch/pointcloud_switcher.launch.xml deleted file mode 100644 index c8d4b97b24f37..0000000000000 --- a/localization/pointcloud_switcher/launch/pointcloud_switcher.launch.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/localization/pointcloud_switcher/package.xml b/localization/pointcloud_switcher/package.xml deleted file mode 100644 index 221725a7eaefe..0000000000000 --- a/localization/pointcloud_switcher/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - pointcloud_switcher - 0.1.0 - TODO: Package description - taikiyamada - - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - ament_lint_auto - autoware_lint_common - - rclcpp - sensor_msgs - - - ament_cmake - - diff --git a/localization/pointcloud_switcher/src/pointcloud_switcher.cpp b/localization/pointcloud_switcher/src/pointcloud_switcher.cpp deleted file mode 100644 index 3161470f66f1f..0000000000000 --- a/localization/pointcloud_switcher/src/pointcloud_switcher.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -PointCloudSwitcher::PointCloudSwitcher() : rclcpp::Node("pointcloud_switcher") -{ - // Get paramters from yaml file - this->declare_parameter>("pointcloud_topic_name", vector()); - this->get_parameter("pointcloud_topic_name", pointcloud_candidates_); - - // Check pointcloud_topic_names and create subscribers for each topic - RCLCPP_INFO(this->get_logger(), "pointcloud_topic_name size: %ld", pointcloud_candidates_.size()); - rclcpp::QoS qos(5); - qos.reliability(rclcpp::ReliabilityPolicy::BestEffort); - for(const auto &topic_name : pointcloud_candidates_) - { - RCLCPP_INFO(this->get_logger(), "pointcloud_topic_name: %s", topic_name.c_str()); - auto callback = [this, topic_name](const sensor_msgs::msg::PointCloud2::SharedPtr pointcloud_msg) { - this->pointcloud_callback(pointcloud_msg, topic_name); - }; - subscribers_.push_back(this->create_subscription(topic_name, qos, callback)); - } - - // Set selected_pointcloud_topic_name_ to the first element of pointcloud_candidates_ - selected_pointcloud_topic_name_ = pointcloud_candidates_[0]; - - // Create publisher of /selected/pointcloud - selected_pointcloud_publisher_ = this->create_publisher("/selected/pointcloud", 10); -} - -void PointCloudSwitcher::pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr pointcloud_msg, const string topic_name) -{ - RCLCPP_DEBUG(this->get_logger(), "pointcloud_callback: %s", topic_name.c_str()); - if(topic_name == selected_pointcloud_topic_name_) - { - selected_pointcloud_publisher_->publish(*pointcloud_msg); - return; - } -} - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/sensing/pointcloud_preprocessor/config/pointcloud_switcher.param.yaml b/sensing/pointcloud_preprocessor/config/pointcloud_switcher.param.yaml new file mode 100644 index 0000000000000..940022ff024b8 --- /dev/null +++ b/sensing/pointcloud_preprocessor/config/pointcloud_switcher.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + pointcloud_topic_names: + - "/sensing/lidar/front_lower/outlier_filtered/pointcloud" + - "/sensing/lidar/left_upper/outlier_filtered/pointcloud" + - "/sensing/lidar/right_upper/outlier_filtered/pointcloud" + - "/sensing/lidar/concatenated/pointcloud" + subscription_period_confirmation_time_span: 1.0 # in seconds + steps_for_moving_average: 4 + average_subscription_period_threshold: 0.5 # in seconds diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_switcher/pointcloud_switcher.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_switcher/pointcloud_switcher.hpp index 867082d559723..e39184874d1cc 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_switcher/pointcloud_switcher.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_switcher/pointcloud_switcher.hpp @@ -16,6 +16,7 @@ #define POINTCLOUD_PREPROCESSOR__POINTCLOUD_SWITCHER__POINTCLOUD_SWITCHER_HPP_ #include +#include #include #include @@ -34,7 +35,7 @@ namespace pointcloud_preprocessor private: void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg, const string topic_name); void initialization_state_callback(const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::SharedPtr msg); - void check_heartbeat(); + void check_subscription_period(); string next_pointcloud_topic(); //(const string current_pointcloud_topic_name); vector pointcloud_candidates_; @@ -42,6 +43,7 @@ namespace pointcloud_preprocessor string selected_pointcloud_topic_name_; vector::SharedPtr> pointcloud_subscribers_; rclcpp::Publisher::SharedPtr selected_pointcloud_publisher_; + rclcpp::Publisher::SharedPtr debug_info_publisher_; rclcpp::Subscription::SharedPtr initialization_state_subscriber_; autoware_adapi_v1_msgs::msg::LocalizationInitializationState last_initialization_state_; diff --git a/sensing/pointcloud_preprocessor/src/pointcloud_switcher/pointcloud_switcher.cpp b/sensing/pointcloud_preprocessor/src/pointcloud_switcher/pointcloud_switcher.cpp index f1f1b585c65dd..292e3beb9f4bb 100644 --- a/sensing/pointcloud_preprocessor/src/pointcloud_switcher/pointcloud_switcher.cpp +++ b/sensing/pointcloud_preprocessor/src/pointcloud_switcher/pointcloud_switcher.cpp @@ -55,10 +55,10 @@ namespace pointcloud_preprocessor pending_delta_flag_[topic_name] = false; } - // Create a timer to check heartbeat + // Create a timer to check subscription_period timer_ = this->create_wall_timer( std::chrono::duration(subscription_period_confimation_time_span), - std::bind(&PointCloudSwitcher::check_heartbeat, this) + std::bind(&PointCloudSwitcher::check_subscription_period, this) ); // Set selected_pointcloud_topic_name_ to the first element of pointcloud_candidates_ @@ -67,6 +67,9 @@ namespace pointcloud_preprocessor // Create publisher of /selected/pointcloud selected_pointcloud_publisher_ = this->create_publisher("output/pointcloud", 10); + // Create publisher of pointcloud_switcher/debug_info + debug_info_publisher_ = this->create_publisher("pointcloud_switcher/debug/info", 10); + // Create subscriber of /api/localization/initialization_state rclcpp::QoS qos_initialization_state(1); qos_initialization_state.reliability(rclcpp::ReliabilityPolicy::Reliable); @@ -119,14 +122,19 @@ namespace pointcloud_preprocessor if (next_pointcloud_topic_name_.empty()) { RCLCPP_ERROR(this->get_logger(), "No available pointcloud topic!!"); } else { - if(next_pointcloud_topic_name_ != selected_pointcloud_topic_name_) RCLCPP_WARN(this->get_logger(), "Switching pointcloud from %s to %s.", selected_pointcloud_topic_name_.c_str(), next_pointcloud_topic_name_.c_str()); + if(next_pointcloud_topic_name_ != selected_pointcloud_topic_name_) { + RCLCPP_WARN(this->get_logger(), "Switching pointcloud from %s to %s.", selected_pointcloud_topic_name_.c_str(), next_pointcloud_topic_name_.c_str()); + auto message = std_msgs::msg::String(); + message.data = selected_pointcloud_topic_name_ + next_pointcloud_topic_name_; + debug_info_publisher_->publish(message); + } selected_pointcloud_topic_name_ = next_pointcloud_topic_name_; } } last_initialization_state_ = *msg; } - void PointCloudSwitcher::check_heartbeat() // Timer callback + void PointCloudSwitcher::check_subscription_period() // Timer callback { rclcpp::Time current_time = this->now(); @@ -138,7 +146,7 @@ namespace pointcloud_preprocessor RCLCPP_WARN(this->get_logger(), "Haven't received pointcloud from %s yet", topic_name.c_str()); continue; } else { - // If delta time is greater than heartbeat_confirmation_time_span, print warning and update delta_times_ by adding it to the last element + // If delta time is greater than subscription_period_confirmation_time_span, print warning and update delta_times_ by adding it to the last element if ((current_time - last_received_time_[topic_name]).seconds() > subscription_period_confimation_time_span) { // If pending_delta_flag_ is false, push_back a new delta time, if not, edit the last element // pending_delta_flag_ is used to avoid pushing back a new delta time when "the last element is edited" (= "a new pointcloud topic is still not subscribed") @@ -175,8 +183,13 @@ namespace pointcloud_preprocessor if (next_pointcloud_topic_name_.empty()) { RCLCPP_ERROR(this->get_logger(), "No available pointcloud topic!!"); } else { - if(next_pointcloud_topic_name_ != selected_pointcloud_topic_name_) RCLCPP_WARN(this->get_logger(), "Switching pointcloud from %s to %s.", selected_pointcloud_topic_name_.c_str(), next_pointcloud_topic_name_.c_str()); - selected_pointcloud_topic_name_ = next_pointcloud_topic_name_; + if(next_pointcloud_topic_name_ != selected_pointcloud_topic_name_) { + RCLCPP_WARN(this->get_logger(), "Switching pointcloud from %s to %s.", selected_pointcloud_topic_name_.c_str(), next_pointcloud_topic_name_.c_str()); + auto message = std_msgs::msg::String(); + message.data = selected_pointcloud_topic_name_ + "," + next_pointcloud_topic_name_; + debug_info_publisher_->publish(message); + } + selected_pointcloud_topic_name_ = next_pointcloud_topic_name_; } }