feat(driving_environment_analyzer): add new package (#13)

* feat(driving_environment_analyzer): add package Signed-off-by: satoshi-ota <[email protected]> * fix: typo Signed-off-by: satoshi-ota <[email protected]> * fix: rename func Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Feb 27, 2024 · e90db5d · e90db5d
1 parent e48d519
commit e90db5d
Show file tree

Hide file tree

Showing 6 changed files with 614 additions and 0 deletions.
diff --git a/driving_environment_analyzer/CMakeLists.txt b/driving_environment_analyzer/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.14)
+project(driving_environment_analyzer)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  DIRECTORY src
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "driving_environment_analyzer::DrivingEnvironmentAnalyzer"
+  EXECUTABLE driving_environment_analyzer
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+)
diff --git a/driving_environment_analyzer/README.md b/driving_environment_analyzer/README.md
@@ -0,0 +1,54 @@
+# Driving Environment Analyzer
+
+このツールはROSBAGに含まれる走行履歴を元に走行環境のODDを解析するツールです。
+
+## How to use
+
+現在以下の情報が出力可能です。
+
+- 走行経路の長さ
+- 走行経路の車線情報
+- 走行経路の最大・最小勾配
+- 走行経路の最大曲率
+- 走行経路の最大・最小車線幅
+- 交差点の有無
+- 信号機の有無
+- 横断歩道の有無
+
+起動時に`bag_path`オプションで解析したいROSBAGを指定してください。（ディレクトリの指定も.db3ファイルの直接指定もサポートしています。）
+
+解析に必要なtopicは以下のとおりです。（今後増える可能性もあります。）
+
+- `/planning/mission_planning/route`
+- `/map/vector_map`
+
+以下のようにlaunchすることでODDの解析結果が得られます。
+
+`ros2 launch driving_environment_analyzer driving_environment_analyzer.launch.xml use_map_in_bag:=true bag_path:=<ROSBAG>`
+
+```bash
+[component_container-1] [INFO 1708999777.768870564] [driving_environment_analyzer]: ======================================
+[component_container-1] [INFO 1708999777.768922452] [driving_environment_analyzer]:  data is ready. start ODD analysis...
+[component_container-1] [INFO 1708999777.768933574] [driving_environment_analyzer]: ======================================
+[component_container-1] [INFO 1708999777.768967412] [driving_environment_analyzer]: - Length of total lanes : 2357.50 [m]
+[component_container-1] [INFO 1708999777.769031174] [driving_environment_analyzer]: - Length of lane that has adjacent lane : 2080.43 [m]
+[component_container-1] [INFO 1708999777.769076141] [driving_environment_analyzer]: - Length of lane that has opposite lane : 0.00 [m]
+[component_container-1] [INFO 1708999777.769101793] [driving_environment_analyzer]: - Length of lane that has no adjacent lane : 277.07 [m]
+[component_container-1] [INFO 1708999777.769225729] [driving_environment_analyzer]: - Min lane width: 3.14 [m] Max lane width: 4.94 [m]
+[component_container-1] [INFO 1708999777.769278698] [driving_environment_analyzer]: - Max curvature: 0.007967 [1/m]
+[component_container-1] [INFO 1708999777.769293161] [driving_environment_analyzer]: - Min curve radius: 125.52 [m]
+[component_container-1] [INFO 1708999777.769336094] [driving_environment_analyzer]: - Min elevation angle: -0.033037 [rad] Max elevation angle: 0.026073 [rad]
+[component_container-1] [INFO 1708999777.769403870] [driving_environment_analyzer]: - Min speed limit: 13.89 [m/s] Max speed limit: 16.67 [m/s]
+[component_container-1] [INFO 1708999777.769424648] [driving_environment_analyzer]: - Exist traffic light: true
+[component_container-1] [INFO 1708999777.769435813] [driving_environment_analyzer]: - Exist intersection: true
+[component_container-1] [INFO 1708999777.769620035] [driving_environment_analyzer]: - Exist crosswalk: true
+[component_container-1] [INFO 1708999777.769634980] [driving_environment_analyzer]: ======================================
+[component_container-1] [INFO 1708999777.769642769] [driving_environment_analyzer]:  complete ODD analysis. shutdown.
+[component_container-1] [INFO 1708999777.769650034] [driving_environment_analyzer]: ======================================
+```
+
+ただし、`map/vector_map`に関しては`use_map_in_bag`を`false`にすることでローカル環境に保存されている地図を使用してODD解析を行うことも可能です。その場合、`map_path`オプションで地図のパスを指定してください。
+
+`ros2 launch driving_environment_analyzer driving_environment_analyzer.launch.xml use_map_in_bag:=false map_path:=<MAP> bag_path:=<ROSBAG>`
+
+以上のようにオプションを指定することでROSBAGに地図情報が保存されていなくてもODD解析が可能です。
diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/node.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/node.hpp
@@ -0,0 +1,68 @@
+// 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.
+
+#ifndef DRIVING_ENVIRONMENT_ANALYZER__NODE_HPP_
+#define DRIVING_ENVIRONMENT_ANALYZER__NODE_HPP_
+
+#include "rosbag2_cpp/reader.hpp"
+#include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+
+#include <route_handler/route_handler.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace driving_environment_analyzer
+{
+using autoware_auto_mapping_msgs::msg::HADMapBin;
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_planning_msgs::msg::LaneletRoute;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
+using nav_msgs::msg::Odometry;
+
+class DrivingEnvironmentAnalyzer : public rclcpp::Node
+{
+public:
+  explicit DrivingEnvironmentAnalyzer(const rclcpp::NodeOptions & node_options);
+
+private:
+  bool isDataReady(const bool use_map_in_bag);
+  void onMap(const HADMapBin::ConstSharedPtr map_msg);
+  void analyze();
+
+  bool has_map_data_{false};
+
+  std::vector<LaneletRoute> route_msgs_;
+
+  route_handler::RouteHandler route_handler_;
+  LaneletRoute::ConstSharedPtr route_ptr_{nullptr};
+  rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rosbag2_cpp::Reader reader_;
+};
+}  // namespace driving_environment_analyzer
+
+#endif  // DRIVING_ENVIRONMENT_ANALYZER__NODE_HPP_
diff --git a/driving_environment_analyzer/launch/driving_environment_analyzer.launch.xml b/driving_environment_analyzer/launch/driving_environment_analyzer.launch.xml
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="map_path" description="point cloud and lanelet2 map directory path"/>
+  <arg name="bag_path" description="bagfile path"/>
+  <arg name="use_map_in_bag" default="false"/>
+  <arg name="lanelet2_map_loader_param_path" default="$(find-pkg-share autoware_launch)/config/map/lanelet2_map_loader.param.yaml"/>
+  <arg name="map_projection_loader_param_path" default="$(find-pkg-share autoware_launch)/config/map/map_projection_loader.param.yaml"/>
+
+  <node_container pkg="rclcpp_components" exec="component_container" name="map_container" namespace="" output="screen">
+    <composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader">
+      <param from="$(var lanelet2_map_loader_param_path)"/>
+      <param name="lanelet2_map_path" value="$(var map_path)/lanelet2_map.osm"/>
+      <remap from="output/lanelet2_map" to="/map/vector_map"/>
+    </composable_node>
+
+    <composable_node pkg="driving_environment_analyzer" plugin="driving_environment_analyzer::DrivingEnvironmentAnalyzer" name="driving_environment_analyzer">
+      <param name="bag_path" value="$(var bag_path)"/>
+      <param name="use_map_in_bag" value="$(var use_map_in_bag)"/>
+      <remap from="input/lanelet2_map" to="/map/vector_map"/>
+    </composable_node>
+  </node_container>
+
+  <include file="$(find-pkg-share map_projection_loader)/launch/map_projection_loader.launch.xml">
+    <arg name="param_path" value="$(var map_projection_loader_param_path)"/>
+    <arg name="map_projector_info_path" value="$(var map_path)/map_projector_info.yaml"/>
+    <arg name="lanelet2_map_path" value="$(var map_path)/lanelet2_map.osm"/>
+  </include>
+</launch>
diff --git a/driving_environment_analyzer/package.xml b/driving_environment_analyzer/package.xml
@@ -0,0 +1,51 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>driving_environment_analyzer</name>
+  <version>0.1.0</version>
+  <description>The driving_environment_analyzer package</description>
+
+  <maintainer email="[email protected]">Satoshi Ota</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Satoshi Ota</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_auto_tf2</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
+  <depend>behavior_path_planner_common</depend>
+  <depend>geometry_msgs</depend>
+  <depend>interpolation</depend>
+  <depend>lane_departure_checker</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>libboost-dev</depend>
+  <depend>motion_utils</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>route_handler</depend>
+  <depend>sensor_msgs</depend>
+  <depend>signal_processing</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_planning_msgs</depend>
+  <depend>vehicle_info_util</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>