diff --git a/driving_environment_analyzer/CMakeLists.txt b/driving_environment_analyzer/CMakeLists.txt new file mode 100644 index 00000000..d1b9fbd2 --- /dev/null +++ b/driving_environment_analyzer/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(driving_environment_analyzer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + DIRECTORY src +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "driving_environment_analyzer::DrivingEnvironmentAnalyzer" + EXECUTABLE driving_environment_analyzer +) + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/driving_environment_analyzer/README.md b/driving_environment_analyzer/README.md new file mode 100644 index 00000000..3e31b78c --- /dev/null +++ b/driving_environment_analyzer/README.md @@ -0,0 +1,54 @@ +# Driving Environment Analyzer + +このツールはROSBAGに含まれる走行履歴を元に走行環境のODDを解析するツールです。 + +## How to use + +現在以下の情報が出力可能です。 + +- 走行経路の長さ +- 走行経路の車線情報 +- 走行経路の最大・最小勾配 +- 走行経路の最大曲率 +- 走行経路の最大・最小車線幅 +- 交差点の有無 +- 信号機の有無 +- 横断歩道の有無 + +起動時に`bag_path`オプションで解析したいROSBAGを指定してください。（ディレクトリの指定も.db3ファイルの直接指定もサポートしています。） + +解析に必要なtopicは以下のとおりです。（今後増える可能性もあります。） + +- `/planning/mission_planning/route` +- `/map/vector_map` + +以下のようにlaunchすることでODDの解析結果が得られます。 + +`ros2 launch driving_environment_analyzer driving_environment_analyzer.launch.xml use_map_in_bag:=true bag_path:=` + +```bash +[component_container-1] [INFO 1708999777.768870564] [driving_environment_analyzer]: ====================================== +[component_container-1] [INFO 1708999777.768922452] [driving_environment_analyzer]: data is ready. start ODD analysis... +[component_container-1] [INFO 1708999777.768933574] [driving_environment_analyzer]: ====================================== +[component_container-1] [INFO 1708999777.768967412] [driving_environment_analyzer]: - Length of total lanes : 2357.50 [m] +[component_container-1] [INFO 1708999777.769031174] [driving_environment_analyzer]: - Length of lane that has adjacent lane : 2080.43 [m] +[component_container-1] [INFO 1708999777.769076141] [driving_environment_analyzer]: - Length of lane that has opposite lane : 0.00 [m] +[component_container-1] [INFO 1708999777.769101793] [driving_environment_analyzer]: - Length of lane that has no adjacent lane : 277.07 [m] +[component_container-1] [INFO 1708999777.769225729] [driving_environment_analyzer]: - Min lane width: 3.14 [m] Max lane width: 4.94 [m] +[component_container-1] [INFO 1708999777.769278698] [driving_environment_analyzer]: - Max curvature: 0.007967 [1/m] +[component_container-1] [INFO 1708999777.769293161] [driving_environment_analyzer]: - Min curve radius: 125.52 [m] +[component_container-1] [INFO 1708999777.769336094] [driving_environment_analyzer]: - Min elevation angle: -0.033037 [rad] Max elevation angle: 0.026073 [rad] +[component_container-1] [INFO 1708999777.769403870] [driving_environment_analyzer]: - Min speed limit: 13.89 [m/s] Max speed limit: 16.67 [m/s] +[component_container-1] [INFO 1708999777.769424648] [driving_environment_analyzer]: - Exist traffic light: true +[component_container-1] [INFO 1708999777.769435813] [driving_environment_analyzer]: - Exist intersection: true +[component_container-1] [INFO 1708999777.769620035] [driving_environment_analyzer]: - Exist crosswalk: true +[component_container-1] [INFO 1708999777.769634980] [driving_environment_analyzer]: ====================================== +[component_container-1] [INFO 1708999777.769642769] [driving_environment_analyzer]: complete ODD analysis. shutdown. +[component_container-1] [INFO 1708999777.769650034] [driving_environment_analyzer]: ====================================== +``` + +ただし、`map/vector_map`に関しては`use_map_in_bag`を`false`にすることでローカル環境に保存されている地図を使用してODD解析を行うことも可能です。その場合、`map_path`オプションで地図のパスを指定してください。 + +`ros2 launch driving_environment_analyzer driving_environment_analyzer.launch.xml use_map_in_bag:=false map_path:= bag_path:=` + +以上のようにオプションを指定することでROSBAGに地図情報が保存されていなくてもODD解析が可能です。 diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/node.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/node.hpp new file mode 100644 index 00000000..da2677b0 --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/node.hpp @@ -0,0 +1,68 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DRIVING_ENVIRONMENT_ANALYZER__NODE_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__NODE_HPP_ + +#include "rosbag2_cpp/reader.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" + +#include + +#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/launch/driving_environment_analyzer.launch.xml b/driving_environment_analyzer/launch/driving_environment_analyzer.launch.xml new file mode 100644 index 00000000..ff84bc98 --- /dev/null +++ b/driving_environment_analyzer/launch/driving_environment_analyzer.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/driving_environment_analyzer/package.xml b/driving_environment_analyzer/package.xml new file mode 100644 index 00000000..3446de71 --- /dev/null +++ b/driving_environment_analyzer/package.xml @@ -0,0 +1,51 @@ + + + + driving_environment_analyzer + 0.1.0 + The driving_environment_analyzer package + + Satoshi Ota + + Apache License 2.0 + + Satoshi Ota + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_tf2 + autoware_auto_vehicle_msgs + autoware_perception_msgs + behavior_path_planner_common + geometry_msgs + interpolation + lane_departure_checker + lanelet2_extension + libboost-dev + motion_utils + rclcpp + rclcpp_components + route_handler + sensor_msgs + signal_processing + tf2 + tf2_eigen + tf2_geometry_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/src/node.cpp b/driving_environment_analyzer/src/node.cpp new file mode 100644 index 00000000..b9496710 --- /dev/null +++ b/driving_environment_analyzer/src/node.cpp @@ -0,0 +1,394 @@ +// 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)