refactored launch file.

evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp

@@ -93,10 +93,16 @@ class ControlEvaluatorNode : public rclcpp::Node
 
   // Parameters
   bool output_metrics_;
-  
+
   // Metric
-  std::vector<Metric> metrics_;
-  std::array<Accumulator<double>, static_cast<size_t>(Metric::SIZE)> metric_accumulators_; // 3(min, max, mean) * metric_size
+  std::vector<Metric> metrics_ = {
+    // collect all metrics
+    Metric::lateral_deviation,      Metric::yaw_deviation,      Metric::goal_longitudinal_deviation,
+    Metric::goal_lateral_deviation, Metric::goal_yaw_deviation,
+  };
+
+  std::array<Accumulator<double>, static_cast<size_t>(Metric::SIZE)>
+    metric_accumulators_;  // 3(min, max, mean) * metric_size
 
   MetricArrayMsg metrics_msg_;
   autoware::route_handler::RouteHandler route_handler_;

evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef AUTOWARE__CONTROL_EVALUATOR__METRICS_METRIC_HPP_
-#define AUTOWARE__CONTROL_EVALUATOR__METRICS_METRIC_HPP_
+#ifndef AUTOWARE__CONTROL_EVALUATOR__METRICS__METRIC_HPP_
+#define AUTOWARE__CONTROL_EVALUATOR__METRICS__METRIC_HPP_
 
 #include <iostream>
 #include <string>
@@ -56,8 +56,7 @@ static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::yaw_deviation, "Yaw deviation from the reference trajectory[rad]"},
   {Metric::goal_longitudinal_deviation, "Longitudinal deviation from the goal point[m]"},
   {Metric::goal_lateral_deviation, "Lateral deviation from the goal point[m]"},
-  {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"}
-};
+  {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"}};
 
 namespace details
 {

evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml

@@ -1,20 +1,19 @@
 <launch>
-  <arg name="input/diagnostics" default="/diagnostics"/>
   <arg name="input/odometry" default="/localization/kinematic_state"/>
   <arg name="input/acceleration" default="/localization/acceleration"/>
   <arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
-  <arg name="map_topic_name" default="/map/vector_map"/>
-  <arg name="route_topic_name" default="/planning/mission_planning/route"/>
+  <arg name="input/vector_map" default="/map/vector_map"/>
+  <arg name="input/route" default="/planning/mission_planning/route"/>
+
   <!-- control evaluator -->
   <group>
     <node name="control_evaluator" exec="control_evaluator" pkg="autoware_control_evaluator">
       <param from="$(find-pkg-share autoware_control_evaluator)/param/control_evaluator.defaults.yaml"/>
-      <remap from="~/input/diagnostics" to="$(var input/diagnostics)"/>
       <remap from="~/input/odometry" to="$(var input/odometry)"/>
       <remap from="~/input/acceleration" to="$(var input/acceleration)"/>
       <remap from="~/input/trajectory" to="$(var input/trajectory)"/>
-      <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
-      <remap from="~/input/route" to="$(var route_topic_name)"/>
+      <remap from="~/input/vector_map" to="$(var input/vector_map)"/>
+      <remap from="~/input/route" to="$(var input/route)"/>
     </node>
   </group>
 </launch>

evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml

@@ -1,3 +1,3 @@
 /**:
   ros__parameters:
-    output_metrics: false # if true, metrics are written to `<ros2_logging_directory>/autoware_metrics/<node_name>-<time_stamp>.json`.
+    output_metrics: false # if true, metrics are written to `<ros2_logging_directory>/autoware_metrics/<node_name>-<time_stamp>.json`.

evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

@@ -16,12 +16,11 @@
 
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
-
 #include <nlohmann/json.hpp>
 
-#include <fstream>
 #include <chrono>
 #include <filesystem>
+#include <fstream>
 #include <limits>
 #include <string>
 #include <vector>
@@ -33,24 +32,23 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti
 {
   using std::placeholders::_1;
 
-  // Parameters
-  output_metrics_ = declare_parameter<bool>("output_metrics");
-
   // Publisher
   processing_time_pub_ =
     this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
   metrics_pub_ = create_publisher<MetricArrayMsg>("~/metrics", 1);
 
+  // Parameters
+  output_metrics_ = declare_parameter<bool>("output_metrics");
+
   // Timer callback to publish evaluator diagnostics
   using namespace std::literals::chrono_literals;
   timer_ =
     rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ControlEvaluatorNode::onTimer, this));
-
 }
 
 ControlEvaluatorNode::~ControlEvaluatorNode()
 {
-  if (!output_metrics_){
+  if (!output_metrics_) {
     return;
   }
 
@@ -65,23 +63,26 @@ ControlEvaluatorNode::~ControlEvaluatorNode()
   }
 
   // get output folder
-  const std::string output_folder_str = rclcpp::get_logging_directory().string() + "/autoware_metrics";
+  const std::string output_folder_str =
+    rclcpp::get_logging_directory().string() + "/autoware_metrics";
   if (!std::filesystem::exists(output_folder_str)) {
     if (!std::filesystem::create_directories(output_folder_str)) {
-      RCLCPP_ERROR(this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str());
+      RCLCPP_ERROR(
+        this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str());
       return;
     }
   }
 
   // get time stamp
   std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
-  std::tm* local_time = std::localtime(&now_time_t);
+  std::tm * local_time = std::localtime(&now_time_t);
   std::ostringstream oss;
   oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S");
   std::string cur_time_str = oss.str();
 
   // Write metrics .json to file
-  const std::string output_file_str = output_folder_str + "/autoware_control_evaluator-"+cur_time_str+".json";
+  const std::string output_file_str =
+    output_folder_str + "/autoware_control_evaluator-" + cur_time_str + ".json";
   std::ofstream f(output_file_str);
   if (f.is_open()) {
     f << j.dump(4);
@@ -238,7 +239,6 @@ void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg(const Pose & ego_pos
 
 void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg(const Pose & ego_pose)
 {
-
   const Metric metric = Metric::goal_yaw_deviation;
   const double metric_value = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose);
 

launch/tier4_control_launch/launch/control.launch.xml

@@ -269,21 +269,11 @@
           </composable_node>
         </load_composable_node>
       </group>
+    </group>
 
-      <!-- control evaluator -->
-      <group if="$(var launch_control_evaluator)">
-        <load_composable_node target="/control/control_check_container">
-          <composable_node pkg="autoware_control_evaluator" plugin="control_diagnostics::ControlEvaluatorNode" name="control_evaluator">
-            <remap from="~/input/diagnostics" to="/diagnostics"/>
-            <remap from="~/input/odometry" to="/localization/kinematic_state"/>
-            <remap from="~/input/acceleration" to="/localization/acceleration"/>
-            <remap from="~/input/trajectory" to="/planning/scenario_planning/trajectory"/>
-            <remap from="~/metrics" to="/control/control_evaluator/metrics"/>
-            <remap from="~/input/vector_map" to="/map/vector_map"/>
-            <remap from="~/input/route" to="/planning/mission_planning/route"/>
-          </composable_node>
-        </load_composable_node>
-      </group>
+    <!-- control evaluator -->
+    <group if="$(var launch_control_evaluator)">
+      <include file="$(find-pkg-share autoware_control_evaluator)/launch/control_evaluator.launch.xml"/>
     </group>
   </group>
 </launch>