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 <[email protected]>

launch/tier4_localization_launch/launch/localization.launch.xml

@@ -15,8 +15,8 @@
   <arg name="pose_initializer_param_path"/>
   <arg name="eagleye_param_path"/>
 
-  <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud" description="The topic will be used in the localization util module"/>
-  <arg name="enable_pointcloud_switch" default="true" description="enable to switch pointcloud topic to subscribe on failure of pointcloud subscription"/>
+  <arg name="input_pointcloud" default="/sensing/lidar/concatenated/pointcloud" description="The topic will be used in the localization util module"/>
+  <arg name="enable_pointcloud_switch" default="false" description="enable to switch pointcloud topic to subscribe on failure of pointcloud subscription"/>
   <arg name="use_pointcloud_container" default="true" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
 

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

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_switcher/pointcloud_switcher.hpp

@@ -16,6 +16,7 @@
 #define POINTCLOUD_PREPROCESSOR__POINTCLOUD_SWITCHER__POINTCLOUD_SWITCHER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
 
@@ -34,14 +35,15 @@ 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<string> pointcloud_candidates_;
       map<string, int> pointcloud_priority_;
       string selected_pointcloud_topic_name_;
       vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr> pointcloud_subscribers_;
       rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr selected_pointcloud_publisher_;
+      rclcpp::Publisher<std_msgs::msg::String>::SharedPtr debug_info_publisher_;
       rclcpp::Subscription<autoware_adapi_v1_msgs::msg::LocalizationInitializationState>::SharedPtr initialization_state_subscriber_;
       autoware_adapi_v1_msgs::msg::LocalizationInitializationState last_initialization_state_;
 

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<double>(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<sensor_msgs::msg::PointCloud2>("output/pointcloud", 10);
 
+    // Create publisher of pointcloud_switcher/debug_info
+    debug_info_publisher_ = this->create_publisher<std_msgs::msg::String>("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_;
     }
   }