From 7d9c5f6ce66fe62d44001cc0084b0b23f026a372 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 23 Apr 2024 20:41:49 +0900 Subject: [PATCH] feat(driving_environment_analyzer): add rviz plugin Signed-off-by: satoshi-ota --- driving_environment_analyzer/CMakeLists.txt | 23 +- .../analyzer_core.hpp | 101 +++++++ .../driving_environment_analyzer_node.hpp | 45 +++ ...iving_environment_analyzer_rviz_plugin.hpp | 82 ++++++ .../driving_environment_analyzer/node.hpp | 68 ----- .../type_alias.hpp | 49 ++++ .../driving_environment_analyzer/utils.hpp | 75 +++++ driving_environment_analyzer/package.xml | 12 +- .../plugins/plugin_description.xml | 9 + .../src/analyzer_core.cpp | 268 ++++++++++++++++++ .../src/driving_environment_analyzer_node.cpp | 62 ++++ ...iving_environment_analyzer_rviz_plugin.cpp | 180 ++++++++++++ .../src/{node.cpp => utils.cpp} | 232 ++++++--------- 13 files changed, 990 insertions(+), 216 deletions(-) create mode 100644 driving_environment_analyzer/include/driving_environment_analyzer/analyzer_core.hpp create mode 100644 driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_node.hpp create mode 100644 driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp delete mode 100644 driving_environment_analyzer/include/driving_environment_analyzer/node.hpp create mode 100644 driving_environment_analyzer/include/driving_environment_analyzer/type_alias.hpp create mode 100644 driving_environment_analyzer/include/driving_environment_analyzer/utils.hpp create mode 100644 driving_environment_analyzer/plugins/plugin_description.xml create mode 100644 driving_environment_analyzer/src/analyzer_core.cpp create mode 100644 driving_environment_analyzer/src/driving_environment_analyzer_node.cpp create mode 100644 driving_environment_analyzer/src/driving_environment_analyzer_rviz_plugin.cpp rename driving_environment_analyzer/src/{node.cpp => utils.cpp} (52%) 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/include/driving_environment_analyzer/analyzer_core.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/analyzer_core.hpp new file mode 100644 index 000000000..c9fc30104 --- /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 getNextTopic(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..5b0f6bc3e --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp @@ -0,0 +1,82 @@ +// 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 onClickLoadListFromFile(); + void onClickAnalyzeStaticODDFactor(); + void onClickAnalyzeDynamicODDFactor(); + +private: + void onMap(const HADMapBin::ConstSharedPtr map_msg); + + std::shared_ptr analyzer_; + + QSpinBox * bag_time_label_; + QSlider * bag_time_slider_; + QLabel * file_name_label_ptr_; + QLabel * bag_time_line_; + QPushButton * load_file_btn_ptr_; + QPushButton * btn_dynamic_odd_analyze_ptr_; + QPushButton * btn_static_odd_analyze_ptr_; + 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..473bbd6c5 --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/type_alias.hpp @@ -0,0 +1,49 @@ +// 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/cooperate_status_array.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::CooperateStatusArray; + +} // 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..6d7d57a7e --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/utils.hpp @@ -0,0 +1,75 @@ +// 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 existTrafficLight(const lanelet::ConstLanelets & lanes); + +bool existIntersection(const lanelet::ConstLanelets & lanes); + +bool existCrosswalk( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +size_t getObjectNumber(const PredictedObjects & objects, const std::uint8_t target_class); + +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..15c752cf7 --- /dev/null +++ b/driving_environment_analyzer/src/analyzer_core.cpp @@ -0,0 +1,268 @@ +// 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 = getNextTopic("/planning/mission_planning/route"); + if (opt_route.has_value()) { + route_handler_.setRoute(opt_route.value()); + } + + const auto opt_map = getNextTopic("/map/vector_map"); + if (opt_map.has_value()) { + route_handler_.setMap(opt_map.value()); + } +} + +template +std::optional AnalyzerCore::getNextTopic(const std::string & topic_name) +{ + rosbag2_storage::StorageFilter filter; + filter.topics.emplace_back(topic_name); + reader_.set_filter(filter); + + rclcpp::Serialization serializer; + + 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; +} + +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\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); + }; + + ss << "- EGO INFO\n"; + ss << " speed : " << getEgoSpeed() << " [m/s]\n"; + ss << " elevation angle: " << utils::calcElevationAngle(closest_lanelet, getEgoPose()) + << " [rad]\n"; + ss << " behavior : " + << utils::getEgoBehavior(closest_lanelet, route_handler_, getEgoPose()) << "\n"; + ss << "\n"; + ss << "- CURRENT LANE INFO\n"; + ss << " lane num : " + << "4" + << "\n"; + ss << " width : " << utils::getLaneWidth(closest_lanelet) << "\n"; + ss << " lane id : " << closest_lanelet.id() << "\n"; + ss << " exist same direction lane : " + << utils::existSameDirectionLane(closest_lanelet, route_handler_) << "\n"; + ss << " exist opposite direction lane: " + << utils::existOppositeDirectionLane(closest_lanelet, route_handler_) << "\n"; + ss << "\n"; + ss << "- SURROUND OBJECT INFO\n"; + ss << " object num [UNKNOWN] : " << number(ObjectClassification::UNKNOWN) << "\n"; + ss << " object num [CAR] : " << number(ObjectClassification::CAR) << "\n"; + ss << " object num [TRUCK] : " << number(ObjectClassification::TRUCK) << "\n"; + ss << " object num [BUS] : " << number(ObjectClassification::BUS) << "\n"; + ss << " object num [TRAILER] : " << number(ObjectClassification::TRAILER) << "\n"; + ss << " object num [MOTORCYCLE]: " << number(ObjectClassification::MOTORCYCLE) << "\n"; + ss << " object num [BICYCLE] : " << number(ObjectClassification::BICYCLE) << "\n"; + ss << " object num [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\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 diredtion 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..1c909f3fe --- /dev/null +++ b/driving_environment_analyzer/src/driving_environment_analyzer_rviz_plugin.cpp @@ -0,0 +1,180 @@ +// 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); + { + load_file_btn_ptr_ = new QPushButton("Load from file"); + connect(load_file_btn_ptr_, SIGNAL(clicked()), SLOT(onClickLoadListFromFile())); + v_layout->addWidget(load_file_btn_ptr_); + } + + { + bag_time_label_ = new QSpinBox(); + bag_time_label_->setValue(0); + bag_time_label_->setSingleStep(1); + connect(bag_time_label_, SIGNAL(valueChanged(int)), SLOT(onBoxUpdate())); + + bag_time_line_ = new QLabel; + + auto * layout = new QHBoxLayout(this); + layout->addWidget(new QLabel("Bag Time:")); + layout->addWidget(bag_time_line_); + layout->addWidget(bag_time_label_); + set_timestamp_btn_ = new QPushButton("Set time stamp"); + connect(set_timestamp_btn_, SIGNAL(clicked()), SLOT(onClickSetTimeStamp())); + layout->addWidget(set_timestamp_btn_); + + btn_dynamic_odd_analyze_ptr_ = new QPushButton("Analyze dynamic ODD factor"); + connect( + btn_dynamic_odd_analyze_ptr_, SIGNAL(clicked()), SLOT(onClickAnalyzeDynamicODDFactor())); + layout->addWidget(btn_dynamic_odd_analyze_ptr_); + + btn_static_odd_analyze_ptr_ = new QPushButton("Analyze static ODD factor"); + connect(btn_static_odd_analyze_ptr_, SIGNAL(clicked()), SLOT(onClickAnalyzeStaticODDFactor())); + layout->addWidget(btn_static_odd_analyze_ptr_); + + 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_); + } + + { + file_name_label_ptr_ = new QLabel(); + file_name_label_ptr_->setAlignment(Qt::AlignLeft); + auto * layout = new QHBoxLayout(this); + layout->addWidget(new QLabel("BagFile:")); + layout->addWidget(file_name_label_ptr_); + 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_label_->value()); + bag_time_slider_->setValue(bag_time_label_->value()); +} + +void DrivingEnvironmentAnalyzerPanel::onSliderUpdate() +{ + set_format_time(bag_time_line_, bag_time_slider_->value()); + bag_time_label_->setValue(bag_time_slider_->value()); +} + +void DrivingEnvironmentAnalyzerPanel::onClickLoadListFromFile() +{ + QString file_name = + QFileDialog::getOpenFileName(this, tr("Open ROSBAG file"), "/tmp", tr("ROSBAG (*.db3)")); + if (file_name.count() > 0) analyzer_->setBagFile(file_name.toStdString()); + file_name_label_ptr_->setText(file_name); + const auto [start_time, end_time] = analyzer_->getBagStartEndTime(); + bag_time_label_->setRange(start_time, end_time); + bag_time_slider_->setRange(start_time, end_time); +} + +void DrivingEnvironmentAnalyzerPanel::onClickSetTimeStamp() +{ + analyzer_->clearData(); + analyzer_->setTimeStamp(bag_time_label_->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/utils.cpp similarity index 52% rename from driving_environment_analyzer/src/node.cpp rename to driving_environment_analyzer/src/utils.cpp index b9496710f..729d03f06 100644 --- a/driving_environment_analyzer/src/node.cpp +++ b/driving_environment_analyzer/src/utils.cpp @@ -12,10 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "driving_environment_analyzer/node.hpp" +#include "driving_environment_analyzer/utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "rosbag2_storage/storage_filter.hpp" #include #include @@ -27,10 +26,24 @@ #include #include -namespace driving_environment_analyzer +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) { @@ -49,6 +62,43 @@ std::vector calcElevationAngle(const T & points) 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); @@ -171,9 +221,7 @@ double getRouteLengthWithSameDirectionLane( 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()) { + if (existSameDirectionLane(lane, route_handler)) { value += lanelet::utils::getLaneletLength3d(lane); } } @@ -187,9 +235,7 @@ double getRouteLengthWithOppositeDirectionLane( 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()) { + if (existOppositeDirectionLane(lane, route_handler)) { value += lanelet::utils::getLaneletLength3d(lane); } } @@ -203,20 +249,12 @@ double getRouteLengthWithNoAdjacentLane( 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; - } + if (existSameDirectionLane(lane, route_handler)) { + 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; - } + if (existOppositeDirectionLane(lane, route_handler)) { + continue; } value += lanelet::utils::getLaneletLength3d(lane); @@ -225,6 +263,22 @@ double getRouteLengthWithNoAdjacentLane( return value; } +bool existSameDirectionLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + const auto right_lanelet = route_handler.getRightLanelet(lane, false, false); + const auto left_lanelet = route_handler.getLeftLanelet(lane, false, false); + return right_lanelet.has_value() || left_lanelet.has_value(); +} + +bool existOppositeDirectionLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + const auto right_lanelet = route_handler.getRightOppositeLanelets(lane); + const auto left_lanelet = route_handler.getLeftOppositeLanelets(lane); + return !right_lanelet.empty() || !left_lanelet.empty(); +} + bool existTrafficLight(const lanelet::ConstLanelets & lanes) { for (const auto & lane : lanes) { @@ -261,134 +315,34 @@ bool existCrosswalk( return false; } -} // namespace -DrivingEnvironmentAnalyzer::DrivingEnvironmentAnalyzer(const rclcpp::NodeOptions & node_options) -: Node("driving_environment_analyzer", node_options) +size_t getObjectNumber(const PredictedObjects & objects, const std::uint8_t target_class) { - 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(); - } + return std::count_if( + objects.objects.begin(), objects.objects.end(), [&target_class](const auto & o) { + return getHighestProbLabel(o.classification) == target_class; + }); } -bool DrivingEnvironmentAnalyzer::isDataReady(const bool use_map_in_bag) +std::string getEgoBehavior( + const lanelet::ConstLanelet & lane, + [[maybe_unused]] const route_handler::RouteHandler & route_handler, + [[maybe_unused]] const Pose & pose) { - 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; - } - } + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - if (route_msgs_.empty()) { - RCLCPP_ERROR(get_logger(), "topic: %s is not included. shutdown.", topic_route.c_str()); - return false; + if (turn_direction == "straight") { + return "[INTERSECTION] GO STRAIGHT"; } - if (use_map_in_bag && !has_map_data_) { - RCLCPP_ERROR(get_logger(), "topic: %s is not included. shutdown.", topic_map.c_str()); - return false; + if (turn_direction == "right") { + return "[INTERSECTION] TURN RIGHT"; } - 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; + if (turn_direction == "left") { + return "[INTERSECTION] TURN LEFT"; } - 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(); + return "LANE FOLLOWING"; } -} // namespace driving_environment_analyzer - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(driving_environment_analyzer::DrivingEnvironmentAnalyzer) +} // namespace driving_environment_analyzer::utils