Merge branch 'autowarefoundation:main' into feature/average_tp_collector

common/autoware_debug_tools/README.md

@@ -119,3 +119,20 @@ ros2 run autoware_debug_tools rosout_log_reconstructor
 ```
 
 ![rosout_log_example](images/rosout_log_example.png)
+
+## Frequent Log Checker
+
+This script shows the frequent log from the `launch.log`.
+In detail, the log which appears more than the threshold times during the duration will be list down.
+
+### Usage
+
+```bash
+ros2 run autoware_debug_tools frequent_log_checker <launch-log-path>
+```
+
+The command has following options.
+
+- `-d`: duration to count the number of log
+- `-c`: threshold of the log count during the duration
+- `-f`: log format. Several log formats are pre-defined in the script.

common/autoware_debug_tools/autoware_debug_tools/frequent_log_checker.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 TIER IV, Inc.
+#
+# 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.
+
+import argparse
+
+
+class Log:
+    def __init__(self, node_name, timestamp, content, file_and_line, full_message):
+        self.node_name = node_name
+        self.timestamp = timestamp
+        self.content = content
+        self.file_and_line = file_and_line
+        self.full_message = full_message
+
+    def is_same(self, log):
+        return self.node_name == log.node_name and self.file_and_line == log.file_and_line
+
+    def is_in(self, log_list):
+        for target_log in log_list:
+            if self.is_same(target_log):
+                return True
+        return False
+
+
+def check(log_file, duration_to_count, log_count_threshold, log_format):
+    recent_log_list = []
+    unique_frequent_log_list = []
+
+    with open(log_file, "r") as f:
+        for full_message in f.readlines():
+            try:
+                # The following implementation depends on the log format.
+                if log_format == "1":
+                    node_name = full_message.split("[")[4].split("]")[0]
+                    timestamp = float(full_message.split("[")[0][:-1])
+                    content = full_message.split("]")[3].split(" at ")[0]
+                    file_and_line = full_message.split("]")[3].split(" at ")[1]
+                    recent_log = Log(node_name, timestamp, content, file_and_line, full_message)
+                elif log_format == "2":
+                    node_name = full_message.split("]")[0][1:]
+                    timestamp = float(full_message.split("]")[1].split(" ")[2])
+                    content = full_message.split("]")[3].split(" at ")[0]
+                    file_and_line = full_message.split("]")[3].split(" at ")[1]
+                    recent_log = Log(node_name, timestamp, content, file_and_line, full_message)
+                else:
+                    continue
+            except IndexError:
+                continue
+            except ValueError:
+                continue
+
+            # skip if the log is already considered as frequent
+            if recent_log.is_in(unique_frequent_log_list):
+                continue
+            recent_log_list.append(recent_log)
+
+            # remove obsolete or already frequent log
+            for log in recent_log_list[:]:
+                duration = timestamp - log.timestamp
+                if duration_to_count < duration:
+                    recent_log_list.remove(log)
+
+            # extract duplicated (= frequent) log
+            for i in range(len(recent_log_list)):
+                log_count = 0
+                if recent_log_list[i].is_in(unique_frequent_log_list):
+                    continue
+
+                for j in range(len(recent_log_list)):
+                    if i <= j:
+                        continue
+                    if recent_log_list[i].is_same(recent_log_list[j]):
+                        log_count += 1
+
+                if log_count_threshold <= log_count:
+                    unique_frequent_log_list.append(recent_log_list[i])
+
+    if len(unique_frequent_log_list) == 0:
+        print(
+            "No frequent log. The log format designated by the `-f` option may be different from the actual log format."
+        )
+    else:
+        for frequent_log in unique_frequent_log_list:
+            print(frequent_log.full_message)
+
+
+def main():
+    parser = argparse.ArgumentParser(description="frequent log checker")
+    parser.add_argument("log_file", help="launch log file")
+    parser.add_argument("-d", "--log-duration", default=1.0, help="duration to count log")
+    parser.add_argument("-c", "--log-count", default=2, help="log count threshold")
+    parser.add_argument("-f", "--log-format", default="1", help="log format")
+    args = parser.parse_args()
+
+    check(args.log_file, args.log_duration, args.log_count, args.log_format)
+
+
+if __name__ == "__main__":
+    main()

common/autoware_debug_tools/setup.py

@@ -28,6 +28,7 @@
             "topic_connection_checker = autoware_debug_tools.topic_connection_checker.topic_connection_checker_node:main",
             "topic_localizer = autoware_debug_tools.topic_connection_checker.localize_topic:main",
             "rosout_log_reconstructor = autoware_debug_tools.rosout_log_reconstructor:main",
+            "frequent_log_checker = autoware_debug_tools.frequent_log_checker:main",
         ],
     },
 )

driving_environment_analyzer/src/utils.cpp

@@ -15,6 +15,7 @@
 #include "driving_environment_analyzer/utils.hpp"
 
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
 
 #include <autoware_lanelet2_extension/regulatory_elements/Forward.hpp>
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>

planning/autoware_static_centerline_generator/src/type_alias.hpp

@@ -32,14 +32,14 @@ using autoware::route_handler::RouteHandler;
 using autoware::universe_utils::LinearRing2d;
 using autoware::universe_utils::LineString2d;
 using autoware::universe_utils::Point2d;
+using autoware_internal_planning_msgs::msg::PathWithLaneId;
 using autoware_map_msgs::msg::LaneletMapBin;
 using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_planning_msgs::msg::LaneletRoute;
 using autoware_planning_msgs::msg::Path;
 using autoware_planning_msgs::msg::PathPoint;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
-using tier4_planning_msgs::msg::PathWithLaneId;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 }  // namespace autoware::static_centerline_generator

planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp

@@ -22,10 +22,10 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "autoware_internal_debug_msgs/msg/float64_multi_array_stamped.hpp"
+#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"
 #include "autoware_planning_msgs/msg/path.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "nav_msgs/msg/odometry.hpp"
-#include "tier4_planning_msgs/msg/path_with_lane_id.hpp"
 
 #include <iostream>
 #include <map>
@@ -35,11 +35,11 @@
 
 namespace planning_debug_tools
 {
+using autoware_internal_planning_msgs::msg::PathWithLaneId;
 using autoware_planning_msgs::msg::Path;
 using autoware_planning_msgs::msg::Trajectory;
 using nav_msgs::msg::Odometry;
 using planning_debug_tools::msg::TrajectoryDebugInfo;
-using tier4_planning_msgs::msg::PathWithLaneId;
 
 template <typename T>
 class TrajectoryAnalyzer

planning/planning_debug_tools/include/planning_debug_tools/util.hpp

@@ -19,9 +19,9 @@
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"
 #include "autoware_planning_msgs/msg/path.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
-#include "tier4_planning_msgs/msg/path_with_lane_id.hpp"
 
 #include <vector>
 
@@ -31,9 +31,9 @@ namespace planning_debug_tools
 using autoware::universe_utils::calcDistance2d;
 using autoware::universe_utils::getPoint;
 using autoware::universe_utils::getRPY;
+using autoware_internal_planning_msgs::msg::PathPointWithLaneId;
 using autoware_planning_msgs::msg::PathPoint;
 using autoware_planning_msgs::msg::TrajectoryPoint;
-using tier4_planning_msgs::msg::PathPointWithLaneId;
 
 double getVelocity(const PathPoint & p)
 {

planning/planning_debug_tools/package.xml

@@ -18,6 +18,7 @@
   <build_depend>rosidl_default_generators</build_depend>
 
   <depend>autoware_internal_debug_msgs</depend>
+  <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>

planning/planning_debug_tools/scripts/closest_velocity_checker.py

@@ -20,6 +20,7 @@
 from autoware_control_msgs.msg import Control as AckermannControlCommand
 from autoware_internal_debug_msgs.msg import Float32MultiArrayStamped
 from autoware_internal_debug_msgs.msg import Float32Stamped
+from autoware_internal_planning_msgs.msg import PathWithLaneId
 from autoware_planning_msgs.msg import Path
 from autoware_planning_msgs.msg import Trajectory
 from autoware_vehicle_msgs.msg import VelocityReport
@@ -31,7 +32,6 @@
 from tf2_ros import LookupException
 from tf2_ros.buffer import Buffer
 from tf2_ros.transform_listener import TransformListener
-from tier4_planning_msgs.msg import PathWithLaneId
 from tier4_planning_msgs.msg import VelocityLimit
 
 REF_LINK = "map"

planning/planning_debug_tools/scripts/trajectory_visualizer.py

@@ -16,6 +16,8 @@
 
 import argparse
 
+from autoware_internal_planning_msgs.msg import PathPointWithLaneId
+from autoware_internal_planning_msgs.msg import PathWithLaneId
 from autoware_planning_msgs.msg import Path
 from autoware_planning_msgs.msg import PathPoint
 from autoware_planning_msgs.msg import Trajectory
@@ -31,8 +33,6 @@
 from rclpy.node import Node
 from tf2_ros.buffer import Buffer
 from tf2_ros.transform_listener import TransformListener
-from tier4_planning_msgs.msg import PathPointWithLaneId
-from tier4_planning_msgs.msg import PathWithLaneId
 from tier4_planning_msgs.msg import VelocityLimit
 
 parser = argparse.ArgumentParser()