diff --git a/__pycache__/mkdocs_macros.cpython-310.pyc b/__pycache__/mkdocs_macros.cpython-310.pyc new file mode 100644 index 000000000..d3a6c1980 Binary files /dev/null and b/__pycache__/mkdocs_macros.cpython-310.pyc differ diff --git a/driving_environment_analyzer/CMakeLists.txt b/driving_environment_analyzer/CMakeLists.txt index d1b9fbd25..6641bcf87 100644 --- a/driving_environment_analyzer/CMakeLists.txt +++ b/driving_environment_analyzer/CMakeLists.txt @@ -3,17 +3,30 @@ project(driving_environment_analyzer) find_package(autoware_cmake REQUIRED) autoware_package() +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) -ament_auto_add_library(${PROJECT_NAME}_node SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED + include/${PROJECT_NAME}/driving_environment_analyzer_node.hpp + include/${PROJECT_NAME}/driving_environment_analyzer_rviz_plugin.hpp DIRECTORY src ) -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "driving_environment_analyzer::DrivingEnvironmentAnalyzer" - EXECUTABLE driving_environment_analyzer +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "driving_environment_analyzer::DrivingEnvironmentAnalyzerNode" + EXECUTABLE driving_environment_analyzer_node +) + +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + ament_auto_package( INSTALL_TO_SHARE - launch + launch + plugins ) diff --git a/driving_environment_analyzer/README.md b/driving_environment_analyzer/README.md index 3e31b78c8..28d3ef30f 100644 --- a/driving_environment_analyzer/README.md +++ b/driving_environment_analyzer/README.md @@ -2,7 +2,94 @@ このツールはROSBAGに含まれる走行履歴を元に走行環境のODDを解析するツールです。 -## How to use +## ROSBAGの特定時刻における周囲のODDを解析する場合 + +この場合にはRvizプラグインである`driving_environment_analyzer_rviz_panel`を使用することをおすすめします。 + +現在以下の情報が出力可能です。 + +- EGOの現在車速 +- 現在位置の勾配 +- EGOの挙動 +- 現在の車線情報 + +こちらのツールはautoware_launchに含まれる`logging_simulator`を使用します。まず以下のコマンドからシミュレータを起動してください。 + +`ros2 launch autoware_launch logging_simulator.launch.xml map_path:= vehicle_model:= sensor_model:= sensing:=false control:=false planning:=false perception:=false localization:=false system:=false` + +![fig1](./images/launch_rviz.png) + +シミュレータ起動時に地図を読み込むためROSBAGに地図情報が含まれていなくてもODDの解析が可能です。（ただし、その場合にはROSBAG取得の際に使用した地図を指定してシミュレータを起動するようにしてください。） + +次に本パッケージに含まれる解析ツールを起動します。Rviz画面左上部のAdd New PanelからDrivingEnvironmentAnalyzerPanelを選択してください。これでRviz左下に新しく操作パネルが追加されます。 + +![fig1](./images/launch_tool.png) +![fig1](./images/rviz_overview_1.png) + +本ツールはROSBAGファイル指定してロードできる他、複数のROSBAGファイルが格納されているディレクトリを指定することも可能です。ただし、その場合には事前に以下のコマンドで`metadata.yaml`の生成が必要になります。 + +`ros2 bag reindex sqlite3` + +![fig1](./images/select_directory.png) + +ROSBAGの読み込みが完了したらODDを解析したい時刻を指定します。時刻の指定にはUnix timeを直接指定するほかスライドバーも使用可能です。左に表示されている日時を参考に調整してください。 + +![fig1](./images/select_time .png) + +また、このときViewsのTarget Flameを`base_link`にしておくことで、指定した時刻のEGOの位置と周囲の状況をRvizで可視化可能です。 + +![fig1](./images/select_target_frame.png) + +時刻の指定が完了したら、`Set time stamp`ボタンを押し、最後に`Analyze dynamic ODD factor`を押すことで解析が始まります。 + +![fig1](./images/rviz_overview_2.png) + +```bash +[rviz2-11] *********************************************************** +[rviz2-11] ODD analysis result +[rviz2-11] *********************************************************** +[rviz2-11] Type: TIME SPECIFIED +[rviz2-11] Time: 2024-04-22 14:48:05 +[rviz2-11] +[rviz2-11] +[rviz2-11] - EGO INFO +[rviz2-11] [SPEED] : 0 [m/s] +[rviz2-11] [ELEVATION ANGLE] : 0.00963597 [rad] +[rviz2-11] +[rviz2-11] - EGO BEHAIOVR +[rviz2-11] [AVOIDANCE(R)] : NONE +[rviz2-11] [AVOIDANCE(L)] : NONE +[rviz2-11] [LANE_CHANGE(R)] : NONE +[rviz2-11] [LANE_CHANGE(L)] : NONE +[rviz2-11] [START_PLANNER] : SAFE: true COMMAND: deactivate +[rviz2-11] [GOAL_PLANNER] : NONE +[rviz2-11] [CROSSWALK] : NONE +[rviz2-11] [INTERSECTION] : NONE +[rviz2-11] +[rviz2-11] - LANE INFO +[rviz2-11] [ID] : 176126 +[rviz2-11] [WIDTH] : 4.24132 [m] +[rviz2-11] [SHAPE] : STRAIGHT +[rviz2-11] [RIGHT LANE NUM] : 0 +[rviz2-11] [LEFT LANE NUM] : 0 +[rviz2-11] [TOTAL LANE NUM] : 1 +[rviz2-11] [SAME DIRECTION LANE] : NONE +[rviz2-11] [OPPOSITE DIRECTION LANE] : NONE +[rviz2-11] [ROAD SHOULDER] : EXIST +[rviz2-11] +[rviz2-11] - SURROUND OBJECT NUM +[rviz2-11] [UNKNOWN] : 0 +[rviz2-11] [CAR] : 6 +[rviz2-11] [TRUCK] : 0 +[rviz2-11] [BUS] : 3 +[rviz2-11] [TRAILER] : 2 +[rviz2-11] [MOTORCYCLE] : 0 +[rviz2-11] [BICYCLE] : 0 +[rviz2-11] [PEDESTRIAN] : 7 +[rviz2-11] *********************************************************** +``` + +## ROSBAG全体に対して経路沿いのODDを解析する場合 現在以下の情報が出力可能です。 diff --git a/driving_environment_analyzer/images/launch_rviz.png b/driving_environment_analyzer/images/launch_rviz.png new file mode 100644 index 000000000..fa2adcf74 Binary files /dev/null and b/driving_environment_analyzer/images/launch_rviz.png differ diff --git a/driving_environment_analyzer/images/launch_tool.png b/driving_environment_analyzer/images/launch_tool.png new file mode 100644 index 000000000..5db44cdbe Binary files /dev/null and b/driving_environment_analyzer/images/launch_tool.png differ diff --git a/driving_environment_analyzer/images/rviz_overview_1.png b/driving_environment_analyzer/images/rviz_overview_1.png new file mode 100644 index 000000000..b5b269922 Binary files /dev/null and b/driving_environment_analyzer/images/rviz_overview_1.png differ diff --git a/driving_environment_analyzer/images/rviz_overview_2.png b/driving_environment_analyzer/images/rviz_overview_2.png new file mode 100644 index 000000000..e626873df Binary files /dev/null and b/driving_environment_analyzer/images/rviz_overview_2.png differ diff --git a/driving_environment_analyzer/images/select_directory.png b/driving_environment_analyzer/images/select_directory.png new file mode 100644 index 000000000..6b40fe48f Binary files /dev/null and b/driving_environment_analyzer/images/select_directory.png differ diff --git a/driving_environment_analyzer/images/select_target_frame.png b/driving_environment_analyzer/images/select_target_frame.png new file mode 100644 index 000000000..97c1342ac Binary files /dev/null and b/driving_environment_analyzer/images/select_target_frame.png differ diff --git a/driving_environment_analyzer/images/select_time.png b/driving_environment_analyzer/images/select_time.png new file mode 100644 index 000000000..8ff092d0a Binary files /dev/null and b/driving_environment_analyzer/images/select_time.png differ diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/analyzer_core.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/analyzer_core.hpp new file mode 100644 index 000000000..df8400314 --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/analyzer_core.hpp @@ -0,0 +1,101 @@ +// 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__ANALYZER_CORE_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_ + +#include "driving_environment_analyzer/type_alias.hpp" +#include "rosbag2_cpp/reader.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace driving_environment_analyzer::analyzer_core +{ + +struct ODDRawData +{ + rcutils_time_point_value_t timestamp; + Odometry odometry; + PredictedObjects objects; + TFMessage tf; + TFMessage tf_static; + CooperateStatusArray rtc_status; +}; + +class AnalyzerCore +{ +public: + explicit AnalyzerCore(rclcpp::Node & node); + ~AnalyzerCore(); + + bool isDataReadyForStaticODDAnalysis() const; + bool isDataReadyForDynamicODDAnalysis() const { return odd_raw_data_.has_value(); } + + void analyzeStaticODDFactor() const; + void analyzeDynamicODDFactor() const; + + void setBagFile(const std::string & file_name); + + void setTimeStamp(const rcutils_time_point_value_t & timestamp) + { + odd_raw_data_ = getRawData(timestamp); + } + + void setMap(const HADMapBin & msg) { route_handler_.setMap(msg); } + + void clearData() { odd_raw_data_ = std::nullopt; } + + std::pair getBagStartEndTime() + { + const auto metadata = reader_.get_metadata(); + const auto start_time = + duration_cast(metadata.starting_time.time_since_epoch()).count(); + const auto duration_time = duration_cast(metadata.duration).count(); + return {start_time, start_time + duration_time}; + } + + Odometry getOdometry() const { return odd_raw_data_.value().odometry; } + PredictedObjects getObjects() const { return odd_raw_data_.value().objects; } + TFMessage getTF() const { return odd_raw_data_.value().tf; } + TFMessage getTFStatic() const { return odd_raw_data_.value().tf_static; } + +private: + Pose getEgoPose() const { return odd_raw_data_.value().odometry.pose.pose; } + + double getEgoSpeed() const { return odd_raw_data_.value().odometry.twist.twist.linear.x; } + + template + std::optional getLastTopic(const std::string & topic_name); + template + std::optional seekTopic( + const std::string & topic_name, const rcutils_time_point_value_t & timestamp); + std::optional getRawData(const rcutils_time_point_value_t & timestamp); + + std::optional odd_raw_data_{std::nullopt}; + + route_handler::RouteHandler route_handler_; + + rosbag2_cpp::Reader reader_; + + rclcpp::Logger logger_; +}; +} // namespace driving_environment_analyzer::analyzer_core + +#endif // DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_ diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_node.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_node.hpp new file mode 100644 index 000000000..b6ddc1ad4 --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_node.hpp @@ -0,0 +1,45 @@ +// 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__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_ + +#include "driving_environment_analyzer/analyzer_core.hpp" +#include "driving_environment_analyzer/type_alias.hpp" + +#include +#include +#include + +namespace driving_environment_analyzer +{ + +class DrivingEnvironmentAnalyzerNode : public rclcpp::Node +{ +public: + explicit DrivingEnvironmentAnalyzerNode(const rclcpp::NodeOptions & node_options); + +private: + void onMap(const HADMapBin::ConstSharedPtr map_msg); + void analyze(); + + std::shared_ptr analyzer_; + + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::TimerBase::SharedPtr timer_; + rosbag2_cpp::Reader reader_; +}; +} // namespace driving_environment_analyzer + +#endif // DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_ diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp new file mode 100644 index 000000000..c69ea0a6a --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp @@ -0,0 +1,84 @@ +// 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__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_ + +#include "driving_environment_analyzer/analyzer_core.hpp" +#include "driving_environment_analyzer/type_alias.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace driving_environment_analyzer +{ + +class DrivingEnvironmentAnalyzerPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit DrivingEnvironmentAnalyzerPanel(QWidget * parent = nullptr); + ~DrivingEnvironmentAnalyzerPanel() override; + void onInitialize() override; + +public Q_SLOTS: + void onBoxUpdate(); + void onSliderUpdate(); + void onClickSetTimeStamp(); + void onSelectBagFile(); + void onSelectDirectory(); + void onClickAnalyzeStaticODDFactor(); + void onClickAnalyzeDynamicODDFactor(); + +private: + void onMap(const HADMapBin::ConstSharedPtr map_msg); + + std::shared_ptr analyzer_; + + QSpinBox * bag_time_selector_; + QSlider * bag_time_slider_; + QLabel * bag_name_label_; + QLabel * bag_time_line_; + QPushButton * dir_button_ptr_; + QPushButton * file_button_ptr_; + QPushButton * analyze_static_odd_button_; + QPushButton * analyze_dynamic_odd_button_; + QPushButton * set_timestamp_btn_; + + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Publisher::SharedPtr pub_odometry_; + rclcpp::Publisher::SharedPtr pub_objects_; + rclcpp::Publisher::SharedPtr pub_tf_; + rclcpp::Publisher::SharedPtr pub_tf_static_; +}; +} // namespace driving_environment_analyzer + +#endif // DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_ diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/node.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/node.hpp deleted file mode 100644 index da2677b0f..000000000 --- a/driving_environment_analyzer/include/driving_environment_analyzer/node.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// 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 - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -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 route_msgs_; - - route_handler::RouteHandler route_handler_; - LaneletRoute::ConstSharedPtr route_ptr_{nullptr}; - rclcpp::Subscription::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/include/driving_environment_analyzer/type_alias.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/type_alias.hpp new file mode 100644 index 000000000..30721beda --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/type_alias.hpp @@ -0,0 +1,53 @@ +// 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__TYPE_ALIAS_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__TYPE_ALIAS_HPP_ + +#include +#include + +#include "tier4_rtc_msgs/msg/command.hpp" +#include "tier4_rtc_msgs/msg/cooperate_status_array.hpp" +#include "tier4_rtc_msgs/msg/module.hpp" +#include +#include +#include +#include +#include + +namespace driving_environment_analyzer +{ + +// std +using std::chrono::duration_cast; +using std::chrono::seconds; + +// ros2 +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using tf2_msgs::msg::TFMessage; + +// autoware +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::LaneletRoute; +using tier4_rtc_msgs::msg::Command; +using tier4_rtc_msgs::msg::CooperateStatusArray; +using tier4_rtc_msgs::msg::Module; + +} // namespace driving_environment_analyzer + +#endif // DRIVING_ENVIRONMENT_ANALYZER__TYPE_ALIAS_HPP_ diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/utils.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/utils.hpp new file mode 100644 index 000000000..8bb570231 --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/utils.hpp @@ -0,0 +1,96 @@ +// 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__UTILS_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__UTILS_HPP_ + +#include "driving_environment_analyzer/type_alias.hpp" + +#include +#include +#include + +namespace driving_environment_analyzer::utils +{ + +template +std::vector calcElevationAngle(const T & points); + +double calcElevationAngle(const lanelet::ConstLanelet & lane, const Pose & pose); + +double getRouteLength(const lanelet::ConstLanelets & lanes); + +double getMaxCurvature(const lanelet::ConstLanelets & lanes); + +double getLaneWidth(const lanelet::ConstLanelet & lane); + +std::pair getLaneWidth(const lanelet::ConstLanelets & lanes); + +std::pair getElevation(const lanelet::ConstLanelets & lanes); + +std::pair getSpeedLimit( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +double getRouteLengthWithSameDirectionLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +double getRouteLengthWithOppositeDirectionLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +double getRouteLengthWithNoAdjacentLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +bool existSameDirectionLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +bool existOppositeDirectionLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +bool existRoadShoulderLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +int getLeftLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +int getRightLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +int getTotalLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +bool existTrafficLight(const lanelet::ConstLanelets & lanes); + +bool existIntersection(const lanelet::ConstLanelets & lanes); + +bool existCrosswalk( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +bool existCrosswalk( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +bool isStraightLane(const lanelet::ConstLanelet & lane); + +size_t getObjectNumber(const PredictedObjects & objects, const std::uint8_t target_class); + +std::string getLaneShape(const lanelet::ConstLanelet & lane); + +std::string getModuleStatus(const CooperateStatusArray & status_array, const uint8_t module_type); + +std::string getEgoBehavior( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler, + const Pose & pose); + +} // namespace driving_environment_analyzer::utils + +#endif // DRIVING_ENVIRONMENT_ANALYZER__UTILS_HPP_ diff --git a/driving_environment_analyzer/package.xml b/driving_environment_analyzer/package.xml index 3446de71f..025e91be1 100644 --- a/driving_environment_analyzer/package.xml +++ b/driving_environment_analyzer/package.xml @@ -26,26 +26,30 @@ lane_departure_checker lanelet2_extension libboost-dev + libqt5-core + libqt5-gui + libqt5-widgets motion_utils + qtbase5-dev rclcpp rclcpp_components route_handler + rviz_common + rviz_default_plugins sensor_msgs signal_processing tf2 tf2_eigen tf2_geometry_msgs + tf2_msgs tf2_ros tier4_autoware_utils tier4_planning_msgs vehicle_info_util visualization_msgs - ament_cmake_ros - ament_lint_auto - autoware_lint_common - ament_cmake + diff --git a/driving_environment_analyzer/plugins/plugin_description.xml b/driving_environment_analyzer/plugins/plugin_description.xml new file mode 100644 index 000000000..fe19f8191 --- /dev/null +++ b/driving_environment_analyzer/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + DrivingEnvironmentAnalyzerPanel + + + diff --git a/driving_environment_analyzer/src/analyzer_core.cpp b/driving_environment_analyzer/src/analyzer_core.cpp new file mode 100644 index 000000000..8a90e1ebe --- /dev/null +++ b/driving_environment_analyzer/src/analyzer_core.cpp @@ -0,0 +1,298 @@ +// 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. + +#include "driving_environment_analyzer/analyzer_core.hpp" + +#include "driving_environment_analyzer/utils.hpp" + +#include +#include +#include + +namespace driving_environment_analyzer::analyzer_core +{ + +AnalyzerCore::AnalyzerCore(rclcpp::Node & node) : logger_{node.get_logger()} +{ +} + +bool AnalyzerCore::isDataReadyForStaticODDAnalysis() const +{ + if (!route_handler_.isMapMsgReady()) { + RCLCPP_WARN(logger_, "Map data is not ready."); + return false; + } + + if (!route_handler_.isHandlerReady()) { + RCLCPP_WARN(logger_, "Route data is not ready."); + return false; + } + + return true; +} + +void AnalyzerCore::setBagFile(const std::string & file_name) +{ + reader_.open(file_name); + + const auto opt_route = getLastTopic("/planning/mission_planning/route"); + if (opt_route.has_value()) { + route_handler_.setRoute(opt_route.value()); + } + + const auto opt_map = getLastTopic("/map/vector_map"); + if (opt_map.has_value()) { + route_handler_.setMap(opt_map.value()); + } +} + +template +std::optional AnalyzerCore::getLastTopic(const std::string & topic_name) +{ + rosbag2_storage::StorageFilter filter; + filter.topics.emplace_back(topic_name); + reader_.set_filter(filter); + + if (!reader_.has_next()) { + return std::nullopt; + } + + rclcpp::Serialization serializer; + + const auto deserialized_message = std::make_shared(); + while (reader_.has_next()) { + rclcpp::SerializedMessage serialized_msg(*reader_.read_next()->serialized_data); + serializer.deserialize_message(&serialized_msg, deserialized_message.get()); + } + + return *deserialized_message; +} + +template +std::optional AnalyzerCore::seekTopic( + const std::string & topic_name, const rcutils_time_point_value_t & timestamp) +{ + rosbag2_storage::StorageFilter filter; + filter.topics.emplace_back(topic_name); + reader_.set_filter(filter); + + rclcpp::Serialization serializer; + reader_.seek(timestamp); + + if (!reader_.has_next()) { + return std::nullopt; + } + + rclcpp::SerializedMessage serialized_msg(*reader_.read_next()->serialized_data); + const auto deserialized_message = std::make_shared(); + serializer.deserialize_message(&serialized_msg, deserialized_message.get()); + + return *deserialized_message; +} + +std::optional AnalyzerCore::getRawData(const rcutils_time_point_value_t & timestamp) +{ + ODDRawData odd_raw_data; + + odd_raw_data.timestamp = timestamp; + + const auto opt_odometry = seekTopic("/localization/kinematic_state", timestamp * 1e9); + if (!opt_odometry.has_value()) { + return std::nullopt; + } else { + odd_raw_data.odometry = opt_odometry.value(); + } + + const auto opt_objects = + seekTopic("/perception/object_recognition/objects", timestamp * 1e9); + if (!opt_objects.has_value()) { + return std::nullopt; + } else { + odd_raw_data.objects = opt_objects.value(); + } + + const auto opt_rtc_status = + seekTopic("/api/external/get/rtc_status", timestamp * 1e9); + if (!opt_rtc_status.has_value()) { + return std::nullopt; + } else { + odd_raw_data.rtc_status = opt_rtc_status.value(); + } + + const auto opt_tf = seekTopic("/tf", timestamp * 1e9); + if (!opt_tf.has_value()) { + return std::nullopt; + } else { + odd_raw_data.tf = opt_tf.value(); + } + + const auto [start_time, end_time] = getBagStartEndTime(); + const auto opt_tf_static = seekTopic("/tf_static", start_time * 1e9); + if (!opt_tf_static.has_value()) { + return std::nullopt; + } else { + odd_raw_data.tf_static = opt_tf_static.value(); + } + + return odd_raw_data; +} + +void AnalyzerCore::analyzeDynamicODDFactor() const +{ + std::ostringstream ss; + ss << std::boolalpha << "\n"; + ss << "***********************************************************\n"; + ss << " ODD analysis result\n"; + ss << "***********************************************************\n"; + ss << "Type: TIME SPECIFIED\n"; + + char buffer[128]; + auto seconds = static_cast(odd_raw_data_.value().timestamp); + strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", localtime(&seconds)); + ss << "Time: " << buffer << "\n"; + ss << "\n"; + ss << "\n"; + + lanelet::ConstLanelet closest_lanelet; + if (!route_handler_.getClosestLaneletWithinRoute(getEgoPose(), &closest_lanelet)) { + return; + } + + const auto number = [this](const auto & target_class) { + return utils::getObjectNumber(odd_raw_data_.value().objects, target_class); + }; + + const auto status = [this](const auto & module_type) { + return utils::getModuleStatus(odd_raw_data_.value().rtc_status, module_type); + }; + + const auto exist_crosswalk = [this, &closest_lanelet]() { + return utils::existCrosswalk(closest_lanelet, route_handler_) ? "EXIST" : "NONE"; + }; + + const auto to_string = [](const bool exist) { return exist ? "EXIST" : "NONE"; }; + + ss << "- EGO INFO\n"; + ss << " [SPEED] : " << getEgoSpeed() << " [m/s]\n"; + ss << " [ELEVATION ANGLE] : " + << utils::calcElevationAngle(closest_lanelet, getEgoPose()) << " [rad]\n"; + ss << "\n"; + + ss << "- EGO BEHAVIOR\n"; + ss << " [AVOIDANCE(R)] : " << status(Module::AVOIDANCE_RIGHT) << "\n"; + ss << " [AVOIDANCE(L)] : " << status(Module::AVOIDANCE_LEFT) << "\n"; + ss << " [LANE_CHANGE(R)] : " << status(Module::LANE_CHANGE_RIGHT) << "\n"; + ss << " [LANE_CHANGE(L)] : " << status(Module::LANE_CHANGE_LEFT) << "\n"; + ss << " [START_PLANNER] : " << status(Module::START_PLANNER) << "\n"; + ss << " [GOAL_PLANNER] : " << status(Module::GOAL_PLANNER) << "\n"; + ss << " [CROSSWALK] : " << exist_crosswalk() << "\n"; + ss << " [INTERSECTION] : " + << utils::getEgoBehavior(closest_lanelet, route_handler_, getEgoPose()) << "\n"; + ss << "\n"; + + ss << "- LANE INFO\n"; + ss << " [ID] : " << closest_lanelet.id() << "\n"; + ss << " [WIDTH] : " << utils::getLaneWidth(closest_lanelet) << " [m]\n"; + ss << " [SHAPE] : " << utils::getLaneShape(closest_lanelet) << "\n"; + ss << " [RIGHT LANE NUM] : " + << utils::getRightLaneletNum(closest_lanelet, route_handler_) << "\n"; + ss << " [LEFT LANE NUM] : " + << utils::getLeftLaneletNum(closest_lanelet, route_handler_) << "\n"; + ss << " [TOTAL LANE NUM] : " + << utils::getTotalLaneletNum(closest_lanelet, route_handler_) << "\n"; + ss << " [SAME DIRECTION LANE] : " + << to_string(utils::existSameDirectionLane(closest_lanelet, route_handler_)) << "\n"; + ss << " [OPPOSITE DIRECTION LANE] : " + << to_string(utils::existOppositeDirectionLane(closest_lanelet, route_handler_)) << "\n"; + ss << " [ROAD SHOULDER] : " + << to_string(utils::existRoadShoulderLane(closest_lanelet, route_handler_)) << "\n"; + ss << "\n"; + + ss << "- SURROUND OBJECT NUM\n"; + ss << " [UNKNOWN] : " << number(ObjectClassification::UNKNOWN) << "\n"; + ss << " [CAR] : " << number(ObjectClassification::CAR) << "\n"; + ss << " [TRUCK] : " << number(ObjectClassification::TRUCK) << "\n"; + ss << " [BUS] : " << number(ObjectClassification::BUS) << "\n"; + ss << " [TRAILER] : " << number(ObjectClassification::TRAILER) << "\n"; + ss << " [MOTORCYCLE] : " << number(ObjectClassification::MOTORCYCLE) << "\n"; + ss << " [BICYCLE] : " << number(ObjectClassification::BICYCLE) << "\n"; + ss << " [PEDESTRIAN] : " << number(ObjectClassification::PEDESTRIAN) << "\n"; + + ss << "***********************************************************\n"; + + RCLCPP_INFO_STREAM(logger_, ss.str()); +} + +void AnalyzerCore::analyzeStaticODDFactor() const +{ + std::ostringstream ss; + ss << std::boolalpha << "\n"; + ss << "***********************************************************\n"; + ss << " ODD analysis result\n"; + ss << "***********************************************************\n"; + ss << "Type: ROUTE SPECIFIED\n"; + ss << "\n"; + ss << "\n"; + + const auto preferred_lanes = route_handler_.getPreferredLanelets(); + ss << "- ROUTE INFO\n"; + ss << " total length : " << utils::getRouteLength(preferred_lanes) + << " [m]\n"; + ss << " exist same direction lane section : " + << utils::getRouteLengthWithSameDirectionLane(preferred_lanes, route_handler_) << " [m]\n"; + ss << " exist same opposite lane section : " + << utils::getRouteLengthWithOppositeDirectionLane(preferred_lanes, route_handler_) << " [m]\n"; + ss << " no adjacent lane section : " + << utils::getRouteLengthWithNoAdjacentLane(preferred_lanes, route_handler_) << " [m]\n"; + + ss << " exist traffic light : " << utils::existTrafficLight(preferred_lanes) + << "\n"; + ss << " exist intersection : " << utils::existIntersection(preferred_lanes) + << "\n"; + ss << " exist crosswalk : " + << utils::existCrosswalk(preferred_lanes, route_handler_) << "\n"; + ss << "\n"; + + const auto [min_width, max_width] = utils::getLaneWidth(preferred_lanes); + ss << "- LANE WIDTH\n"; + ss << " max : " << max_width << " [m]\n"; + ss << " min : " << min_width << " [m]\n"; + ss << "\n"; + + const auto max_curvature = utils::getMaxCurvature(preferred_lanes); + ss << "- LANE CURVATURE\n"; + ss << " max : " << max_curvature << " [1/m]\n"; + ss << " max : " << 1.0 / max_curvature << " [m]\n"; + ss << "\n"; + + const auto [min_elevation, max_elevation] = utils::getElevation(preferred_lanes); + ss << "- ELEVATION ANGLE\n"; + ss << " max : " << max_elevation << " [rad]\n"; + ss << " min : " << min_elevation << " [rad]\n"; + ss << "\n"; + + const auto [min_speed_limit, max_speed_limit] = + utils::getSpeedLimit(preferred_lanes, route_handler_); + ss << "- SPEED LIMIT\n"; + ss << " max : " << max_speed_limit << " [m/s]\n"; + ss << " min : " << min_speed_limit << " [m/s]\n"; + ss << "\n"; + + ss << "***********************************************************\n"; + + RCLCPP_INFO_STREAM(logger_, ss.str()); +} + +AnalyzerCore::~AnalyzerCore() = default; +} // namespace driving_environment_analyzer::analyzer_core diff --git a/driving_environment_analyzer/src/driving_environment_analyzer_node.cpp b/driving_environment_analyzer/src/driving_environment_analyzer_node.cpp new file mode 100644 index 000000000..2517509d5 --- /dev/null +++ b/driving_environment_analyzer/src/driving_environment_analyzer_node.cpp @@ -0,0 +1,62 @@ +// 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. + +#include "driving_environment_analyzer/driving_environment_analyzer_node.hpp" + +#include "driving_environment_analyzer/analyzer_core.hpp" + +#include +#include +#include + +namespace driving_environment_analyzer +{ + +DrivingEnvironmentAnalyzerNode::DrivingEnvironmentAnalyzerNode( + const rclcpp::NodeOptions & node_options) +: Node("driving_environment_analyzer", node_options) +{ + using std::placeholders::_1; + using std::chrono_literals::operator""ms; + + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&DrivingEnvironmentAnalyzerNode::analyze, this)); + + sub_map_ = create_subscription( + "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), + std::bind(&DrivingEnvironmentAnalyzerNode::onMap, this, _1)); + + analyzer_ = std::make_shared(*this); + + analyzer_->setBagFile(declare_parameter("bag_path")); +} + +void DrivingEnvironmentAnalyzerNode::onMap(const HADMapBin::ConstSharedPtr msg) +{ + analyzer_->setMap(*msg); +} + +void DrivingEnvironmentAnalyzerNode::analyze() +{ + if (!analyzer_->isDataReadyForStaticODDAnalysis()) { + return; + } + + analyzer_->analyzeStaticODDFactor(); + rclcpp::shutdown(); +} +} // namespace driving_environment_analyzer + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(driving_environment_analyzer::DrivingEnvironmentAnalyzerNode) diff --git a/driving_environment_analyzer/src/driving_environment_analyzer_rviz_plugin.cpp b/driving_environment_analyzer/src/driving_environment_analyzer_rviz_plugin.cpp new file mode 100644 index 000000000..3f9a3c7d2 --- /dev/null +++ b/driving_environment_analyzer/src/driving_environment_analyzer_rviz_plugin.cpp @@ -0,0 +1,216 @@ +// 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. + +#include "driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp" + +#include "driving_environment_analyzer/analyzer_core.hpp" + +#include +#include +#include + +namespace driving_environment_analyzer +{ +namespace +{ +void set_format_time(QLabel * label, double time) +{ + char buffer[128]; + auto seconds = static_cast(time); + strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", localtime(&seconds)); + label->setText(QString(buffer) + QString::number((time - seconds), 'f', 3).rightRef(4)); +} +} // namespace + +DrivingEnvironmentAnalyzerPanel::DrivingEnvironmentAnalyzerPanel(QWidget * parent) +: rviz_common::Panel(parent) +{ + auto * v_layout = new QVBoxLayout(this); + { + auto * layout = new QHBoxLayout(this); + + // add button to load bag file. + file_button_ptr_ = new QPushButton("Select bag file"); + connect(file_button_ptr_, SIGNAL(clicked()), SLOT(onSelectBagFile())); + layout->addWidget(file_button_ptr_); + + // add button to load directory. + dir_button_ptr_ = new QPushButton("Select directory"); + connect(dir_button_ptr_, SIGNAL(clicked()), SLOT(onSelectDirectory())); + layout->addWidget(dir_button_ptr_); + + v_layout->addLayout(layout); + } + + { + auto * layout = new QHBoxLayout(this); + + // add label to show target time. + layout->addWidget(new QLabel("Bag Time:")); + bag_time_line_ = new QLabel; + layout->addWidget(bag_time_line_); + + // add spin box to select target time. + bag_time_selector_ = new QSpinBox(); + bag_time_selector_->setValue(0); + bag_time_selector_->setSingleStep(1); + connect(bag_time_selector_, SIGNAL(valueChanged(int)), SLOT(onBoxUpdate())); + layout->addWidget(bag_time_selector_); + + // add button to set target time. + set_timestamp_btn_ = new QPushButton("Set time stamp"); + connect(set_timestamp_btn_, SIGNAL(clicked()), SLOT(onClickSetTimeStamp())); + layout->addWidget(set_timestamp_btn_); + + // add button to start analyzing dynamic ODD. + analyze_static_odd_button_ = new QPushButton("Analyze dynamic ODD factor"); + connect(analyze_static_odd_button_, SIGNAL(clicked()), SLOT(onClickAnalyzeDynamicODDFactor())); + layout->addWidget(analyze_static_odd_button_); + + // add button to start analyzing static ODD. + analyze_dynamic_odd_button_ = new QPushButton("Analyze static ODD factor"); + connect(analyze_dynamic_odd_button_, SIGNAL(clicked()), SLOT(onClickAnalyzeStaticODDFactor())); + layout->addWidget(analyze_dynamic_odd_button_); + + v_layout->addLayout(layout); + } + + { + bag_time_slider_ = new QSlider(); + bag_time_slider_->setOrientation(Qt::Horizontal); + bag_time_slider_->setValue(0); + connect(bag_time_slider_, SIGNAL(valueChanged(int)), SLOT(onSliderUpdate())); + v_layout->addWidget(bag_time_slider_); + } + + { + bag_name_label_ = new QLabel(); + bag_name_label_->setAlignment(Qt::AlignLeft); + auto * layout = new QHBoxLayout(this); + layout->addWidget(new QLabel("BagFile:")); + layout->addWidget(bag_name_label_); + v_layout->addLayout(layout); + } + + setLayout(v_layout); +} + +void DrivingEnvironmentAnalyzerPanel::onInitialize() +{ + using std::placeholders::_1; + + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_map_ = raw_node_->create_subscription( + "/map/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&DrivingEnvironmentAnalyzerPanel::onMap, this, _1)); + + pub_odometry_ = + raw_node_->create_publisher("/localization/kinematic_state", rclcpp::QoS(1)); + pub_objects_ = raw_node_->create_publisher( + "/perception/object_recognition/objects", rclcpp::QoS(1)); + pub_tf_ = raw_node_->create_publisher("/tf", rclcpp::QoS(1)); + pub_tf_static_ = raw_node_->create_publisher("/tf_static", rclcpp::QoS(1)); + + analyzer_ = std::make_shared(*raw_node_); +} + +void DrivingEnvironmentAnalyzerPanel::onMap(const HADMapBin::ConstSharedPtr msg) +{ + analyzer_->setMap(*msg); +} + +void DrivingEnvironmentAnalyzerPanel::onBoxUpdate() +{ + set_format_time(bag_time_line_, bag_time_selector_->value()); + bag_time_slider_->setValue(bag_time_selector_->value()); +} + +void DrivingEnvironmentAnalyzerPanel::onSliderUpdate() +{ + set_format_time(bag_time_line_, bag_time_slider_->value()); + bag_time_selector_->setValue(bag_time_slider_->value()); +} + +void DrivingEnvironmentAnalyzerPanel::onSelectDirectory() +{ + QString file_name = QFileDialog::getExistingDirectory(this, tr("Open ROSBAG file"), "/tmp"); + if (file_name.count() == 0) { + return; + } + + analyzer_->setBagFile(file_name.toStdString()); + + const auto [start_time, end_time] = analyzer_->getBagStartEndTime(); + bag_time_selector_->setRange(start_time, end_time); + bag_time_slider_->setRange(start_time, end_time); + bag_name_label_->setText(file_name); +} + +void DrivingEnvironmentAnalyzerPanel::onSelectBagFile() +{ + QString file_name = + QFileDialog::getOpenFileName(this, tr("Open ROSBAG file"), "/tmp", tr("ROSBAG (*.db3)")); + + if (file_name.count() == 0) { + return; + } + + analyzer_->setBagFile(file_name.toStdString()); + + const auto [start_time, end_time] = analyzer_->getBagStartEndTime(); + bag_time_selector_->setRange(start_time, end_time); + bag_time_slider_->setRange(start_time, end_time); + bag_name_label_->setText(file_name); +} + +void DrivingEnvironmentAnalyzerPanel::onClickSetTimeStamp() +{ + analyzer_->clearData(); + analyzer_->setTimeStamp(bag_time_selector_->value()); + + if (!analyzer_->isDataReadyForDynamicODDAnalysis()) { + return; + } + + pub_odometry_->publish(analyzer_->getOdometry()); + pub_objects_->publish(analyzer_->getObjects()); + pub_tf_->publish(analyzer_->getTF()); + pub_tf_static_->publish(analyzer_->getTFStatic()); +} + +void DrivingEnvironmentAnalyzerPanel::onClickAnalyzeDynamicODDFactor() +{ + if (!analyzer_->isDataReadyForDynamicODDAnalysis()) { + return; + } + + analyzer_->analyzeDynamicODDFactor(); +} + +void DrivingEnvironmentAnalyzerPanel::onClickAnalyzeStaticODDFactor() +{ + if (!analyzer_->isDataReadyForStaticODDAnalysis()) { + return; + } + + analyzer_->analyzeStaticODDFactor(); +} + +DrivingEnvironmentAnalyzerPanel::~DrivingEnvironmentAnalyzerPanel() = default; +} // namespace driving_environment_analyzer + +#include +PLUGINLIB_EXPORT_CLASS( + driving_environment_analyzer::DrivingEnvironmentAnalyzerPanel, rviz_common::Panel) diff --git a/driving_environment_analyzer/src/node.cpp b/driving_environment_analyzer/src/node.cpp deleted file mode 100644 index b9496710f..000000000 --- a/driving_environment_analyzer/src/node.cpp +++ /dev/null @@ -1,394 +0,0 @@ -// 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. - -#include "driving_environment_analyzer/node.hpp" - -#include "motion_utils/trajectory/trajectory.hpp" -#include "rosbag2_storage/storage_filter.hpp" - -#include -#include -#include - -#include - -#include -#include -#include - -namespace driving_environment_analyzer -{ -namespace -{ -template -std::vector calcElevationAngle(const T & points) -{ - std::vector elevation_vec(points.size(), 0.0); - if (points.size() < 2) { - return elevation_vec; - } - - for (size_t i = 0; i < points.size() - 1; ++i) { - const auto p1 = tier4_autoware_utils::getPoint(points.at(i)); - const auto p2 = tier4_autoware_utils::getPoint(points.at(i + 1)); - elevation_vec.at(i) = tier4_autoware_utils::calcElevationAngle(p1, p2); - } - elevation_vec.at(elevation_vec.size() - 1) = elevation_vec.at(elevation_vec.size() - 2); - - return elevation_vec; -} - -double getRouteLength(const lanelet::ConstLanelets & lanes) -{ - return lanelet::utils::getLaneletLength3d(lanes); -} - -double getMaxCurvature(const lanelet::ConstLanelets & lanes) -{ - const auto to_ros_msg = [](const auto & line) { - std::vector points; - std::for_each(line.begin(), line.end(), [&points](const auto & p) { - points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); - }); - return points; - }; - - double max_value = 0.0; - - for (const auto & lane : lanes) { - const auto values = motion_utils::calcCurvature(to_ros_msg(lane.centerline())); - const auto max_value_itr = std::max_element(values.begin(), values.end()); - if (max_value_itr == values.end()) { - continue; - } - - if (max_value < *max_value_itr) { - max_value = *max_value_itr; - } - } - - return max_value; -} - -std::pair getLaneWidth(const lanelet::ConstLanelets & lanes) -{ - const auto to_ros_msg = [](const auto & line) { - std::vector points; - std::for_each(line.begin(), line.end(), [&points](const auto & p) { - points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); - }); - return points; - }; - - double min_value = std::numeric_limits::max(); - double max_value = 0.0; - - for (const auto & lane : lanes) { - const auto lon_length = motion_utils::calcArcLength(to_ros_msg(lane.centerline())); - const auto width = boost::geometry::area(lane.polygon2d().basicPolygon()) / lon_length; - - if (min_value > width) { - min_value = width; - } - - if (max_value < width) { - max_value = width; - } - } - - return std::make_pair(min_value, max_value); -} - -std::pair getElevation(const lanelet::ConstLanelets & lanes) -{ - const auto to_ros_msg = [](const auto & line) { - std::vector points; - std::for_each(line.begin(), line.end(), [&points](const auto & p) { - points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); - }); - return points; - }; - - double min_value = std::numeric_limits::max(); - double max_value = 0.0; - - for (const auto & lane : lanes) { - const auto values = calcElevationAngle(to_ros_msg(lane.centerline())); - const auto max_value_itr = std::max_element(values.begin(), values.end()); - const auto min_value_itr = std::min_element(values.begin(), values.end()); - - if (min_value_itr == values.end()) { - continue; - } - - if (min_value > *min_value_itr) { - min_value = *min_value_itr; - } - - if (max_value_itr == values.end()) { - continue; - } - - if (max_value < *max_value_itr) { - max_value = *max_value_itr; - } - } - - return std::make_pair(min_value, max_value); -} - -std::pair getSpeedLimit( - const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) -{ - const auto traffic_rule = route_handler.getTrafficRulesPtr(); - - double min_value = std::numeric_limits::max(); - double max_value = 0.0; - - for (const auto & lane : lanes) { - const auto limit = traffic_rule->speedLimit(lane).speedLimit; - min_value = std::min(limit.value(), min_value); - max_value = std::max(limit.value(), max_value); - } - - return std::make_pair(min_value, max_value); -} - -double getRouteLengthWithSameDirectionLane( - const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) -{ - double value = 0.0; - - for (const auto & lane : lanes) { - const auto right_lanelet = route_handler.getRightLanelet(lane, false, false); - const auto left_lanelet = route_handler.getLeftLanelet(lane, false, false); - if (right_lanelet.has_value() || left_lanelet.has_value()) { - value += lanelet::utils::getLaneletLength3d(lane); - } - } - - return value; -} - -double getRouteLengthWithOppositeDirectionLane( - const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) -{ - double value = 0.0; - - for (const auto & lane : lanes) { - const auto right_lanelet = route_handler.getRightOppositeLanelets(lane); - const auto left_lanelet = route_handler.getLeftOppositeLanelets(lane); - if (!right_lanelet.empty() || !left_lanelet.empty()) { - value += lanelet::utils::getLaneletLength3d(lane); - } - } - - return value; -} - -double getRouteLengthWithNoAdjacentLane( - const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) -{ - double value = 0.0; - - for (const auto & lane : lanes) { - { - const auto right_lanelet = route_handler.getRightLanelet(lane, false, false); - const auto left_lanelet = route_handler.getLeftLanelet(lane, false, false); - if (right_lanelet.has_value() || left_lanelet.has_value()) { - continue; - } - } - - { - const auto right_lanelet = route_handler.getRightOppositeLanelets(lane); - const auto left_lanelet = route_handler.getLeftOppositeLanelets(lane); - if (!right_lanelet.empty() || !left_lanelet.empty()) { - continue; - } - } - - value += lanelet::utils::getLaneletLength3d(lane); - } - - return value; -} - -bool existTrafficLight(const lanelet::ConstLanelets & lanes) -{ - for (const auto & lane : lanes) { - if (!lane.regulatoryElementsAs().empty()) { - return true; - } - } - - return false; -} - -bool existIntersection(const lanelet::ConstLanelets & lanes) -{ - for (const auto & lane : lanes) { - std::string turn_direction = lane.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { - return true; - } - } - - return false; -} - -bool existCrosswalk( - const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) -{ - const auto overall_graphs = route_handler.getOverallGraphPtr(); - for (const auto & lane : lanes) { - constexpr int PEDESTRIAN_GRAPH_ID = 1; - if (!overall_graphs->conflictingInGraph(lane, PEDESTRIAN_GRAPH_ID).empty()) { - return true; - } - } - - return false; -} -} // namespace - -DrivingEnvironmentAnalyzer::DrivingEnvironmentAnalyzer(const rclcpp::NodeOptions & node_options) -: Node("driving_environment_analyzer", node_options) -{ - using std::placeholders::_1; - using std::chrono_literals::operator""ms; - - const auto bag_filename = declare_parameter("bag_path"); - const auto use_map_in_bag = declare_parameter("use_map_in_bag"); - - timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&DrivingEnvironmentAnalyzer::analyze, this)); - - sub_map_ = create_subscription( - "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), - std::bind(&DrivingEnvironmentAnalyzer::onMap, this, _1)); - - reader_.open(bag_filename); - - if (!isDataReady(use_map_in_bag)) { - rclcpp::shutdown(); - } -} - -bool DrivingEnvironmentAnalyzer::isDataReady(const bool use_map_in_bag) -{ - const std::string topic_route = "/planning/mission_planning/route"; - const std::string topic_map = "/map/vector_map"; - - rclcpp::Serialization serializer_route; - rclcpp::Serialization serializer_map; - - rosbag2_storage::StorageFilter filter; - filter.topics.emplace_back(topic_route); - filter.topics.emplace_back(topic_map); - reader_.set_filter(filter); - - while (reader_.has_next()) { - const auto data = reader_.read_next(); - - if (data->topic_name == topic_route) { - rclcpp::SerializedMessage serialized_msg(*data->serialized_data); - const auto deserialized_message = std::make_shared(); - serializer_route.deserialize_message(&serialized_msg, deserialized_message.get()); - route_msgs_.push_back(*deserialized_message); - } - - if (data->topic_name == topic_map) { - rclcpp::SerializedMessage serialized_msg(*data->serialized_data); - const auto deserialized_message = std::make_shared(); - serializer_route.deserialize_message(&serialized_msg, deserialized_message.get()); - route_handler_.setMap(*deserialized_message); - has_map_data_ = true; - } - } - - if (route_msgs_.empty()) { - RCLCPP_ERROR(get_logger(), "topic: %s is not included. shutdown.", topic_route.c_str()); - return false; - } - - if (use_map_in_bag && !has_map_data_) { - RCLCPP_ERROR(get_logger(), "topic: %s is not included. shutdown.", topic_map.c_str()); - return false; - } - - return true; -} - -void DrivingEnvironmentAnalyzer::onMap(const HADMapBin::ConstSharedPtr msg) -{ - route_handler_.setMap(*msg); - has_map_data_ = true; -} - -void DrivingEnvironmentAnalyzer::analyze() -{ - if (!has_map_data_) { - return; - } - - route_handler_.setRoute(route_msgs_.front()); - - RCLCPP_INFO(get_logger(), "======================================"); - RCLCPP_INFO(get_logger(), " data is ready. start ODD analysis... "); - RCLCPP_INFO(get_logger(), "======================================"); - - const auto preferred_lanes = route_handler_.getPreferredLanelets(); - RCLCPP_INFO(get_logger(), "- Length of total lanes : %.2f [m]", getRouteLength(preferred_lanes)); - RCLCPP_INFO( - get_logger(), "- Length of lane that has adjacent lane : %.2f [m]", - getRouteLengthWithSameDirectionLane(preferred_lanes, route_handler_)); - RCLCPP_INFO( - get_logger(), "- Length of lane that has opposite lane : %.2f [m]", - getRouteLengthWithOppositeDirectionLane(preferred_lanes, route_handler_)); - RCLCPP_INFO( - get_logger(), "- Length of lane that has no adjacent lane : %.2f [m]", - getRouteLengthWithNoAdjacentLane(preferred_lanes, route_handler_)); - const auto [min_width, max_width] = getLaneWidth(preferred_lanes); - RCLCPP_INFO( - get_logger(), "- Min lane width: %.2f [m] Max lane width: %.2f [m]", min_width, max_width); - const auto max_curvature = getMaxCurvature(preferred_lanes); - RCLCPP_INFO(get_logger(), "- Max curvature: %f [1/m]", max_curvature); - RCLCPP_INFO(get_logger(), "- Min curve radius: %.2f [m]", 1.0 / max_curvature); - const auto [min_elevation, max_elevation] = getElevation(preferred_lanes); - RCLCPP_INFO( - get_logger(), "- Min elevation angle: %f [rad] Max elevation angle: %f [rad]", min_elevation, - max_elevation); - const auto [min_speed_limit, max_speed_limit] = getSpeedLimit(preferred_lanes, route_handler_); - RCLCPP_INFO( - get_logger(), "- Min speed limit: %.2f [m/s] Max speed limit: %.2f [m/s]", min_speed_limit, - max_speed_limit); - RCLCPP_INFO_STREAM( - get_logger(), - "- Exist traffic light: " << std::boolalpha << existTrafficLight(preferred_lanes)); - RCLCPP_INFO_STREAM( - get_logger(), "- Exist intersection: " << std::boolalpha << existIntersection(preferred_lanes)); - RCLCPP_INFO_STREAM( - get_logger(), - "- Exist crosswalk: " << std::boolalpha << existCrosswalk(preferred_lanes, route_handler_)); - - RCLCPP_INFO(get_logger(), "======================================"); - RCLCPP_INFO(get_logger(), " complete ODD analysis. shutdown. "); - RCLCPP_INFO(get_logger(), "======================================"); - rclcpp::shutdown(); -} -} // namespace driving_environment_analyzer - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(driving_environment_analyzer::DrivingEnvironmentAnalyzer) diff --git a/driving_environment_analyzer/src/utils.cpp b/driving_environment_analyzer/src/utils.cpp new file mode 100644 index 000000000..b42dd3d4d --- /dev/null +++ b/driving_environment_analyzer/src/utils.cpp @@ -0,0 +1,462 @@ +// 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. + +#include "driving_environment_analyzer/utils.hpp" + +#include "motion_utils/trajectory/trajectory.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace driving_environment_analyzer::utils +{ +namespace +{ +std::uint8_t getHighestProbLabel(const std::vector & classification) +{ + std::uint8_t label = ObjectClassification::UNKNOWN; + float highest_prob = 0.0; + for (const auto & _class : classification) { + if (highest_prob < _class.probability) { + highest_prob = _class.probability; + label = _class.label; + } + } + return label; +} +} // namespace + +template +std::vector calcElevationAngle(const T & points) +{ + std::vector elevation_vec(points.size(), 0.0); + if (points.size() < 2) { + return elevation_vec; + } + + for (size_t i = 0; i < points.size() - 1; ++i) { + const auto p1 = tier4_autoware_utils::getPoint(points.at(i)); + const auto p2 = tier4_autoware_utils::getPoint(points.at(i + 1)); + elevation_vec.at(i) = tier4_autoware_utils::calcElevationAngle(p1, p2); + } + elevation_vec.at(elevation_vec.size() - 1) = elevation_vec.at(elevation_vec.size() - 2); + + return elevation_vec; +} + +double calcElevationAngle(const lanelet::ConstLanelet & lane, const Pose & pose) +{ + const auto to_ros_msg = [](const auto & line) { + std::vector points; + std::for_each(line.begin(), line.end(), [&points](const auto & p) { + points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return points; + }; + + const auto points = to_ros_msg(lane.centerline()); + + if (points.size() < 2) { + return 0.0; + } + + const size_t idx = motion_utils::findNearestSegmentIndex(points, pose.position); + + const auto p1 = tier4_autoware_utils::getPoint(points.at(idx)); + const auto p2 = tier4_autoware_utils::getPoint(points.at(idx + 1)); + return tier4_autoware_utils::calcElevationAngle(p1, p2); +} + +double getLaneWidth(const lanelet::ConstLanelet & lane) +{ + const auto to_ros_msg = [](const auto & line) { + std::vector points; + std::for_each(line.begin(), line.end(), [&points](const auto & p) { + points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return points; + }; + + const auto lon_length = motion_utils::calcArcLength(to_ros_msg(lane.centerline())); + return boost::geometry::area(lane.polygon2d().basicPolygon()) / lon_length; +} + +double getRouteLength(const lanelet::ConstLanelets & lanes) +{ + return lanelet::utils::getLaneletLength3d(lanes); +} + +double getMaxCurvature(const lanelet::ConstLanelets & lanes) +{ + const auto to_ros_msg = [](const auto & line) { + std::vector points; + std::for_each(line.begin(), line.end(), [&points](const auto & p) { + points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return points; + }; + + double max_value = 0.0; + + for (const auto & lane : lanes) { + const auto values = motion_utils::calcCurvature(to_ros_msg(lane.centerline())); + const auto max_value_itr = std::max_element(values.begin(), values.end()); + if (max_value_itr == values.end()) { + continue; + } + + if (max_value < *max_value_itr) { + max_value = *max_value_itr; + } + } + + return max_value; +} + +std::pair getLaneWidth(const lanelet::ConstLanelets & lanes) +{ + const auto to_ros_msg = [](const auto & line) { + std::vector points; + std::for_each(line.begin(), line.end(), [&points](const auto & p) { + points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return points; + }; + + double min_value = std::numeric_limits::max(); + double max_value = 0.0; + + for (const auto & lane : lanes) { + const auto lon_length = motion_utils::calcArcLength(to_ros_msg(lane.centerline())); + const auto width = boost::geometry::area(lane.polygon2d().basicPolygon()) / lon_length; + + if (min_value > width) { + min_value = width; + } + + if (max_value < width) { + max_value = width; + } + } + + return std::make_pair(min_value, max_value); +} + +std::pair getElevation(const lanelet::ConstLanelets & lanes) +{ + const auto to_ros_msg = [](const auto & line) { + std::vector points; + std::for_each(line.begin(), line.end(), [&points](const auto & p) { + points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return points; + }; + + double min_value = std::numeric_limits::max(); + double max_value = 0.0; + + for (const auto & lane : lanes) { + const auto values = calcElevationAngle(to_ros_msg(lane.centerline())); + const auto max_value_itr = std::max_element(values.begin(), values.end()); + const auto min_value_itr = std::min_element(values.begin(), values.end()); + + if (min_value_itr == values.end()) { + continue; + } + + if (min_value > *min_value_itr) { + min_value = *min_value_itr; + } + + if (max_value_itr == values.end()) { + continue; + } + + if (max_value < *max_value_itr) { + max_value = *max_value_itr; + } + } + + return std::make_pair(min_value, max_value); +} + +std::pair getSpeedLimit( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) +{ + const auto traffic_rule = route_handler.getTrafficRulesPtr(); + + double min_value = std::numeric_limits::max(); + double max_value = 0.0; + + for (const auto & lane : lanes) { + const auto limit = traffic_rule->speedLimit(lane).speedLimit; + min_value = std::min(limit.value(), min_value); + max_value = std::max(limit.value(), max_value); + } + + return std::make_pair(min_value, max_value); +} + +double getRouteLengthWithSameDirectionLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) +{ + double value = 0.0; + + for (const auto & lane : lanes) { + if (existSameDirectionLane(lane, route_handler)) { + value += lanelet::utils::getLaneletLength3d(lane); + } + } + + return value; +} + +double getRouteLengthWithOppositeDirectionLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) +{ + double value = 0.0; + + for (const auto & lane : lanes) { + if (existOppositeDirectionLane(lane, route_handler)) { + value += lanelet::utils::getLaneletLength3d(lane); + } + } + + return value; +} + +double getRouteLengthWithNoAdjacentLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) +{ + double value = 0.0; + + for (const auto & lane : lanes) { + if (existSameDirectionLane(lane, route_handler)) { + continue; + } + + if (existOppositeDirectionLane(lane, route_handler)) { + continue; + } + + value += lanelet::utils::getLaneletLength3d(lane); + } + + return value; +} + +bool existSameDirectionLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + const auto right_lanelet = route_handler.getRightLanelet(lane, true, false); + const auto left_lanelet = route_handler.getLeftLanelet(lane, true, false); + return right_lanelet.has_value() || left_lanelet.has_value(); +} + +bool existOppositeDirectionLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + { + const auto lanelet = route_handler.getMostRightLanelet(lane, true, false); + if (!route_handler.getRightOppositeLanelets(lanelet).empty()) { + return true; + } + } + + { + const auto lanelet = route_handler.getMostLeftLanelet(lane, true, false); + if (!route_handler.getRightOppositeLanelets(lanelet).empty()) { + return true; + } + } + + return false; +} + +bool existRoadShoulderLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + { + const auto lanelet = route_handler.getMostRightLanelet(lane, true, true); + const lanelet::Attribute sub_type = lanelet.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + return true; + } + } + + { + const auto lanelet = route_handler.getMostLeftLanelet(lane, true, true); + const lanelet::Attribute sub_type = lanelet.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + return true; + } + } + + return false; +} + +int getLeftLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + const auto left_lanelet = route_handler.getLeftLanelet(lane, true, false); + if (left_lanelet.has_value()) { + return getLeftLaneletNum(left_lanelet.value(), route_handler) + 1; + } + + const auto opposite_lanelets = route_handler.getLeftOppositeLanelets(lane); + if (!opposite_lanelets.empty()) { + return getRightLaneletNum(opposite_lanelets.front(), route_handler) + 1; + } + + return 0; +} + +int getRightLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + const auto right_lanelet = route_handler.getRightLanelet(lane, true, false); + if (right_lanelet.has_value()) { + return getRightLaneletNum(right_lanelet.value(), route_handler) + 1; + } + + const auto opposite_lanelets = route_handler.getRightOppositeLanelets(lane); + if (!opposite_lanelets.empty()) { + return getLeftLaneletNum(opposite_lanelets.front(), route_handler) + 1; + } + + return 0; +} + +int getTotalLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + return getRightLaneletNum(lane, route_handler) + getLeftLaneletNum(lane, route_handler) + 1; +} + +bool existTrafficLight(const lanelet::ConstLanelets & lanes) +{ + for (const auto & lane : lanes) { + if (!lane.regulatoryElementsAs().empty()) { + return true; + } + } + + return false; +} + +bool existIntersection(const lanelet::ConstLanelets & lanes) +{ + for (const auto & lane : lanes) { + std::string turn_direction = lane.attributeOr("turn_direction", "else"); + if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + return true; + } + } + + return false; +} + +bool existCrosswalk( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto overall_graphs = route_handler.getOverallGraphPtr(); + return !overall_graphs->conflictingInGraph(lane, PEDESTRIAN_GRAPH_ID).empty(); +} + +bool existCrosswalk( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) +{ + for (const auto & lane : lanes) { + if (existCrosswalk(lane, route_handler)) { + return true; + } + } + + return false; +} + +bool isStraightLane(const lanelet::ConstLanelet & lane) +{ + constexpr double THRESHOLD = 50.0; // radius [m] + return 1.0 / getMaxCurvature({lane}) > THRESHOLD; +} + +size_t getObjectNumber(const PredictedObjects & objects, const std::uint8_t target_class) +{ + return std::count_if( + objects.objects.begin(), objects.objects.end(), [&target_class](const auto & o) { + return getHighestProbLabel(o.classification) == target_class; + }); +} + +std::string getLaneShape(const lanelet::ConstLanelet & lane) +{ + if (isStraightLane(lane)) { + return "STRAIGHT"; + } + + return "CURVE"; +} + +std::string getModuleStatus(const CooperateStatusArray & status_array, const uint8_t module_type) +{ + const auto itr = std::find_if( + status_array.statuses.begin(), status_array.statuses.end(), + [&module_type](const auto & s) { return s.module.type == module_type; }); + + if (itr == status_array.statuses.end()) { + return "NONE"; + } + + const auto to_string = [](const auto & command_type) { + return command_type == Command::DEACTIVATE ? "deactivate" : "activate"; + }; + + std::ostringstream ss; + ss << std::boolalpha; + ss << "SAFE: " << itr->safe << " COMMAND: " << to_string(itr->command_status.type); + return ss.str(); +} + +std::string getEgoBehavior( + const lanelet::ConstLanelet & lane, + [[maybe_unused]] const route_handler::RouteHandler & route_handler, + [[maybe_unused]] const Pose & pose) +{ + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + + if (turn_direction == "straight") { + return "GO STRAIGHT"; + } + + if (turn_direction == "right") { + return "TURN RIGHT"; + } + + if (turn_direction == "left") { + return "TURN LEFT"; + } + + return "NONE"; +} +} // namespace driving_environment_analyzer::utils