From dace67fac302a47d320e633a6d6d696db23b5ca4 Mon Sep 17 00:00:00 2001 From: suchang Date: Thu, 27 Feb 2025 18:00:13 +0800 Subject: [PATCH 01/19] feat: modify autoware_universe_utils to autoware_utils Signed-off-by: suchang --- .../CMakeLists.txt | 29 + .../README.md | 3 + ...ehavior_velocity_planner_common.param.yaml | 7 + .../planner_data.hpp | 86 +++ .../plugin_interface.hpp | 42 ++ .../plugin_wrapper.hpp | 49 ++ .../scene_module_interface.hpp | 288 +++++++ .../utilization/arc_lane_util.hpp | 201 +++++ .../utilization/boost_geometry_helper.hpp | 82 ++ .../utilization/debug.hpp | 49 ++ .../utilization/path_utilization.hpp | 36 + .../utilization/state_machine.hpp | 95 +++ .../utilization/trajectory_utils.hpp | 45 ++ .../utilization/util.hpp | 245 ++++++ .../package.xml | 59 ++ ...havior_velocity_planner_common.schema.json | 59 ++ .../src/planner_data.cpp | 72 ++ .../src/scene_module_interface.cpp | 61 ++ .../src/utilization/arc_lane_util.cpp | 129 ++++ .../src/utilization/boost_geometry_helper.cpp | 64 ++ .../src/utilization/debug.cpp | 129 ++++ .../src/utilization/path_utilization.cpp | 169 +++++ .../src/utilization/trajectory_utils.cpp | 99 +++ .../src/utilization/util.cpp | 700 ++++++++++++++++++ .../test/src/test_arc_lane_util.cpp | 242 ++++++ .../test/src/test_path_utilization.cpp | 245 ++++++ .../test/src/test_state_machine.cpp | 87 +++ .../test/src/test_trajectory_utils.cpp | 113 +++ .../test/src/test_util.cpp | 308 ++++++++ .../test/src/utils.hpp | 57 ++ 30 files changed, 3850 insertions(+) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt new file mode 100644 index 0000000000..4c06c427a0 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_planner_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene_module_interface.cpp + src/planner_data.cpp + src/utilization/path_utilization.cpp + src/utilization/trajectory_utils.cpp + src/utilization/arc_lane_util.cpp + src/utilization/boost_geometry_helper.cpp + src/utilization/util.cpp + src/utilization/debug.cpp +) + +if(BUILD_TESTING) + file(GLOB TEST_SOURCES test/src/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${TEST_SOURCES}) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) +endif() + +ament_auto_package(INSTALL_TO_SHARE + config +) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md new file mode 100644 index 0000000000..c4c8d97b4b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md @@ -0,0 +1,3 @@ +# Behavior Velocity Planner Common + +This package provides a behavior velocity interface without RTC, and common functions as a library, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml new file mode 100644 index 0000000000..aff2aec9cf --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + max_accel: -2.8 + max_jerk: -5.0 + system_delay: 0.5 + delay_response_time: 0.5 + is_publish_debug_path: false # publish all debug path with lane id in each module diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp new file mode 100644 index 0000000000..f8b37999cb --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -0,0 +1,86 @@ +// Copyright 2019 Autoware Foundation +// +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ + +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/route_handler/route_handler.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +struct PlannerData +{ + explicit PlannerData(rclcpp::Node & node); + + rclcpp::Clock::SharedPtr clock_; + + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; + static constexpr double velocity_buffer_time_sec = 10.0; + std::deque velocity_buffer; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + pcl::PointCloud::ConstPtr no_ground_pointcloud; + + nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; + + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; + std::optional external_velocity_limit; + + bool is_simulation = false; + + std::shared_ptr velocity_smoother_; + std::shared_ptr route_handler_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + + double max_stop_acceleration_threshold; + double max_stop_jerk_threshold; + double system_delay; + double delay_response_time; + double stop_line_extend_length; + + bool isVehicleStopped(const double stop_duration = 0.0) const; + + std::optional getTrafficSignal( + const lanelet::Id id, const bool keep_last_observation = false) const; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp new file mode 100644 index 0000000000..ce6d828779 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp @@ -0,0 +1,42 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ + +#include +#include + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ + +class PluginInterface +{ +public: + virtual ~PluginInterface() = default; + virtual void init(rclcpp::Node & node) = 0; + virtual void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) = 0; + virtual void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual const char * getModuleName() = 0; +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp new file mode 100644 index 0000000000..a4f2bc0467 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -0,0 +1,49 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ + +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +template +class PluginWrapper : public PluginInterface +{ +public: + void init(rclcpp::Node & node) override { scene_manager_ = std::make_unique(node); } + void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) override + { + scene_manager_->plan(path); + }; + void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override + { + scene_manager_->updateSceneModuleInstances(planner_data, path); + } + const char * getModuleName() override { return scene_manager_->getModuleName(); } + +private: + std::unique_ptr scene_manager_; +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp new file mode 100644 index 0000000000..3b5ab16c3f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -0,0 +1,288 @@ +// Copyright 2020 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// Debug +#include +#include + +#include +namespace autoware::behavior_velocity_planner +{ + +using autoware::objects_of_interest_marker_interface::ColorName; +using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using autoware_utils::DebugPublisher; +using autoware_utils::get_or_declare_parameter; +using autoware_utils::StopWatch; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using builtin_interfaces::msg::Time; +using unique_identifier_msgs::msg::UUID; + +struct ObjectOfInterest +{ + geometry_msgs::msg::Pose pose; + autoware_perception_msgs::msg::Shape shape; + ColorName color; + ObjectOfInterest( + const geometry_msgs::msg::Pose & pose, autoware_perception_msgs::msg::Shape shape, + const ColorName & color_name) + : pose(pose), shape(std::move(shape)), color(color_name) + { + } +}; + +class SceneModuleInterface +{ +public: + explicit SceneModuleInterface( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); + virtual ~SceneModuleInterface() = default; + + virtual bool modifyPathVelocity(PathWithLaneId * path) = 0; + + virtual visualization_msgs::msg::MarkerArray createDebugMarkerArray() = 0; + virtual std::vector createVirtualWalls() = 0; + + int64_t getModuleId() const { return module_id_; } + + void setPlannerData(const std::shared_ptr & planner_data) + { + planner_data_ = planner_data; + } + + std::vector getObjectsOfInterestData() const { return objects_of_interest_; } + void clearObjectsOfInterestData() { objects_of_interest_.clear(); } + +protected: + const int64_t module_id_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr planner_data_; + std::vector objects_of_interest_; + mutable std::shared_ptr time_keeper_; + std::shared_ptr planning_factor_interface_; + + void setObjectsOfInterestData( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, + const ColorName & color_name) + { + objects_of_interest_.emplace_back(pose, shape, color_name); + } + + size_t findEgoSegmentIndex( + const std::vector & points) const; +}; + +template +class SceneModuleManagerInterface +{ +public: + SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name) + : node_(node), clock_(node.get_clock()), logger_(node.get_logger()) + { + const auto ns = std::string("~/debug/") + module_name; + pub_debug_ = node.create_publisher(ns, 1); + if (!node.has_parameter("is_publish_debug_path")) { + is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); + } else { + is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); + } + if (is_publish_debug_path_) { + pub_debug_path_ = node.create_publisher( + std::string("~/debug/path_with_lane_id/") + module_name, 1); + } + pub_virtual_wall_ = node.create_publisher( + std::string("~/virtual_wall/") + module_name, 5); + planning_factor_interface_ = + std::make_shared(&node, module_name); + + processing_time_publisher_ = std::make_shared(&node, "~/debug"); + + pub_processing_time_detail_ = node.create_publisher( + "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); + + time_keeper_ = std::make_shared(pub_processing_time_detail_); + } + + virtual ~SceneModuleManagerInterface() = default; + + virtual const char * getModuleName() = 0; + + void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) + { + planner_data_ = planner_data; + + launchNewModules(path); + deleteExpiredModules(path); + } + + virtual void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) + { + modifyPathVelocity(path); + } + +protected: + virtual void modifyPathVelocity(autoware_internal_planning_msgs::msg::PathWithLaneId * path) + { + autoware_utils::ScopedTimeTrack st( + "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); + StopWatch stop_watch; + stop_watch.tic("Total"); + visualization_msgs::msg::MarkerArray debug_marker_array; + + for (const auto & scene_module : scene_modules_) { + scene_module->setPlannerData(planner_data_); + scene_module->modifyPathVelocity(path); + + // The velocity factor must be called after modifyPathVelocity. + + for (const auto & marker : scene_module->createDebugMarkerArray().markers) { + debug_marker_array.markers.push_back(marker); + } + + virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); + } + + planning_factor_interface_->publish(); + pub_debug_->publish(debug_marker_array); + if (is_publish_debug_path_) { + autoware_internal_planning_msgs::msg::PathWithLaneId debug_path; + debug_path.header = path->header; + debug_path.points = path->points; + pub_debug_path_->publish(debug_path); + } + pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); + processing_time_publisher_->publish( + std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); + } + + virtual void launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; + + virtual std::function &)> getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; + + virtual void deleteExpiredModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) + { + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } + } + + bool isModuleRegistered(const int64_t module_id) + { + return registered_module_id_set_.count(module_id) != 0; + } + + void registerModule(const std::shared_ptr & scene_module) + { + RCLCPP_DEBUG( + logger_, "register task: module = %s, id = %lu", getModuleName(), + scene_module->getModuleId()); + registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_modules_.insert(scene_module); + } + + size_t findEgoSegmentIndex( + const std::vector & points) const + { + const auto & p = planner_data_; + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, + p->ego_nearest_yaw_threshold); + } + + std::set> scene_modules_; + std::set registered_module_id_set_; + + std::shared_ptr planner_data_; + autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_; + + rclcpp::Node & node_; + rclcpp::Clock::SharedPtr clock_; + // Debug + bool is_publish_debug_path_ = {false}; // note : this is very heavy debug topic option + rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; + rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr + pub_debug_path_; + + std::shared_ptr processing_time_publisher_; + + rclcpp::Publisher::SharedPtr pub_processing_time_detail_; + + std::shared_ptr time_keeper_; + + std::shared_ptr planning_factor_interface_; +}; +extern template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +extern template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +extern template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::modifyPathVelocity( + autoware_internal_planning_msgs::msg::PathWithLaneId * path); +extern template void SceneModuleManagerInterface::deleteExpiredModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp new file mode 100644 index 0000000000..215419ee4f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -0,0 +1,201 @@ +// Copyright 2021 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ + +#include +#include + +#include + +#include +#include + +#define EIGEN_MPL2_ONLY +#include + +namespace autoware::behavior_velocity_planner +{ + +inline geometry_msgs::msg::Point convertToGeomPoint(const autoware_utils::Point2d & p) +{ + geometry_msgs::msg::Point geom_p; + geom_p.x = p.x(); + geom_p.y = p.y(); + + return geom_p; +} + +namespace arc_lane_utils +{ +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = + std::pair; // front index, point2d +using PathIndexWithPoint = std::pair; // front index, point2d +using PathIndexWithOffset = std::pair; // front index, offset + +double calcSignedDistance( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2); + +// calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) +std::optional checkCollision( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); + +template +std::optional findCollisionSegment( + const T & path, const geometry_msgs::msg::Point & stop_line_p1, + const geometry_msgs::msg::Point & stop_line_p2) +{ + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto & p1 = + autoware_utils::get_point(path.points.at(i)); // Point before collision point + const auto & p2 = + autoware_utils::get_point(path.points.at(i + 1)); // Point after collision point + + const auto collision_point = checkCollision(p1, p2, stop_line_p1, stop_line_p2); + + if (collision_point) { + return std::make_pair(i, collision_point.value()); + } + } + + return {}; +} + +template +std::optional findCollisionSegment( + const T & path, const LineString2d & stop_line) +{ + const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0)); + const auto stop_line_p2 = convertToGeomPoint(stop_line.at(1)); + + return findCollisionSegment(path, stop_line_p1, stop_line_p2); +} + +template +std::optional findForwardOffsetSegment( + const T & path, const size_t base_idx, const double offset_length) +{ + double sum_length = 0.0; + for (size_t i = base_idx; i < path.points.size() - 1; ++i) { + const double segment_length = + autoware_utils::calc_distance2d(path.points.at(i), path.points.at(i + 1)); + + // If it's over offset point, return front index and remain offset length + /** + * (base_idx) --- offset_length ---------> + * --------- (i) <-- remain -->-----------> (i+1) + */ + if (sum_length + segment_length >= offset_length) { + return std::make_pair(i, offset_length - sum_length); + } + + sum_length += segment_length; + } + + // No enough path length + return {}; +} + +template +std::optional findBackwardOffsetSegment( + const T & path, const size_t base_idx, const double offset_length) +{ + double sum_length = 0.0; + const auto start = static_cast(base_idx) - 1; + for (std::int32_t i = start; i >= 0; --i) { + sum_length += + autoware_utils::calc_distance2d(path.points.at(i), path.points.at(i + 1)); + + // If it's over offset point, return front index and remain offset length + /** + * <-------- offset_length --- (base_idx) + * ----- (i) <-- remain -->-------> (i+1) + */ + if (sum_length >= offset_length) { + const auto k = static_cast(i); + return std::make_pair(k, sum_length - offset_length); + } + } + + // No enough path length + return {}; +} + +template +std::optional findOffsetSegment( + const T & path, const PathIndexWithPoint & collision_segment, const double offset_length) +{ + const size_t collision_idx = collision_segment.first; + const auto & collision_point = collision_segment.second; + + if (offset_length >= 0) { + return findForwardOffsetSegment( + path, collision_idx, + offset_length + + autoware_utils::calc_distance2d(path.points.at(collision_idx), collision_point)); + } + + return findBackwardOffsetSegment( + path, collision_idx + 1, + -offset_length + + autoware_utils::calc_distance2d(path.points.at(collision_idx + 1), collision_point)); +} + +std::optional findOffsetSegment( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t index, + const double offset); + +template +geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment) +{ + const size_t offset_idx = offset_segment.first; + const double remain_offset_length = offset_segment.second; + const auto & p_front = path.points.at(offset_idx).point.pose.position; + const auto & p_back = path.points.at(offset_idx + 1).point.pose.position; + + // To Eigen point + const auto p_eigen_front = Eigen::Vector2d(p_front.x, p_front.y); + const auto p_eigen_back = Eigen::Vector2d(p_back.x, p_back.y); + + // Calculate interpolation ratio + /** + * (front) <-- remain_length --> (interp) <----> (back) + */ + const auto interpolate_ratio = remain_offset_length / (p_eigen_back - p_eigen_front).norm(); + + // Add offset to front point + const auto target_point_2d = p_eigen_front + interpolate_ratio * (p_eigen_back - p_eigen_front); + const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); + + // Calculate orientation so that X-axis would be along the trajectory + geometry_msgs::msg::Pose target_pose; + target_pose.position.x = target_point_2d.x(); + target_pose.position.y = target_point_2d.y(); + target_pose.position.z = interpolated_z; + const double yaw = autoware_utils::calc_azimuth_angle(p_front, p_back); + target_pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw); + return target_pose; +} + +std::optional createTargetPoint( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const double margin, const double vehicle_offset); + +} // namespace arc_lane_utils +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp new file mode 100644 index 0000000000..cdbf22368b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -0,0 +1,82 @@ +// Copyright 2020 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +// cppcheck-suppress unknownMacro +BOOST_GEOMETRY_REGISTER_POINT_3D(geometry_msgs::msg::Point, double, cs::cartesian, x, y, z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + geometry_msgs::msg::Pose, double, cs::cartesian, position.x, position.y, position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + geometry_msgs::msg::PoseWithCovarianceStamped, double, cs::cartesian, pose.pose.position.x, + pose.pose.position.y, pose.pose.position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + autoware_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, pose.position.y, + pose.position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + autoware_internal_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, + point.pose.position.x, point.pose.position.y, point.pose.position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, + pose.position.y, pose.position.z) + +namespace autoware::behavior_velocity_planner +{ +namespace bg = boost::geometry; + +using Point2d = autoware_utils::Point2d; +using LineString2d = autoware_utils::LineString2d; +using Polygon2d = autoware_utils::Polygon2d; + +template +Point2d to_bg2d(const T & p) +{ + return Point2d(bg::get<0>(p), bg::get<1>(p)); +} + +template +LineString2d to_bg2d(const std::vector & vec) +{ + LineString2d ps; + for (const auto & p : vec) { + ps.push_back(to_bg2d(p)); + } + return ps; +} + +Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line); + +Polygon2d upScalePolygon( + const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale); + +geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon); + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp new file mode 100644 index 0000000000..ba103e01f8 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp @@ -0,0 +1,49 @@ +// Copyright 2020 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::behavior_velocity_planner::debug +{ +visualization_msgs::msg::MarkerArray createPolygonMarkerArray( + const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, + const rclcpp::Time & now, const double x, const double y, const double z, const double r, + const double g, const double b); +visualization_msgs::msg::MarkerArray createPathMarkerArray( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b); +visualization_msgs::msg::MarkerArray createObjectsMarkerArray( + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double r, const double g, + const double b); +visualization_msgs::msg::MarkerArray createPointsMarkerArray( + const std::vector & points, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b); +} // namespace autoware::behavior_velocity_planner::debug + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp new file mode 100644 index 0000000000..770feb3e0a --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -0,0 +1,36 @@ +// Copyright 2020 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ + +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +bool splineInterpolate( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_internal_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger); +autoware_planning_msgs::msg::Path interpolatePath( + const autoware_planning_msgs::msg::Path & path, const double length, const double interval); +autoware_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_planning_msgs::msg::Path & path); +autoware_planning_msgs::msg::Path filterStopPathPoint( + const autoware_planning_msgs::msg::Path & path); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp new file mode 100644 index 0000000000..dfe70d3766 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp @@ -0,0 +1,95 @@ +// Copyright 2021 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ + +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +/** + * @brief Manage stop-go states with safety margin time. + */ +class StateMachine +{ +public: + enum State { + STOP = 0, + GO, + }; + static std::string toString(const State & state) + { + if (state == State::STOP) { + return "STOP"; + } + if (state == State::GO) { + return "GO"; + } + return ""; + } + StateMachine() + { + state_ = State::GO; + margin_time_ = 0.0; + duration_ = 0.0; + } + void setStateWithMarginTime(State state, rclcpp::Logger logger, rclcpp::Clock & clock) + { + /* same state request */ + if (state_ == state) { + start_time_ = nullptr; // reset timer + return; + } + + /* GO -> STOP */ + if (state == State::STOP) { + state_ = State::STOP; + start_time_ = nullptr; // reset timer + return; + } + + /* STOP -> GO */ + if (state == State::GO) { + if (start_time_ == nullptr) { + start_time_ = std::make_shared(clock.now()); + } else { + duration_ = (clock.now() - *start_time_).seconds(); + if (duration_ > margin_time_) { + state_ = State::GO; + start_time_ = nullptr; // reset timer + } + } + return; + } + RCLCPP_ERROR(logger, "Unsuitable state. ignore request."); + } + + void setMarginTime(const double t) { margin_time_ = t; } + void setState(State state) { state_ = state; } + State getState() const { return state_; } + double getDuration() const { return duration_; } + +private: + State state_; //! current state + double margin_time_; //! margin time when transit to Go from Stop + double duration_; //! duration time when transit to Go from Stop + std::shared_ptr start_time_; //! first time received GO when STOP state +}; + +} // namespace autoware::behavior_velocity_planner +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp new file mode 100644 index 0000000000..c82d3d5ed0 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -0,0 +1,45 @@ +// Copyright 2021 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Quaternion; +using TrajectoryPointWithIdx = std::pair; + +//! smooth path point with lane id starts from ego position on path to the path end +bool smoothPath( + const PathWithLaneId & in_path, PathWithLaneId & out_path, + const std::shared_ptr & planner_data); + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp new file mode 100644 index 0000000000..6535905926 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp @@ -0,0 +1,245 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ + +#include "autoware_utils/geometry/boost_geometry.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +/** + * @brief Represents detection range parameters. + */ +struct DetectionRange +{ + bool use_right = true; ///< Whether to use the right side. + bool use_left = true; ///< Whether to use the left side. + double interval{0.0}; ///< Interval of detection points. + double min_longitudinal_distance{0.0}; ///< Minimum longitudinal detection distance. + double max_longitudinal_distance{0.0}; ///< Maximum longitudinal detection distance. + double max_lateral_distance{0.0}; ///< Maximum lateral detection distance. + double wheel_tread{0.0}; ///< Wheel tread of the vehicle. + double right_overhang{0.0}; ///< Right overhang of the vehicle. + double left_overhang{0.0}; ///< Left overhang of the vehicle. +}; + +/** + * @brief Represents a traffic signal with a timestamp. + */ +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; ///< Timestamp of the signal. + autoware_perception_msgs::msg::TrafficLightGroup signal; ///< Traffic light group. +}; + +using Pose = geometry_msgs::msg::Pose; +using Point2d = autoware_utils::Point2d; +using LineString2d = autoware_utils::LineString2d; +using Polygon2d = autoware_utils::Polygon2d; +using BasicPolygons2d = std::vector; +using Polygons2d = std::vector; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; + +namespace planning_utils +{ +size_t calcSegmentIndexFromPointIndex( + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t idx); + +bool createDetectionAreaPolygons( + Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, + const double min_velocity = 1.0); + +Point2d calculateOffsetPoint2d( + const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y); + +void extractClosePartition( + const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, + BasicPolygons2d & close_partition, const double distance_thresh = 30.0); + +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr & ll, BasicPolygons2d & polys); + +void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input); + +void insertVelocity( + PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, + size_t & insert_index, const double min_distance = 0.001); + +inline int64_t bitShift(int64_t original_id) +{ + return original_id << (sizeof(int32_t) * 8 / 2); +} + +bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); +geometry_msgs::msg::Pose getAheadPose( + const size_t start_idx, const double ahead_dist, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); +Polygon2d generatePathPolygon( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); + +double calcJudgeLineDistWithAccLimit( + const double velocity, const double max_stop_acceleration, const double delay_response_time); + +double calcJudgeLineDistWithJerkLimit( + const double velocity, const double acceleration, const double max_stop_acceleration, + const double max_stop_jerk, const double delay_response_time); + +double calcDecelerationVelocityFromDistanceToTarget( + const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel, + const double current_velocity, const double distance_to_target); + +double findReachTime( + const double jerk, const double accel, const double velocity, const double distance, + const double t_min, const double t_max); + +std::vector toRosPoints(const PredictedObjects & object); + +LineString2d extendLine( + const lanelet::ConstPoint3d & lanelet_point1, const lanelet::ConstPoint3d & lanelet_point2, + const double & length); + +template +std::vector concatVector(const std::vector & vec1, const std::vector & vec2) +{ + auto concat_vec = vec1; + concat_vec.insert(std::end(concat_vec), std::begin(vec2), std::end(vec2)); + return concat_vec; +} + +std::optional getNearestLaneId( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); + +std::vector getSortedLaneIdsFromPath(const PathWithLaneId & path); + +// return the set of lane_ids in the path after base_lane_id +std::vector getSubsequentLaneIdsSetOnPath( + const PathWithLaneId & path, int64_t base_lane_id); + +template +std::unordered_map, lanelet::ConstLanelet> getRegElemMapOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::unordered_map, lanelet::ConstLanelet> reg_elem_map_on_path; + + // Add current lane id + const auto nearest_lane_id = getNearestLaneId(path, lanelet_map, current_pose); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + // Add subsequent lane_ids from nearest lane_id + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); + } else { + // Add all lane_ids in path + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + } + + for (const auto lane_id : unique_lane_ids) { + const auto ll = lanelet_map->laneletLayer.get(lane_id); + + for (const auto & reg_elem : ll.regulatoryElementsAs()) { + reg_elem_map_on_path.insert(std::make_pair(reg_elem, ll)); + } + } + + return reg_elem_map_on_path; +} + +template +std::set getRegElemIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::set reg_elem_id_set; + for (const auto & m : getRegElemMapOnPath(path, lanelet_map, current_pose)) { + reg_elem_id_set.insert(m.first->id()); + } + return reg_elem_id_set; +} + +template +std::set getLaneletIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::set id_set; + for (const auto & m : getRegElemMapOnPath(path, lanelet_map, current_pose)) { + id_set.insert(m.second.id()); + } + return id_set; +} + +std::optional insertDecelPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output, + const float target_velocity); + +std::vector getLaneletsOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); + +std::set getLaneIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); + +bool isOverLine( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, + const double offset = 0.0); + +std::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); + +std::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output); + +/* + @brief return 'associative' lanes in the intersection. 'associative' means that a lane shares same + or lane-changeable parent lanes with `lane` and has same turn_direction value. + */ +std::set getAssociativeIntersectionLanelets( + const lanelet::ConstLanelet & lane, const lanelet::LaneletMapPtr lanelet_map, + const lanelet::routing::RoutingGraphPtr routing_graph); + +lanelet::ConstLanelets getConstLaneletsFromIds( + const lanelet::LaneletMapConstPtr & map, const std::set & ids); + +} // namespace planning_utils +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml new file mode 100644 index 0000000000..a66faa2e0a --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -0,0 +1,59 @@ + + + + autoware_behavior_velocity_planner_common + 0.41.0 + The autoware_behavior_velocity_planner_common package + + Tomoya Kimura + Shumpei Wakabayashi + Takagi, Isamu + Fumiya Watanabe + Mamoru Sobue + + Apache License 2.0 + + Yukihiro Saito + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_adapi_v1_msgs + autoware_internal_debug_msgs + autoware_interpolation + autoware_lanelet2_extension + autoware_map_msgs + autoware_motion_utils + autoware_objects_of_interest_marker_interface + autoware_perception_msgs + autoware_planning_factor_interface + autoware_planning_msgs + autoware_route_handler + autoware_universe_utils + autoware_vehicle_info_utils + autoware_velocity_smoother + diagnostic_msgs + eigen + geometry_msgs + nav_msgs + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_test_utils + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json new file mode 100644 index 0000000000..2468b71aa9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json @@ -0,0 +1,59 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Behavior Velocity Planner Common", + "type": "object", + "definitions": { + "behavior_velocity_planner_common": { + "type": "object", + "properties": { + "max_accel": { + "type": "number", + "default": "-2.8", + "description": "(to be a global parameter) max acceleration of the vehicle" + }, + "system_delay": { + "type": "number", + "default": "0.5", + "description": "(to be a global parameter) delay time until output control command" + }, + "delay_response_time": { + "type": "number", + "default": "0.5", + "description": "(to be a global parameter) delay time of the vehicle's response to control commands" + }, + "max_jerk": { + "type": "number", + "default": "-5.0", + "description": "max jerk of the vehicle" + }, + "is_publish_debug_path": { + "type": "boolean", + "default": "false", + "description": "is publish debug path?" + } + }, + "required": [ + "max_accel", + "system_delay", + "delay_response_time", + "max_jerk", + "is_publish_debug_path" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/behavior_velocity_planner_common" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp new file mode 100644 index 0000000000..df2a183e3b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp @@ -0,0 +1,72 @@ +// Copyright 2019 Autoware Foundation +// +// 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 "autoware/behavior_velocity_planner_common/planner_data.hpp" + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include + +namespace autoware::behavior_velocity_planner +{ +PlannerData::PlannerData(rclcpp::Node & node) +: clock_(node.get_clock()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) +{ + max_stop_acceleration_threshold = node.declare_parameter("max_accel"); + max_stop_jerk_threshold = node.declare_parameter("max_jerk"); + system_delay = node.declare_parameter("system_delay"); + delay_response_time = node.declare_parameter("delay_response_time"); +} + +bool PlannerData::isVehicleStopped(const double stop_duration) const +{ + if (velocity_buffer.empty()) { + return false; + } + + const auto now = clock_->now(); + std::vector vs; + for (const auto & velocity : velocity_buffer) { + vs.push_back(velocity.twist.linear.x); + + const auto & s = velocity.header.stamp; + const auto time_diff = + now >= s ? now - s : rclcpp::Duration(0, 0); // Note: negative time throws an exception. + if (time_diff.seconds() >= stop_duration) { + break; + } + } + + constexpr double stop_velocity = 1e-3; + for (const auto & v : vs) { + if (v >= stop_velocity) { + return false; + } + } + + return true; +} + +std::optional PlannerData::getTrafficSignal( + const lanelet::Id id, const bool keep_last_observation) const +{ + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; + if (traffic_light_id_map.count(id) == 0) { + return std::nullopt; + } + return std::make_optional(traffic_light_id_map.at(id)); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp new file mode 100644 index 0000000000..789b5ee412 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -0,0 +1,61 @@ +// Copyright 2023 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 +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +SceneModuleInterface::SceneModuleInterface( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: module_id_(module_id), + logger_(logger), + clock_(clock), + time_keeper_(time_keeper), + planning_factor_interface_(planning_factor_interface) +{ +} + +size_t SceneModuleInterface::findEgoSegmentIndex( + const std::vector & points) const +{ + const auto & p = planner_data_; + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold); +} + +template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::modifyPathVelocity( + autoware_internal_planning_msgs::msg::PathWithLaneId * path); +template void SceneModuleManagerInterface::deleteExpiredModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp new file mode 100644 index 0000000000..24a2d4d7eb --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -0,0 +1,129 @@ +// Copyright 2023 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 +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#include + +namespace +{ +geometry_msgs::msg::Point operator+( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + geometry_msgs::msg::Point p; + p.x = p1.x + p2.x; + p.y = p1.y + p2.y; + p.z = p1.z + p2.z; + + return p; +} + +geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & p, const double v) +{ + geometry_msgs::msg::Point multiplied_p; + multiplied_p.x = p.x * v; + multiplied_p.y = p.y * v; + multiplied_p.z = p.z * v; + + return multiplied_p; +} + +} // namespace + +namespace autoware::behavior_velocity_planner::arc_lane_utils +{ + +double calcSignedDistance(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) +{ + Eigen::Affine3d map2p1; + tf2::fromMsg(p1, map2p1); + const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z); + return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); +} + +// calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) + +std::optional checkCollision( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) +{ + const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x); + + if (det == 0.0) { + // collision is not one point. + return std::nullopt; + } + + const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det; + const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det; + + // check collision is outside the segment line + if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) { + return std::nullopt; + } + + return p1 * (1.0 - t1) + p2 * t1; +} + +std::optional findOffsetSegment( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t index, + const double offset) +{ + if (offset >= 0) { + return findForwardOffsetSegment(path, index, offset); + } + + return findBackwardOffsetSegment(path, index, -offset); +} + +std::optional createTargetPoint( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const double margin, const double vehicle_offset) +{ + // Find collision segment + const auto collision_segment = findCollisionSegment(path, stop_line); + if (!collision_segment) { + // No collision + return {}; + } + + // Calculate offset length from stop line + // Use '-' to make the positive direction is forward + const double offset_length = -(margin + vehicle_offset); + + // Find offset segment + const auto offset_segment = findOffsetSegment(path, *collision_segment, offset_length); + if (!offset_segment) { + // No enough path length + return {}; + } + + const auto target_pose = calcTargetPose(path, *offset_segment); + + const auto front_idx = offset_segment->first; + return std::make_pair(front_idx, target_pose); +} +} // namespace autoware::behavior_velocity_planner::arc_lane_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp new file mode 100644 index 0000000000..d9162419ec --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 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 + +#include + +namespace autoware::behavior_velocity_planner +{ + +Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) +{ + Polygon2d polygon; + + polygon.outer().push_back(left_line.front()); + + for (const auto & itr : right_line) { + polygon.outer().push_back(itr); + } + + for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) { + polygon.outer().push_back(*itr); + } + + bg::correct(polygon); + return polygon; +} + +Polygon2d upScalePolygon( + const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale) +{ + Polygon2d transformed_polygon; + // upscale + for (size_t i = 0; i < polygon.outer().size(); i++) { + const double upscale_x = (polygon.outer().at(i).x() - position.x) * scale + position.x; + const double upscale_y = (polygon.outer().at(i).y() - position.y) * scale + position.y; + transformed_polygon.outer().emplace_back(Point2d(upscale_x, upscale_y)); + } + return transformed_polygon; +} + +geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) +{ + geometry_msgs::msg::Polygon polygon_msg; + geometry_msgs::msg::Point32 point_msg; + for (const auto & p : polygon.outer()) { + point_msg.x = static_cast(p.x()); + point_msg.y = static_cast(p.y()); + polygon_msg.points.push_back(point_msg); + } + return polygon_msg; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp new file mode 100644 index 0000000000..fd6a61a0ba --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -0,0 +1,129 @@ +// Copyright 2020 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 +#include +#include + +#include +#include +namespace autoware::behavior_velocity_planner::debug +{ +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; + +visualization_msgs::msg::MarkerArray createPolygonMarkerArray( + const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, + const rclcpp::Time & now, const double x, const double y, const double z, const double r, + const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + { + auto marker = create_default_marker( + "map", now, ns, static_cast(module_id), visualization_msgs::msg::Marker::LINE_STRIP, + create_marker_scale(static_cast(x), static_cast(y), static_cast(z)), + create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.8f)); + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + + for (const auto & p : polygon.points) { + geometry_msgs::msg::Point point; + point.x = p.x; + point.y = p.y; + point.z = p.z; + marker.points.push_back(point); + } + + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + msg.markers.push_back(marker); + } + return msg; +} + +visualization_msgs::msg::MarkerArray createPathMarkerArray( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + for (size_t i = 0; i < path.points.size(); ++i) { + const auto & p = path.points.at(i); + + auto marker = create_default_marker( + "map", now, ns, static_cast(planning_utils::bitShift(lane_id) + i), + visualization_msgs::msg::Marker::ARROW, + create_marker_scale(static_cast(x), static_cast(y), static_cast(z)), + create_marker_color( + static_cast(r), static_cast(g), static_cast(b), 0.999f)); + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.pose = p.point.pose; + + if (std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end()) { + // if p.lane_ids has lane_id + marker.color = create_marker_color( + static_cast(r), static_cast(g), static_cast(b), 0.999f); + } else { + marker.color = create_marker_color(0.5, 0.5, 0.5, 0.999); + } + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createObjectsMarkerArray( + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + auto marker = create_default_marker( + "map", now, ns, 0, visualization_msgs::msg::Marker::CUBE, create_marker_scale(3.0, 1.0, 1.0), + create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.8f)); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + + marker.id = static_cast(planning_utils::bitShift(module_id) + i); + marker.pose = object.kinematics.initial_pose_with_covariance.pose; + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createPointsMarkerArray( + const std::vector & points, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + auto marker = create_default_marker( + "map", now, ns, 0, visualization_msgs::msg::Marker::SPHERE, create_marker_scale(x, y, z), + create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.999f)); + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + for (size_t i = 0; i < points.size(); ++i) { + marker.id = static_cast(i + planning_utils::bitShift(module_id)); + marker.pose.position = points.at(i); + msg.markers.push_back(marker); + } + + return msg; +} +} // namespace autoware::behavior_velocity_planner::debug diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp new file mode 100644 index 0000000000..38f7442999 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -0,0 +1,169 @@ +// Copyright 2020 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 +#include +#include +#include +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +bool splineInterpolate( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_internal_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger) +{ + if (input.points.size() < 2) { + RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1."); + return false; + } + + output = autoware::motion_utils::resamplePath(input, interval, false, true, true, false); + + return true; +} + +/* + * Interpolate the path with a fixed interval by spline. + * In order to correctly inherit the position of the planned velocity points, the position of the + * existing points in the input path are kept in the interpolated path. + * The velocity is interpolated by zero-order hold, that is, the velocity of the interpolated point + * is the velocity of the closest point for the input "sub-path" which consists of the points before + * the interpolated point. + */ +autoware_planning_msgs::msg::Path interpolatePath( + const autoware_planning_msgs::msg::Path & path, const double length, const double interval) +{ + const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("path_utilization")}; + + const double epsilon = 0.01; + std::vector s_in; + if (2000 < path.points.size()) { + RCLCPP_WARN( + logger, "because path size is too large, calculation cost is high. size is %d.", + (int)path.points.size()); + } + if (path.points.size() < 2) { + RCLCPP_WARN(logger, "Do not interpolate because path size is 1."); + return path; + } + + double path_len = std::min(length, autoware::motion_utils::calcArcLength(path.points)); + { + std::vector x; + std::vector y; + std::vector z; + std::vector v; + double s = 0.0; + for (size_t idx = 0; idx < path.points.size(); ++idx) { + const auto path_point = path.points.at(idx); + x.push_back(path_point.pose.position.x); + y.push_back(path_point.pose.position.y); + z.push_back(path_point.pose.position.z); + v.push_back(path_point.longitudinal_velocity_mps); + if (idx != 0) { + const auto path_point_prev = path.points.at(idx - 1); + s += autoware_utils::calc_distance2d(path_point_prev.pose, path_point.pose); + } + if (s > path_len) { + break; + } + s_in.push_back(s); + } + + // update path length + path_len = std::min(path_len, s_in.back()); + + // Check Terminal Points + if (std::fabs(s_in.back() - path_len) < epsilon) { + s_in.back() = path_len; + } else { + s_in.push_back(path_len); + } + } + + // Calculate query points + // Use all values of s_in to inherit the velocity-planned point, and add some points for + // interpolation with a constant interval if the point is not closed to the original s_in points. + // (because if this interval is very short, the interpolation will be less accurate and may fail.) + std::vector s_out = s_in; + + const auto has_almost_same_value = [&](const auto & vec, const auto x) { + if (vec.empty()) return false; + const auto has_close = [&](const auto v) { return std::abs(v - x) < epsilon; }; + return std::find_if(vec.begin(), vec.end(), has_close) != vec.end(); + }; + for (double s = 0.0; s < path_len; s += interval) { + if (!has_almost_same_value(s_out, s)) { + s_out.push_back(s); + } + } + + std::sort(s_out.begin(), s_out.end()); + + if (s_out.empty()) { + RCLCPP_WARN(logger, "Do not interpolate because s_out is empty."); + return path; + } + + return autoware::motion_utils::resamplePath(path, s_out); +} + +autoware_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_planning_msgs::msg::Path & path) +{ + autoware_planning_msgs::msg::Path filtered_path; + + const double epsilon = 0.01; + size_t latest_id = 0; + for (size_t i = 0; i < path.points.size(); ++i) { + double dist = 0.0; + if (i != 0) { + const double x = + path.points.at(i).pose.position.x - path.points.at(latest_id).pose.position.x; + const double y = + path.points.at(i).pose.position.y - path.points.at(latest_id).pose.position.y; + dist = std::sqrt(x * x + y * y); + } + if (i == 0 || epsilon < dist /*init*/) { + latest_id = i; + filtered_path.points.push_back(path.points.at(latest_id)); + } else { + filtered_path.points.back().longitudinal_velocity_mps = std::min( + filtered_path.points.back().longitudinal_velocity_mps, + path.points.at(i).longitudinal_velocity_mps); + } + } + + return filtered_path; +} +autoware_planning_msgs::msg::Path filterStopPathPoint( + const autoware_planning_msgs::msg::Path & path) +{ + autoware_planning_msgs::msg::Path filtered_path = path; + bool found_stop = false; + for (auto & point : filtered_path.points) { + if (std::fabs(point.longitudinal_velocity_mps) < 0.01) { + found_stop = true; + } + if (found_stop) { + point.longitudinal_velocity_mps = 0.0; + } + } + return filtered_path; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp new file mode 100644 index 0000000000..c0d6eadaac --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -0,0 +1,99 @@ +// Copyright 2023 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 +#include "autoware/motion_utils/trajectory/conversion.hpp" + +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Quaternion; +using TrajectoryPointWithIdx = std::pair; + +//! smooth path point with lane id starts from ego position on path to the path end +bool smoothPath( + const PathWithLaneId & in_path, PathWithLaneId & out_path, + const std::shared_ptr & planner_data) +{ + const geometry_msgs::msg::Pose current_pose = planner_data->current_odometry->pose; + const double v0 = planner_data->current_velocity->twist.linear.x; + const double a0 = planner_data->current_acceleration->accel.accel.linear.x; + const auto & external_v_limit = planner_data->external_velocity_limit; + const auto & smoother = planner_data->velocity_smoother_; + + auto trajectory = autoware::motion_utils::convertToTrajectoryPoints< + autoware_internal_planning_msgs::msg::PathWithLaneId>(in_path); + const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); + + const auto traj_steering_rate_limited = + smoother->applySteeringRateLimit(traj_lateral_acc_filtered, false); + + // Resample trajectory with ego-velocity based interval distances + auto traj_resampled = smoother->resampleTrajectory( + traj_steering_rate_limited, v0, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + const size_t traj_resampled_closest = + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + std::vector debug_trajectories; + // Clip trajectory from closest point + TrajectoryPoints clipped; + TrajectoryPoints traj_smoothed; + clipped.insert( + clipped.end(), traj_resampled.begin() + static_cast(traj_resampled_closest), + traj_resampled.end()); + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { + std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; + return false; + } + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled.begin(), + traj_resampled.begin() + static_cast(traj_resampled_closest)); + + if (external_v_limit) { + autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + } + out_path = autoware::motion_utils::convertToPathWithLaneId(traj_smoothed); + return true; +} + +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp new file mode 100644 index 0000000000..4bf6d98e31 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -0,0 +1,700 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "autoware/behavior_velocity_planner_common/utilization/util.hpp" + +#include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_lanelet2_extension/utility/query.hpp" + +#include + +#include + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace +{ +size_t calcPointIndexFromSegmentIndex( + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t seg_idx) +{ + const size_t prev_point_idx = seg_idx; + const size_t next_point_idx = seg_idx + 1; + + const double prev_dist = + autoware_utils::calc_distance2d(point, points.at(prev_point_idx)); + const double next_dist = + autoware_utils::calc_distance2d(point, points.at(next_point_idx)); + + if (prev_dist < next_dist) { + return prev_point_idx; + } + return next_point_idx; +} + +using autoware_planning_msgs::msg::PathPoint; + +PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio) +{ + auto lerp = [](const double a, const double b, const double t) { return a + t * (b - a); }; + PathPoint p; + p.pose = autoware_utils::calc_interpolated_pose(p0, p1, ratio); + const double v = lerp(p0.longitudinal_velocity_mps, p1.longitudinal_velocity_mps, ratio); + p.longitudinal_velocity_mps = static_cast(v); + return p; +} + +geometry_msgs::msg::Pose transformRelCoordinate2D( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) +{ + // translation + geometry_msgs::msg::Point trans_p; + trans_p.x = target.position.x - origin.position.x; + trans_p.y = target.position.y - origin.position.y; + + // rotation (use inverse matrix of rotation) + double yaw = tf2::getYaw(origin.orientation); + + geometry_msgs::msg::Pose res; + res.position.x = (std::cos(yaw) * trans_p.x) + (std::sin(yaw) * trans_p.y); + res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y); + res.position.z = target.position.z - origin.position.z; + res.orientation = + autoware_utils::create_quaternion_from_yaw(tf2::getYaw(target.orientation) - yaw); + + return res; +} + +} // namespace + +namespace autoware::behavior_velocity_planner::planning_utils +{ +using autoware::motion_utils::calcSignedArcLength; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_offset_pose; +using autoware_planning_msgs::msg::PathPoint; + +size_t calcSegmentIndexFromPointIndex( + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t idx) +{ + if (idx == 0) { + return 0; + } + if (idx == points.size() - 1) { + return idx - 1; + } + if (points.size() < 3) { + return 0; + } + + const double offset_to_seg = + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); + if (0 < offset_to_seg) { + return idx; + } + return idx - 1; +} + +Point2d calculateOffsetPoint2d( + const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y) +{ + return to_bg2d(calc_offset_pose(pose, offset_x, offset_y, 0.0)); +} + +bool createDetectionAreaPolygons( + Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, + const double min_velocity) +{ + /** + * @brief relationships for interpolated polygon + * + * +(min_length,max_distance)-+ - +---+(max_length,max_distance) = outer_polygons + * | | + * +--------------------------+ - +---+(max_length,min_distance) = inner_polygons + */ + const double min_len = da_range.min_longitudinal_distance; + const double max_len = da_range.max_longitudinal_distance; + const double max_dst = da_range.max_lateral_distance; + const double interval = da_range.interval; + const double offset_left = (da_range.wheel_tread / 2.0) + da_range.left_overhang; + const double offset_right = (da_range.wheel_tread / 2.0) + da_range.right_overhang; + + //! max index is the last index of path point + const auto max_index = static_cast(path.points.size() - 1); + //! avoid bug with same point polygon + const double eps = 1e-3; + auto nearest_idx = + calcPointIndexFromSegmentIndex(path.points, target_pose.position, target_seg_idx); + if (max_index == nearest_idx) return false; // case of path point is not enough size + auto p0 = path.points.at(nearest_idx).point; + auto first_idx = nearest_idx + 1; + + // use ego point as start point if same point as ego is not in the path + const auto dist_to_nearest = + std::fabs(calcSignedArcLength(path.points, target_pose.position, target_seg_idx, nearest_idx)); + if (dist_to_nearest > eps) { + // interpolate ego point + const auto & pp = path.points; + const double ds = calc_distance2d(pp.at(target_seg_idx), pp.at(target_seg_idx + 1)); + const double dist_to_target_seg = + calcSignedArcLength(path.points, target_seg_idx, target_pose.position, target_seg_idx); + const double ratio = dist_to_target_seg / ds; + p0 = getLerpPathPointWithLaneId( + pp.at(target_seg_idx).point, pp.at(target_seg_idx + 1).point, ratio); + + // new first index should be ahead of p0 + first_idx = target_seg_idx + 1; + } + + double ttc = 0.0; + double dist_sum = 0.0; + double length = 0; + // initial point of detection area polygon + LineString2d left_inner_bound = {calculateOffsetPoint2d(p0.pose, min_len, offset_left)}; + LineString2d left_outer_bound = {calculateOffsetPoint2d(p0.pose, min_len, offset_left + eps)}; + LineString2d right_inner_bound = {calculateOffsetPoint2d(p0.pose, min_len, -offset_right)}; + LineString2d right_outer_bound = {calculateOffsetPoint2d(p0.pose, min_len, -offset_right - eps)}; + for (size_t s = first_idx; s <= max_index; s++) { + const auto p1 = path.points.at(s).point; + const double ds = calc_distance2d(p0, p1); + dist_sum += ds; + length += ds; + // calculate the distance that obstacles can move until ego reach the trajectory point + const double v_average = 0.5 * (p0.longitudinal_velocity_mps + p1.longitudinal_velocity_mps); + const double v = std::max(v_average, min_velocity); + const double dt = ds / v; + ttc += dt; + + // for offset calculation + const double max_lateral_distance_right = + std::min(max_dst, offset_right + ttc * obstacle_vel_mps + eps); + const double max_lateral_distance_left = + std::min(max_dst, offset_left + ttc * obstacle_vel_mps + eps); + + // left bound + if (da_range.use_left) { + left_inner_bound.emplace_back(calculateOffsetPoint2d(p1.pose, min_len, offset_left)); + left_outer_bound.emplace_back( + calculateOffsetPoint2d(p1.pose, min_len, max_lateral_distance_left)); + } + // right bound + if (da_range.use_right) { + right_inner_bound.emplace_back(calculateOffsetPoint2d(p1.pose, min_len, -offset_right)); + right_outer_bound.emplace_back( + calculateOffsetPoint2d(p1.pose, min_len, -max_lateral_distance_right)); + } + // replace previous point with next point + p0 = p1; + // separate detection area polygon with fixed interval or at the end of detection max length + if (length > interval || max_len < dist_sum || s == max_index) { + if (left_inner_bound.size() > 1) + da_polys.emplace_back(lines2polygon(left_inner_bound, left_outer_bound)); + if (right_inner_bound.size() > 1) + da_polys.emplace_back(lines2polygon(right_outer_bound, right_inner_bound)); + left_inner_bound = {left_inner_bound.back()}; + left_outer_bound = {left_outer_bound.back()}; + right_inner_bound = {right_inner_bound.back()}; + right_outer_bound = {right_outer_bound.back()}; + length = 0; + if (max_len < dist_sum || s == max_index) return true; + } + } + return true; +} + +void extractClosePartition( + const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, + BasicPolygons2d & close_partition, const double distance_thresh) +{ + close_partition.clear(); + for (const auto & p : all_partitions) { + if (boost::geometry::distance(Point2d(position.x, position.y), p) < distance_thresh) { + close_partition.emplace_back(p); + } + } +} + +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr & ll, BasicPolygons2d & polys) +{ + const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll); + for (const auto & partition : partitions) { + lanelet::BasicLineString2d line; + for (const auto & p : partition) { + line.emplace_back(lanelet::BasicPoint2d{p.x(), p.y()}); + } + // correct line to calculate distance in accurate + boost::geometry::correct(line); + polys.emplace_back(line); + } +} + +void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input) +{ + for (size_t i = begin_idx; i < input->points.size(); ++i) { + input->points.at(i).point.longitudinal_velocity_mps = + std::min(static_cast(vel), input->points.at(i).point.longitudinal_velocity_mps); + } +} + +void insertVelocity( + PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, + size_t & insert_index, const double min_distance) +{ + bool already_has_path_point = false; + // consider front/back point is near to insert point or not + int min_idx = std::max(0, static_cast(insert_index - 1)); + int max_idx = + std::min(static_cast(insert_index + 1), static_cast(path.points.size() - 1)); + for (int i = min_idx; i <= max_idx; i++) { + if (calc_distance2d(path.points.at(static_cast(i)), path_point) < min_distance) { + path.points.at(i).point.longitudinal_velocity_mps = v; + already_has_path_point = true; + insert_index = static_cast(i); + // set velocity from is going to insert min velocity later + break; + } + } + //! insert velocity point only if there is no close point on path + if (!already_has_path_point) { + path.points.insert(path.points.begin() + insert_index, path_point); + } + // set zero velocity from insert index + setVelocityFromIndex(insert_index, v, &path); +} + +bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) +{ + geometry_msgs::msg::Pose p = transformRelCoordinate2D(target, origin); + const bool is_target_ahead = (p.position.x > 0.0); + return is_target_ahead; +} + +geometry_msgs::msg::Pose getAheadPose( + const size_t start_idx, const double ahead_dist, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) +{ + if (path.points.empty()) { + return geometry_msgs::msg::Pose{}; + } + + double curr_dist = 0.0; + double prev_dist = 0.0; + for (size_t i = start_idx; i < path.points.size() - 1; ++i) { + const geometry_msgs::msg::Pose p0 = path.points.at(i).point.pose; + const geometry_msgs::msg::Pose p1 = path.points.at(i + 1).point.pose; + curr_dist += autoware_utils::calc_distance2d(p0, p1); + if (curr_dist > ahead_dist) { + const double dl = std::max(curr_dist - prev_dist, 0.0001 /* avoid 0 divide */); + const double w_p0 = (curr_dist - ahead_dist) / dl; + const double w_p1 = (ahead_dist - prev_dist) / dl; + geometry_msgs::msg::Pose p; + p.position.x = w_p0 * p0.position.x + w_p1 * p1.position.x; + p.position.y = w_p0 * p0.position.y + w_p1 * p1.position.y; + p.position.z = w_p0 * p0.position.z + w_p1 * p1.position.z; + tf2::Quaternion q0_tf; + tf2::Quaternion q1_tf; + tf2::fromMsg(p0.orientation, q0_tf); + tf2::fromMsg(p1.orientation, q1_tf); + p.orientation = tf2::toMsg(q0_tf.slerp(q1_tf, w_p1)); + return p; + } + prev_dist = curr_dist; + } + return path.points.back().point.pose; +} + +Polygon2d generatePathPolygon( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width) +{ + Polygon2d ego_area; // open polygon + for (size_t i = start_idx; i <= end_idx; ++i) { + const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); + const double x = path.points.at(i).point.pose.position.x + width * std::sin(yaw); + const double y = path.points.at(i).point.pose.position.y - width * std::cos(yaw); + ego_area.outer().push_back(Point2d(x, y)); + } + for (size_t i = end_idx; i >= start_idx; --i) { + const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); + const double x = path.points.at(i).point.pose.position.x - width * std::sin(yaw); + const double y = path.points.at(i).point.pose.position.y + width * std::cos(yaw); + ego_area.outer().push_back(Point2d(x, y)); + } + + bg::correct(ego_area); + return ego_area; +} + +double calcJudgeLineDistWithAccLimit( + const double velocity, const double max_stop_acceleration, const double delay_response_time) +{ + double judge_line_dist = + (velocity * velocity) / (2.0 * (-max_stop_acceleration)) + delay_response_time * velocity; + return judge_line_dist; +} + +double calcJudgeLineDistWithJerkLimit( + const double velocity, const double acceleration, const double max_stop_acceleration, + const double max_stop_jerk, const double delay_response_time) +{ + if (velocity <= 0.0) { + return 0.0; + } + + /* t0: subscribe traffic light state and decide to stop */ + /* t1: braking start (with jerk limitation) */ + /* t2: reach max stop acceleration */ + /* t3: stop */ + + const double t1 = delay_response_time; + const double x1 = velocity * t1; + + const double v2 = velocity + (std::pow(max_stop_acceleration, 2) - std::pow(acceleration, 2)) / + (2.0 * max_stop_jerk); + + if (v2 <= 0.0) { + const double t2 = -1.0 * + (max_stop_acceleration + + std::sqrt(acceleration * acceleration - 2.0 * max_stop_jerk * velocity)) / + max_stop_jerk; + const double x2 = + velocity * t2 + acceleration * std::pow(t2, 2) / 2.0 + max_stop_jerk * std::pow(t2, 3) / 6.0; + return std::max(0.0, x1 + x2); + } + + const double t2 = (max_stop_acceleration - acceleration) / max_stop_jerk; + const double x2 = + velocity * t2 + acceleration * std::pow(t2, 2) / 2.0 + max_stop_jerk * std::pow(t2, 3) / 6.0; + + const double x3 = -1.0 * std::pow(v2, 2) / (2.0 * max_stop_acceleration); + return std::max(0.0, x1 + x2 + x3); +} + +double findReachTime( + const double jerk, const double accel, const double velocity, const double distance, + const double t_min, const double t_max) +{ + const double j = jerk; + const double a = accel; + const double v = velocity; + const double d = distance; + const double min = t_min; + const double max = t_max; + auto f = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + if (f(min, j, a, v, d) > 0 || f(max, j, a, v, d) < 0) { + throw std::logic_error("[behavior_velocity](findReachTime): search range is invalid"); + } + const double eps = 1e-5; + const int warn_iter = 100; + double lower = min; + double upper = max; + double t = NAN; + int iter = 0; + while (true) { + t = 0.5 * (lower + upper); + const double fx = f(t, j, a, v, d); + // std::cout<<"fx: "< 0.0) { + upper = t; + } else { + lower = t; + } + iter++; + if (iter > warn_iter) + std::cerr << "[behavior_velocity](findReachTime): current iter is over warning" << std::endl; + } + // std::cout<<"iter: "< 0 || max_slowdown_accel > 0) { + throw std::logic_error("max_slowdown_jerk and max_slowdown_accel should be negative"); + } + // case0: distance to target is behind ego + if (distance_to_target <= 0) return current_velocity; + auto ft = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + auto vt = [](const double t, const double j, const double a, const double v) { + return j * t * t / 2.0 + a * t + v; + }; + const double j_max = max_slowdown_jerk; + const double a0 = current_accel; + const double a_max = max_slowdown_accel; + const double v0 = current_velocity; + const double l = distance_to_target; + const double t_const_jerk = (a_max - a0) / j_max; + const double d_const_jerk_stop = ft(t_const_jerk, j_max, a0, v0, 0.0); + const double d_const_acc_stop = l - d_const_jerk_stop; + + if (d_const_acc_stop < 0) { + // case0: distance to target is within constant jerk deceleration + // use binary search instead of solving cubic equation + const double t_jerk = findReachTime(j_max, a0, v0, l, 0, t_const_jerk); + const double velocity = vt(t_jerk, j_max, a0, v0); + return velocity; + } + + const double v1 = vt(t_const_jerk, j_max, a0, v0); + const double discriminant_of_stop = 2.0 * a_max * d_const_acc_stop + v1 * v1; + // case3: distance to target is farther than distance to stop + if (discriminant_of_stop <= 0) { + return 0.0; + } + // case2: distance to target is within constant accel deceleration + // solve d = 0.5*a^2+v*t by t + const double t_acc = (-v1 + std::sqrt(discriminant_of_stop)) / a_max; + return vt(t_acc, 0.0, a_max, v1); +} + +std::vector toRosPoints(const PredictedObjects & object) +{ + std::vector points; + for (const auto & obj : object.objects) { + points.emplace_back(obj.kinematics.initial_pose_with_covariance.pose.position); + } + return points; +} + +LineString2d extendLine( + const lanelet::ConstPoint3d & lanelet_point1, const lanelet::ConstPoint3d & lanelet_point2, + const double & length) +{ + const Eigen::Vector2d p1(lanelet_point1.x(), lanelet_point1.y()); + const Eigen::Vector2d p2(lanelet_point2.x(), lanelet_point2.y()); + const Eigen::Vector2d t = (p2 - p1).normalized(); + return { + {(p1 - length * t).x(), (p1 - length * t).y()}, {(p2 + length * t).x(), (p2 + length * t).y()}}; +} + +std::optional getNearestLaneId( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + lanelet::ConstLanelets lanes; + const auto lane_ids = getSortedLaneIdsFromPath(path); + for (const auto & lane_id : lane_ids) { + lanes.push_back(lanelet_map->laneletLayer.get(lane_id)); + } + + lanelet::Lanelet closest_lane; + if (lanelet::utils::query::getClosestLanelet(lanes, current_pose, &closest_lane)) { + return closest_lane.id(); + } + return std::nullopt; +} + +std::vector getLaneletsOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + const auto nearest_lane_id = getNearestLaneId(path, lanelet_map, current_pose); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + // Add subsequent lane_ids from nearest lane_id + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); + } else { + // Add all lane_ids in path + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + } + + std::vector lanelets; + lanelets.reserve(unique_lane_ids.size()); + for (const auto lane_id : unique_lane_ids) { + lanelets.push_back(lanelet_map->laneletLayer.get(lane_id)); + } + + return lanelets; +} + +std::set getLaneIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::set lane_id_set; + for (const auto & lane : getLaneletsOnPath(path, lanelet_map, current_pose)) { + lane_id_set.insert(lane.id()); + } + + return lane_id_set; +} + +std::vector getSortedLaneIdsFromPath(const PathWithLaneId & path) +{ + std::vector sorted_lane_ids; + for (const auto & path_points : path.points) { + for (const auto lane_id : path_points.lane_ids) + if ( + std::find(sorted_lane_ids.begin(), sorted_lane_ids.end(), lane_id) == + sorted_lane_ids.end()) { + sorted_lane_ids.emplace_back(lane_id); + } + } + return sorted_lane_ids; +} + +std::vector getSubsequentLaneIdsSetOnPath( + const PathWithLaneId & path, int64_t base_lane_id) +{ + const auto all_lane_ids = getSortedLaneIdsFromPath(path); + const auto base_index = std::find(all_lane_ids.begin(), all_lane_ids.end(), base_lane_id); + + // cannot find base_index in all_lane_ids + if (base_index == all_lane_ids.end()) { + return {}; + } + + std::vector subsequent_lane_ids; + + std::copy(base_index, all_lane_ids.end(), std::back_inserter(subsequent_lane_ids)); + return subsequent_lane_ids; +} + +// TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex +bool isOverLine( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, + const double offset) +{ + return autoware::motion_utils::calcSignedArcLength( + path.points, self_pose.position, line_pose.position) + + offset < + 0.0; +} + +std::optional insertDecelPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output, + const float target_velocity) +{ + // TODO(tanaka): consider proper overlap threshold for inserting decel point + const double overlap_threshold = 5e-2; + // TODO(murooka): remove this function for u-turn and crossing-path + const size_t base_idx = + autoware::motion_utils::findNearestSegmentIndex(output.points, stop_point); + const auto insert_idx = autoware::motion_utils::insertTargetPoint( + base_idx, stop_point, output.points, overlap_threshold); + + if (!insert_idx) { + return {}; + } + + for (size_t i = insert_idx.value(); i < output.points.size(); ++i) { + const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps; + output.points.at(i).point.longitudinal_velocity_mps = + std::min(original_velocity, target_velocity); + } + return autoware_utils::get_pose(output.points.at(insert_idx.value())); +} + +// TODO(murooka): remove this function for u-turn and crossing-path +std::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output) +{ + const size_t base_idx = + autoware::motion_utils::findNearestSegmentIndex(output.points, stop_point); + const auto insert_idx = + autoware::motion_utils::insertStopPoint(base_idx, stop_point, output.points); + + if (!insert_idx) { + return {}; + } + + return autoware_utils::get_pose(output.points.at(insert_idx.value())); +} + +std::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output) +{ + const auto insert_idx = + autoware::motion_utils::insertStopPoint(stop_seg_idx, stop_point, output.points); + + if (!insert_idx) { + return {}; + } + + return autoware_utils::get_pose(output.points.at(insert_idx.value())); +} + +std::set getAssociativeIntersectionLanelets( + const lanelet::ConstLanelet & lane, const lanelet::LaneletMapPtr lanelet_map, + const lanelet::routing::RoutingGraphPtr routing_graph) +{ + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + if (turn_direction == "else") { + return {}; + } + + const auto parents = routing_graph->previous(lane); + std::set parent_neighbors; + for (const auto & parent : parents) { + const auto neighbors = routing_graph->besides(parent); + for (const auto & neighbor : neighbors) parent_neighbors.insert(neighbor.id()); + } + std::set associative_intersection_lanelets; + associative_intersection_lanelets.insert(lane.id()); + for (const auto & parent_neighbor_id : parent_neighbors) { + const auto parent_neighbor = lanelet_map->laneletLayer.get(parent_neighbor_id); + const auto followings = routing_graph->following(parent_neighbor); + for (const auto & following : followings) { + if (following.attributeOr("turn_direction", "else") == turn_direction) { + associative_intersection_lanelets.insert(following.id()); + } + } + } + return associative_intersection_lanelets; +} + +lanelet::ConstLanelets getConstLaneletsFromIds( + const lanelet::LaneletMapConstPtr & map, const std::set & ids) +{ + lanelet::ConstLanelets ret{}; + for (const auto & id : ids) { + const auto ll = map->laneletLayer.get(id); + ret.push_back(ll); + } + return ret; +} + +} // namespace autoware::behavior_velocity_planner::planning_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp new file mode 100644 index 0000000000..db9a63169e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -0,0 +1,242 @@ +// Copyright 2021 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 "utils.hpp" + +#include +#include + +#include + +#include + +using PathIndexWithPoint2d = + autoware::behavior_velocity_planner::arc_lane_utils::PathIndexWithPoint2d; +using LineString2d = autoware::behavior_velocity_planner::LineString2d; +using Point2d = autoware::behavior_velocity_planner::Point2d; +namespace arc_lane_utils = autoware::behavior_velocity_planner::arc_lane_utils; + +namespace +{ +geometry_msgs::msg::Point createPoint(const double x, const double y, const double z) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + p.z = z; + return p; +} +} // namespace + +TEST(findCollisionSegment, nominal) +{ + /** + * find forward collision segment by stop line + * s + * | = | = | = | s | = | + * 0 1 2 3 s 4 5 + * + **/ + auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + + LineString2d stop_line; + stop_line.emplace_back(Point2d(3.5, 3.0)); + stop_line.emplace_back(Point2d(3.5, -3.0)); + auto segment = arc_lane_utils::findCollisionSegment(path, stop_line); + EXPECT_EQ(segment->first, static_cast(3)); + EXPECT_DOUBLE_EQ(segment->second.x, 3.5); + EXPECT_DOUBLE_EQ(segment->second.y, 0.0); +} + +TEST(findOffsetSegment, case_forward_offset_segment) +{ + auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + /** + * find offset segment forward + * | = | = | = | c | o | + * 0 1 2 3 4 5 + **/ + const int collision_segment_idx = 3; + const auto collision_point = createPoint(3.5, 0.0, 0.0); + const auto & collision_segment = std::make_pair(collision_segment_idx, collision_point); + // nominal + { + double offset_length = 1.0; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + const auto front_idx = offset_segment->first; + EXPECT_EQ(front_idx, static_cast(4)); + EXPECT_DOUBLE_EQ(offset_segment->second, 0.5); + } + // boundary condition + { + double offset_length = INFINITY; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + EXPECT_FALSE(offset_segment); + } +} + +TEST(findOffsetSegment, case_backward_offset_segment) +{ + auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + /** + * find offset segment forward + * | = | = | o | c | = | + * 0 1 2 3 4 5 + **/ + const int collision_segment_idx = 3; + const auto collision_point = createPoint(3.5, 0.0, 0.0); + const auto & collision_segment = std::make_pair(collision_segment_idx, collision_point); + // nominal + { + double offset_length = -1.0; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + const auto front_idx = offset_segment->first; + EXPECT_EQ(front_idx, static_cast(2)); + EXPECT_DOUBLE_EQ(offset_segment->second, 0.5); + } + // boundary condition + { + double offset_length = -INFINITY; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + EXPECT_FALSE(offset_segment); + } +} + +TEST(checkCollision, various_cases) +{ + using autoware::behavior_velocity_planner::arc_lane_utils::checkCollision; + constexpr double epsilon = 1e-6; + + { // normal case with collision + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // normal case without collision + const auto p1 = createPoint(1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, std::nullopt); + } + + { // normal case without collision + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, std::nullopt); + } + + { // line and point + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, std::nullopt); + } + + { // point and line + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(-1.0, 0.0, 0.0); + const auto p4 = createPoint(2.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, std::nullopt); + } + + { // collision with edges + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp new file mode 100644 index 0000000000..51c6b5b9dd --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp @@ -0,0 +1,245 @@ +// Copyright 2021 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 "autoware/motion_utils/trajectory/trajectory.hpp" +#include "utils.hpp" + +#include +#include +#include + +#include + +#include +#include + +#define DEBUG_PRINT_PATH(path) \ + { \ + std::stringstream ss; \ + ss << #path << "(px, vx): "; \ + for (const auto p : path.points) { \ + ss << "(" << p.pose.position.x << ", " << p.longitudinal_velocity_mps << "), "; \ + } \ + std::cerr << ss.str() << std::endl; \ + } + +TEST(is_ahead_of, nominal) +{ + using autoware::behavior_velocity_planner::planning_utils::isAheadOf; + geometry_msgs::msg::Pose target = test::generatePose(0); + geometry_msgs::msg::Pose origin = test::generatePose(1); + bool is_ahead = isAheadOf(target, origin); + EXPECT_FALSE(is_ahead); + target = test::generatePose(2); + is_ahead = isAheadOf(target, origin); + EXPECT_TRUE(is_ahead); +} + +TEST(smoothDeceleration, calculateMaxSlowDownVelocity) +{ + using autoware::behavior_velocity_planner::planning_utils:: + calcDecelerationVelocityFromDistanceToTarget; + const double current_accel = 1.0; + const double current_velocity = 5.0; + const double max_slow_down_jerk = -1.0; + const double max_slow_down_accel = -2.0; + const double eps = 1e-3; + { + for (int i = -8; i <= 24; i += 8) { + // arc length in path point + const double l = i * 1.0; + const double v = calcDecelerationVelocityFromDistanceToTarget( + max_slow_down_jerk, max_slow_down_accel, current_accel, current_velocity, l); + // case 0 : behind ego + if (i == -8) EXPECT_NEAR(v, 5.0, eps); + // case 1 : const jerk + else if (i == 0) + EXPECT_NEAR(v, 5.0, eps); + // case 1 : const jerk + else if (i == 8) + EXPECT_NEAR(v, 5.380, eps); + // case 2 : const accel + else if (i == 16) + EXPECT_NEAR(v, 2.872, eps); + // case 3 : after stop + else if (i == 24) + EXPECT_NEAR(v, 0.00, eps); + else + continue; + std::cout << "s: " << l << " v: " << v << std::endl; + } + } +} + +TEST(specialInterpolation, specialInterpolation) +{ + using autoware::behavior_velocity_planner::interpolatePath; + using autoware::motion_utils::calcSignedArcLength; + using autoware::motion_utils::searchZeroVelocityIndex; + using autoware_planning_msgs::msg::Path; + using autoware_planning_msgs::msg::PathPoint; + + const auto genPath = [](const auto p, const auto v) { + if (p.size() != v.size()) throw std::invalid_argument("different size is not expected"); + Path path; + for (size_t i = 0; i < p.size(); ++i) { + PathPoint pp; + pp.pose.position.x = p.at(i); + pp.longitudinal_velocity_mps = v.at(i); + path.points.push_back(pp); + } + return path; + }; + + constexpr auto length = 5.0; + constexpr auto interval = 1.0; + + const auto calcInterpolatedStopDist = [&](const auto & px, const auto & vx) { + const auto path = genPath(px, vx); + const auto res = interpolatePath(path, length, interval); + // DEBUG_PRINT_PATH(path); + // DEBUG_PRINT_PATH(res); + return calcSignedArcLength(res.points, 0, *searchZeroVelocityIndex(res.points)); + }; + + // expected stop position: s=2.0 + { + const std::vector px{0.0, 1.0, 2.0, 3.0}; + const std::vector vx{5.5, 5.5, 0.0, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), 2.0); + } + + // expected stop position: s=2.1 + { + constexpr auto expected = 2.1; + const std::vector px{0.0, 1.0, 2.1, 3.0}; + const std::vector vx{5.5, 5.5, 0.0, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=2.001 + { + constexpr auto expected = 2.001; + const std::vector px{0.0, 1.0, 2.001, 3.0}; + const std::vector vx{5.5, 5.5, 0.000, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=2.001 + { + constexpr auto expected = 2.001; + const std::vector px{0.0, 1.0, 1.999, 2.0, 2.001, 3.0}; + const std::vector vx{5.5, 5.5, 5.555, 5.5, 0.000, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=2.0 + { + constexpr auto expected = 2.0; + const std::vector px{0.0, 1.0, 1.999, 2.0, 2.001, 3.0}; + const std::vector vx{5.5, 5.5, 5.555, 0.0, 0.000, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=1.999 + { + constexpr auto expected = 1.999; + const std::vector px{0.0, 1.0, 1.999, 3.0}; + const std::vector vx{5.5, 5.5, 0.000, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=0.2 + { + constexpr auto expected = 0.2; + const std::vector px{0.0, 0.1, 0.2, 0.3, 0.4}; + const std::vector vx{5.5, 5.5, 0.0, 0.0, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=0.4 + { + constexpr auto expected = 0.4; + const std::vector px{0.0, 0.1, 0.2, 0.3, 0.4}; + const std::vector vx{5.5, 5.5, 5.5, 5.5, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } +} + +TEST(filterLitterPathPoint, nominal) +{ + using autoware::behavior_velocity_planner::filterLitterPathPoint; + using autoware_planning_msgs::msg::Path; + using autoware_planning_msgs::msg::PathPoint; + + const auto genPath = [](const std::vector & px, const std::vector & vx) { + Path path; + for (size_t i = 0; i < px.size(); ++i) { + PathPoint point; + point.pose.position.x = px[i]; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = static_cast(vx[i]); + path.points.push_back(point); + } + return path; + }; + + const std::vector px{0.0, 1.0, 1.001, 2.0, 3.0}; + const std::vector vx{5.0, 3.5, 3.5, 3.0, 2.5}; + + const auto path = genPath(px, vx); + const auto filtered_path = filterLitterPathPoint(path); + + ASSERT_EQ(filtered_path.points.size(), 4U); // Expected: Points at x = {0.0, 1.0, 2.0, 3.0} + EXPECT_DOUBLE_EQ(filtered_path.points[0].pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(filtered_path.points[1].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(filtered_path.points[2].pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(filtered_path.points[3].pose.position.x, 3.0); + + EXPECT_DOUBLE_EQ(filtered_path.points[0].longitudinal_velocity_mps, 5.0); + EXPECT_DOUBLE_EQ(filtered_path.points[1].longitudinal_velocity_mps, 3.5); + EXPECT_DOUBLE_EQ(filtered_path.points[2].longitudinal_velocity_mps, 3.0); + EXPECT_DOUBLE_EQ(filtered_path.points[3].longitudinal_velocity_mps, 2.5); +} + +TEST(filterStopPathPoint, nominal) +{ + using autoware::behavior_velocity_planner::filterStopPathPoint; + using autoware_planning_msgs::msg::Path; + using autoware_planning_msgs::msg::PathPoint; + + const auto genPath = [](const std::vector & px, const std::vector & vx) { + Path path; + for (size_t i = 0; i < px.size(); ++i) { + PathPoint point; + point.pose.position.x = px[i]; + point.longitudinal_velocity_mps = static_cast(vx[i]); + path.points.push_back(point); + } + return path; + }; + + const std::vector px{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector vx{5.0, 4.0, 0.0, 2.0, 3.0}; + + const auto path = genPath(px, vx); + const auto filtered_path = filterStopPathPoint(path); + + ASSERT_EQ(filtered_path.points.size(), 5U); + EXPECT_DOUBLE_EQ(filtered_path.points[0].longitudinal_velocity_mps, 5.0); + EXPECT_DOUBLE_EQ(filtered_path.points[1].longitudinal_velocity_mps, 4.0); + EXPECT_DOUBLE_EQ(filtered_path.points[2].longitudinal_velocity_mps, 0.0); + EXPECT_DOUBLE_EQ(filtered_path.points[3].longitudinal_velocity_mps, 0.0); + EXPECT_DOUBLE_EQ(filtered_path.points[4].longitudinal_velocity_mps, 0.0); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp new file mode 100644 index 0000000000..37ab955b9c --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp @@ -0,0 +1,87 @@ +// Copyright 2021 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 "utils.hpp" + +#include +#include + +#include + +#include +#include +#include + +using StateMachine = autoware::behavior_velocity_planner::StateMachine; +using State = autoware::behavior_velocity_planner::StateMachine::State; + +int enumToInt(State s) +{ + return static_cast(s); +} + +TEST(state_machine, on_initialized) +{ + StateMachine state_machine = StateMachine(); + EXPECT_EQ(enumToInt(State::GO), enumToInt(state_machine.getState())); +} + +TEST(state_machine, set_state_stop) +{ + StateMachine state_machine = StateMachine(); + state_machine.setState(State::STOP); + EXPECT_EQ(enumToInt(State::STOP), enumToInt(state_machine.getState())); +} + +TEST(state_machine, set_state_stop_with_margin_time) +{ + StateMachine state_machine = StateMachine(); + const double margin_time = 1.0; + state_machine.setMarginTime(margin_time); + rclcpp::Clock current_time = rclcpp::Clock(RCL_ROS_TIME); + rclcpp::Logger logger = rclcpp::get_logger("test_set_state_with_margin_time"); + // DO NOT SET GO until margin time past + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::GO)); + state_machine.setStateWithMarginTime(State::STOP, logger, current_time); + // set STOP immediately when stop is set + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::STOP)); +} + +TEST(state_machine, set_state_go_with_margin_time) +{ + StateMachine state_machine = StateMachine(); + const double margin_time = 0.2; + state_machine.setMarginTime(margin_time); + rclcpp::Logger logger = rclcpp::get_logger("test_set_state_with_margin_time"); + state_machine.setState(State::STOP); + size_t loop_counter = 0; + // loop until state change from STOP -> GO + while (state_machine.getState() == State::STOP) { + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::STOP)); + rclcpp::Clock current_time = rclcpp::Clock(RCL_ROS_TIME); + if (state_machine.getDuration() > margin_time) { + std::cerr << "stop duration is larger than margin time" << std::endl; + } + EXPECT_FALSE(state_machine.getDuration() > margin_time); + state_machine.setStateWithMarginTime(State::GO, logger, current_time); + loop_counter++; + } + // time past STOP -> GO + if (loop_counter > 2) { + EXPECT_TRUE(state_machine.getDuration() > margin_time); + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::GO)); + } else { + std::cerr << "[Warning] computational resource is not enough" << std::endl; + } +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp new file mode 100644 index 0000000000..ae4d248120 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp @@ -0,0 +1,113 @@ +// 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 "autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp" +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" + +#include +#include + +#include +#include + +#include + +#include + +TEST(smoothPath, nominal) +{ + using autoware::behavior_velocity_planner::smoothPath; + using autoware_internal_planning_msgs::msg::PathPointWithLaneId; + using autoware_internal_planning_msgs::msg::PathWithLaneId; + + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + options.arguments( + {"--ros-args", "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common") + + "/config/behavior_velocity_planner_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_vehicle_info.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother") + + "/config/default_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother") + + "/config/default_velocity_smoother.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother") + + "/config/JerkFiltered.param.yaml"}); + auto node = std::make_shared("test_node", options); + + auto planner_data = std::make_shared(*node); + planner_data->stop_line_extend_length = 5.0; + planner_data->vehicle_info_.max_longitudinal_offset_m = 1.0; + + planner_data->current_odometry = std::make_shared([]() { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 2.0; + pose.pose.position.y = 1.0; + return pose; + }()); + + planner_data->current_velocity = std::make_shared([]() { + geometry_msgs::msg::TwistStamped twist; + twist.twist.linear.x = 3.0; + return twist; + }()); + + planner_data->current_acceleration = + std::make_shared([]() { + geometry_msgs::msg::AccelWithCovarianceStamped accel; + accel.accel.accel.linear.x = 1.0; + return accel; + }()); + + planner_data->velocity_smoother_ = + std::make_shared( + *node, std::make_shared()); + + // Input path + PathWithLaneId in_path; + for (double i = 0; i <= 10.0; i += 1.0) { + PathPointWithLaneId point; + point.point.pose.position.x = i; + point.point.pose.position.y = 0.0; + point.point.longitudinal_velocity_mps = 5.0; // Set constant velocity + in_path.points.emplace_back(point); + } + + // Output path + PathWithLaneId out_path; + + // Execute smoothing + auto result = smoothPath(in_path, out_path, planner_data); + + // Check results + EXPECT_TRUE(result); + + // Check initial and last points + EXPECT_DOUBLE_EQ(out_path.points.front().point.pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(out_path.points.front().point.pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(out_path.points.back().point.pose.position.x, 10.0); + EXPECT_DOUBLE_EQ(out_path.points.back().point.pose.position.y, 0.0); + + for (auto & point : out_path.points) { + // Check velocities + EXPECT_LE( + point.point.longitudinal_velocity_mps, + 5.0); // Smoothed velocity must not exceed initial + } +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp new file mode 100644 index 0000000000..bc5abee0ca --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp @@ -0,0 +1,308 @@ +// 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 "./utils.hpp" +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" + +#include +#include +#include +#include +#include + +#include + +using namespace autoware::behavior_velocity_planner; // NOLINT +using namespace autoware::behavior_velocity_planner::planning_utils; // NOLINT +using autoware_planning_msgs::msg::PathPoint; + +TEST(PlanningUtilsTest, calcSegmentIndexFromPointIndex) +{ + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point point; + point.x = 4.5; + point.y = 0.0; + + size_t result = calcSegmentIndexFromPointIndex(path.points, point, 4); + + EXPECT_EQ(result, 4); +} + +TEST(PlanningUtilsTest, calculateOffsetPoint2d) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + pose.orientation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1)); // No rotation + + double offset_x = 1.0; + double offset_y = 1.0; + + auto result = calculateOffsetPoint2d(pose, offset_x, offset_y); + + EXPECT_NEAR(result.x(), 1.0, 0.1); + EXPECT_NEAR(result.y(), 1.0, 0.1); +} + +TEST(PlanningUtilsTest, createDetectionAreaPolygons) +{ + // using namespace autoware::behavior_velocity_planner::planning_utils; + + // Input parameters + Polygons2d da_polys; + autoware_internal_planning_msgs::msg::PathWithLaneId path; + geometry_msgs::msg::Pose target_pose; + size_t target_seg_idx = 0; + autoware::behavior_velocity_planner::DetectionRange da_range; + + da_range.min_longitudinal_distance = 1.0; + da_range.max_longitudinal_distance = 10.0; + da_range.max_lateral_distance = 2.0; + da_range.interval = 5.0; + da_range.wheel_tread = 1.0; + da_range.left_overhang = 0.5; + da_range.right_overhang = 0.5; + da_range.use_left = true; + da_range.use_right = true; + + double obstacle_vel_mps = 0.5; + double min_velocity = 1.0; + + // Path with some points + for (double i = 0.0; i < 3.0; ++i) { + autoware_internal_planning_msgs::msg::PathPointWithLaneId point; + point.point.pose.position.x = i * 5.0; + point.point.pose.position.y = 0.0; + point.point.longitudinal_velocity_mps = 1.0; + path.points.push_back(point); + } + + // Target pose + target_pose.position.x = 0.0; + target_pose.position.y = 0.0; + + // Call the function + bool success = createDetectionAreaPolygons( + da_polys, path, target_pose, target_seg_idx, da_range, obstacle_vel_mps, min_velocity); + + // Assert success + EXPECT_TRUE(success); + + // Validate results + ASSERT_FALSE(da_polys.empty()); + EXPECT_EQ(da_polys.size(), 2); // Expect polygons for left and right bounds + + // Check the first polygon + auto & polygon = da_polys.front(); + EXPECT_EQ(polygon.outer().size(), 7); // Each polygon should be a rectangle + + // Check some specific points + EXPECT_NEAR(polygon.outer()[0].x(), 1.0, 0.1); // Left inner bound + EXPECT_NEAR(polygon.outer()[0].y(), 1.0, 0.1); +} + +// Test for calcJudgeLineDistWithAccLimit +TEST(PlanningUtilsTest, calcJudgeLineDistWithAccLimit) +{ + double velocity = 10.0; // m/s + double max_stop_acceleration = -3.0; // m/s^2 + double delay_response_time = 1.0; // s + + double result = + calcJudgeLineDistWithAccLimit(velocity, max_stop_acceleration, delay_response_time); + + EXPECT_NEAR(result, 26.67, 0.01); // Updated expected value +} + +// Test for calcJudgeLineDistWithJerkLimit +TEST(PlanningUtilsTest, calcJudgeLineDistWithJerkLimit) +{ + double velocity = 10.0; // m/s + double acceleration = 0.0; // m/s^2 + double max_stop_acceleration = -3.0; // m/s^2 + double max_stop_jerk = -1.0; // m/s^3 + double delay_response_time = 1.0; // s + + double result = calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); + + EXPECT_GT(result, 0.0); // The result should be positive +} + +// Test for isAheadOf +TEST(PlanningUtilsTest, isAheadOf) +{ + geometry_msgs::msg::Pose target; + geometry_msgs::msg::Pose origin; + target.position.x = 10.0; + target.position.y = 0.0; + origin.position.x = 0.0; + origin.position.y = 0.0; + origin.orientation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1)); // No rotation + + EXPECT_TRUE(isAheadOf(target, origin)); + + target.position.x = -10.0; + EXPECT_FALSE(isAheadOf(target, origin)); +} + +TEST(PlanningUtilsTest, insertDecelPoint) +{ + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point stop_point; + stop_point.x = 5.0; + stop_point.y = 0.0; + + auto stop_pose = insertDecelPoint(stop_point, path, 5.0); + ASSERT_TRUE(stop_pose.has_value()); + EXPECT_NEAR(stop_pose->position.x, 5.0, 0.1); +} + +// Test for insertVelocity +TEST(PlanningUtilsTest, insertVelocity) +{ + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point; + path_point.point.pose.position.x = 5.0; + path_point.point.pose.position.y = 0.0; + path_point.point.longitudinal_velocity_mps = 10.0; + + size_t insert_index = 5; + insertVelocity(path, path_point, 10.0, insert_index); + + EXPECT_EQ(path.points.size(), 11); + EXPECT_NEAR(path.points.at(insert_index).point.longitudinal_velocity_mps, 10.0, 0.1); +} + +// Test for insertStopPoint +TEST(PlanningUtilsTest, insertStopPoint) +{ + { + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point stop_point; + stop_point.x = 5.0; + stop_point.y = 0.0; + + auto stop_pose = insertStopPoint(stop_point, path); + ASSERT_TRUE(stop_pose.has_value()); + EXPECT_NEAR(stop_pose->position.x, 5.0, 0.1); + } + { + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point stop_point; + stop_point.x = 5.0; + stop_point.y = 0.0; + + auto stop_pose = insertStopPoint(stop_point, 4, path); + ASSERT_TRUE(stop_pose.has_value()); + EXPECT_NEAR(stop_pose->position.x, 5.0, 0.1); + } +} + +// Test for getAheadPose +TEST(PlanningUtilsTest, getAheadPose) +{ + autoware_internal_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point1; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point2; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point3; + point1.point.pose.position.x = 0.0; + point2.point.pose.position.x = 5.0; + point3.point.pose.position.x = 10.0; + + path.points.emplace_back(point1); + path.points.emplace_back(point2); + path.points.emplace_back(point3); + + double ahead_dist = 7.0; + auto pose = getAheadPose(0, ahead_dist, path); + + EXPECT_NEAR(pose.position.x, 7.0, 0.1); +} + +TEST(PlanningUtilsTest, calcDecelerationVelocityFromDistanceToTarget) +{ + double max_slowdown_jerk = -1.0; // m/s^3 + double max_slowdown_accel = -3.0; // m/s^2 + double current_accel = -1.0; // m/s^2 + double current_velocity = 10.0; // m/s + double distance_to_target = 100.0; // m + + double result = calcDecelerationVelocityFromDistanceToTarget( + max_slowdown_jerk, max_slowdown_accel, current_accel, current_velocity, distance_to_target); + + EXPECT_LT(result, current_velocity); +} + +// Test for toRosPoints +TEST(PlanningUtilsTest, ToRosPoints) +{ + using autoware_perception_msgs::msg::PredictedObject; + PredictedObjects objects; + + // Add a predicted object + PredictedObject obj1; + obj1.kinematics.initial_pose_with_covariance.pose.position.x = 1.0; + obj1.kinematics.initial_pose_with_covariance.pose.position.y = 2.0; + obj1.kinematics.initial_pose_with_covariance.pose.position.z = 3.0; + objects.objects.push_back(obj1); + + // Add another predicted object + PredictedObject obj2; + obj2.kinematics.initial_pose_with_covariance.pose.position.x = 4.0; + obj2.kinematics.initial_pose_with_covariance.pose.position.y = 5.0; + obj2.kinematics.initial_pose_with_covariance.pose.position.z = 6.0; + objects.objects.push_back(obj2); + + auto points = toRosPoints(objects); + + ASSERT_EQ(points.size(), 2); // Verify the number of points + EXPECT_EQ(points[0].x, 1.0); + EXPECT_EQ(points[0].y, 2.0); + EXPECT_EQ(points[0].z, 3.0); + EXPECT_EQ(points[1].x, 4.0); + EXPECT_EQ(points[1].y, 5.0); + EXPECT_EQ(points[1].z, 6.0); +} + +// Test for extendLine +TEST(PlanningUtilsTest, ExtendLine) +{ + lanelet::ConstPoint3d point1(lanelet::InvalId, 0.0, 0.0, 0.0); + lanelet::ConstPoint3d point2(lanelet::InvalId, 1.0, 1.0, 0.0); + double length = 1.0; + + auto extended_line = extendLine(point1, point2, length); + + ASSERT_EQ(extended_line.size(), 2); // Verify the line has two points + + // Check the extended line coordinates + EXPECT_NEAR(extended_line[0].x(), -0.707, 0.001); // Extended in the reverse direction + EXPECT_NEAR(extended_line[0].y(), -0.707, 0.001); + EXPECT_NEAR(extended_line[1].x(), 1.707, 0.001); // Extended in the forward direction + EXPECT_NEAR(extended_line[1].y(), 1.707, 0.001); +} + +TEST(PlanningUtilsTest, getConstLaneletsFromIds) +{ + const auto package_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); + lanelet::LaneletMapPtr map = + autoware::test_utils::loadMap(package_dir + "/test_map/lanelet2_map.osm"); + + auto lanelets = getConstLaneletsFromIds(map, {10333, 10310, 10291}); + + EXPECT_EQ(lanelets.size(), 3); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp new file mode 100644 index 0000000000..f61bec186e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include + +#include + +namespace test +{ + +inline autoware_internal_planning_msgs::msg::PathWithLaneId generatePath( + double x0, double y0, double x, double y, int nb_points) +{ + autoware_internal_planning_msgs::msg::PathWithLaneId path{}; + double x_step = (x - x0) / (nb_points - 1); + double y_step = (y - y0) / (nb_points - 1); + for (int i = 0; i < nb_points; ++i) { + autoware_internal_planning_msgs::msg::PathPointWithLaneId point{}; + point.point.pose.position.x = x0 + x_step * i; + point.point.pose.position.y = y0 + y_step * i; + point.point.pose.position.z = 0.0; + path.points.push_back(point); + } + return path; +} + +inline geometry_msgs::msg::Pose generatePose(double x) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = 0.0; + p.position.z = 1.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0); + p.orientation = tf2::toMsg(q); + return p; +} + +} // namespace test + +#endif // UTILS_HPP_ From 75671beb9fd32fc613f04099ee45bb7038fefb24 Mon Sep 17 00:00:00 2001 From: suchang Date: Fri, 28 Feb 2025 10:48:05 +0800 Subject: [PATCH 02/19] feat: remove useless dependecy Signed-off-by: suchang --- .../autoware_behavior_velocity_planner_common/package.xml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index a66faa2e0a..f4bdea2b1c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_planner_common - 0.41.0 + 0.0.0 The autoware_behavior_velocity_planner_common package Tomoya Kimura @@ -28,7 +28,7 @@ autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_factor_interface - autoware_planning_msgs + autoware_internal_planning_msgs autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils @@ -45,7 +45,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs visualization_msgs ament_cmake_ros From 89c8a0c64f51be27d4663d0ee3823e8bbc3f9269 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 28 Feb 2025 02:54:29 +0000 Subject: [PATCH 03/19] style(pre-commit): autofix --- .../scene_module_interface.hpp | 4 ++-- .../utilization/arc_lane_util.hpp | 11 ++++------- .../package.xml | 2 +- .../src/utilization/debug.cpp | 6 ++++-- .../src/utilization/util.cpp | 10 ++++------ 5 files changed, 15 insertions(+), 18 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 3b5ab16c3f..2340770c62 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -50,11 +50,11 @@ namespace autoware::behavior_velocity_planner using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_utils::DebugPublisher; using autoware_utils::get_or_declare_parameter; using autoware_utils::StopWatch; -using autoware_internal_debug_msgs::msg::Float64Stamped; -using autoware_internal_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; using unique_identifier_msgs::msg::UUID; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 215419ee4f..ce84a4512b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -40,9 +40,8 @@ inline geometry_msgs::msg::Point convertToGeomPoint(const autoware_utils::Point2 namespace arc_lane_utils { -using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = - std::pair; // front index, point2d +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d using PathIndexWithPoint = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset @@ -60,8 +59,7 @@ std::optional findCollisionSegment( const geometry_msgs::msg::Point & stop_line_p2) { for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & p1 = - autoware_utils::get_point(path.points.at(i)); // Point before collision point + const auto & p1 = autoware_utils::get_point(path.points.at(i)); // Point before collision point const auto & p2 = autoware_utils::get_point(path.points.at(i + 1)); // Point after collision point @@ -117,8 +115,7 @@ std::optional findBackwardOffsetSegment( double sum_length = 0.0; const auto start = static_cast(base_idx) - 1; for (std::int32_t i = start; i >= 0; --i) { - sum_length += - autoware_utils::calc_distance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += autoware_utils::calc_distance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length /** diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index f4bdea2b1c..4e44d334cf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_internal_debug_msgs + autoware_internal_planning_msgs autoware_interpolation autoware_lanelet2_extension autoware_map_msgs @@ -28,7 +29,6 @@ autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_factor_interface - autoware_internal_planning_msgs autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index fd6a61a0ba..36319f3b43 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -34,7 +34,8 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( auto marker = create_default_marker( "map", now, ns, static_cast(module_id), visualization_msgs::msg::Marker::LINE_STRIP, create_marker_scale(static_cast(x), static_cast(y), static_cast(z)), - create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.8f)); + create_marker_color( + static_cast(r), static_cast(g), static_cast(b), 0.8f)); marker.lifetime = rclcpp::Duration::from_seconds(0.3); for (const auto & p : polygon.points) { @@ -116,7 +117,8 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( auto marker = create_default_marker( "map", now, ns, 0, visualization_msgs::msg::Marker::SPHERE, create_marker_scale(x, y, z), - create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.999f)); + create_marker_color( + static_cast(r), static_cast(g), static_cast(b), 0.999f)); marker.lifetime = rclcpp::Duration::from_seconds(0.3); for (size_t i = 0; i < points.size(); ++i) { marker.id = static_cast(i + planning_utils::bitShift(module_id)); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index 4bf6d98e31..a11ca779b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -16,8 +16,8 @@ #include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware_utils/geometry/geometry.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -49,10 +49,8 @@ size_t calcPointIndexFromSegmentIndex( const size_t prev_point_idx = seg_idx; const size_t next_point_idx = seg_idx + 1; - const double prev_dist = - autoware_utils::calc_distance2d(point, points.at(prev_point_idx)); - const double next_dist = - autoware_utils::calc_distance2d(point, points.at(next_point_idx)); + const double prev_dist = autoware_utils::calc_distance2d(point, points.at(prev_point_idx)); + const double next_dist = autoware_utils::calc_distance2d(point, points.at(next_point_idx)); if (prev_dist < next_dist) { return prev_point_idx; @@ -98,9 +96,9 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( namespace autoware::behavior_velocity_planner::planning_utils { using autoware::motion_utils::calcSignedArcLength; +using autoware_planning_msgs::msg::PathPoint; using autoware_utils::calc_distance2d; using autoware_utils::calc_offset_pose; -using autoware_planning_msgs::msg::PathPoint; size_t calcSegmentIndexFromPointIndex( const std::vector & points, From 09c1478fe5b1875eeedb6e2e68b737c9ecab9559 Mon Sep 17 00:00:00 2001 From: suchang Date: Tue, 4 Mar 2025 09:39:46 +0800 Subject: [PATCH 04/19] feat: modify autoware_universe_utils to autoware_utils Signed-off-by: suchang --- .../CMakeLists.txt | 26 ++ .../README.md | 110 ++++++++ .../config/stop_line.param.yaml | 6 + .../docs/calculate_stop_pose.drawio.svg | 99 +++++++ .../docs/find_collision_segment.drawio.svg | 99 +++++++ .../docs/find_offset_segment.drawio.svg | 143 ++++++++++ .../docs/keep_stopping.svg | 91 +++++++ .../docs/node_and_segment.drawio.svg | 251 ++++++++++++++++++ .../docs/restart.svg | 93 +++++++ .../docs/restart_prevention.svg | 131 +++++++++ .../docs/stop_line.svg | 151 +++++++++++ .../package.xml | 44 +++ .../plugins.xml | 3 + .../src/debug.cpp | 37 +++ .../src/manager.cpp | 109 ++++++++ .../src/manager.hpp | 66 +++++ .../src/scene.cpp | 172 ++++++++++++ .../src/scene.hpp | 121 +++++++++ .../test/test_node_interface.cpp | 75 ++++++ .../test/test_scene.cpp | 146 ++++++++++ 20 files changed, 1973 insertions(+) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/stop_line.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/plugins.xml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt new file mode 100644 index 0000000000..a187b4cda9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_stop_line_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_scene.cpp + test/test_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md new file mode 100644 index 0000000000..963d75e5a8 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md @@ -0,0 +1,110 @@ +# Stop Line + +## Role + +This module plans the vehicle's velocity to ensure it stops just before stop lines and can resume movement after stopping. + +![stop line](docs/stop_line.svg) + +## Activation Timing + +This module is activated when there is a stop line in a target lane. + +## Module Parameters + +| Parameter | Type | Description | +| ------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_margin` | double | Margin that the vehicle tries to stop in before stop_line | +| `stop_duration_sec` | double | [s] Time parameter for the ego vehicle to stop before stop line | +| `hold_stop_margin_distance` | double | [m] Parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | +| `use_initialization_stop_state` | bool | Flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | + +## Inner-workings / Algorithms + +- Gets a stop line from map information. +- Inserts a stop point on the path from the stop line defined in the map and the ego vehicle length. +- Sets velocities of the path after the stop point to 0[m/s]. +- Releases the inserted stop velocity when the vehicle halts at the stop point for `stop_duration_sec` seconds. + +### Flowchart + +```plantuml +@startuml +title modifyPathVelocity +start + +:find collision between path and stop_line; + +if (collision is found?) then (yes) +else (no) + stop +endif + +:find offset segment; + +:calculate stop pose; + +:calculate distance to stop line; + +if (state is APPROACH) then (yes) + :set stop velocity; + + if (vehicle is within hold_stop_margin_distance?) then (yes) + if (vehicle is stopped?) then (yes) + :change state to STOPPED; + endif + endif +else if (state is STOPPED) then (yes) + if (stopping time is longer than stop_duration_sec ?) then (yes) + :change state to START; + endif +else if (state is START) then (yes) + if ([optional] far from stop line?) then (yes) + :change state to APPROACH; + endif +endif + +stop +@enduml +``` + +This algorithm is based on `segment`. +`segment` consists of two node points. It's useful for removing boundary conditions because if `segment(i)` exists we can assume `node(i)` and `node(i+1)` exist. + +![node_and_segment](docs/./node_and_segment.drawio.svg) + +First, this algorithm finds a collision between reference path and stop line. +Then, we can get `collision segment` and `collision point`. + +![find_collision_segment](docs/./find_collision_segment.drawio.svg) + +Next, based on `collision point`, it finds `offset segment` by iterating backward points up to a specific offset length. +The offset length is `stop_margin`(parameter) + `base_link to front`(to adjust head pose to stop line). +Then, we can get `offset segment` and `offset from segment start`. + +![find_offset_segment](docs/./find_offset_segment.drawio.svg) + +After that, we can calculate an offset point from `offset segment` and `offset`. This will be `stop_pose`. + +![calculate_stop_pose](docs/./calculate_stop_pose.drawio.svg) + +### Restart Prevention + +If the vehicle requires X meters (e.g. 0.5 meters) to stop once it starts moving due to poor vehicle control performance, it may overshoot the stopping position, which must be strictly observed. This happens when the vehicle begins to move in order to approach a nearby stop point (e.g. 0.3 meters away). + +This module includes the parameter `hold_stop_margin_distance` to prevent redundant restarts in such a situation. If the vehicle is stopped within `hold_stop_margin_distance` meters of the stop point (\_front_to_stop_line < hold_stop_margin_distance), the module determines that the vehicle has already stopped at the stop point and will maintain the current stopping position, even if the vehicle has come to a stop due to other factors. + +
+ ![example](docs/restart_prevention.svg){width=1000} +
parameters
+
+ +
+ ![example](docs/restart.svg){width=1000} +
outside the hold_stop_margin_distance
+
+ +
+ ![example](docs/keep_stopping.svg){width=1000} +
inside the hold_stop_margin_distance
+
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml new file mode 100644 index 0000000000..f5e00fa708 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + stop_line: + stop_margin: 0.0 + stop_duration_sec: 1.0 + hold_stop_margin_distance: 2.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg new file mode 100644 index 0000000000..a4cc2451d5 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ offset segment +
+
+
+ + offset segment + + + + + + + + +
+
+
+ offset point +
+ = stop pose +
+
+
+ + offset point... + + + + + + +
+
+
+ offset from segment start +
+
+
+ + offset from segme... + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg new file mode 100644 index 0000000000..fee520e242 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop line +
+
+
+ + stop line + + + + + + + + +
+
+
+ collision segment +
+
+
+ + collision segment + + + + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg new file mode 100644 index 0000000000..aed520b236 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg @@ -0,0 +1,143 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + + + + + +
+
+
+ stop margin +
+
+
+ + stop margin + + + + + + + + +
+
+
+ base_link to front +
+
+
+ + base_link to front + + + + + + + + +
+
+
+ offset segment +
+
+
+ + offset segment + + + + + + +
+
+
+ offset from segment start +
+
+
+ + offset from segme... + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg new file mode 100644 index 0000000000..e6233f7fd0 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + +
+
+
+ keep stopping at current position +
+
+
+ + keep stopping at curr... + + + + + + +
+
+
+ target stop position +
+
+
+ + target stop position + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg new file mode 100644 index 0000000000..6cd47ffc14 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg @@ -0,0 +1,251 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
node 1
+
+
+ + node 1 + + + + + + +
+
+
node 2
+
+
+ + node 2 + + + + + + +
+
+
node 3
+
+
+ + node 3 + + + + + + +
+
+
node 4
+
+
+ + node 4 + + + + + + +
+
+
node 5
+
+
+ + node 5 + + + + + + +
+
+
node 6
+
+
+ + node 6 + + + + + + +
+
+
node 7
+
+
+ + node 7 + + + + + + +
+
+
+ segment 1 +
+
+
+ + segment 1 + + + + + + +
+
+
+ segment 3 +
+
+
+ + segment 3 + + + + + + +
+
+
+ segment 2 +
+
+
+ + segment 2 + + + + + + +
+
+
+ segment 4 +
+
+
+ + segment 4 + + + + + + +
+
+
+ segment 5 +
+
+
+ + segment 5 + + + + + + +
+
+
+ segment 6 +
+
+
+ + segment 6 + + + + + + +
+
+
+ segment(i) +
+ = node(i) + node(i+1) +
+ = all segments have two points +
+
+
+ + segment(i)... + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart.svg new file mode 100644 index 0000000000..d0ee659189 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart.svg @@ -0,0 +1,93 @@ + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + + + + + + +
+
+
+ move toward stop line +
+
+
+ + move toward stop line + + + + + + +
+
+
+ target stop position +
+
+
+ + target stop position + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg new file mode 100644 index 0000000000..160df47d81 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg @@ -0,0 +1,131 @@ + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + + + +
+
+
+ target stop position +
+
+
+ + target stop position + + + + + + + + + + + + +
+
+
+ hold_stop_margin_distance + [m] +
+
+
+ + hold_stop_margin_distance [m] + + + + + + +
+
+
+ front_to_stop_line + [m] +
+
+
+ + front_to_stop_line [... + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/stop_line.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/stop_line.svg new file mode 100644 index 0000000000..a090a90e2e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/stop_line.svg @@ -0,0 +1,151 @@ + + + + + + + + + + + + + + + + + +
+
+
+ +

+ l + [ + m + ] + : + base link to front +

+ +
+
+
+ + l [m]  : base link to front + + + + + + + +
+
+
+ Stop Line +
+
+
+ + Stop Line + + + + + + +
+
+
+ Stop Point +
+
+
+ + Stop Point + + + + + + + + + + + + + + +
+
+
+ velocity 0 +
+
+
+ + velocity 0 + + + + + +
+
+
+ 2[m] +
+
+
+ + 2[m] + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml new file mode 100644 index 0000000000..4f1ab8f9a1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml @@ -0,0 +1,44 @@ + + + + autoware_behavior_velocity_stop_line_module + 0.0.0 + The autoware_behavior_velocity_stop_line_module package + + Yukinari Hisaki + Satoshi Ota + Fumiya Watanabe + Zhe Shen + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Yutaka Shimizu + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_velocity_planner + autoware_behavior_velocity_planner_common + autoware_lanelet2_extension + autoware_motion_utils + autoware_internal_planning_msgs + autoware_route_handler + autoware_trajectory + eigen + geometry_msgs + pluginlib + rclcpp + tf2_geometry_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/plugins.xml new file mode 100644 index 0000000000..6765a4bc1b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp new file mode 100644 index 0000000000..cb29858bce --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp @@ -0,0 +1,37 @@ +// Copyright 2020 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 "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "scene.hpp" + +namespace autoware::behavior_velocity_planner +{ + +autoware::motion_utils::VirtualWalls StopLineModule::createVirtualWalls() +{ + autoware::motion_utils::VirtualWalls virtual_walls; + + if (debug_data_.stop_pose && (state_ == State::APPROACH || state_ == State::STOPPED)) { + autoware::motion_utils::VirtualWall wall; + wall.text = "stopline"; + wall.style = autoware::motion_utils::VirtualWallType::stop; + wall.ns = std::to_string(module_id_) + "_"; + wall.pose = autoware_utils::calc_offset_pose( + *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); + virtual_walls.push_back(wall); + } + return virtual_walls; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp new file mode 100644 index 0000000000..bd0c446b4e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -0,0 +1,109 @@ +// Copyright 2020 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 "manager.hpp" + +#include "autoware_utils/ros/parameter.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware_utils::get_or_declare_parameter; +using lanelet::TrafficSign; + +StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()), planner_param_() +{ + const std::string ns(StopLineModuleManager::getModuleName()); + auto & p = planner_param_; + p.stop_margin = get_or_declare_parameter(node, ns + ".stop_margin"); + p.hold_stop_margin_distance = + get_or_declare_parameter(node, ns + ".hold_stop_margin_distance"); + p.stop_duration_sec = get_or_declare_parameter(node, ns + ".stop_duration_sec"); +} + +std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::vector stop_lines_with_lane_id; + + for (const auto & m : planning_utils::getRegElemMapOnPath( + path, lanelet_map, planner_data_->current_odometry->pose)) { + const auto & traffic_sign_reg_elem = m.first; + const int64_t lane_id = m.second.id(); + // Is stop sign? + if (traffic_sign_reg_elem->type() != "stop_sign") { + continue; + } + + for (const auto & stop_line : traffic_sign_reg_elem->refLines()) { + stop_lines_with_lane_id.emplace_back(stop_line, lane_id); + } + } + + return stop_lines_with_lane_id; +} + +std::set StopLineModuleManager::getStopLineIdSetOnPath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::set stop_line_id_set; + + for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, lanelet_map)) { + stop_line_id_set.insert(stop_line_with_lane_id.first.id()); + } + + return stop_line_id_set; +} + +void StopLineModuleManager::launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & stop_line_with_lane_id : + getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { + const auto module_id = stop_line_with_lane_id.first.id(); + if (!isModuleRegistered(module_id)) { + registerModule(std::make_shared( + module_id, stop_line_with_lane_id.first, planner_param_, + logger_.get_child("stop_line_module"), clock_, time_keeper_, planning_factor_interface_)); + } + } +} + +std::function &)> +StopLineModuleManager::getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) +{ + const auto stop_line_id_set = + getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); + + return [stop_line_id_set](const std::shared_ptr & scene_module) { + return stop_line_id_set.count(scene_module->getModuleId()) == 0; + }; +} + +} // namespace autoware::behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::StopLineModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp new file mode 100644 index 0000000000..70ad0a6b8d --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp @@ -0,0 +1,66 @@ +// Copyright 2020 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "autoware/behavior_velocity_planner_common/plugin_wrapper.hpp" +#include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" +#include "scene.hpp" + +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using StopLineWithLaneId = std::pair; + +class StopLineModuleManager : public SceneModuleManagerInterface<> +{ +public: + explicit StopLineModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "stop_line"; } + +private: + StopLineModule::PlannerParam planner_param_; + + std::vector getStopLinesWithLaneIdOnPath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map); + + std::set getStopLineIdSetOnPath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map); + + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class StopLineModulePlugin : public PluginWrapper +{ +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp new file mode 100644 index 0000000000..87b75b7126 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -0,0 +1,172 @@ +// Copyright 2020 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 "scene.hpp" + +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/trajectory/utils/closest.hpp" +#include "autoware/trajectory/utils/crossed.hpp" + +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +StopLineModule::StopLineModule( + const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr & time_keeper, + const std::shared_ptr & + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), + stop_line_(std::move(stop_line)), + planner_param_(planner_param), + state_(State::APPROACH), + debug_data_() +{ +} + +bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) +{ + auto trajectory = + trajectory::Trajectory::Builder{} + .build(path->points); + + if (!trajectory) { + return true; + } + + auto [ego_s, stop_point] = + getEgoAndStopPoint(*trajectory, planner_data_->current_odometry->pose, state_); + + if (!stop_point) { + return true; + } + + trajectory->longitudinal_velocity_mps().range(*stop_point, trajectory->length()).set(0.0); + + path->points = trajectory->restore(); + + // TODO(soblin): PlanningFactorInterface use trajectory class + planning_factor_interface_->add( + path->points, trajectory->compute(*stop_point).point.pose, + planner_data_->current_odometry->pose, planner_data_->current_odometry->pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); + + updateStateAndStoppedTime( + &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); + + geometry_msgs::msg::Pose stop_pose = trajectory->compute(*stop_point).point.pose; + + updateDebugData(&debug_data_, stop_pose, state_); + + return true; +} + +std::pair> StopLineModule::getEgoAndStopPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & ego_pose, + const State & state) const +{ + const double ego_s = autoware::trajectory::closest(trajectory, ego_pose); + std::optional stop_point_s; + + switch (state) { + case State::APPROACH: { + const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const LineString2d stop_line = planning_utils::extendLine( + stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); + + // Calculate intersection with stop line + const auto trajectory_stop_line_intersection = + autoware::trajectory::crossed(trajectory, stop_line); + + // If no collision found, do nothing + if (trajectory_stop_line_intersection.size() == 0) { + stop_point_s = std::nullopt; + break; + } + + stop_point_s = + trajectory_stop_line_intersection.at(0) - + (base_link2front + planner_param_.stop_margin); // consider vehicle length and stop margin + + if (*stop_point_s < 0.0) { + stop_point_s = std::nullopt; + } + break; + } + + case State::STOPPED: { + stop_point_s = ego_s; + break; + } + + case State::START: { + stop_point_s = std::nullopt; + break; + } + } + return {ego_s, stop_point_s}; +} + +void StopLineModule::updateStateAndStoppedTime( + State * state, std::optional * stopped_time, const rclcpp::Time & now, + const double & distance_to_stop_point, const bool & is_vehicle_stopped) const +{ + switch (*state) { + case State::APPROACH: { + if (distance_to_stop_point < planner_param_.hold_stop_margin_distance && is_vehicle_stopped) { + *state = State::STOPPED; + *stopped_time = now; + RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); + + if (distance_to_stop_point < 0.0) { + RCLCPP_WARN(logger_, "Vehicle cannot stop before stop line"); + } + } + break; + } + case State::STOPPED: { + double stop_duration = (now - **stopped_time).seconds(); + if (stop_duration > planner_param_.stop_duration_sec) { + *state = State::START; + stopped_time->reset(); + RCLCPP_INFO(logger_, "STOPPED -> START"); + } + break; + } + case State::START: { + break; + } + } +} + +void StopLineModule::updateDebugData( + DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const +{ + debug_data->base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + debug_data->stop_pose = stop_pose; + if (state == State::START) { + debug_data->stop_pose = std::nullopt; + } +} + +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp new file mode 100644 index 0000000000..7973e46db9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -0,0 +1,121 @@ +// Copyright 2020 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 SCENE_HPP_ +#define SCENE_HPP_ +#define EIGEN_MPL2_ONLY + +#include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/trajectory/path_point_with_lane_id.hpp" + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +class StopLineModule : public SceneModuleInterface +{ +public: + using StopLineWithLaneId = std::pair; + using Trajectory = + autoware::trajectory::Trajectory; + enum class State { APPROACH, STOPPED, START }; + + struct DebugData + { + double base_link2front; ///< Distance from the base link to the vehicle front. + std::optional stop_pose; ///< Pose of the stop position. + }; + + struct PlannerParam + { + double stop_margin; ///< Margin to the stop line. + double stop_duration_sec; ///< Required stop duration at the stop line. + double + hold_stop_margin_distance; ///< Distance threshold for transitioning to the STOPPED state + }; + + /** + * @brief Constructor for StopLineModule. + * @param module_id Unique ID for the module. + * @param stop_line Stop line data. + * @param planner_param Planning parameters. + * @param logger Logger for output messages. + * @param clock Shared clock instance. + * @param time_keeper Time keeper for the module. + * @param planning_factor_interface Planning factor interface. + */ + StopLineModule( + const int64_t module_id, lanelet::ConstLineString3d stop_line, + const PlannerParam & planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr & time_keeper, + const std::shared_ptr & + planning_factor_interface); + + bool modifyPathVelocity(PathWithLaneId * path) override; + + /** + * @brief Calculate ego position and stop point. + * @param trajectory Current trajectory. + * @param ego_pose Current pose of the vehicle. + * @param state Current state of the stop line module. + * @return Pair of ego position and optional stop point. + */ + std::pair> getEgoAndStopPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & ego_pose, + const State & state) const; + + /** + * @brief Update the state and stopped time of the module. + * @param state Pointer to the current state. + * @param stopped_time Pointer to the stopped time. + * @param now Current time. + * @param distance_to_stop_point Distance to the stop point. + * @param is_vehicle_stopped Flag indicating if the vehicle is stopped. + */ + void updateStateAndStoppedTime( + State * state, std::optional * stopped_time, const rclcpp::Time & now, + const double & distance_to_stop_point, const bool & is_vehicle_stopped) const; + + void updateDebugData( + DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override + { + return visualization_msgs::msg::MarkerArray{}; + } + autoware::motion_utils::VirtualWalls createVirtualWalls() override; + +private: + const lanelet::ConstLineString3d stop_line_; ///< Stop line geometry. + const PlannerParam planner_param_; ///< Parameters for the planner. + State state_; ///< Current state of the module. + std::optional stopped_time_; ///< Time when the vehicle stopped. + DebugData debug_data_; ///< Debug information. +}; +} // namespace autoware::behavior_velocity_planner + +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp new file mode 100644 index 0000000000..ac158a0e59 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp @@ -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. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "stop_line", "autoware::behavior_velocity_planner::StopLineModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp new file mode 100644 index 0000000000..a98d66c6f7 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -0,0 +1,146 @@ +// 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 "../src/scene.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include +#include + +using autoware::behavior_velocity_planner::StopLineModule; + +autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point(double x, double y) +{ + autoware_internal_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = x; + p.point.pose.position.y = y; + return p; +} + +class StopLineModuleTest : public ::testing::Test +{ +protected: + // Set up the test case + void SetUp() override + { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + options.arguments( + {"--ros-args", "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common") + + "/config/behavior_velocity_planner_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_vehicle_info.param.yaml"}); + node_ = std::make_shared("test_node", options); + + // Initialize parameters, logger, and clock + planner_param_.stop_margin = 0.5; + planner_param_.stop_duration_sec = 2.0; + planner_param_.hold_stop_margin_distance = 0.5; + + planner_data_ = std::make_shared(*node_); + planner_data_->stop_line_extend_length = 5.0; + planner_data_->vehicle_info_.max_longitudinal_offset_m = 1.0; + + stop_line_ = lanelet::ConstLineString3d( + lanelet::utils::getId(), {lanelet::Point3d(lanelet::utils::getId(), 7.0, -1.0, 0.0), + lanelet::Point3d(lanelet::utils::getId(), 7.0, 1.0, 0.0)}); + + trajectory_ = *StopLineModule::Trajectory::Builder{}.build( + {path_point(0.0, 0.0), path_point(1.0, 0.0), path_point(2.0, 0.0), path_point(3.0, 0.0), + path_point(4.0, 0.0), path_point(5.0, 0.0), path_point(6.0, 0.0), path_point(7.0, 0.0), + path_point(8.0, 0.0), path_point(9.0, 0.0), path_point(10.0, 0.0)}); + + clock_ = std::make_shared(); + + module_ = std::make_shared( + 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_, + std::make_shared(), + std::make_shared( + node_.get(), "test_stopline")); + + module_->setPlannerData(planner_data_); + } + + void TearDown() override { rclcpp::shutdown(); } + + StopLineModule::Trajectory trajectory_; + StopLineModule::PlannerParam planner_param_{}; + lanelet::ConstLineString3d stop_line_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr module_; + std::shared_ptr planner_data_; + + rclcpp::Node::SharedPtr node_; +}; + +TEST_F(StopLineModuleTest, TestGetEgoAndStopPoint) +{ + // Prepare trajectory and other parameters + geometry_msgs::msg::Pose ego_pose; + ego_pose.position.x = 5.0; + ego_pose.position.y = 1.0; + + // Execute the function + auto [ego_s, stop_point_s] = + module_->getEgoAndStopPoint(trajectory_, ego_pose, StopLineModule::State::APPROACH); + + // Verify results + EXPECT_DOUBLE_EQ(ego_s, 5.0); + EXPECT_DOUBLE_EQ(stop_point_s.value(), 7.0 - 0.5 - 1.0); + + std::tie(ego_s, stop_point_s) = + module_->getEgoAndStopPoint(trajectory_, ego_pose, StopLineModule::State::STOPPED); + + EXPECT_DOUBLE_EQ(ego_s, 5.0); + EXPECT_DOUBLE_EQ(stop_point_s.value(), 5.0); +} + +TEST_F(StopLineModuleTest, TestUpdateStateAndStoppedTime) +{ + StopLineModule::State state = StopLineModule::State::APPROACH; + std::optional stopped_time; + double distance_to_stop_point = 0.1; + bool is_vehicle_stopped = true; + + // Simulate clock progression + auto test_start_time = clock_->now(); + stopped_time = test_start_time; + + // Execute the function + module_->updateStateAndStoppedTime( + &state, &stopped_time, test_start_time, distance_to_stop_point, is_vehicle_stopped); + + // Verify state transition to STOPPED + EXPECT_EQ(state, StopLineModule::State::STOPPED); + EXPECT_TRUE(stopped_time.has_value()); + + // Simulate time elapsed to exceed stop duration + auto now = test_start_time + rclcpp::Duration::from_seconds(3.0); + module_->updateStateAndStoppedTime( + &state, &stopped_time, now, distance_to_stop_point, is_vehicle_stopped); + + // Verify state transition to START + EXPECT_EQ(state, StopLineModule::State::START); + EXPECT_FALSE(stopped_time.has_value()); +} From 149320d7971589745a531840cf8c524f897ad2d7 Mon Sep 17 00:00:00 2001 From: suchang Date: Thu, 27 Feb 2025 18:00:13 +0800 Subject: [PATCH 05/19] feat: modify autoware_universe_utils to autoware_utils Signed-off-by: suchang --- .../CMakeLists.txt | 29 + .../README.md | 3 + ...ehavior_velocity_planner_common.param.yaml | 7 + .../planner_data.hpp | 86 +++ .../plugin_interface.hpp | 42 ++ .../plugin_wrapper.hpp | 49 ++ .../scene_module_interface.hpp | 288 +++++++ .../utilization/arc_lane_util.hpp | 201 +++++ .../utilization/boost_geometry_helper.hpp | 82 ++ .../utilization/debug.hpp | 49 ++ .../utilization/path_utilization.hpp | 36 + .../utilization/state_machine.hpp | 95 +++ .../utilization/trajectory_utils.hpp | 45 ++ .../utilization/util.hpp | 245 ++++++ .../package.xml | 59 ++ ...havior_velocity_planner_common.schema.json | 59 ++ .../src/planner_data.cpp | 72 ++ .../src/scene_module_interface.cpp | 61 ++ .../src/utilization/arc_lane_util.cpp | 129 ++++ .../src/utilization/boost_geometry_helper.cpp | 64 ++ .../src/utilization/debug.cpp | 129 ++++ .../src/utilization/path_utilization.cpp | 169 +++++ .../src/utilization/trajectory_utils.cpp | 99 +++ .../src/utilization/util.cpp | 700 ++++++++++++++++++ .../test/src/test_arc_lane_util.cpp | 242 ++++++ .../test/src/test_path_utilization.cpp | 245 ++++++ .../test/src/test_state_machine.cpp | 87 +++ .../test/src/test_trajectory_utils.cpp | 113 +++ .../test/src/test_util.cpp | 308 ++++++++ .../test/src/utils.hpp | 57 ++ 30 files changed, 3850 insertions(+) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt new file mode 100644 index 0000000000..4c06c427a0 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_planner_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene_module_interface.cpp + src/planner_data.cpp + src/utilization/path_utilization.cpp + src/utilization/trajectory_utils.cpp + src/utilization/arc_lane_util.cpp + src/utilization/boost_geometry_helper.cpp + src/utilization/util.cpp + src/utilization/debug.cpp +) + +if(BUILD_TESTING) + file(GLOB TEST_SOURCES test/src/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${TEST_SOURCES}) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) +endif() + +ament_auto_package(INSTALL_TO_SHARE + config +) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md new file mode 100644 index 0000000000..c4c8d97b4b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md @@ -0,0 +1,3 @@ +# Behavior Velocity Planner Common + +This package provides a behavior velocity interface without RTC, and common functions as a library, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml new file mode 100644 index 0000000000..aff2aec9cf --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/config/behavior_velocity_planner_common.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + max_accel: -2.8 + max_jerk: -5.0 + system_delay: 0.5 + delay_response_time: 0.5 + is_publish_debug_path: false # publish all debug path with lane id in each module diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp new file mode 100644 index 0000000000..f8b37999cb --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -0,0 +1,86 @@ +// Copyright 2019 Autoware Foundation +// +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ + +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/route_handler/route_handler.hpp" +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +struct PlannerData +{ + explicit PlannerData(rclcpp::Node & node); + + rclcpp::Clock::SharedPtr clock_; + + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; + static constexpr double velocity_buffer_time_sec = 10.0; + std::deque velocity_buffer; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + pcl::PointCloud::ConstPtr no_ground_pointcloud; + + nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; + + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; + std::optional external_velocity_limit; + + bool is_simulation = false; + + std::shared_ptr velocity_smoother_; + std::shared_ptr route_handler_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + + double max_stop_acceleration_threshold; + double max_stop_jerk_threshold; + double system_delay; + double delay_response_time; + double stop_line_extend_length; + + bool isVehicleStopped(const double stop_duration = 0.0) const; + + std::optional getTrafficSignal( + const lanelet::Id id, const bool keep_last_observation = false) const; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp new file mode 100644 index 0000000000..ce6d828779 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp @@ -0,0 +1,42 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ + +#include +#include + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ + +class PluginInterface +{ +public: + virtual ~PluginInterface() = default; + virtual void init(rclcpp::Node & node) = 0; + virtual void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) = 0; + virtual void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual const char * getModuleName() = 0; +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp new file mode 100644 index 0000000000..a4f2bc0467 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -0,0 +1,49 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ + +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +template +class PluginWrapper : public PluginInterface +{ +public: + void init(rclcpp::Node & node) override { scene_manager_ = std::make_unique(node); } + void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) override + { + scene_manager_->plan(path); + }; + void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override + { + scene_manager_->updateSceneModuleInstances(planner_data, path); + } + const char * getModuleName() override { return scene_manager_->getModuleName(); } + +private: + std::unique_ptr scene_manager_; +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp new file mode 100644 index 0000000000..3b5ab16c3f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -0,0 +1,288 @@ +// Copyright 2020 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// Debug +#include +#include + +#include +namespace autoware::behavior_velocity_planner +{ + +using autoware::objects_of_interest_marker_interface::ColorName; +using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using autoware_utils::DebugPublisher; +using autoware_utils::get_or_declare_parameter; +using autoware_utils::StopWatch; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using builtin_interfaces::msg::Time; +using unique_identifier_msgs::msg::UUID; + +struct ObjectOfInterest +{ + geometry_msgs::msg::Pose pose; + autoware_perception_msgs::msg::Shape shape; + ColorName color; + ObjectOfInterest( + const geometry_msgs::msg::Pose & pose, autoware_perception_msgs::msg::Shape shape, + const ColorName & color_name) + : pose(pose), shape(std::move(shape)), color(color_name) + { + } +}; + +class SceneModuleInterface +{ +public: + explicit SceneModuleInterface( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); + virtual ~SceneModuleInterface() = default; + + virtual bool modifyPathVelocity(PathWithLaneId * path) = 0; + + virtual visualization_msgs::msg::MarkerArray createDebugMarkerArray() = 0; + virtual std::vector createVirtualWalls() = 0; + + int64_t getModuleId() const { return module_id_; } + + void setPlannerData(const std::shared_ptr & planner_data) + { + planner_data_ = planner_data; + } + + std::vector getObjectsOfInterestData() const { return objects_of_interest_; } + void clearObjectsOfInterestData() { objects_of_interest_.clear(); } + +protected: + const int64_t module_id_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr planner_data_; + std::vector objects_of_interest_; + mutable std::shared_ptr time_keeper_; + std::shared_ptr planning_factor_interface_; + + void setObjectsOfInterestData( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, + const ColorName & color_name) + { + objects_of_interest_.emplace_back(pose, shape, color_name); + } + + size_t findEgoSegmentIndex( + const std::vector & points) const; +}; + +template +class SceneModuleManagerInterface +{ +public: + SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name) + : node_(node), clock_(node.get_clock()), logger_(node.get_logger()) + { + const auto ns = std::string("~/debug/") + module_name; + pub_debug_ = node.create_publisher(ns, 1); + if (!node.has_parameter("is_publish_debug_path")) { + is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); + } else { + is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); + } + if (is_publish_debug_path_) { + pub_debug_path_ = node.create_publisher( + std::string("~/debug/path_with_lane_id/") + module_name, 1); + } + pub_virtual_wall_ = node.create_publisher( + std::string("~/virtual_wall/") + module_name, 5); + planning_factor_interface_ = + std::make_shared(&node, module_name); + + processing_time_publisher_ = std::make_shared(&node, "~/debug"); + + pub_processing_time_detail_ = node.create_publisher( + "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); + + time_keeper_ = std::make_shared(pub_processing_time_detail_); + } + + virtual ~SceneModuleManagerInterface() = default; + + virtual const char * getModuleName() = 0; + + void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) + { + planner_data_ = planner_data; + + launchNewModules(path); + deleteExpiredModules(path); + } + + virtual void plan(autoware_internal_planning_msgs::msg::PathWithLaneId * path) + { + modifyPathVelocity(path); + } + +protected: + virtual void modifyPathVelocity(autoware_internal_planning_msgs::msg::PathWithLaneId * path) + { + autoware_utils::ScopedTimeTrack st( + "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); + StopWatch stop_watch; + stop_watch.tic("Total"); + visualization_msgs::msg::MarkerArray debug_marker_array; + + for (const auto & scene_module : scene_modules_) { + scene_module->setPlannerData(planner_data_); + scene_module->modifyPathVelocity(path); + + // The velocity factor must be called after modifyPathVelocity. + + for (const auto & marker : scene_module->createDebugMarkerArray().markers) { + debug_marker_array.markers.push_back(marker); + } + + virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); + } + + planning_factor_interface_->publish(); + pub_debug_->publish(debug_marker_array); + if (is_publish_debug_path_) { + autoware_internal_planning_msgs::msg::PathWithLaneId debug_path; + debug_path.header = path->header; + debug_path.points = path->points; + pub_debug_path_->publish(debug_path); + } + pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); + processing_time_publisher_->publish( + std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); + } + + virtual void launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; + + virtual std::function &)> getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) = 0; + + virtual void deleteExpiredModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) + { + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } + } + + bool isModuleRegistered(const int64_t module_id) + { + return registered_module_id_set_.count(module_id) != 0; + } + + void registerModule(const std::shared_ptr & scene_module) + { + RCLCPP_DEBUG( + logger_, "register task: module = %s, id = %lu", getModuleName(), + scene_module->getModuleId()); + registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_modules_.insert(scene_module); + } + + size_t findEgoSegmentIndex( + const std::vector & points) const + { + const auto & p = planner_data_; + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, + p->ego_nearest_yaw_threshold); + } + + std::set> scene_modules_; + std::set registered_module_id_set_; + + std::shared_ptr planner_data_; + autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_; + + rclcpp::Node & node_; + rclcpp::Clock::SharedPtr clock_; + // Debug + bool is_publish_debug_path_ = {false}; // note : this is very heavy debug topic option + rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; + rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr + pub_debug_path_; + + std::shared_ptr processing_time_publisher_; + + rclcpp::Publisher::SharedPtr pub_processing_time_detail_; + + std::shared_ptr time_keeper_; + + std::shared_ptr planning_factor_interface_; +}; +extern template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +extern template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +extern template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::modifyPathVelocity( + autoware_internal_planning_msgs::msg::PathWithLaneId * path); +extern template void SceneModuleManagerInterface::deleteExpiredModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp new file mode 100644 index 0000000000..215419ee4f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -0,0 +1,201 @@ +// Copyright 2021 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ + +#include +#include + +#include + +#include +#include + +#define EIGEN_MPL2_ONLY +#include + +namespace autoware::behavior_velocity_planner +{ + +inline geometry_msgs::msg::Point convertToGeomPoint(const autoware_utils::Point2d & p) +{ + geometry_msgs::msg::Point geom_p; + geom_p.x = p.x(); + geom_p.y = p.y(); + + return geom_p; +} + +namespace arc_lane_utils +{ +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = + std::pair; // front index, point2d +using PathIndexWithPoint = std::pair; // front index, point2d +using PathIndexWithOffset = std::pair; // front index, offset + +double calcSignedDistance( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2); + +// calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) +std::optional checkCollision( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); + +template +std::optional findCollisionSegment( + const T & path, const geometry_msgs::msg::Point & stop_line_p1, + const geometry_msgs::msg::Point & stop_line_p2) +{ + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto & p1 = + autoware_utils::get_point(path.points.at(i)); // Point before collision point + const auto & p2 = + autoware_utils::get_point(path.points.at(i + 1)); // Point after collision point + + const auto collision_point = checkCollision(p1, p2, stop_line_p1, stop_line_p2); + + if (collision_point) { + return std::make_pair(i, collision_point.value()); + } + } + + return {}; +} + +template +std::optional findCollisionSegment( + const T & path, const LineString2d & stop_line) +{ + const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0)); + const auto stop_line_p2 = convertToGeomPoint(stop_line.at(1)); + + return findCollisionSegment(path, stop_line_p1, stop_line_p2); +} + +template +std::optional findForwardOffsetSegment( + const T & path, const size_t base_idx, const double offset_length) +{ + double sum_length = 0.0; + for (size_t i = base_idx; i < path.points.size() - 1; ++i) { + const double segment_length = + autoware_utils::calc_distance2d(path.points.at(i), path.points.at(i + 1)); + + // If it's over offset point, return front index and remain offset length + /** + * (base_idx) --- offset_length ---------> + * --------- (i) <-- remain -->-----------> (i+1) + */ + if (sum_length + segment_length >= offset_length) { + return std::make_pair(i, offset_length - sum_length); + } + + sum_length += segment_length; + } + + // No enough path length + return {}; +} + +template +std::optional findBackwardOffsetSegment( + const T & path, const size_t base_idx, const double offset_length) +{ + double sum_length = 0.0; + const auto start = static_cast(base_idx) - 1; + for (std::int32_t i = start; i >= 0; --i) { + sum_length += + autoware_utils::calc_distance2d(path.points.at(i), path.points.at(i + 1)); + + // If it's over offset point, return front index and remain offset length + /** + * <-------- offset_length --- (base_idx) + * ----- (i) <-- remain -->-------> (i+1) + */ + if (sum_length >= offset_length) { + const auto k = static_cast(i); + return std::make_pair(k, sum_length - offset_length); + } + } + + // No enough path length + return {}; +} + +template +std::optional findOffsetSegment( + const T & path, const PathIndexWithPoint & collision_segment, const double offset_length) +{ + const size_t collision_idx = collision_segment.first; + const auto & collision_point = collision_segment.second; + + if (offset_length >= 0) { + return findForwardOffsetSegment( + path, collision_idx, + offset_length + + autoware_utils::calc_distance2d(path.points.at(collision_idx), collision_point)); + } + + return findBackwardOffsetSegment( + path, collision_idx + 1, + -offset_length + + autoware_utils::calc_distance2d(path.points.at(collision_idx + 1), collision_point)); +} + +std::optional findOffsetSegment( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t index, + const double offset); + +template +geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment) +{ + const size_t offset_idx = offset_segment.first; + const double remain_offset_length = offset_segment.second; + const auto & p_front = path.points.at(offset_idx).point.pose.position; + const auto & p_back = path.points.at(offset_idx + 1).point.pose.position; + + // To Eigen point + const auto p_eigen_front = Eigen::Vector2d(p_front.x, p_front.y); + const auto p_eigen_back = Eigen::Vector2d(p_back.x, p_back.y); + + // Calculate interpolation ratio + /** + * (front) <-- remain_length --> (interp) <----> (back) + */ + const auto interpolate_ratio = remain_offset_length / (p_eigen_back - p_eigen_front).norm(); + + // Add offset to front point + const auto target_point_2d = p_eigen_front + interpolate_ratio * (p_eigen_back - p_eigen_front); + const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); + + // Calculate orientation so that X-axis would be along the trajectory + geometry_msgs::msg::Pose target_pose; + target_pose.position.x = target_point_2d.x(); + target_pose.position.y = target_point_2d.y(); + target_pose.position.z = interpolated_z; + const double yaw = autoware_utils::calc_azimuth_angle(p_front, p_back); + target_pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw); + return target_pose; +} + +std::optional createTargetPoint( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const double margin, const double vehicle_offset); + +} // namespace arc_lane_utils +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp new file mode 100644 index 0000000000..cdbf22368b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -0,0 +1,82 @@ +// Copyright 2020 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +// cppcheck-suppress unknownMacro +BOOST_GEOMETRY_REGISTER_POINT_3D(geometry_msgs::msg::Point, double, cs::cartesian, x, y, z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + geometry_msgs::msg::Pose, double, cs::cartesian, position.x, position.y, position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + geometry_msgs::msg::PoseWithCovarianceStamped, double, cs::cartesian, pose.pose.position.x, + pose.pose.position.y, pose.pose.position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + autoware_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, pose.position.y, + pose.position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + autoware_internal_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, + point.pose.position.x, point.pose.position.y, point.pose.position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, + pose.position.y, pose.position.z) + +namespace autoware::behavior_velocity_planner +{ +namespace bg = boost::geometry; + +using Point2d = autoware_utils::Point2d; +using LineString2d = autoware_utils::LineString2d; +using Polygon2d = autoware_utils::Polygon2d; + +template +Point2d to_bg2d(const T & p) +{ + return Point2d(bg::get<0>(p), bg::get<1>(p)); +} + +template +LineString2d to_bg2d(const std::vector & vec) +{ + LineString2d ps; + for (const auto & p : vec) { + ps.push_back(to_bg2d(p)); + } + return ps; +} + +Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line); + +Polygon2d upScalePolygon( + const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale); + +geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon); + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp new file mode 100644 index 0000000000..ba103e01f8 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp @@ -0,0 +1,49 @@ +// Copyright 2020 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::behavior_velocity_planner::debug +{ +visualization_msgs::msg::MarkerArray createPolygonMarkerArray( + const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, + const rclcpp::Time & now, const double x, const double y, const double z, const double r, + const double g, const double b); +visualization_msgs::msg::MarkerArray createPathMarkerArray( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b); +visualization_msgs::msg::MarkerArray createObjectsMarkerArray( + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double r, const double g, + const double b); +visualization_msgs::msg::MarkerArray createPointsMarkerArray( + const std::vector & points, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b); +} // namespace autoware::behavior_velocity_planner::debug + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp new file mode 100644 index 0000000000..770feb3e0a --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -0,0 +1,36 @@ +// Copyright 2020 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ + +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +bool splineInterpolate( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_internal_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger); +autoware_planning_msgs::msg::Path interpolatePath( + const autoware_planning_msgs::msg::Path & path, const double length, const double interval); +autoware_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_planning_msgs::msg::Path & path); +autoware_planning_msgs::msg::Path filterStopPathPoint( + const autoware_planning_msgs::msg::Path & path); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp new file mode 100644 index 0000000000..dfe70d3766 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp @@ -0,0 +1,95 @@ +// Copyright 2021 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ + +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +/** + * @brief Manage stop-go states with safety margin time. + */ +class StateMachine +{ +public: + enum State { + STOP = 0, + GO, + }; + static std::string toString(const State & state) + { + if (state == State::STOP) { + return "STOP"; + } + if (state == State::GO) { + return "GO"; + } + return ""; + } + StateMachine() + { + state_ = State::GO; + margin_time_ = 0.0; + duration_ = 0.0; + } + void setStateWithMarginTime(State state, rclcpp::Logger logger, rclcpp::Clock & clock) + { + /* same state request */ + if (state_ == state) { + start_time_ = nullptr; // reset timer + return; + } + + /* GO -> STOP */ + if (state == State::STOP) { + state_ = State::STOP; + start_time_ = nullptr; // reset timer + return; + } + + /* STOP -> GO */ + if (state == State::GO) { + if (start_time_ == nullptr) { + start_time_ = std::make_shared(clock.now()); + } else { + duration_ = (clock.now() - *start_time_).seconds(); + if (duration_ > margin_time_) { + state_ = State::GO; + start_time_ = nullptr; // reset timer + } + } + return; + } + RCLCPP_ERROR(logger, "Unsuitable state. ignore request."); + } + + void setMarginTime(const double t) { margin_time_ = t; } + void setState(State state) { state_ = state; } + State getState() const { return state_; } + double getDuration() const { return duration_; } + +private: + State state_; //! current state + double margin_time_; //! margin time when transit to Go from Stop + double duration_; //! duration time when transit to Go from Stop + std::shared_ptr start_time_; //! first time received GO when STOP state +}; + +} // namespace autoware::behavior_velocity_planner +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp new file mode 100644 index 0000000000..c82d3d5ed0 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -0,0 +1,45 @@ +// Copyright 2021 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Quaternion; +using TrajectoryPointWithIdx = std::pair; + +//! smooth path point with lane id starts from ego position on path to the path end +bool smoothPath( + const PathWithLaneId & in_path, PathWithLaneId & out_path, + const std::shared_ptr & planner_data); + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp new file mode 100644 index 0000000000..6535905926 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp @@ -0,0 +1,245 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ + +#include "autoware_utils/geometry/boost_geometry.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +/** + * @brief Represents detection range parameters. + */ +struct DetectionRange +{ + bool use_right = true; ///< Whether to use the right side. + bool use_left = true; ///< Whether to use the left side. + double interval{0.0}; ///< Interval of detection points. + double min_longitudinal_distance{0.0}; ///< Minimum longitudinal detection distance. + double max_longitudinal_distance{0.0}; ///< Maximum longitudinal detection distance. + double max_lateral_distance{0.0}; ///< Maximum lateral detection distance. + double wheel_tread{0.0}; ///< Wheel tread of the vehicle. + double right_overhang{0.0}; ///< Right overhang of the vehicle. + double left_overhang{0.0}; ///< Left overhang of the vehicle. +}; + +/** + * @brief Represents a traffic signal with a timestamp. + */ +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; ///< Timestamp of the signal. + autoware_perception_msgs::msg::TrafficLightGroup signal; ///< Traffic light group. +}; + +using Pose = geometry_msgs::msg::Pose; +using Point2d = autoware_utils::Point2d; +using LineString2d = autoware_utils::LineString2d; +using Polygon2d = autoware_utils::Polygon2d; +using BasicPolygons2d = std::vector; +using Polygons2d = std::vector; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; + +namespace planning_utils +{ +size_t calcSegmentIndexFromPointIndex( + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t idx); + +bool createDetectionAreaPolygons( + Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, + const double min_velocity = 1.0); + +Point2d calculateOffsetPoint2d( + const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y); + +void extractClosePartition( + const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, + BasicPolygons2d & close_partition, const double distance_thresh = 30.0); + +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr & ll, BasicPolygons2d & polys); + +void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input); + +void insertVelocity( + PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, + size_t & insert_index, const double min_distance = 0.001); + +inline int64_t bitShift(int64_t original_id) +{ + return original_id << (sizeof(int32_t) * 8 / 2); +} + +bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); +geometry_msgs::msg::Pose getAheadPose( + const size_t start_idx, const double ahead_dist, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); +Polygon2d generatePathPolygon( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); + +double calcJudgeLineDistWithAccLimit( + const double velocity, const double max_stop_acceleration, const double delay_response_time); + +double calcJudgeLineDistWithJerkLimit( + const double velocity, const double acceleration, const double max_stop_acceleration, + const double max_stop_jerk, const double delay_response_time); + +double calcDecelerationVelocityFromDistanceToTarget( + const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel, + const double current_velocity, const double distance_to_target); + +double findReachTime( + const double jerk, const double accel, const double velocity, const double distance, + const double t_min, const double t_max); + +std::vector toRosPoints(const PredictedObjects & object); + +LineString2d extendLine( + const lanelet::ConstPoint3d & lanelet_point1, const lanelet::ConstPoint3d & lanelet_point2, + const double & length); + +template +std::vector concatVector(const std::vector & vec1, const std::vector & vec2) +{ + auto concat_vec = vec1; + concat_vec.insert(std::end(concat_vec), std::begin(vec2), std::end(vec2)); + return concat_vec; +} + +std::optional getNearestLaneId( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); + +std::vector getSortedLaneIdsFromPath(const PathWithLaneId & path); + +// return the set of lane_ids in the path after base_lane_id +std::vector getSubsequentLaneIdsSetOnPath( + const PathWithLaneId & path, int64_t base_lane_id); + +template +std::unordered_map, lanelet::ConstLanelet> getRegElemMapOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::unordered_map, lanelet::ConstLanelet> reg_elem_map_on_path; + + // Add current lane id + const auto nearest_lane_id = getNearestLaneId(path, lanelet_map, current_pose); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + // Add subsequent lane_ids from nearest lane_id + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); + } else { + // Add all lane_ids in path + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + } + + for (const auto lane_id : unique_lane_ids) { + const auto ll = lanelet_map->laneletLayer.get(lane_id); + + for (const auto & reg_elem : ll.regulatoryElementsAs()) { + reg_elem_map_on_path.insert(std::make_pair(reg_elem, ll)); + } + } + + return reg_elem_map_on_path; +} + +template +std::set getRegElemIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::set reg_elem_id_set; + for (const auto & m : getRegElemMapOnPath(path, lanelet_map, current_pose)) { + reg_elem_id_set.insert(m.first->id()); + } + return reg_elem_id_set; +} + +template +std::set getLaneletIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::set id_set; + for (const auto & m : getRegElemMapOnPath(path, lanelet_map, current_pose)) { + id_set.insert(m.second.id()); + } + return id_set; +} + +std::optional insertDecelPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output, + const float target_velocity); + +std::vector getLaneletsOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); + +std::set getLaneIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); + +bool isOverLine( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, + const double offset = 0.0); + +std::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); + +std::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output); + +/* + @brief return 'associative' lanes in the intersection. 'associative' means that a lane shares same + or lane-changeable parent lanes with `lane` and has same turn_direction value. + */ +std::set getAssociativeIntersectionLanelets( + const lanelet::ConstLanelet & lane, const lanelet::LaneletMapPtr lanelet_map, + const lanelet::routing::RoutingGraphPtr routing_graph); + +lanelet::ConstLanelets getConstLaneletsFromIds( + const lanelet::LaneletMapConstPtr & map, const std::set & ids); + +} // namespace planning_utils +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml new file mode 100644 index 0000000000..a66faa2e0a --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -0,0 +1,59 @@ + + + + autoware_behavior_velocity_planner_common + 0.41.0 + The autoware_behavior_velocity_planner_common package + + Tomoya Kimura + Shumpei Wakabayashi + Takagi, Isamu + Fumiya Watanabe + Mamoru Sobue + + Apache License 2.0 + + Yukihiro Saito + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_adapi_v1_msgs + autoware_internal_debug_msgs + autoware_interpolation + autoware_lanelet2_extension + autoware_map_msgs + autoware_motion_utils + autoware_objects_of_interest_marker_interface + autoware_perception_msgs + autoware_planning_factor_interface + autoware_planning_msgs + autoware_route_handler + autoware_universe_utils + autoware_vehicle_info_utils + autoware_velocity_smoother + diagnostic_msgs + eigen + geometry_msgs + nav_msgs + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_test_utils + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json new file mode 100644 index 0000000000..2468b71aa9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/schema/behavior_velocity_planner_common.schema.json @@ -0,0 +1,59 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Behavior Velocity Planner Common", + "type": "object", + "definitions": { + "behavior_velocity_planner_common": { + "type": "object", + "properties": { + "max_accel": { + "type": "number", + "default": "-2.8", + "description": "(to be a global parameter) max acceleration of the vehicle" + }, + "system_delay": { + "type": "number", + "default": "0.5", + "description": "(to be a global parameter) delay time until output control command" + }, + "delay_response_time": { + "type": "number", + "default": "0.5", + "description": "(to be a global parameter) delay time of the vehicle's response to control commands" + }, + "max_jerk": { + "type": "number", + "default": "-5.0", + "description": "max jerk of the vehicle" + }, + "is_publish_debug_path": { + "type": "boolean", + "default": "false", + "description": "is publish debug path?" + } + }, + "required": [ + "max_accel", + "system_delay", + "delay_response_time", + "max_jerk", + "is_publish_debug_path" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/behavior_velocity_planner_common" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp new file mode 100644 index 0000000000..df2a183e3b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp @@ -0,0 +1,72 @@ +// Copyright 2019 Autoware Foundation +// +// 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 "autoware/behavior_velocity_planner_common/planner_data.hpp" + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include + +namespace autoware::behavior_velocity_planner +{ +PlannerData::PlannerData(rclcpp::Node & node) +: clock_(node.get_clock()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) +{ + max_stop_acceleration_threshold = node.declare_parameter("max_accel"); + max_stop_jerk_threshold = node.declare_parameter("max_jerk"); + system_delay = node.declare_parameter("system_delay"); + delay_response_time = node.declare_parameter("delay_response_time"); +} + +bool PlannerData::isVehicleStopped(const double stop_duration) const +{ + if (velocity_buffer.empty()) { + return false; + } + + const auto now = clock_->now(); + std::vector vs; + for (const auto & velocity : velocity_buffer) { + vs.push_back(velocity.twist.linear.x); + + const auto & s = velocity.header.stamp; + const auto time_diff = + now >= s ? now - s : rclcpp::Duration(0, 0); // Note: negative time throws an exception. + if (time_diff.seconds() >= stop_duration) { + break; + } + } + + constexpr double stop_velocity = 1e-3; + for (const auto & v : vs) { + if (v >= stop_velocity) { + return false; + } + } + + return true; +} + +std::optional PlannerData::getTrafficSignal( + const lanelet::Id id, const bool keep_last_observation) const +{ + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; + if (traffic_light_id_map.count(id) == 0) { + return std::nullopt; + } + return std::make_optional(traffic_light_id_map.at(id)); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp new file mode 100644 index 0000000000..789b5ee412 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -0,0 +1,61 @@ +// Copyright 2023 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 +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +SceneModuleInterface::SceneModuleInterface( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: module_id_(module_id), + logger_(logger), + clock_(clock), + time_keeper_(time_keeper), + planning_factor_interface_(planning_factor_interface) +{ +} + +size_t SceneModuleInterface::findEgoSegmentIndex( + const std::vector & points) const +{ + const auto & p = planner_data_; + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold); +} + +template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::modifyPathVelocity( + autoware_internal_planning_msgs::msg::PathWithLaneId * path); +template void SceneModuleManagerInterface::deleteExpiredModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp new file mode 100644 index 0000000000..24a2d4d7eb --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -0,0 +1,129 @@ +// Copyright 2023 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 +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#include + +namespace +{ +geometry_msgs::msg::Point operator+( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + geometry_msgs::msg::Point p; + p.x = p1.x + p2.x; + p.y = p1.y + p2.y; + p.z = p1.z + p2.z; + + return p; +} + +geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & p, const double v) +{ + geometry_msgs::msg::Point multiplied_p; + multiplied_p.x = p.x * v; + multiplied_p.y = p.y * v; + multiplied_p.z = p.z * v; + + return multiplied_p; +} + +} // namespace + +namespace autoware::behavior_velocity_planner::arc_lane_utils +{ + +double calcSignedDistance(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) +{ + Eigen::Affine3d map2p1; + tf2::fromMsg(p1, map2p1); + const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z); + return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); +} + +// calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) + +std::optional checkCollision( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) +{ + const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x); + + if (det == 0.0) { + // collision is not one point. + return std::nullopt; + } + + const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det; + const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det; + + // check collision is outside the segment line + if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) { + return std::nullopt; + } + + return p1 * (1.0 - t1) + p2 * t1; +} + +std::optional findOffsetSegment( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const size_t index, + const double offset) +{ + if (offset >= 0) { + return findForwardOffsetSegment(path, index, offset); + } + + return findBackwardOffsetSegment(path, index, -offset); +} + +std::optional createTargetPoint( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const double margin, const double vehicle_offset) +{ + // Find collision segment + const auto collision_segment = findCollisionSegment(path, stop_line); + if (!collision_segment) { + // No collision + return {}; + } + + // Calculate offset length from stop line + // Use '-' to make the positive direction is forward + const double offset_length = -(margin + vehicle_offset); + + // Find offset segment + const auto offset_segment = findOffsetSegment(path, *collision_segment, offset_length); + if (!offset_segment) { + // No enough path length + return {}; + } + + const auto target_pose = calcTargetPose(path, *offset_segment); + + const auto front_idx = offset_segment->first; + return std::make_pair(front_idx, target_pose); +} +} // namespace autoware::behavior_velocity_planner::arc_lane_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp new file mode 100644 index 0000000000..d9162419ec --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 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 + +#include + +namespace autoware::behavior_velocity_planner +{ + +Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) +{ + Polygon2d polygon; + + polygon.outer().push_back(left_line.front()); + + for (const auto & itr : right_line) { + polygon.outer().push_back(itr); + } + + for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) { + polygon.outer().push_back(*itr); + } + + bg::correct(polygon); + return polygon; +} + +Polygon2d upScalePolygon( + const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale) +{ + Polygon2d transformed_polygon; + // upscale + for (size_t i = 0; i < polygon.outer().size(); i++) { + const double upscale_x = (polygon.outer().at(i).x() - position.x) * scale + position.x; + const double upscale_y = (polygon.outer().at(i).y() - position.y) * scale + position.y; + transformed_polygon.outer().emplace_back(Point2d(upscale_x, upscale_y)); + } + return transformed_polygon; +} + +geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) +{ + geometry_msgs::msg::Polygon polygon_msg; + geometry_msgs::msg::Point32 point_msg; + for (const auto & p : polygon.outer()) { + point_msg.x = static_cast(p.x()); + point_msg.y = static_cast(p.y()); + polygon_msg.points.push_back(point_msg); + } + return polygon_msg; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp new file mode 100644 index 0000000000..fd6a61a0ba --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -0,0 +1,129 @@ +// Copyright 2020 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 +#include +#include + +#include +#include +namespace autoware::behavior_velocity_planner::debug +{ +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; + +visualization_msgs::msg::MarkerArray createPolygonMarkerArray( + const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, + const rclcpp::Time & now, const double x, const double y, const double z, const double r, + const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + { + auto marker = create_default_marker( + "map", now, ns, static_cast(module_id), visualization_msgs::msg::Marker::LINE_STRIP, + create_marker_scale(static_cast(x), static_cast(y), static_cast(z)), + create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.8f)); + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + + for (const auto & p : polygon.points) { + geometry_msgs::msg::Point point; + point.x = p.x; + point.y = p.y; + point.z = p.z; + marker.points.push_back(point); + } + + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + msg.markers.push_back(marker); + } + return msg; +} + +visualization_msgs::msg::MarkerArray createPathMarkerArray( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + for (size_t i = 0; i < path.points.size(); ++i) { + const auto & p = path.points.at(i); + + auto marker = create_default_marker( + "map", now, ns, static_cast(planning_utils::bitShift(lane_id) + i), + visualization_msgs::msg::Marker::ARROW, + create_marker_scale(static_cast(x), static_cast(y), static_cast(z)), + create_marker_color( + static_cast(r), static_cast(g), static_cast(b), 0.999f)); + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.pose = p.point.pose; + + if (std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end()) { + // if p.lane_ids has lane_id + marker.color = create_marker_color( + static_cast(r), static_cast(g), static_cast(b), 0.999f); + } else { + marker.color = create_marker_color(0.5, 0.5, 0.5, 0.999); + } + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createObjectsMarkerArray( + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + auto marker = create_default_marker( + "map", now, ns, 0, visualization_msgs::msg::Marker::CUBE, create_marker_scale(3.0, 1.0, 1.0), + create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.8f)); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + + marker.id = static_cast(planning_utils::bitShift(module_id) + i); + marker.pose = object.kinematics.initial_pose_with_covariance.pose; + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createPointsMarkerArray( + const std::vector & points, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + auto marker = create_default_marker( + "map", now, ns, 0, visualization_msgs::msg::Marker::SPHERE, create_marker_scale(x, y, z), + create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.999f)); + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + for (size_t i = 0; i < points.size(); ++i) { + marker.id = static_cast(i + planning_utils::bitShift(module_id)); + marker.pose.position = points.at(i); + msg.markers.push_back(marker); + } + + return msg; +} +} // namespace autoware::behavior_velocity_planner::debug diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp new file mode 100644 index 0000000000..38f7442999 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -0,0 +1,169 @@ +// Copyright 2020 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 +#include +#include +#include +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +bool splineInterpolate( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_internal_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger & logger) +{ + if (input.points.size() < 2) { + RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1."); + return false; + } + + output = autoware::motion_utils::resamplePath(input, interval, false, true, true, false); + + return true; +} + +/* + * Interpolate the path with a fixed interval by spline. + * In order to correctly inherit the position of the planned velocity points, the position of the + * existing points in the input path are kept in the interpolated path. + * The velocity is interpolated by zero-order hold, that is, the velocity of the interpolated point + * is the velocity of the closest point for the input "sub-path" which consists of the points before + * the interpolated point. + */ +autoware_planning_msgs::msg::Path interpolatePath( + const autoware_planning_msgs::msg::Path & path, const double length, const double interval) +{ + const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("path_utilization")}; + + const double epsilon = 0.01; + std::vector s_in; + if (2000 < path.points.size()) { + RCLCPP_WARN( + logger, "because path size is too large, calculation cost is high. size is %d.", + (int)path.points.size()); + } + if (path.points.size() < 2) { + RCLCPP_WARN(logger, "Do not interpolate because path size is 1."); + return path; + } + + double path_len = std::min(length, autoware::motion_utils::calcArcLength(path.points)); + { + std::vector x; + std::vector y; + std::vector z; + std::vector v; + double s = 0.0; + for (size_t idx = 0; idx < path.points.size(); ++idx) { + const auto path_point = path.points.at(idx); + x.push_back(path_point.pose.position.x); + y.push_back(path_point.pose.position.y); + z.push_back(path_point.pose.position.z); + v.push_back(path_point.longitudinal_velocity_mps); + if (idx != 0) { + const auto path_point_prev = path.points.at(idx - 1); + s += autoware_utils::calc_distance2d(path_point_prev.pose, path_point.pose); + } + if (s > path_len) { + break; + } + s_in.push_back(s); + } + + // update path length + path_len = std::min(path_len, s_in.back()); + + // Check Terminal Points + if (std::fabs(s_in.back() - path_len) < epsilon) { + s_in.back() = path_len; + } else { + s_in.push_back(path_len); + } + } + + // Calculate query points + // Use all values of s_in to inherit the velocity-planned point, and add some points for + // interpolation with a constant interval if the point is not closed to the original s_in points. + // (because if this interval is very short, the interpolation will be less accurate and may fail.) + std::vector s_out = s_in; + + const auto has_almost_same_value = [&](const auto & vec, const auto x) { + if (vec.empty()) return false; + const auto has_close = [&](const auto v) { return std::abs(v - x) < epsilon; }; + return std::find_if(vec.begin(), vec.end(), has_close) != vec.end(); + }; + for (double s = 0.0; s < path_len; s += interval) { + if (!has_almost_same_value(s_out, s)) { + s_out.push_back(s); + } + } + + std::sort(s_out.begin(), s_out.end()); + + if (s_out.empty()) { + RCLCPP_WARN(logger, "Do not interpolate because s_out is empty."); + return path; + } + + return autoware::motion_utils::resamplePath(path, s_out); +} + +autoware_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_planning_msgs::msg::Path & path) +{ + autoware_planning_msgs::msg::Path filtered_path; + + const double epsilon = 0.01; + size_t latest_id = 0; + for (size_t i = 0; i < path.points.size(); ++i) { + double dist = 0.0; + if (i != 0) { + const double x = + path.points.at(i).pose.position.x - path.points.at(latest_id).pose.position.x; + const double y = + path.points.at(i).pose.position.y - path.points.at(latest_id).pose.position.y; + dist = std::sqrt(x * x + y * y); + } + if (i == 0 || epsilon < dist /*init*/) { + latest_id = i; + filtered_path.points.push_back(path.points.at(latest_id)); + } else { + filtered_path.points.back().longitudinal_velocity_mps = std::min( + filtered_path.points.back().longitudinal_velocity_mps, + path.points.at(i).longitudinal_velocity_mps); + } + } + + return filtered_path; +} +autoware_planning_msgs::msg::Path filterStopPathPoint( + const autoware_planning_msgs::msg::Path & path) +{ + autoware_planning_msgs::msg::Path filtered_path = path; + bool found_stop = false; + for (auto & point : filtered_path.points) { + if (std::fabs(point.longitudinal_velocity_mps) < 0.01) { + found_stop = true; + } + if (found_stop) { + point.longitudinal_velocity_mps = 0.0; + } + } + return filtered_path; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp new file mode 100644 index 0000000000..c0d6eadaac --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -0,0 +1,99 @@ +// Copyright 2023 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 +#include "autoware/motion_utils/trajectory/conversion.hpp" + +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Quaternion; +using TrajectoryPointWithIdx = std::pair; + +//! smooth path point with lane id starts from ego position on path to the path end +bool smoothPath( + const PathWithLaneId & in_path, PathWithLaneId & out_path, + const std::shared_ptr & planner_data) +{ + const geometry_msgs::msg::Pose current_pose = planner_data->current_odometry->pose; + const double v0 = planner_data->current_velocity->twist.linear.x; + const double a0 = planner_data->current_acceleration->accel.accel.linear.x; + const auto & external_v_limit = planner_data->external_velocity_limit; + const auto & smoother = planner_data->velocity_smoother_; + + auto trajectory = autoware::motion_utils::convertToTrajectoryPoints< + autoware_internal_planning_msgs::msg::PathWithLaneId>(in_path); + const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); + + const auto traj_steering_rate_limited = + smoother->applySteeringRateLimit(traj_lateral_acc_filtered, false); + + // Resample trajectory with ego-velocity based interval distances + auto traj_resampled = smoother->resampleTrajectory( + traj_steering_rate_limited, v0, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + const size_t traj_resampled_closest = + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + std::vector debug_trajectories; + // Clip trajectory from closest point + TrajectoryPoints clipped; + TrajectoryPoints traj_smoothed; + clipped.insert( + clipped.end(), traj_resampled.begin() + static_cast(traj_resampled_closest), + traj_resampled.end()); + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { + std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; + return false; + } + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled.begin(), + traj_resampled.begin() + static_cast(traj_resampled_closest)); + + if (external_v_limit) { + autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + } + out_path = autoware::motion_utils::convertToPathWithLaneId(traj_smoothed); + return true; +} + +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp new file mode 100644 index 0000000000..4bf6d98e31 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -0,0 +1,700 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "autoware/behavior_velocity_planner_common/utilization/util.hpp" + +#include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_lanelet2_extension/utility/query.hpp" + +#include + +#include + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace +{ +size_t calcPointIndexFromSegmentIndex( + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t seg_idx) +{ + const size_t prev_point_idx = seg_idx; + const size_t next_point_idx = seg_idx + 1; + + const double prev_dist = + autoware_utils::calc_distance2d(point, points.at(prev_point_idx)); + const double next_dist = + autoware_utils::calc_distance2d(point, points.at(next_point_idx)); + + if (prev_dist < next_dist) { + return prev_point_idx; + } + return next_point_idx; +} + +using autoware_planning_msgs::msg::PathPoint; + +PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio) +{ + auto lerp = [](const double a, const double b, const double t) { return a + t * (b - a); }; + PathPoint p; + p.pose = autoware_utils::calc_interpolated_pose(p0, p1, ratio); + const double v = lerp(p0.longitudinal_velocity_mps, p1.longitudinal_velocity_mps, ratio); + p.longitudinal_velocity_mps = static_cast(v); + return p; +} + +geometry_msgs::msg::Pose transformRelCoordinate2D( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) +{ + // translation + geometry_msgs::msg::Point trans_p; + trans_p.x = target.position.x - origin.position.x; + trans_p.y = target.position.y - origin.position.y; + + // rotation (use inverse matrix of rotation) + double yaw = tf2::getYaw(origin.orientation); + + geometry_msgs::msg::Pose res; + res.position.x = (std::cos(yaw) * trans_p.x) + (std::sin(yaw) * trans_p.y); + res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y); + res.position.z = target.position.z - origin.position.z; + res.orientation = + autoware_utils::create_quaternion_from_yaw(tf2::getYaw(target.orientation) - yaw); + + return res; +} + +} // namespace + +namespace autoware::behavior_velocity_planner::planning_utils +{ +using autoware::motion_utils::calcSignedArcLength; +using autoware_utils::calc_distance2d; +using autoware_utils::calc_offset_pose; +using autoware_planning_msgs::msg::PathPoint; + +size_t calcSegmentIndexFromPointIndex( + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t idx) +{ + if (idx == 0) { + return 0; + } + if (idx == points.size() - 1) { + return idx - 1; + } + if (points.size() < 3) { + return 0; + } + + const double offset_to_seg = + autoware::motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); + if (0 < offset_to_seg) { + return idx; + } + return idx - 1; +} + +Point2d calculateOffsetPoint2d( + const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y) +{ + return to_bg2d(calc_offset_pose(pose, offset_x, offset_y, 0.0)); +} + +bool createDetectionAreaPolygons( + Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, + const double min_velocity) +{ + /** + * @brief relationships for interpolated polygon + * + * +(min_length,max_distance)-+ - +---+(max_length,max_distance) = outer_polygons + * | | + * +--------------------------+ - +---+(max_length,min_distance) = inner_polygons + */ + const double min_len = da_range.min_longitudinal_distance; + const double max_len = da_range.max_longitudinal_distance; + const double max_dst = da_range.max_lateral_distance; + const double interval = da_range.interval; + const double offset_left = (da_range.wheel_tread / 2.0) + da_range.left_overhang; + const double offset_right = (da_range.wheel_tread / 2.0) + da_range.right_overhang; + + //! max index is the last index of path point + const auto max_index = static_cast(path.points.size() - 1); + //! avoid bug with same point polygon + const double eps = 1e-3; + auto nearest_idx = + calcPointIndexFromSegmentIndex(path.points, target_pose.position, target_seg_idx); + if (max_index == nearest_idx) return false; // case of path point is not enough size + auto p0 = path.points.at(nearest_idx).point; + auto first_idx = nearest_idx + 1; + + // use ego point as start point if same point as ego is not in the path + const auto dist_to_nearest = + std::fabs(calcSignedArcLength(path.points, target_pose.position, target_seg_idx, nearest_idx)); + if (dist_to_nearest > eps) { + // interpolate ego point + const auto & pp = path.points; + const double ds = calc_distance2d(pp.at(target_seg_idx), pp.at(target_seg_idx + 1)); + const double dist_to_target_seg = + calcSignedArcLength(path.points, target_seg_idx, target_pose.position, target_seg_idx); + const double ratio = dist_to_target_seg / ds; + p0 = getLerpPathPointWithLaneId( + pp.at(target_seg_idx).point, pp.at(target_seg_idx + 1).point, ratio); + + // new first index should be ahead of p0 + first_idx = target_seg_idx + 1; + } + + double ttc = 0.0; + double dist_sum = 0.0; + double length = 0; + // initial point of detection area polygon + LineString2d left_inner_bound = {calculateOffsetPoint2d(p0.pose, min_len, offset_left)}; + LineString2d left_outer_bound = {calculateOffsetPoint2d(p0.pose, min_len, offset_left + eps)}; + LineString2d right_inner_bound = {calculateOffsetPoint2d(p0.pose, min_len, -offset_right)}; + LineString2d right_outer_bound = {calculateOffsetPoint2d(p0.pose, min_len, -offset_right - eps)}; + for (size_t s = first_idx; s <= max_index; s++) { + const auto p1 = path.points.at(s).point; + const double ds = calc_distance2d(p0, p1); + dist_sum += ds; + length += ds; + // calculate the distance that obstacles can move until ego reach the trajectory point + const double v_average = 0.5 * (p0.longitudinal_velocity_mps + p1.longitudinal_velocity_mps); + const double v = std::max(v_average, min_velocity); + const double dt = ds / v; + ttc += dt; + + // for offset calculation + const double max_lateral_distance_right = + std::min(max_dst, offset_right + ttc * obstacle_vel_mps + eps); + const double max_lateral_distance_left = + std::min(max_dst, offset_left + ttc * obstacle_vel_mps + eps); + + // left bound + if (da_range.use_left) { + left_inner_bound.emplace_back(calculateOffsetPoint2d(p1.pose, min_len, offset_left)); + left_outer_bound.emplace_back( + calculateOffsetPoint2d(p1.pose, min_len, max_lateral_distance_left)); + } + // right bound + if (da_range.use_right) { + right_inner_bound.emplace_back(calculateOffsetPoint2d(p1.pose, min_len, -offset_right)); + right_outer_bound.emplace_back( + calculateOffsetPoint2d(p1.pose, min_len, -max_lateral_distance_right)); + } + // replace previous point with next point + p0 = p1; + // separate detection area polygon with fixed interval or at the end of detection max length + if (length > interval || max_len < dist_sum || s == max_index) { + if (left_inner_bound.size() > 1) + da_polys.emplace_back(lines2polygon(left_inner_bound, left_outer_bound)); + if (right_inner_bound.size() > 1) + da_polys.emplace_back(lines2polygon(right_outer_bound, right_inner_bound)); + left_inner_bound = {left_inner_bound.back()}; + left_outer_bound = {left_outer_bound.back()}; + right_inner_bound = {right_inner_bound.back()}; + right_outer_bound = {right_outer_bound.back()}; + length = 0; + if (max_len < dist_sum || s == max_index) return true; + } + } + return true; +} + +void extractClosePartition( + const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, + BasicPolygons2d & close_partition, const double distance_thresh) +{ + close_partition.clear(); + for (const auto & p : all_partitions) { + if (boost::geometry::distance(Point2d(position.x, position.y), p) < distance_thresh) { + close_partition.emplace_back(p); + } + } +} + +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr & ll, BasicPolygons2d & polys) +{ + const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll); + for (const auto & partition : partitions) { + lanelet::BasicLineString2d line; + for (const auto & p : partition) { + line.emplace_back(lanelet::BasicPoint2d{p.x(), p.y()}); + } + // correct line to calculate distance in accurate + boost::geometry::correct(line); + polys.emplace_back(line); + } +} + +void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input) +{ + for (size_t i = begin_idx; i < input->points.size(); ++i) { + input->points.at(i).point.longitudinal_velocity_mps = + std::min(static_cast(vel), input->points.at(i).point.longitudinal_velocity_mps); + } +} + +void insertVelocity( + PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, + size_t & insert_index, const double min_distance) +{ + bool already_has_path_point = false; + // consider front/back point is near to insert point or not + int min_idx = std::max(0, static_cast(insert_index - 1)); + int max_idx = + std::min(static_cast(insert_index + 1), static_cast(path.points.size() - 1)); + for (int i = min_idx; i <= max_idx; i++) { + if (calc_distance2d(path.points.at(static_cast(i)), path_point) < min_distance) { + path.points.at(i).point.longitudinal_velocity_mps = v; + already_has_path_point = true; + insert_index = static_cast(i); + // set velocity from is going to insert min velocity later + break; + } + } + //! insert velocity point only if there is no close point on path + if (!already_has_path_point) { + path.points.insert(path.points.begin() + insert_index, path_point); + } + // set zero velocity from insert index + setVelocityFromIndex(insert_index, v, &path); +} + +bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) +{ + geometry_msgs::msg::Pose p = transformRelCoordinate2D(target, origin); + const bool is_target_ahead = (p.position.x > 0.0); + return is_target_ahead; +} + +geometry_msgs::msg::Pose getAheadPose( + const size_t start_idx, const double ahead_dist, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) +{ + if (path.points.empty()) { + return geometry_msgs::msg::Pose{}; + } + + double curr_dist = 0.0; + double prev_dist = 0.0; + for (size_t i = start_idx; i < path.points.size() - 1; ++i) { + const geometry_msgs::msg::Pose p0 = path.points.at(i).point.pose; + const geometry_msgs::msg::Pose p1 = path.points.at(i + 1).point.pose; + curr_dist += autoware_utils::calc_distance2d(p0, p1); + if (curr_dist > ahead_dist) { + const double dl = std::max(curr_dist - prev_dist, 0.0001 /* avoid 0 divide */); + const double w_p0 = (curr_dist - ahead_dist) / dl; + const double w_p1 = (ahead_dist - prev_dist) / dl; + geometry_msgs::msg::Pose p; + p.position.x = w_p0 * p0.position.x + w_p1 * p1.position.x; + p.position.y = w_p0 * p0.position.y + w_p1 * p1.position.y; + p.position.z = w_p0 * p0.position.z + w_p1 * p1.position.z; + tf2::Quaternion q0_tf; + tf2::Quaternion q1_tf; + tf2::fromMsg(p0.orientation, q0_tf); + tf2::fromMsg(p1.orientation, q1_tf); + p.orientation = tf2::toMsg(q0_tf.slerp(q1_tf, w_p1)); + return p; + } + prev_dist = curr_dist; + } + return path.points.back().point.pose; +} + +Polygon2d generatePathPolygon( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width) +{ + Polygon2d ego_area; // open polygon + for (size_t i = start_idx; i <= end_idx; ++i) { + const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); + const double x = path.points.at(i).point.pose.position.x + width * std::sin(yaw); + const double y = path.points.at(i).point.pose.position.y - width * std::cos(yaw); + ego_area.outer().push_back(Point2d(x, y)); + } + for (size_t i = end_idx; i >= start_idx; --i) { + const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); + const double x = path.points.at(i).point.pose.position.x - width * std::sin(yaw); + const double y = path.points.at(i).point.pose.position.y + width * std::cos(yaw); + ego_area.outer().push_back(Point2d(x, y)); + } + + bg::correct(ego_area); + return ego_area; +} + +double calcJudgeLineDistWithAccLimit( + const double velocity, const double max_stop_acceleration, const double delay_response_time) +{ + double judge_line_dist = + (velocity * velocity) / (2.0 * (-max_stop_acceleration)) + delay_response_time * velocity; + return judge_line_dist; +} + +double calcJudgeLineDistWithJerkLimit( + const double velocity, const double acceleration, const double max_stop_acceleration, + const double max_stop_jerk, const double delay_response_time) +{ + if (velocity <= 0.0) { + return 0.0; + } + + /* t0: subscribe traffic light state and decide to stop */ + /* t1: braking start (with jerk limitation) */ + /* t2: reach max stop acceleration */ + /* t3: stop */ + + const double t1 = delay_response_time; + const double x1 = velocity * t1; + + const double v2 = velocity + (std::pow(max_stop_acceleration, 2) - std::pow(acceleration, 2)) / + (2.0 * max_stop_jerk); + + if (v2 <= 0.0) { + const double t2 = -1.0 * + (max_stop_acceleration + + std::sqrt(acceleration * acceleration - 2.0 * max_stop_jerk * velocity)) / + max_stop_jerk; + const double x2 = + velocity * t2 + acceleration * std::pow(t2, 2) / 2.0 + max_stop_jerk * std::pow(t2, 3) / 6.0; + return std::max(0.0, x1 + x2); + } + + const double t2 = (max_stop_acceleration - acceleration) / max_stop_jerk; + const double x2 = + velocity * t2 + acceleration * std::pow(t2, 2) / 2.0 + max_stop_jerk * std::pow(t2, 3) / 6.0; + + const double x3 = -1.0 * std::pow(v2, 2) / (2.0 * max_stop_acceleration); + return std::max(0.0, x1 + x2 + x3); +} + +double findReachTime( + const double jerk, const double accel, const double velocity, const double distance, + const double t_min, const double t_max) +{ + const double j = jerk; + const double a = accel; + const double v = velocity; + const double d = distance; + const double min = t_min; + const double max = t_max; + auto f = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + if (f(min, j, a, v, d) > 0 || f(max, j, a, v, d) < 0) { + throw std::logic_error("[behavior_velocity](findReachTime): search range is invalid"); + } + const double eps = 1e-5; + const int warn_iter = 100; + double lower = min; + double upper = max; + double t = NAN; + int iter = 0; + while (true) { + t = 0.5 * (lower + upper); + const double fx = f(t, j, a, v, d); + // std::cout<<"fx: "< 0.0) { + upper = t; + } else { + lower = t; + } + iter++; + if (iter > warn_iter) + std::cerr << "[behavior_velocity](findReachTime): current iter is over warning" << std::endl; + } + // std::cout<<"iter: "< 0 || max_slowdown_accel > 0) { + throw std::logic_error("max_slowdown_jerk and max_slowdown_accel should be negative"); + } + // case0: distance to target is behind ego + if (distance_to_target <= 0) return current_velocity; + auto ft = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + auto vt = [](const double t, const double j, const double a, const double v) { + return j * t * t / 2.0 + a * t + v; + }; + const double j_max = max_slowdown_jerk; + const double a0 = current_accel; + const double a_max = max_slowdown_accel; + const double v0 = current_velocity; + const double l = distance_to_target; + const double t_const_jerk = (a_max - a0) / j_max; + const double d_const_jerk_stop = ft(t_const_jerk, j_max, a0, v0, 0.0); + const double d_const_acc_stop = l - d_const_jerk_stop; + + if (d_const_acc_stop < 0) { + // case0: distance to target is within constant jerk deceleration + // use binary search instead of solving cubic equation + const double t_jerk = findReachTime(j_max, a0, v0, l, 0, t_const_jerk); + const double velocity = vt(t_jerk, j_max, a0, v0); + return velocity; + } + + const double v1 = vt(t_const_jerk, j_max, a0, v0); + const double discriminant_of_stop = 2.0 * a_max * d_const_acc_stop + v1 * v1; + // case3: distance to target is farther than distance to stop + if (discriminant_of_stop <= 0) { + return 0.0; + } + // case2: distance to target is within constant accel deceleration + // solve d = 0.5*a^2+v*t by t + const double t_acc = (-v1 + std::sqrt(discriminant_of_stop)) / a_max; + return vt(t_acc, 0.0, a_max, v1); +} + +std::vector toRosPoints(const PredictedObjects & object) +{ + std::vector points; + for (const auto & obj : object.objects) { + points.emplace_back(obj.kinematics.initial_pose_with_covariance.pose.position); + } + return points; +} + +LineString2d extendLine( + const lanelet::ConstPoint3d & lanelet_point1, const lanelet::ConstPoint3d & lanelet_point2, + const double & length) +{ + const Eigen::Vector2d p1(lanelet_point1.x(), lanelet_point1.y()); + const Eigen::Vector2d p2(lanelet_point2.x(), lanelet_point2.y()); + const Eigen::Vector2d t = (p2 - p1).normalized(); + return { + {(p1 - length * t).x(), (p1 - length * t).y()}, {(p2 + length * t).x(), (p2 + length * t).y()}}; +} + +std::optional getNearestLaneId( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + lanelet::ConstLanelets lanes; + const auto lane_ids = getSortedLaneIdsFromPath(path); + for (const auto & lane_id : lane_ids) { + lanes.push_back(lanelet_map->laneletLayer.get(lane_id)); + } + + lanelet::Lanelet closest_lane; + if (lanelet::utils::query::getClosestLanelet(lanes, current_pose, &closest_lane)) { + return closest_lane.id(); + } + return std::nullopt; +} + +std::vector getLaneletsOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + const auto nearest_lane_id = getNearestLaneId(path, lanelet_map, current_pose); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + // Add subsequent lane_ids from nearest lane_id + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); + } else { + // Add all lane_ids in path + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + } + + std::vector lanelets; + lanelets.reserve(unique_lane_ids.size()); + for (const auto lane_id : unique_lane_ids) { + lanelets.push_back(lanelet_map->laneletLayer.get(lane_id)); + } + + return lanelets; +} + +std::set getLaneIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::set lane_id_set; + for (const auto & lane : getLaneletsOnPath(path, lanelet_map, current_pose)) { + lane_id_set.insert(lane.id()); + } + + return lane_id_set; +} + +std::vector getSortedLaneIdsFromPath(const PathWithLaneId & path) +{ + std::vector sorted_lane_ids; + for (const auto & path_points : path.points) { + for (const auto lane_id : path_points.lane_ids) + if ( + std::find(sorted_lane_ids.begin(), sorted_lane_ids.end(), lane_id) == + sorted_lane_ids.end()) { + sorted_lane_ids.emplace_back(lane_id); + } + } + return sorted_lane_ids; +} + +std::vector getSubsequentLaneIdsSetOnPath( + const PathWithLaneId & path, int64_t base_lane_id) +{ + const auto all_lane_ids = getSortedLaneIdsFromPath(path); + const auto base_index = std::find(all_lane_ids.begin(), all_lane_ids.end(), base_lane_id); + + // cannot find base_index in all_lane_ids + if (base_index == all_lane_ids.end()) { + return {}; + } + + std::vector subsequent_lane_ids; + + std::copy(base_index, all_lane_ids.end(), std::back_inserter(subsequent_lane_ids)); + return subsequent_lane_ids; +} + +// TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex +bool isOverLine( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, + const double offset) +{ + return autoware::motion_utils::calcSignedArcLength( + path.points, self_pose.position, line_pose.position) + + offset < + 0.0; +} + +std::optional insertDecelPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output, + const float target_velocity) +{ + // TODO(tanaka): consider proper overlap threshold for inserting decel point + const double overlap_threshold = 5e-2; + // TODO(murooka): remove this function for u-turn and crossing-path + const size_t base_idx = + autoware::motion_utils::findNearestSegmentIndex(output.points, stop_point); + const auto insert_idx = autoware::motion_utils::insertTargetPoint( + base_idx, stop_point, output.points, overlap_threshold); + + if (!insert_idx) { + return {}; + } + + for (size_t i = insert_idx.value(); i < output.points.size(); ++i) { + const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps; + output.points.at(i).point.longitudinal_velocity_mps = + std::min(original_velocity, target_velocity); + } + return autoware_utils::get_pose(output.points.at(insert_idx.value())); +} + +// TODO(murooka): remove this function for u-turn and crossing-path +std::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output) +{ + const size_t base_idx = + autoware::motion_utils::findNearestSegmentIndex(output.points, stop_point); + const auto insert_idx = + autoware::motion_utils::insertStopPoint(base_idx, stop_point, output.points); + + if (!insert_idx) { + return {}; + } + + return autoware_utils::get_pose(output.points.at(insert_idx.value())); +} + +std::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output) +{ + const auto insert_idx = + autoware::motion_utils::insertStopPoint(stop_seg_idx, stop_point, output.points); + + if (!insert_idx) { + return {}; + } + + return autoware_utils::get_pose(output.points.at(insert_idx.value())); +} + +std::set getAssociativeIntersectionLanelets( + const lanelet::ConstLanelet & lane, const lanelet::LaneletMapPtr lanelet_map, + const lanelet::routing::RoutingGraphPtr routing_graph) +{ + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + if (turn_direction == "else") { + return {}; + } + + const auto parents = routing_graph->previous(lane); + std::set parent_neighbors; + for (const auto & parent : parents) { + const auto neighbors = routing_graph->besides(parent); + for (const auto & neighbor : neighbors) parent_neighbors.insert(neighbor.id()); + } + std::set associative_intersection_lanelets; + associative_intersection_lanelets.insert(lane.id()); + for (const auto & parent_neighbor_id : parent_neighbors) { + const auto parent_neighbor = lanelet_map->laneletLayer.get(parent_neighbor_id); + const auto followings = routing_graph->following(parent_neighbor); + for (const auto & following : followings) { + if (following.attributeOr("turn_direction", "else") == turn_direction) { + associative_intersection_lanelets.insert(following.id()); + } + } + } + return associative_intersection_lanelets; +} + +lanelet::ConstLanelets getConstLaneletsFromIds( + const lanelet::LaneletMapConstPtr & map, const std::set & ids) +{ + lanelet::ConstLanelets ret{}; + for (const auto & id : ids) { + const auto ll = map->laneletLayer.get(id); + ret.push_back(ll); + } + return ret; +} + +} // namespace autoware::behavior_velocity_planner::planning_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp new file mode 100644 index 0000000000..db9a63169e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -0,0 +1,242 @@ +// Copyright 2021 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 "utils.hpp" + +#include +#include + +#include + +#include + +using PathIndexWithPoint2d = + autoware::behavior_velocity_planner::arc_lane_utils::PathIndexWithPoint2d; +using LineString2d = autoware::behavior_velocity_planner::LineString2d; +using Point2d = autoware::behavior_velocity_planner::Point2d; +namespace arc_lane_utils = autoware::behavior_velocity_planner::arc_lane_utils; + +namespace +{ +geometry_msgs::msg::Point createPoint(const double x, const double y, const double z) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + p.z = z; + return p; +} +} // namespace + +TEST(findCollisionSegment, nominal) +{ + /** + * find forward collision segment by stop line + * s + * | = | = | = | s | = | + * 0 1 2 3 s 4 5 + * + **/ + auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + + LineString2d stop_line; + stop_line.emplace_back(Point2d(3.5, 3.0)); + stop_line.emplace_back(Point2d(3.5, -3.0)); + auto segment = arc_lane_utils::findCollisionSegment(path, stop_line); + EXPECT_EQ(segment->first, static_cast(3)); + EXPECT_DOUBLE_EQ(segment->second.x, 3.5); + EXPECT_DOUBLE_EQ(segment->second.y, 0.0); +} + +TEST(findOffsetSegment, case_forward_offset_segment) +{ + auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + /** + * find offset segment forward + * | = | = | = | c | o | + * 0 1 2 3 4 5 + **/ + const int collision_segment_idx = 3; + const auto collision_point = createPoint(3.5, 0.0, 0.0); + const auto & collision_segment = std::make_pair(collision_segment_idx, collision_point); + // nominal + { + double offset_length = 1.0; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + const auto front_idx = offset_segment->first; + EXPECT_EQ(front_idx, static_cast(4)); + EXPECT_DOUBLE_EQ(offset_segment->second, 0.5); + } + // boundary condition + { + double offset_length = INFINITY; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + EXPECT_FALSE(offset_segment); + } +} + +TEST(findOffsetSegment, case_backward_offset_segment) +{ + auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + /** + * find offset segment forward + * | = | = | o | c | = | + * 0 1 2 3 4 5 + **/ + const int collision_segment_idx = 3; + const auto collision_point = createPoint(3.5, 0.0, 0.0); + const auto & collision_segment = std::make_pair(collision_segment_idx, collision_point); + // nominal + { + double offset_length = -1.0; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + const auto front_idx = offset_segment->first; + EXPECT_EQ(front_idx, static_cast(2)); + EXPECT_DOUBLE_EQ(offset_segment->second, 0.5); + } + // boundary condition + { + double offset_length = -INFINITY; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + EXPECT_FALSE(offset_segment); + } +} + +TEST(checkCollision, various_cases) +{ + using autoware::behavior_velocity_planner::arc_lane_utils::checkCollision; + constexpr double epsilon = 1e-6; + + { // normal case with collision + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // normal case without collision + const auto p1 = createPoint(1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, std::nullopt); + } + + { // normal case without collision + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, std::nullopt); + } + + { // line and point + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, std::nullopt); + } + + { // point and line + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(-1.0, 0.0, 0.0); + const auto p4 = createPoint(2.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, std::nullopt); + } + + { // collision with edges + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, std::nullopt); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp new file mode 100644 index 0000000000..51c6b5b9dd --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp @@ -0,0 +1,245 @@ +// Copyright 2021 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 "autoware/motion_utils/trajectory/trajectory.hpp" +#include "utils.hpp" + +#include +#include +#include + +#include + +#include +#include + +#define DEBUG_PRINT_PATH(path) \ + { \ + std::stringstream ss; \ + ss << #path << "(px, vx): "; \ + for (const auto p : path.points) { \ + ss << "(" << p.pose.position.x << ", " << p.longitudinal_velocity_mps << "), "; \ + } \ + std::cerr << ss.str() << std::endl; \ + } + +TEST(is_ahead_of, nominal) +{ + using autoware::behavior_velocity_planner::planning_utils::isAheadOf; + geometry_msgs::msg::Pose target = test::generatePose(0); + geometry_msgs::msg::Pose origin = test::generatePose(1); + bool is_ahead = isAheadOf(target, origin); + EXPECT_FALSE(is_ahead); + target = test::generatePose(2); + is_ahead = isAheadOf(target, origin); + EXPECT_TRUE(is_ahead); +} + +TEST(smoothDeceleration, calculateMaxSlowDownVelocity) +{ + using autoware::behavior_velocity_planner::planning_utils:: + calcDecelerationVelocityFromDistanceToTarget; + const double current_accel = 1.0; + const double current_velocity = 5.0; + const double max_slow_down_jerk = -1.0; + const double max_slow_down_accel = -2.0; + const double eps = 1e-3; + { + for (int i = -8; i <= 24; i += 8) { + // arc length in path point + const double l = i * 1.0; + const double v = calcDecelerationVelocityFromDistanceToTarget( + max_slow_down_jerk, max_slow_down_accel, current_accel, current_velocity, l); + // case 0 : behind ego + if (i == -8) EXPECT_NEAR(v, 5.0, eps); + // case 1 : const jerk + else if (i == 0) + EXPECT_NEAR(v, 5.0, eps); + // case 1 : const jerk + else if (i == 8) + EXPECT_NEAR(v, 5.380, eps); + // case 2 : const accel + else if (i == 16) + EXPECT_NEAR(v, 2.872, eps); + // case 3 : after stop + else if (i == 24) + EXPECT_NEAR(v, 0.00, eps); + else + continue; + std::cout << "s: " << l << " v: " << v << std::endl; + } + } +} + +TEST(specialInterpolation, specialInterpolation) +{ + using autoware::behavior_velocity_planner::interpolatePath; + using autoware::motion_utils::calcSignedArcLength; + using autoware::motion_utils::searchZeroVelocityIndex; + using autoware_planning_msgs::msg::Path; + using autoware_planning_msgs::msg::PathPoint; + + const auto genPath = [](const auto p, const auto v) { + if (p.size() != v.size()) throw std::invalid_argument("different size is not expected"); + Path path; + for (size_t i = 0; i < p.size(); ++i) { + PathPoint pp; + pp.pose.position.x = p.at(i); + pp.longitudinal_velocity_mps = v.at(i); + path.points.push_back(pp); + } + return path; + }; + + constexpr auto length = 5.0; + constexpr auto interval = 1.0; + + const auto calcInterpolatedStopDist = [&](const auto & px, const auto & vx) { + const auto path = genPath(px, vx); + const auto res = interpolatePath(path, length, interval); + // DEBUG_PRINT_PATH(path); + // DEBUG_PRINT_PATH(res); + return calcSignedArcLength(res.points, 0, *searchZeroVelocityIndex(res.points)); + }; + + // expected stop position: s=2.0 + { + const std::vector px{0.0, 1.0, 2.0, 3.0}; + const std::vector vx{5.5, 5.5, 0.0, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), 2.0); + } + + // expected stop position: s=2.1 + { + constexpr auto expected = 2.1; + const std::vector px{0.0, 1.0, 2.1, 3.0}; + const std::vector vx{5.5, 5.5, 0.0, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=2.001 + { + constexpr auto expected = 2.001; + const std::vector px{0.0, 1.0, 2.001, 3.0}; + const std::vector vx{5.5, 5.5, 0.000, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=2.001 + { + constexpr auto expected = 2.001; + const std::vector px{0.0, 1.0, 1.999, 2.0, 2.001, 3.0}; + const std::vector vx{5.5, 5.5, 5.555, 5.5, 0.000, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=2.0 + { + constexpr auto expected = 2.0; + const std::vector px{0.0, 1.0, 1.999, 2.0, 2.001, 3.0}; + const std::vector vx{5.5, 5.5, 5.555, 0.0, 0.000, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=1.999 + { + constexpr auto expected = 1.999; + const std::vector px{0.0, 1.0, 1.999, 3.0}; + const std::vector vx{5.5, 5.5, 0.000, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=0.2 + { + constexpr auto expected = 0.2; + const std::vector px{0.0, 0.1, 0.2, 0.3, 0.4}; + const std::vector vx{5.5, 5.5, 0.0, 0.0, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } + + // expected stop position: s=0.4 + { + constexpr auto expected = 0.4; + const std::vector px{0.0, 0.1, 0.2, 0.3, 0.4}; + const std::vector vx{5.5, 5.5, 5.5, 5.5, 0.0}; + EXPECT_DOUBLE_EQ(calcInterpolatedStopDist(px, vx), expected); + } +} + +TEST(filterLitterPathPoint, nominal) +{ + using autoware::behavior_velocity_planner::filterLitterPathPoint; + using autoware_planning_msgs::msg::Path; + using autoware_planning_msgs::msg::PathPoint; + + const auto genPath = [](const std::vector & px, const std::vector & vx) { + Path path; + for (size_t i = 0; i < px.size(); ++i) { + PathPoint point; + point.pose.position.x = px[i]; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = static_cast(vx[i]); + path.points.push_back(point); + } + return path; + }; + + const std::vector px{0.0, 1.0, 1.001, 2.0, 3.0}; + const std::vector vx{5.0, 3.5, 3.5, 3.0, 2.5}; + + const auto path = genPath(px, vx); + const auto filtered_path = filterLitterPathPoint(path); + + ASSERT_EQ(filtered_path.points.size(), 4U); // Expected: Points at x = {0.0, 1.0, 2.0, 3.0} + EXPECT_DOUBLE_EQ(filtered_path.points[0].pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(filtered_path.points[1].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(filtered_path.points[2].pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(filtered_path.points[3].pose.position.x, 3.0); + + EXPECT_DOUBLE_EQ(filtered_path.points[0].longitudinal_velocity_mps, 5.0); + EXPECT_DOUBLE_EQ(filtered_path.points[1].longitudinal_velocity_mps, 3.5); + EXPECT_DOUBLE_EQ(filtered_path.points[2].longitudinal_velocity_mps, 3.0); + EXPECT_DOUBLE_EQ(filtered_path.points[3].longitudinal_velocity_mps, 2.5); +} + +TEST(filterStopPathPoint, nominal) +{ + using autoware::behavior_velocity_planner::filterStopPathPoint; + using autoware_planning_msgs::msg::Path; + using autoware_planning_msgs::msg::PathPoint; + + const auto genPath = [](const std::vector & px, const std::vector & vx) { + Path path; + for (size_t i = 0; i < px.size(); ++i) { + PathPoint point; + point.pose.position.x = px[i]; + point.longitudinal_velocity_mps = static_cast(vx[i]); + path.points.push_back(point); + } + return path; + }; + + const std::vector px{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector vx{5.0, 4.0, 0.0, 2.0, 3.0}; + + const auto path = genPath(px, vx); + const auto filtered_path = filterStopPathPoint(path); + + ASSERT_EQ(filtered_path.points.size(), 5U); + EXPECT_DOUBLE_EQ(filtered_path.points[0].longitudinal_velocity_mps, 5.0); + EXPECT_DOUBLE_EQ(filtered_path.points[1].longitudinal_velocity_mps, 4.0); + EXPECT_DOUBLE_EQ(filtered_path.points[2].longitudinal_velocity_mps, 0.0); + EXPECT_DOUBLE_EQ(filtered_path.points[3].longitudinal_velocity_mps, 0.0); + EXPECT_DOUBLE_EQ(filtered_path.points[4].longitudinal_velocity_mps, 0.0); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp new file mode 100644 index 0000000000..37ab955b9c --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp @@ -0,0 +1,87 @@ +// Copyright 2021 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 "utils.hpp" + +#include +#include + +#include + +#include +#include +#include + +using StateMachine = autoware::behavior_velocity_planner::StateMachine; +using State = autoware::behavior_velocity_planner::StateMachine::State; + +int enumToInt(State s) +{ + return static_cast(s); +} + +TEST(state_machine, on_initialized) +{ + StateMachine state_machine = StateMachine(); + EXPECT_EQ(enumToInt(State::GO), enumToInt(state_machine.getState())); +} + +TEST(state_machine, set_state_stop) +{ + StateMachine state_machine = StateMachine(); + state_machine.setState(State::STOP); + EXPECT_EQ(enumToInt(State::STOP), enumToInt(state_machine.getState())); +} + +TEST(state_machine, set_state_stop_with_margin_time) +{ + StateMachine state_machine = StateMachine(); + const double margin_time = 1.0; + state_machine.setMarginTime(margin_time); + rclcpp::Clock current_time = rclcpp::Clock(RCL_ROS_TIME); + rclcpp::Logger logger = rclcpp::get_logger("test_set_state_with_margin_time"); + // DO NOT SET GO until margin time past + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::GO)); + state_machine.setStateWithMarginTime(State::STOP, logger, current_time); + // set STOP immediately when stop is set + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::STOP)); +} + +TEST(state_machine, set_state_go_with_margin_time) +{ + StateMachine state_machine = StateMachine(); + const double margin_time = 0.2; + state_machine.setMarginTime(margin_time); + rclcpp::Logger logger = rclcpp::get_logger("test_set_state_with_margin_time"); + state_machine.setState(State::STOP); + size_t loop_counter = 0; + // loop until state change from STOP -> GO + while (state_machine.getState() == State::STOP) { + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::STOP)); + rclcpp::Clock current_time = rclcpp::Clock(RCL_ROS_TIME); + if (state_machine.getDuration() > margin_time) { + std::cerr << "stop duration is larger than margin time" << std::endl; + } + EXPECT_FALSE(state_machine.getDuration() > margin_time); + state_machine.setStateWithMarginTime(State::GO, logger, current_time); + loop_counter++; + } + // time past STOP -> GO + if (loop_counter > 2) { + EXPECT_TRUE(state_machine.getDuration() > margin_time); + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::GO)); + } else { + std::cerr << "[Warning] computational resource is not enough" << std::endl; + } +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp new file mode 100644 index 0000000000..ae4d248120 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp @@ -0,0 +1,113 @@ +// 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 "autoware/behavior_velocity_planner_common/utilization/trajectory_utils.hpp" +#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" + +#include +#include + +#include +#include + +#include + +#include + +TEST(smoothPath, nominal) +{ + using autoware::behavior_velocity_planner::smoothPath; + using autoware_internal_planning_msgs::msg::PathPointWithLaneId; + using autoware_internal_planning_msgs::msg::PathWithLaneId; + + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + options.arguments( + {"--ros-args", "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common") + + "/config/behavior_velocity_planner_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_vehicle_info.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother") + + "/config/default_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother") + + "/config/default_velocity_smoother.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother") + + "/config/JerkFiltered.param.yaml"}); + auto node = std::make_shared("test_node", options); + + auto planner_data = std::make_shared(*node); + planner_data->stop_line_extend_length = 5.0; + planner_data->vehicle_info_.max_longitudinal_offset_m = 1.0; + + planner_data->current_odometry = std::make_shared([]() { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 2.0; + pose.pose.position.y = 1.0; + return pose; + }()); + + planner_data->current_velocity = std::make_shared([]() { + geometry_msgs::msg::TwistStamped twist; + twist.twist.linear.x = 3.0; + return twist; + }()); + + planner_data->current_acceleration = + std::make_shared([]() { + geometry_msgs::msg::AccelWithCovarianceStamped accel; + accel.accel.accel.linear.x = 1.0; + return accel; + }()); + + planner_data->velocity_smoother_ = + std::make_shared( + *node, std::make_shared()); + + // Input path + PathWithLaneId in_path; + for (double i = 0; i <= 10.0; i += 1.0) { + PathPointWithLaneId point; + point.point.pose.position.x = i; + point.point.pose.position.y = 0.0; + point.point.longitudinal_velocity_mps = 5.0; // Set constant velocity + in_path.points.emplace_back(point); + } + + // Output path + PathWithLaneId out_path; + + // Execute smoothing + auto result = smoothPath(in_path, out_path, planner_data); + + // Check results + EXPECT_TRUE(result); + + // Check initial and last points + EXPECT_DOUBLE_EQ(out_path.points.front().point.pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(out_path.points.front().point.pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(out_path.points.back().point.pose.position.x, 10.0); + EXPECT_DOUBLE_EQ(out_path.points.back().point.pose.position.y, 0.0); + + for (auto & point : out_path.points) { + // Check velocities + EXPECT_LE( + point.point.longitudinal_velocity_mps, + 5.0); // Smoothed velocity must not exceed initial + } +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp new file mode 100644 index 0000000000..bc5abee0ca --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp @@ -0,0 +1,308 @@ +// 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 "./utils.hpp" +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" + +#include +#include +#include +#include +#include + +#include + +using namespace autoware::behavior_velocity_planner; // NOLINT +using namespace autoware::behavior_velocity_planner::planning_utils; // NOLINT +using autoware_planning_msgs::msg::PathPoint; + +TEST(PlanningUtilsTest, calcSegmentIndexFromPointIndex) +{ + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point point; + point.x = 4.5; + point.y = 0.0; + + size_t result = calcSegmentIndexFromPointIndex(path.points, point, 4); + + EXPECT_EQ(result, 4); +} + +TEST(PlanningUtilsTest, calculateOffsetPoint2d) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + pose.orientation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1)); // No rotation + + double offset_x = 1.0; + double offset_y = 1.0; + + auto result = calculateOffsetPoint2d(pose, offset_x, offset_y); + + EXPECT_NEAR(result.x(), 1.0, 0.1); + EXPECT_NEAR(result.y(), 1.0, 0.1); +} + +TEST(PlanningUtilsTest, createDetectionAreaPolygons) +{ + // using namespace autoware::behavior_velocity_planner::planning_utils; + + // Input parameters + Polygons2d da_polys; + autoware_internal_planning_msgs::msg::PathWithLaneId path; + geometry_msgs::msg::Pose target_pose; + size_t target_seg_idx = 0; + autoware::behavior_velocity_planner::DetectionRange da_range; + + da_range.min_longitudinal_distance = 1.0; + da_range.max_longitudinal_distance = 10.0; + da_range.max_lateral_distance = 2.0; + da_range.interval = 5.0; + da_range.wheel_tread = 1.0; + da_range.left_overhang = 0.5; + da_range.right_overhang = 0.5; + da_range.use_left = true; + da_range.use_right = true; + + double obstacle_vel_mps = 0.5; + double min_velocity = 1.0; + + // Path with some points + for (double i = 0.0; i < 3.0; ++i) { + autoware_internal_planning_msgs::msg::PathPointWithLaneId point; + point.point.pose.position.x = i * 5.0; + point.point.pose.position.y = 0.0; + point.point.longitudinal_velocity_mps = 1.0; + path.points.push_back(point); + } + + // Target pose + target_pose.position.x = 0.0; + target_pose.position.y = 0.0; + + // Call the function + bool success = createDetectionAreaPolygons( + da_polys, path, target_pose, target_seg_idx, da_range, obstacle_vel_mps, min_velocity); + + // Assert success + EXPECT_TRUE(success); + + // Validate results + ASSERT_FALSE(da_polys.empty()); + EXPECT_EQ(da_polys.size(), 2); // Expect polygons for left and right bounds + + // Check the first polygon + auto & polygon = da_polys.front(); + EXPECT_EQ(polygon.outer().size(), 7); // Each polygon should be a rectangle + + // Check some specific points + EXPECT_NEAR(polygon.outer()[0].x(), 1.0, 0.1); // Left inner bound + EXPECT_NEAR(polygon.outer()[0].y(), 1.0, 0.1); +} + +// Test for calcJudgeLineDistWithAccLimit +TEST(PlanningUtilsTest, calcJudgeLineDistWithAccLimit) +{ + double velocity = 10.0; // m/s + double max_stop_acceleration = -3.0; // m/s^2 + double delay_response_time = 1.0; // s + + double result = + calcJudgeLineDistWithAccLimit(velocity, max_stop_acceleration, delay_response_time); + + EXPECT_NEAR(result, 26.67, 0.01); // Updated expected value +} + +// Test for calcJudgeLineDistWithJerkLimit +TEST(PlanningUtilsTest, calcJudgeLineDistWithJerkLimit) +{ + double velocity = 10.0; // m/s + double acceleration = 0.0; // m/s^2 + double max_stop_acceleration = -3.0; // m/s^2 + double max_stop_jerk = -1.0; // m/s^3 + double delay_response_time = 1.0; // s + + double result = calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); + + EXPECT_GT(result, 0.0); // The result should be positive +} + +// Test for isAheadOf +TEST(PlanningUtilsTest, isAheadOf) +{ + geometry_msgs::msg::Pose target; + geometry_msgs::msg::Pose origin; + target.position.x = 10.0; + target.position.y = 0.0; + origin.position.x = 0.0; + origin.position.y = 0.0; + origin.orientation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1)); // No rotation + + EXPECT_TRUE(isAheadOf(target, origin)); + + target.position.x = -10.0; + EXPECT_FALSE(isAheadOf(target, origin)); +} + +TEST(PlanningUtilsTest, insertDecelPoint) +{ + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point stop_point; + stop_point.x = 5.0; + stop_point.y = 0.0; + + auto stop_pose = insertDecelPoint(stop_point, path, 5.0); + ASSERT_TRUE(stop_pose.has_value()); + EXPECT_NEAR(stop_pose->position.x, 5.0, 0.1); +} + +// Test for insertVelocity +TEST(PlanningUtilsTest, insertVelocity) +{ + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point; + path_point.point.pose.position.x = 5.0; + path_point.point.pose.position.y = 0.0; + path_point.point.longitudinal_velocity_mps = 10.0; + + size_t insert_index = 5; + insertVelocity(path, path_point, 10.0, insert_index); + + EXPECT_EQ(path.points.size(), 11); + EXPECT_NEAR(path.points.at(insert_index).point.longitudinal_velocity_mps, 10.0, 0.1); +} + +// Test for insertStopPoint +TEST(PlanningUtilsTest, insertStopPoint) +{ + { + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point stop_point; + stop_point.x = 5.0; + stop_point.y = 0.0; + + auto stop_pose = insertStopPoint(stop_point, path); + ASSERT_TRUE(stop_pose.has_value()); + EXPECT_NEAR(stop_pose->position.x, 5.0, 0.1); + } + { + auto path = test::generatePath(0.0, 0.0, 10.0, 0.0, 10); + geometry_msgs::msg::Point stop_point; + stop_point.x = 5.0; + stop_point.y = 0.0; + + auto stop_pose = insertStopPoint(stop_point, 4, path); + ASSERT_TRUE(stop_pose.has_value()); + EXPECT_NEAR(stop_pose->position.x, 5.0, 0.1); + } +} + +// Test for getAheadPose +TEST(PlanningUtilsTest, getAheadPose) +{ + autoware_internal_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point1; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point2; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point3; + point1.point.pose.position.x = 0.0; + point2.point.pose.position.x = 5.0; + point3.point.pose.position.x = 10.0; + + path.points.emplace_back(point1); + path.points.emplace_back(point2); + path.points.emplace_back(point3); + + double ahead_dist = 7.0; + auto pose = getAheadPose(0, ahead_dist, path); + + EXPECT_NEAR(pose.position.x, 7.0, 0.1); +} + +TEST(PlanningUtilsTest, calcDecelerationVelocityFromDistanceToTarget) +{ + double max_slowdown_jerk = -1.0; // m/s^3 + double max_slowdown_accel = -3.0; // m/s^2 + double current_accel = -1.0; // m/s^2 + double current_velocity = 10.0; // m/s + double distance_to_target = 100.0; // m + + double result = calcDecelerationVelocityFromDistanceToTarget( + max_slowdown_jerk, max_slowdown_accel, current_accel, current_velocity, distance_to_target); + + EXPECT_LT(result, current_velocity); +} + +// Test for toRosPoints +TEST(PlanningUtilsTest, ToRosPoints) +{ + using autoware_perception_msgs::msg::PredictedObject; + PredictedObjects objects; + + // Add a predicted object + PredictedObject obj1; + obj1.kinematics.initial_pose_with_covariance.pose.position.x = 1.0; + obj1.kinematics.initial_pose_with_covariance.pose.position.y = 2.0; + obj1.kinematics.initial_pose_with_covariance.pose.position.z = 3.0; + objects.objects.push_back(obj1); + + // Add another predicted object + PredictedObject obj2; + obj2.kinematics.initial_pose_with_covariance.pose.position.x = 4.0; + obj2.kinematics.initial_pose_with_covariance.pose.position.y = 5.0; + obj2.kinematics.initial_pose_with_covariance.pose.position.z = 6.0; + objects.objects.push_back(obj2); + + auto points = toRosPoints(objects); + + ASSERT_EQ(points.size(), 2); // Verify the number of points + EXPECT_EQ(points[0].x, 1.0); + EXPECT_EQ(points[0].y, 2.0); + EXPECT_EQ(points[0].z, 3.0); + EXPECT_EQ(points[1].x, 4.0); + EXPECT_EQ(points[1].y, 5.0); + EXPECT_EQ(points[1].z, 6.0); +} + +// Test for extendLine +TEST(PlanningUtilsTest, ExtendLine) +{ + lanelet::ConstPoint3d point1(lanelet::InvalId, 0.0, 0.0, 0.0); + lanelet::ConstPoint3d point2(lanelet::InvalId, 1.0, 1.0, 0.0); + double length = 1.0; + + auto extended_line = extendLine(point1, point2, length); + + ASSERT_EQ(extended_line.size(), 2); // Verify the line has two points + + // Check the extended line coordinates + EXPECT_NEAR(extended_line[0].x(), -0.707, 0.001); // Extended in the reverse direction + EXPECT_NEAR(extended_line[0].y(), -0.707, 0.001); + EXPECT_NEAR(extended_line[1].x(), 1.707, 0.001); // Extended in the forward direction + EXPECT_NEAR(extended_line[1].y(), 1.707, 0.001); +} + +TEST(PlanningUtilsTest, getConstLaneletsFromIds) +{ + const auto package_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); + lanelet::LaneletMapPtr map = + autoware::test_utils::loadMap(package_dir + "/test_map/lanelet2_map.osm"); + + auto lanelets = getConstLaneletsFromIds(map, {10333, 10310, 10291}); + + EXPECT_EQ(lanelets.size(), 3); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp new file mode 100644 index 0000000000..f61bec186e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/utils.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include + +#include + +namespace test +{ + +inline autoware_internal_planning_msgs::msg::PathWithLaneId generatePath( + double x0, double y0, double x, double y, int nb_points) +{ + autoware_internal_planning_msgs::msg::PathWithLaneId path{}; + double x_step = (x - x0) / (nb_points - 1); + double y_step = (y - y0) / (nb_points - 1); + for (int i = 0; i < nb_points; ++i) { + autoware_internal_planning_msgs::msg::PathPointWithLaneId point{}; + point.point.pose.position.x = x0 + x_step * i; + point.point.pose.position.y = y0 + y_step * i; + point.point.pose.position.z = 0.0; + path.points.push_back(point); + } + return path; +} + +inline geometry_msgs::msg::Pose generatePose(double x) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = 0.0; + p.position.z = 1.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0); + p.orientation = tf2::toMsg(q); + return p; +} + +} // namespace test + +#endif // UTILS_HPP_ From b7f5229011f927f77cb5142c4b8fa8d0a2071bfc Mon Sep 17 00:00:00 2001 From: suchang Date: Fri, 28 Feb 2025 10:48:05 +0800 Subject: [PATCH 06/19] feat: remove useless dependecy Signed-off-by: suchang --- .../autoware_behavior_velocity_planner_common/package.xml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index a66faa2e0a..f4bdea2b1c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_planner_common - 0.41.0 + 0.0.0 The autoware_behavior_velocity_planner_common package Tomoya Kimura @@ -28,7 +28,7 @@ autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_factor_interface - autoware_planning_msgs + autoware_internal_planning_msgs autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils @@ -45,7 +45,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_planning_msgs visualization_msgs ament_cmake_ros From be1b6daeed50cd2cb950d317524879bd41909daf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 28 Feb 2025 02:54:29 +0000 Subject: [PATCH 07/19] style(pre-commit): autofix --- .../scene_module_interface.hpp | 4 ++-- .../utilization/arc_lane_util.hpp | 11 ++++------- .../package.xml | 2 +- .../src/utilization/debug.cpp | 6 ++++-- .../src/utilization/util.cpp | 10 ++++------ 5 files changed, 15 insertions(+), 18 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 3b5ab16c3f..2340770c62 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -50,11 +50,11 @@ namespace autoware::behavior_velocity_planner using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_utils::DebugPublisher; using autoware_utils::get_or_declare_parameter; using autoware_utils::StopWatch; -using autoware_internal_debug_msgs::msg::Float64Stamped; -using autoware_internal_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; using unique_identifier_msgs::msg::UUID; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 215419ee4f..ce84a4512b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -40,9 +40,8 @@ inline geometry_msgs::msg::Point convertToGeomPoint(const autoware_utils::Point2 namespace arc_lane_utils { -using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = - std::pair; // front index, point2d +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d using PathIndexWithPoint = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset @@ -60,8 +59,7 @@ std::optional findCollisionSegment( const geometry_msgs::msg::Point & stop_line_p2) { for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & p1 = - autoware_utils::get_point(path.points.at(i)); // Point before collision point + const auto & p1 = autoware_utils::get_point(path.points.at(i)); // Point before collision point const auto & p2 = autoware_utils::get_point(path.points.at(i + 1)); // Point after collision point @@ -117,8 +115,7 @@ std::optional findBackwardOffsetSegment( double sum_length = 0.0; const auto start = static_cast(base_idx) - 1; for (std::int32_t i = start; i >= 0; --i) { - sum_length += - autoware_utils::calc_distance2d(path.points.at(i), path.points.at(i + 1)); + sum_length += autoware_utils::calc_distance2d(path.points.at(i), path.points.at(i + 1)); // If it's over offset point, return front index and remain offset length /** diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index f4bdea2b1c..4e44d334cf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_internal_debug_msgs + autoware_internal_planning_msgs autoware_interpolation autoware_lanelet2_extension autoware_map_msgs @@ -28,7 +29,6 @@ autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_factor_interface - autoware_internal_planning_msgs autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index fd6a61a0ba..36319f3b43 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -34,7 +34,8 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( auto marker = create_default_marker( "map", now, ns, static_cast(module_id), visualization_msgs::msg::Marker::LINE_STRIP, create_marker_scale(static_cast(x), static_cast(y), static_cast(z)), - create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.8f)); + create_marker_color( + static_cast(r), static_cast(g), static_cast(b), 0.8f)); marker.lifetime = rclcpp::Duration::from_seconds(0.3); for (const auto & p : polygon.points) { @@ -116,7 +117,8 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( auto marker = create_default_marker( "map", now, ns, 0, visualization_msgs::msg::Marker::SPHERE, create_marker_scale(x, y, z), - create_marker_color(static_cast(r), static_cast(g), static_cast(b), 0.999f)); + create_marker_color( + static_cast(r), static_cast(g), static_cast(b), 0.999f)); marker.lifetime = rclcpp::Duration::from_seconds(0.3); for (size_t i = 0; i < points.size(); ++i) { marker.id = static_cast(i + planning_utils::bitShift(module_id)); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index 4bf6d98e31..a11ca779b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -16,8 +16,8 @@ #include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware_utils/geometry/geometry.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -49,10 +49,8 @@ size_t calcPointIndexFromSegmentIndex( const size_t prev_point_idx = seg_idx; const size_t next_point_idx = seg_idx + 1; - const double prev_dist = - autoware_utils::calc_distance2d(point, points.at(prev_point_idx)); - const double next_dist = - autoware_utils::calc_distance2d(point, points.at(next_point_idx)); + const double prev_dist = autoware_utils::calc_distance2d(point, points.at(prev_point_idx)); + const double next_dist = autoware_utils::calc_distance2d(point, points.at(next_point_idx)); if (prev_dist < next_dist) { return prev_point_idx; @@ -98,9 +96,9 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( namespace autoware::behavior_velocity_planner::planning_utils { using autoware::motion_utils::calcSignedArcLength; +using autoware_planning_msgs::msg::PathPoint; using autoware_utils::calc_distance2d; using autoware_utils::calc_offset_pose; -using autoware_planning_msgs::msg::PathPoint; size_t calcSegmentIndexFromPointIndex( const std::vector & points, From aa452087c5ac554743464a8688f338a8a7d54552 Mon Sep 17 00:00:00 2001 From: suchang Date: Tue, 4 Mar 2025 09:39:46 +0800 Subject: [PATCH 08/19] feat: modify autoware_universe_utils to autoware_utils Signed-off-by: suchang --- .../CMakeLists.txt | 26 ++ .../README.md | 110 ++++++++ .../config/stop_line.param.yaml | 6 + .../docs/calculate_stop_pose.drawio.svg | 99 +++++++ .../docs/find_collision_segment.drawio.svg | 99 +++++++ .../docs/find_offset_segment.drawio.svg | 143 ++++++++++ .../docs/keep_stopping.svg | 91 +++++++ .../docs/node_and_segment.drawio.svg | 251 ++++++++++++++++++ .../docs/restart.svg | 93 +++++++ .../docs/restart_prevention.svg | 131 +++++++++ .../docs/stop_line.svg | 151 +++++++++++ .../package.xml | 44 +++ .../plugins.xml | 3 + .../src/debug.cpp | 37 +++ .../src/manager.cpp | 109 ++++++++ .../src/manager.hpp | 66 +++++ .../src/scene.cpp | 172 ++++++++++++ .../src/scene.hpp | 121 +++++++++ .../test/test_node_interface.cpp | 75 ++++++ .../test/test_scene.cpp | 146 ++++++++++ 20 files changed, 1973 insertions(+) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/stop_line.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/plugins.xml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt new file mode 100644 index 0000000000..a187b4cda9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_stop_line_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_scene.cpp + test/test_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md new file mode 100644 index 0000000000..963d75e5a8 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md @@ -0,0 +1,110 @@ +# Stop Line + +## Role + +This module plans the vehicle's velocity to ensure it stops just before stop lines and can resume movement after stopping. + +![stop line](docs/stop_line.svg) + +## Activation Timing + +This module is activated when there is a stop line in a target lane. + +## Module Parameters + +| Parameter | Type | Description | +| ------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_margin` | double | Margin that the vehicle tries to stop in before stop_line | +| `stop_duration_sec` | double | [s] Time parameter for the ego vehicle to stop before stop line | +| `hold_stop_margin_distance` | double | [m] Parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | +| `use_initialization_stop_state` | bool | Flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | + +## Inner-workings / Algorithms + +- Gets a stop line from map information. +- Inserts a stop point on the path from the stop line defined in the map and the ego vehicle length. +- Sets velocities of the path after the stop point to 0[m/s]. +- Releases the inserted stop velocity when the vehicle halts at the stop point for `stop_duration_sec` seconds. + +### Flowchart + +```plantuml +@startuml +title modifyPathVelocity +start + +:find collision between path and stop_line; + +if (collision is found?) then (yes) +else (no) + stop +endif + +:find offset segment; + +:calculate stop pose; + +:calculate distance to stop line; + +if (state is APPROACH) then (yes) + :set stop velocity; + + if (vehicle is within hold_stop_margin_distance?) then (yes) + if (vehicle is stopped?) then (yes) + :change state to STOPPED; + endif + endif +else if (state is STOPPED) then (yes) + if (stopping time is longer than stop_duration_sec ?) then (yes) + :change state to START; + endif +else if (state is START) then (yes) + if ([optional] far from stop line?) then (yes) + :change state to APPROACH; + endif +endif + +stop +@enduml +``` + +This algorithm is based on `segment`. +`segment` consists of two node points. It's useful for removing boundary conditions because if `segment(i)` exists we can assume `node(i)` and `node(i+1)` exist. + +![node_and_segment](docs/./node_and_segment.drawio.svg) + +First, this algorithm finds a collision between reference path and stop line. +Then, we can get `collision segment` and `collision point`. + +![find_collision_segment](docs/./find_collision_segment.drawio.svg) + +Next, based on `collision point`, it finds `offset segment` by iterating backward points up to a specific offset length. +The offset length is `stop_margin`(parameter) + `base_link to front`(to adjust head pose to stop line). +Then, we can get `offset segment` and `offset from segment start`. + +![find_offset_segment](docs/./find_offset_segment.drawio.svg) + +After that, we can calculate an offset point from `offset segment` and `offset`. This will be `stop_pose`. + +![calculate_stop_pose](docs/./calculate_stop_pose.drawio.svg) + +### Restart Prevention + +If the vehicle requires X meters (e.g. 0.5 meters) to stop once it starts moving due to poor vehicle control performance, it may overshoot the stopping position, which must be strictly observed. This happens when the vehicle begins to move in order to approach a nearby stop point (e.g. 0.3 meters away). + +This module includes the parameter `hold_stop_margin_distance` to prevent redundant restarts in such a situation. If the vehicle is stopped within `hold_stop_margin_distance` meters of the stop point (\_front_to_stop_line < hold_stop_margin_distance), the module determines that the vehicle has already stopped at the stop point and will maintain the current stopping position, even if the vehicle has come to a stop due to other factors. + +
+ ![example](docs/restart_prevention.svg){width=1000} +
parameters
+
+ +
+ ![example](docs/restart.svg){width=1000} +
outside the hold_stop_margin_distance
+
+ +
+ ![example](docs/keep_stopping.svg){width=1000} +
inside the hold_stop_margin_distance
+
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml new file mode 100644 index 0000000000..f5e00fa708 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + stop_line: + stop_margin: 0.0 + stop_duration_sec: 1.0 + hold_stop_margin_distance: 2.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg new file mode 100644 index 0000000000..a4cc2451d5 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ offset segment +
+
+
+ + offset segment + + + + + + + + +
+
+
+ offset point +
+ = stop pose +
+
+
+ + offset point... + + + + + + +
+
+
+ offset from segment start +
+
+
+ + offset from segme... + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg new file mode 100644 index 0000000000..fee520e242 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop line +
+
+
+ + stop line + + + + + + + + +
+
+
+ collision segment +
+
+
+ + collision segment + + + + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg new file mode 100644 index 0000000000..aed520b236 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg @@ -0,0 +1,143 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + + + + + +
+
+
+ stop margin +
+
+
+ + stop margin + + + + + + + + +
+
+
+ base_link to front +
+
+
+ + base_link to front + + + + + + + + +
+
+
+ offset segment +
+
+
+ + offset segment + + + + + + +
+
+
+ offset from segment start +
+
+
+ + offset from segme... + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg new file mode 100644 index 0000000000..e6233f7fd0 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + +
+
+
+ keep stopping at current position +
+
+
+ + keep stopping at curr... + + + + + + +
+
+
+ target stop position +
+
+
+ + target stop position + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg new file mode 100644 index 0000000000..6cd47ffc14 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg @@ -0,0 +1,251 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
node 1
+
+
+ + node 1 + + + + + + +
+
+
node 2
+
+
+ + node 2 + + + + + + +
+
+
node 3
+
+
+ + node 3 + + + + + + +
+
+
node 4
+
+
+ + node 4 + + + + + + +
+
+
node 5
+
+
+ + node 5 + + + + + + +
+
+
node 6
+
+
+ + node 6 + + + + + + +
+
+
node 7
+
+
+ + node 7 + + + + + + +
+
+
+ segment 1 +
+
+
+ + segment 1 + + + + + + +
+
+
+ segment 3 +
+
+
+ + segment 3 + + + + + + +
+
+
+ segment 2 +
+
+
+ + segment 2 + + + + + + +
+
+
+ segment 4 +
+
+
+ + segment 4 + + + + + + +
+
+
+ segment 5 +
+
+
+ + segment 5 + + + + + + +
+
+
+ segment 6 +
+
+
+ + segment 6 + + + + + + +
+
+
+ segment(i) +
+ = node(i) + node(i+1) +
+ = all segments have two points +
+
+
+ + segment(i)... + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart.svg new file mode 100644 index 0000000000..d0ee659189 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart.svg @@ -0,0 +1,93 @@ + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + + + + + + +
+
+
+ move toward stop line +
+
+
+ + move toward stop line + + + + + + +
+
+
+ target stop position +
+
+
+ + target stop position + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg new file mode 100644 index 0000000000..160df47d81 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg @@ -0,0 +1,131 @@ + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + + + +
+
+
+ target stop position +
+
+
+ + target stop position + + + + + + + + + + + + +
+
+
+ hold_stop_margin_distance + [m] +
+
+
+ + hold_stop_margin_distance [m] + + + + + + +
+
+
+ front_to_stop_line + [m] +
+
+
+ + front_to_stop_line [... + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/stop_line.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/stop_line.svg new file mode 100644 index 0000000000..a090a90e2e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/stop_line.svg @@ -0,0 +1,151 @@ + + + + + + + + + + + + + + + + + +
+
+
+ +

+ l + [ + m + ] + : + base link to front +

+ +
+
+
+ + l [m]  : base link to front + + + + + + + +
+
+
+ Stop Line +
+
+
+ + Stop Line + + + + + + +
+
+
+ Stop Point +
+
+
+ + Stop Point + + + + + + + + + + + + + + +
+
+
+ velocity 0 +
+
+
+ + velocity 0 + + + + + +
+
+
+ 2[m] +
+
+
+ + 2[m] + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml new file mode 100644 index 0000000000..4f1ab8f9a1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml @@ -0,0 +1,44 @@ + + + + autoware_behavior_velocity_stop_line_module + 0.0.0 + The autoware_behavior_velocity_stop_line_module package + + Yukinari Hisaki + Satoshi Ota + Fumiya Watanabe + Zhe Shen + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Yutaka Shimizu + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_velocity_planner + autoware_behavior_velocity_planner_common + autoware_lanelet2_extension + autoware_motion_utils + autoware_internal_planning_msgs + autoware_route_handler + autoware_trajectory + eigen + geometry_msgs + pluginlib + rclcpp + tf2_geometry_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/plugins.xml new file mode 100644 index 0000000000..6765a4bc1b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp new file mode 100644 index 0000000000..cb29858bce --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp @@ -0,0 +1,37 @@ +// Copyright 2020 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 "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "scene.hpp" + +namespace autoware::behavior_velocity_planner +{ + +autoware::motion_utils::VirtualWalls StopLineModule::createVirtualWalls() +{ + autoware::motion_utils::VirtualWalls virtual_walls; + + if (debug_data_.stop_pose && (state_ == State::APPROACH || state_ == State::STOPPED)) { + autoware::motion_utils::VirtualWall wall; + wall.text = "stopline"; + wall.style = autoware::motion_utils::VirtualWallType::stop; + wall.ns = std::to_string(module_id_) + "_"; + wall.pose = autoware_utils::calc_offset_pose( + *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); + virtual_walls.push_back(wall); + } + return virtual_walls; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp new file mode 100644 index 0000000000..bd0c446b4e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -0,0 +1,109 @@ +// Copyright 2020 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 "manager.hpp" + +#include "autoware_utils/ros/parameter.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware_utils::get_or_declare_parameter; +using lanelet::TrafficSign; + +StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()), planner_param_() +{ + const std::string ns(StopLineModuleManager::getModuleName()); + auto & p = planner_param_; + p.stop_margin = get_or_declare_parameter(node, ns + ".stop_margin"); + p.hold_stop_margin_distance = + get_or_declare_parameter(node, ns + ".hold_stop_margin_distance"); + p.stop_duration_sec = get_or_declare_parameter(node, ns + ".stop_duration_sec"); +} + +std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::vector stop_lines_with_lane_id; + + for (const auto & m : planning_utils::getRegElemMapOnPath( + path, lanelet_map, planner_data_->current_odometry->pose)) { + const auto & traffic_sign_reg_elem = m.first; + const int64_t lane_id = m.second.id(); + // Is stop sign? + if (traffic_sign_reg_elem->type() != "stop_sign") { + continue; + } + + for (const auto & stop_line : traffic_sign_reg_elem->refLines()) { + stop_lines_with_lane_id.emplace_back(stop_line, lane_id); + } + } + + return stop_lines_with_lane_id; +} + +std::set StopLineModuleManager::getStopLineIdSetOnPath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::set stop_line_id_set; + + for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, lanelet_map)) { + stop_line_id_set.insert(stop_line_with_lane_id.first.id()); + } + + return stop_line_id_set; +} + +void StopLineModuleManager::launchNewModules( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & stop_line_with_lane_id : + getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { + const auto module_id = stop_line_with_lane_id.first.id(); + if (!isModuleRegistered(module_id)) { + registerModule(std::make_shared( + module_id, stop_line_with_lane_id.first, planner_param_, + logger_.get_child("stop_line_module"), clock_, time_keeper_, planning_factor_interface_)); + } + } +} + +std::function &)> +StopLineModuleManager::getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) +{ + const auto stop_line_id_set = + getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); + + return [stop_line_id_set](const std::shared_ptr & scene_module) { + return stop_line_id_set.count(scene_module->getModuleId()) == 0; + }; +} + +} // namespace autoware::behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::StopLineModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp new file mode 100644 index 0000000000..70ad0a6b8d --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp @@ -0,0 +1,66 @@ +// Copyright 2020 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "autoware/behavior_velocity_planner_common/plugin_wrapper.hpp" +#include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" +#include "scene.hpp" + +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using StopLineWithLaneId = std::pair; + +class StopLineModuleManager : public SceneModuleManagerInterface<> +{ +public: + explicit StopLineModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "stop_line"; } + +private: + StopLineModule::PlannerParam planner_param_; + + std::vector getStopLinesWithLaneIdOnPath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map); + + std::set getStopLineIdSetOnPath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map); + + void launchNewModules(const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class StopLineModulePlugin : public PluginWrapper +{ +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp new file mode 100644 index 0000000000..87b75b7126 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -0,0 +1,172 @@ +// Copyright 2020 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 "scene.hpp" + +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/trajectory/utils/closest.hpp" +#include "autoware/trajectory/utils/crossed.hpp" + +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +StopLineModule::StopLineModule( + const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr & time_keeper, + const std::shared_ptr & + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), + stop_line_(std::move(stop_line)), + planner_param_(planner_param), + state_(State::APPROACH), + debug_data_() +{ +} + +bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) +{ + auto trajectory = + trajectory::Trajectory::Builder{} + .build(path->points); + + if (!trajectory) { + return true; + } + + auto [ego_s, stop_point] = + getEgoAndStopPoint(*trajectory, planner_data_->current_odometry->pose, state_); + + if (!stop_point) { + return true; + } + + trajectory->longitudinal_velocity_mps().range(*stop_point, trajectory->length()).set(0.0); + + path->points = trajectory->restore(); + + // TODO(soblin): PlanningFactorInterface use trajectory class + planning_factor_interface_->add( + path->points, trajectory->compute(*stop_point).point.pose, + planner_data_->current_odometry->pose, planner_data_->current_odometry->pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); + + updateStateAndStoppedTime( + &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); + + geometry_msgs::msg::Pose stop_pose = trajectory->compute(*stop_point).point.pose; + + updateDebugData(&debug_data_, stop_pose, state_); + + return true; +} + +std::pair> StopLineModule::getEgoAndStopPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & ego_pose, + const State & state) const +{ + const double ego_s = autoware::trajectory::closest(trajectory, ego_pose); + std::optional stop_point_s; + + switch (state) { + case State::APPROACH: { + const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const LineString2d stop_line = planning_utils::extendLine( + stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); + + // Calculate intersection with stop line + const auto trajectory_stop_line_intersection = + autoware::trajectory::crossed(trajectory, stop_line); + + // If no collision found, do nothing + if (trajectory_stop_line_intersection.size() == 0) { + stop_point_s = std::nullopt; + break; + } + + stop_point_s = + trajectory_stop_line_intersection.at(0) - + (base_link2front + planner_param_.stop_margin); // consider vehicle length and stop margin + + if (*stop_point_s < 0.0) { + stop_point_s = std::nullopt; + } + break; + } + + case State::STOPPED: { + stop_point_s = ego_s; + break; + } + + case State::START: { + stop_point_s = std::nullopt; + break; + } + } + return {ego_s, stop_point_s}; +} + +void StopLineModule::updateStateAndStoppedTime( + State * state, std::optional * stopped_time, const rclcpp::Time & now, + const double & distance_to_stop_point, const bool & is_vehicle_stopped) const +{ + switch (*state) { + case State::APPROACH: { + if (distance_to_stop_point < planner_param_.hold_stop_margin_distance && is_vehicle_stopped) { + *state = State::STOPPED; + *stopped_time = now; + RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); + + if (distance_to_stop_point < 0.0) { + RCLCPP_WARN(logger_, "Vehicle cannot stop before stop line"); + } + } + break; + } + case State::STOPPED: { + double stop_duration = (now - **stopped_time).seconds(); + if (stop_duration > planner_param_.stop_duration_sec) { + *state = State::START; + stopped_time->reset(); + RCLCPP_INFO(logger_, "STOPPED -> START"); + } + break; + } + case State::START: { + break; + } + } +} + +void StopLineModule::updateDebugData( + DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const +{ + debug_data->base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + debug_data->stop_pose = stop_pose; + if (state == State::START) { + debug_data->stop_pose = std::nullopt; + } +} + +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp new file mode 100644 index 0000000000..7973e46db9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -0,0 +1,121 @@ +// Copyright 2020 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 SCENE_HPP_ +#define SCENE_HPP_ +#define EIGEN_MPL2_ONLY + +#include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" +#include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/trajectory/path_point_with_lane_id.hpp" + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +class StopLineModule : public SceneModuleInterface +{ +public: + using StopLineWithLaneId = std::pair; + using Trajectory = + autoware::trajectory::Trajectory; + enum class State { APPROACH, STOPPED, START }; + + struct DebugData + { + double base_link2front; ///< Distance from the base link to the vehicle front. + std::optional stop_pose; ///< Pose of the stop position. + }; + + struct PlannerParam + { + double stop_margin; ///< Margin to the stop line. + double stop_duration_sec; ///< Required stop duration at the stop line. + double + hold_stop_margin_distance; ///< Distance threshold for transitioning to the STOPPED state + }; + + /** + * @brief Constructor for StopLineModule. + * @param module_id Unique ID for the module. + * @param stop_line Stop line data. + * @param planner_param Planning parameters. + * @param logger Logger for output messages. + * @param clock Shared clock instance. + * @param time_keeper Time keeper for the module. + * @param planning_factor_interface Planning factor interface. + */ + StopLineModule( + const int64_t module_id, lanelet::ConstLineString3d stop_line, + const PlannerParam & planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr & time_keeper, + const std::shared_ptr & + planning_factor_interface); + + bool modifyPathVelocity(PathWithLaneId * path) override; + + /** + * @brief Calculate ego position and stop point. + * @param trajectory Current trajectory. + * @param ego_pose Current pose of the vehicle. + * @param state Current state of the stop line module. + * @return Pair of ego position and optional stop point. + */ + std::pair> getEgoAndStopPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & ego_pose, + const State & state) const; + + /** + * @brief Update the state and stopped time of the module. + * @param state Pointer to the current state. + * @param stopped_time Pointer to the stopped time. + * @param now Current time. + * @param distance_to_stop_point Distance to the stop point. + * @param is_vehicle_stopped Flag indicating if the vehicle is stopped. + */ + void updateStateAndStoppedTime( + State * state, std::optional * stopped_time, const rclcpp::Time & now, + const double & distance_to_stop_point, const bool & is_vehicle_stopped) const; + + void updateDebugData( + DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override + { + return visualization_msgs::msg::MarkerArray{}; + } + autoware::motion_utils::VirtualWalls createVirtualWalls() override; + +private: + const lanelet::ConstLineString3d stop_line_; ///< Stop line geometry. + const PlannerParam planner_param_; ///< Parameters for the planner. + State state_; ///< Current state of the module. + std::optional stopped_time_; ///< Time when the vehicle stopped. + DebugData debug_data_; ///< Debug information. +}; +} // namespace autoware::behavior_velocity_planner + +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp new file mode 100644 index 0000000000..ac158a0e59 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp @@ -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. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "stop_line", "autoware::behavior_velocity_planner::StopLineModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp new file mode 100644 index 0000000000..a98d66c6f7 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -0,0 +1,146 @@ +// 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 "../src/scene.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include +#include + +using autoware::behavior_velocity_planner::StopLineModule; + +autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point(double x, double y) +{ + autoware_internal_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = x; + p.point.pose.position.y = y; + return p; +} + +class StopLineModuleTest : public ::testing::Test +{ +protected: + // Set up the test case + void SetUp() override + { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + options.arguments( + {"--ros-args", "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common") + + "/config/behavior_velocity_planner_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_vehicle_info.param.yaml"}); + node_ = std::make_shared("test_node", options); + + // Initialize parameters, logger, and clock + planner_param_.stop_margin = 0.5; + planner_param_.stop_duration_sec = 2.0; + planner_param_.hold_stop_margin_distance = 0.5; + + planner_data_ = std::make_shared(*node_); + planner_data_->stop_line_extend_length = 5.0; + planner_data_->vehicle_info_.max_longitudinal_offset_m = 1.0; + + stop_line_ = lanelet::ConstLineString3d( + lanelet::utils::getId(), {lanelet::Point3d(lanelet::utils::getId(), 7.0, -1.0, 0.0), + lanelet::Point3d(lanelet::utils::getId(), 7.0, 1.0, 0.0)}); + + trajectory_ = *StopLineModule::Trajectory::Builder{}.build( + {path_point(0.0, 0.0), path_point(1.0, 0.0), path_point(2.0, 0.0), path_point(3.0, 0.0), + path_point(4.0, 0.0), path_point(5.0, 0.0), path_point(6.0, 0.0), path_point(7.0, 0.0), + path_point(8.0, 0.0), path_point(9.0, 0.0), path_point(10.0, 0.0)}); + + clock_ = std::make_shared(); + + module_ = std::make_shared( + 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_, + std::make_shared(), + std::make_shared( + node_.get(), "test_stopline")); + + module_->setPlannerData(planner_data_); + } + + void TearDown() override { rclcpp::shutdown(); } + + StopLineModule::Trajectory trajectory_; + StopLineModule::PlannerParam planner_param_{}; + lanelet::ConstLineString3d stop_line_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr module_; + std::shared_ptr planner_data_; + + rclcpp::Node::SharedPtr node_; +}; + +TEST_F(StopLineModuleTest, TestGetEgoAndStopPoint) +{ + // Prepare trajectory and other parameters + geometry_msgs::msg::Pose ego_pose; + ego_pose.position.x = 5.0; + ego_pose.position.y = 1.0; + + // Execute the function + auto [ego_s, stop_point_s] = + module_->getEgoAndStopPoint(trajectory_, ego_pose, StopLineModule::State::APPROACH); + + // Verify results + EXPECT_DOUBLE_EQ(ego_s, 5.0); + EXPECT_DOUBLE_EQ(stop_point_s.value(), 7.0 - 0.5 - 1.0); + + std::tie(ego_s, stop_point_s) = + module_->getEgoAndStopPoint(trajectory_, ego_pose, StopLineModule::State::STOPPED); + + EXPECT_DOUBLE_EQ(ego_s, 5.0); + EXPECT_DOUBLE_EQ(stop_point_s.value(), 5.0); +} + +TEST_F(StopLineModuleTest, TestUpdateStateAndStoppedTime) +{ + StopLineModule::State state = StopLineModule::State::APPROACH; + std::optional stopped_time; + double distance_to_stop_point = 0.1; + bool is_vehicle_stopped = true; + + // Simulate clock progression + auto test_start_time = clock_->now(); + stopped_time = test_start_time; + + // Execute the function + module_->updateStateAndStoppedTime( + &state, &stopped_time, test_start_time, distance_to_stop_point, is_vehicle_stopped); + + // Verify state transition to STOPPED + EXPECT_EQ(state, StopLineModule::State::STOPPED); + EXPECT_TRUE(stopped_time.has_value()); + + // Simulate time elapsed to exceed stop duration + auto now = test_start_time + rclcpp::Duration::from_seconds(3.0); + module_->updateStateAndStoppedTime( + &state, &stopped_time, now, distance_to_stop_point, is_vehicle_stopped); + + // Verify state transition to START + EXPECT_EQ(state, StopLineModule::State::START); + EXPECT_FALSE(stopped_time.has_value()); +} From 9b1424bec2dcbf2dd32e83b4e0785386e019ef60 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 4 Mar 2025 01:44:24 +0000 Subject: [PATCH 09/19] style(pre-commit): autofix --- .../autoware_behavior_velocity_stop_line_module/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml index 4f1ab8f9a1..e7f2f06210 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml @@ -22,9 +22,9 @@ autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_internal_planning_msgs autoware_lanelet2_extension autoware_motion_utils - autoware_internal_planning_msgs autoware_route_handler autoware_trajectory eigen From c224ef54f9b3510c71eb2cbdcc0b074068a5a12d Mon Sep 17 00:00:00 2001 From: suchang Date: Thu, 20 Mar 2025 14:49:23 +0800 Subject: [PATCH 10/19] feat: port autoware_behavior_velocity_planner to core Signed-off-by: suchang --- .../CMakeLists.txt | 33 + .../README.md | 69 + .../behavior_velocity_planner.param.yaml | 6 + ...iorVelocityPlanner-Architecture.drawio.svg | 2523 +++++++++++++++++ .../docs/set_stop_velocity.drawio.svg | 197 ++ .../behavior_velocity_planner/node.hpp | 145 + .../planner_manager.hpp | 60 + .../behavior_velocity_planner/test_utils.hpp | 47 + .../behavior_velocity_planner.launch.xml | 73 + .../package.xml | 69 + .../behavior_velocity_planner.schema.json | 53 + .../src/node.cpp | 402 +++ .../src/planner_manager.cpp | 91 + .../src/test_utils.cpp | 127 + .../test/test_node_interface.cpp | 72 + 15 files changed, 3967 insertions(+) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt new file mode 100644 index 0000000000..a520b02a43 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_planner) + +find_package(autoware_cmake REQUIRED) + +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + src/node.cpp + src/planner_manager.cpp + src/test_utils.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_lib + PLUGIN "autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME}_lib + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md new file mode 100644 index 0000000000..26d0e26f79 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md @@ -0,0 +1,69 @@ +# Behavior Velocity Planner + +## Overview + +`behavior_velocity_planner` is a planner that adjust velocity based on the traffic rules. +It loads modules as plugins. Please refer to the links listed below for detail on each module. + +![Architecture](./docs/BehaviorVelocityPlanner-Architecture.drawio.svg) + +- [Blind Spot](../autoware_behavior_velocity_blind_spot_module/README.md) +- [Crosswalk](../autoware_behavior_velocity_crosswalk_module/README.md) +- [Walkway](../autoware_behavior_velocity_walkway_module/README.md) +- [Detection Area](../autoware_behavior_velocity_detection_area_module/README.md) +- [Intersection](../autoware_behavior_velocity_intersection_module/README.md) +- [MergeFromPrivate](../autoware_behavior_velocity_intersection_module/README.md#merge-from-private) +- [Stop Line](../autoware_behavior_velocity_stop_line_module/README.md) +- [Virtual Traffic Light](../autoware_behavior_velocity_virtual_traffic_light_module/README.md) +- [Traffic Light](../autoware_behavior_velocity_traffic_light_module/README.md) +- [Occlusion Spot](../autoware_behavior_velocity_occlusion_spot_module/README.md) +- [No Stopping Area](../autoware_behavior_velocity_no_stopping_area_module/README.md) +- [Run Out](../autoware_behavior_velocity_run_out_module/README.md) +- [Speed Bump](../autoware_behavior_velocity_speed_bump_module/README.md) + +When each module plans velocity, it considers based on `base_link`(center of rear-wheel axis) pose. +So for example, in order to stop at a stop line with the vehicles' front on the stop line, it calculates `base_link` position from the distance between `base_link` to front and modifies path velocity from the `base_link` position. + +![set_stop_velocity](./docs/set_stop_velocity.drawio.svg) + +## Input topics + +| Name | Type | Description | +| ----------------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | +| `~input/path_with_lane_id` | autoware_internal_planning_msgs::msg::PathWithLaneId | path with lane_id | +| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | +| `~input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | +| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states | + +## Output topics + +| Name | Type | Description | +| ---------------------- | ----------------------------------------- | -------------------------------------- | +| `~output/path` | autoware_planning_msgs::msg::Path | path to be followed | +| `~output/stop_reasons` | tier4_planning_msgs::msg::StopReasonArray | reasons that cause the vehicle to stop | + +## Node parameters + +| Parameter | Type | Description | +| ---------------------- | -------------------- | ----------------------------------------------------------------------------------- | +| `launch_modules` | vector<string> | module names to launch | +| `forward_path_length` | double | forward path length | +| `backward_path_length` | double | backward path length | +| `max_accel` | double | (to be a global parameter) max acceleration of the vehicle | +| `system_delay` | double | (to be a global parameter) delay time until output control command | +| `delay_response_time` | double | (to be a global parameter) delay time of the vehicle's response to control commands | + +## Traffic Light Handling in sim/real + +The handling of traffic light information varies depending on the usage. In the below table, the traffic signal topic element for the corresponding lane is denoted as `info`, and if `info` is not available, it is denoted as `null`. + +| module \\ case | `info` is `null` | `info` is not `null` | +| :--------------------------------------------------------------------------------------------------------- | ------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| intersection_occlusion(`is_simulation = *`)
  • `info` is the latest non-`null` information
| GO(occlusion is ignored) | intersection_occlusion uses the latest non UNKNOWN observation in the queue up to present.
  • If `info` is `GREEN or UNKNOWN`, occlusion is cared
  • If `info` is `RED or YELLOW`, occlusion is ignored(GO)
  • NOTE: Currently timeout is not considered
| +| traffic_light(sim, `is_simulation = true`)
  • `info` is current information
| GO | traffic_light uses the perceived traffic light information at present directly.
  • If `info` is timeout, STOP whatever the color is
  • If `info` is not timeout, then act according to the color. If `info` is `UNKNOWN`, STOP
{: rowspan=2} | +| traffic_light(real, `is_simulation = false`)
  • `info` is current information
| STOP | ⁠ {: style="padding:0"} | +| crosswalk with Traffic Light(`is_simulation = *`)
  • `info` is current information
| default |
  • If `disable_yield_for_new_stopped_object` is true, each sub scene_module ignore newly detected pedestrians after module instantiation.
  • If `ignore_with_traffic_light` is true, occlusion detection is skipped.
| +| map_based_prediction(`is_simulation = *`)
  • `info` is current information
| default | If a pedestrian traffic light is
  • RED, surrounding pedestrians are not predicted.
  • GREEN, stopped pedestrians are not predicted.
| diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml new file mode 100644 index 0000000000..49749cd129 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + forward_path_length: 1000.0 + backward_path_length: 5.0 + behavior_output_path_interval: 1.0 + stop_line_extend_length: 5.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg new file mode 100644 index 0000000000..f48e1ad630 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg @@ -0,0 +1,2523 @@ + + + + + + + + + + + +
+
+
+ /perception/occupancy_grid_map/map +
+
+
+ + /per... + + + + + + + Blind Spot + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Turn RIght +
+
+
+ + Turn RIght + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Turn Left +
+
+
+ + Turn Left + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + Occlusion Spot + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Private Area +
+
+
+ + Private Area + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Public Area +
+
+
+ + Public Area + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + + Stopline + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Stopline 1 +
+
+
+ + Stopline 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Stopline 2 +
+
+
+ + Stopline 2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + + + + + + + + Detection Area + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Detection Area 1 +
+
+
+ + Detection Area 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Detection Area 2 +
+
+
+ + Detection Area 2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + Run out + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Run out 1 +
+
+
+ + Run out 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Run out 2 +
+
+
+ + Run out 2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + +
+
+
+ /perception/object_recognition/objects +
+
+
+ + /per... + + + + + + + +
+
+
+ /perception/prediction/objects +
+
+
+ + /per... + + + + + + + +
+
+
+ /vehicle/status/twist +
+
+
+ + /veh... + + + + + + + +
+
+
+ /planning/behavior_planning/path_with_lane_id +
+
+
+ + /pla... + + + + + + + Behavior Velocity Planner + + + + + +
+
+
+ UpdateModuleInstance +
+
+
+ + UpdateModuleInstance + + + + + + +
+
+
+ PlanVelocity +
+
+
+ + PlanVelocity + + + + + + +
+
+
+ RefinePath +
+
+
+ + RefinePath + + + + + + + +
+
+
+ path_with_lane_id +
+
+
+ + path_w... + + + + + + + +
+
+
+ path_with_lane_id +
+
+
+ + path_w... + + + + + + + +
+
+
+ path_with_lane_id +
+
+
+ + path_w... + + + + + + + +
+
+
+ Updated +
+ path_with_lane_id +
+
+
+ + Update... + + + + + + + +
+
+
+ Updated +
+ path_with_lane_id +
+
+
+ + Update... + + + + + + + +
+
+
+ Data +
+
+
+ + Data + + + + + + +
+
+
+ Planner Data +
+
+
+ + Planner Data + + + + + + + SceneManagers + + + + + + Crosswalk + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Crosswalk 1 +
+
+
+ + Crosswalk 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Crosswalk2 +
+
+
+ + Crosswalk2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + + + + + + + + Intersection + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Intersection 1 +
+
+
+ + Intersection 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Intersection 2 +
+
+
+ + Intersection 2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + Virtual Traffic Light + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Virtual Traffic Light 1 +
+
+
+ + Virtual Traffic Light 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Virtual Traffic Light 2 +
+
+
+ + Virtual Traffic Light 2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + Traffic LIght + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Traffic LIght 1 +
+
+
+ + Traffic LIght 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Traffic Light 2 +
+
+
+ + Traffic Light 2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + + + + No Stopping Area + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ No Stopping Area1 +
+
+
+ + No Stopping Area1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ No Stopping Area2 +
+
+
+ + No Stopping Area2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + Speed Bump + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Speed Bump 1 +
+
+
+ + Speed Bump 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Speed Bump 2 +
+
+
+ + Speed Bump 2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + No Drivable Lane + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ No Drivable Lanelet 1 +
+
+
+ + No Drivable Lanelet 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ No Drivable Lanelet 2 +
+
+
+ + No Drivable Lanelet 2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + + + + +
+
+
+ /map/semantic +
+
+
+ + /map... + + + + + + + +
+
+
+ /tf +
+
+
+ + /tf + + + + + + + +
+
+
+ /planning/behavior_planning/path +
+
+
+ + /pla... + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg new file mode 100644 index 0000000000..5ae7d7dbb5 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg @@ -0,0 +1,197 @@ + + + + + + + + + + + +
+
+
+ +

+ + stop point + +

+ +
+
+
+ + stop point + + + + + + + + + + + +
+
+
+ +

+ + v + +

+ +
+
+
+ + v + + + + + + +
+
+
+ +

+ + x + +

+ +
+
+
+ + x + + + + + + +
+
+
+ +

+ + baselink to front + +

+ +
+
+
+ + baselink to front + + + + + + + + + + + + + + +
+
+
+ +

+ + output velocity + +

+ +
+
+
+ + output velocity + + + + + + + +
+
+
+ +

+ + reference velocity + +

+ +
+
+
+ + reference velocity + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp new file mode 100644 index 0000000000..25dac976e1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -0,0 +1,145 @@ +// Copyright 2019 Autoware Foundation +// +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ + +#include "autoware/behavior_velocity_planner/planner_manager.hpp" +#include "autoware_utils/ros/logger_level_configure.hpp" +#include "autoware_utils/ros/polling_subscriber.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware_internal_planning_msgs::msg::VelocityLimit; +using autoware_map_msgs::msg::LaneletMapBin; + +class BehaviorVelocityPlannerNode : public rclcpp::Node +{ +public: + explicit BehaviorVelocityPlannerNode(const rclcpp::NodeOptions & node_options); + +private: + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // subscriber + rclcpp::Subscription::SharedPtr + trigger_sub_path_with_lane_id_; + + // polling subscribers + autoware_utils::InterProcessPollingSubscriber + sub_predicted_objects_{this, "~/input/dynamic_objects"}; + + autoware_utils::InterProcessPollingSubscriber + sub_no_ground_pointcloud_{ + this, "~/input/no_ground_pointcloud", autoware_utils::single_depth_sensor_qos()}; + + autoware_utils::InterProcessPollingSubscriber sub_vehicle_odometry_{ + this, "~/input/vehicle_odometry"}; + + autoware_utils::InterProcessPollingSubscriber + sub_acceleration_{this, "~/input/accel"}; + + autoware_utils::InterProcessPollingSubscriber< + autoware_perception_msgs::msg::TrafficLightGroupArray> + sub_traffic_signals_{this, "~/input/traffic_signals"}; + + autoware_utils::InterProcessPollingSubscriber sub_occupancy_grid_{ + this, "~/input/occupancy_grid"}; + + autoware_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware_utils::polling_policy::Newest> + sub_lanelet_map_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; + + autoware_utils::InterProcessPollingSubscriber sub_external_velocity_limit_{ + this, "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local()}; + + void onTrigger( + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); + + void onParam(); + + void processNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void processOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void processTrafficSignals( + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); + bool processData(rclcpp::Clock clock); + + // publisher + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + + void publishDebugMarker(const autoware_planning_msgs::msg::Path & path); + + // parameter + double forward_path_length_; + double backward_path_length_; + double behavior_output_path_interval_; + + // member + PlannerData planner_data_; + BehaviorVelocityPlannerManager planner_manager_; + bool is_driving_forward_{true}; + + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; + void onUnloadPlugin( + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); + void onLoadPlugin( + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); + + // mutex for planner_data_ + std::mutex mutex_; + + // function + bool isDataReady(rclcpp::Clock clock); + autoware_planning_msgs::msg::Path generatePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, + const PlannerData & planner_data); + + std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; + + static constexpr int logger_throttle_interval = 3000; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp new file mode 100644 index 0000000000..42d014f9af --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp @@ -0,0 +1,60 @@ +// Copyright 2019 Autoware Foundation +// +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +class BehaviorVelocityPlannerManager +{ +public: + BehaviorVelocityPlannerManager(); + void launchScenePlugin(rclcpp::Node & node, const std::string & name); + void removeScenePlugin(rclcpp::Node & node, const std::string & name); + + // cppcheck-suppress functionConst + autoware_internal_planning_msgs::msg::PathWithLaneId planPathVelocity( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path_msg); + +private: + pluginlib::ClassLoader plugin_loader_; + std::vector> scene_manager_plugins_; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp new file mode 100644 index 0000000000..2fb08f2ac7 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp @@ -0,0 +1,47 @@ +// 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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ + +#include "autoware/behavior_velocity_planner/node.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; +using autoware::planning_test_manager::PlanningInterfaceTestManager; + +struct PluginInfo +{ + std::string module_name; // e.g. crosswalk + std::string plugin_name; // e.g. autoware::behavior_velocity_planner::CrosswalkModulePlugin +}; + +std::shared_ptr generateTestManager(); + +std::shared_ptr generateNode( + const std::vector & plugin_info_vec); + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml new file mode 100644 index 0000000000..399762f0b8 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml new file mode 100644 index 0000000000..37825eebb2 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -0,0 +1,69 @@ + + + + autoware_behavior_velocity_planner + 0.0.0 + The autoware_behavior_velocity_planner package + + Mamoru Sobue + Takayuki Murooka + Satoshi Ota + Kyoichi Sugahara + Taiki Tanaka + Kosuke Takeuchi + Tomohito Ando + Makoto Kurihara + Maxime Clement + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Taiki Tanaka + Mamoru Sobue + Satoshi Ota + Kyoichi Sugahara + Kosuke Takeuchi + Yutaka Shimizu + Tomohito Ando + Yukihiro Saito + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs + autoware_internal_planning_msgs + autoware_lanelet2_extension + autoware_map_msgs + autoware_motion_utils + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_utils + autoware_velocity_smoother + diagnostic_msgs + eigen + geometry_msgs + libboost-dev + pcl_conversions + pluginlib + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json new file mode 100644 index 0000000000..80044d5c6a --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json @@ -0,0 +1,53 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Behavior Velocity Planner", + "type": "object", + "definitions": { + "behavior_velocity_planner": { + "type": "object", + "properties": { + "forward_path_length": { + "type": "number", + "default": "1000.0", + "description": "forward path" + }, + "backward_path_length": { + "type": "number", + "default": "5.0", + "description": "backward path" + }, + "behavior_output_path_interval": { + "type": "number", + "default": "1.0", + "description": "the output path will be interpolated by this interval" + }, + "stop_line_extend_length": { + "type": "number", + "default": "5.0", + "description": "extend length of stop line" + } + }, + "required": [ + "forward_path_length", + "behavior_output_path_interval", + "backward_path_length", + "stop_line_extend_length" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/behavior_velocity_planner" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp new file mode 100644 index 0000000000..709d5813a6 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -0,0 +1,402 @@ +// Copyright 2020 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 "autoware/behavior_velocity_planner/node.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +namespace +{ + +autoware_planning_msgs::msg::Path to_path( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path_with_id) +{ + autoware_planning_msgs::msg::Path path; + for (const auto & path_point : path_with_id.points) { + path.points.push_back(path_point.point); + } + return path; +} +} // namespace + +BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptions & node_options) +: Node("behavior_velocity_planner_node", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + planner_data_(*this) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + // Trigger Subscriber + trigger_sub_path_with_lane_id_ = + this->create_subscription( + "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1)); + + srv_load_plugin_ = create_service( + "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); + srv_unload_plugin_ = create_service( + "~/service/unload_plugin", + std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2)); + + // set velocity smoother param + onParam(); + + // Publishers + path_pub_ = this->create_publisher("~/output/path", 1); + debug_viz_pub_ = this->create_publisher("~/debug/path", 1); + + // Parameters + forward_path_length_ = declare_parameter("forward_path_length"); + backward_path_length_ = declare_parameter("backward_path_length"); + behavior_output_path_interval_ = declare_parameter("behavior_output_path_interval"); + planner_data_.stop_line_extend_length = declare_parameter("stop_line_extend_length"); + + // nearest search + planner_data_.ego_nearest_dist_threshold = + declare_parameter("ego_nearest_dist_threshold"); + planner_data_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + + // is simulation or not + planner_data_.is_simulation = declare_parameter("is_simulation"); + + // Initialize PlannerManager + for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } + planner_manager_.launchScenePlugin(*this, name); + } + + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); +} + +void BehaviorVelocityPlannerNode::onLoadPlugin( + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.launchScenePlugin(*this, request->data); +} + +void BehaviorVelocityPlannerNode::onUnloadPlugin( + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.removeScenePlugin(*this, request->data); +} + +void BehaviorVelocityPlannerNode::onParam() +{ + // Note(VRichardJP): mutex lock is not necessary as onParam is only called once in the + // constructed. It would be required if it was a callback. std::lock_guard + // lock(mutex_); + planner_data_.velocity_smoother_ = + std::make_unique(*this); + planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); +} + +void BehaviorVelocityPlannerNode::processNoGroundPointCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_.lookupTransform( + "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & e) { + RCLCPP_WARN(get_logger(), "no transform found for no_ground_pointcloud: %s", e.what()); + return; + } + + pcl::PointCloud pc; + pcl::fromROSMsg(*msg, pc); + + Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); + pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); + if (!pc.empty()) { + autoware_utils::transform_pointcloud(pc, *pc_transformed, affine); + } + + planner_data_.no_ground_pointcloud = pc_transformed; +} + +void BehaviorVelocityPlannerNode::processOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + auto current_odometry = std::make_shared(); + current_odometry->header = msg->header; + current_odometry->pose = msg->pose.pose; + planner_data_.current_odometry = current_odometry; + + auto current_velocity = std::make_shared(); + current_velocity->header = msg->header; + current_velocity->twist = msg->twist.twist; + planner_data_.current_velocity = current_velocity; + + // Add velocity to buffer + planner_data_.velocity_buffer.push_front(*current_velocity); + const rclcpp::Time now = this->now(); + while (!planner_data_.velocity_buffer.empty()) { + // Check oldest data time + const auto & s = planner_data_.velocity_buffer.back().header.stamp; + const auto time_diff = + now >= s ? now - s : rclcpp::Duration(0, 0); // Note: negative time throws an exception. + + // Finish when oldest data is newer than threshold + if (time_diff.seconds() <= PlannerData::velocity_buffer_time_sec) { + break; + } + + // Remove old data + planner_data_.velocity_buffer.pop_back(); + } +} + +void BehaviorVelocityPlannerNode::processTrafficSignals( + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) +{ + // clear previous observation + planner_data_.traffic_light_id_map_raw_.clear(); + const auto traffic_light_id_map_last_observed_old = + planner_data_.traffic_light_id_map_last_observed_; + planner_data_.traffic_light_id_map_last_observed_.clear(); + for (const auto & signal : msg->traffic_light_groups) { + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; + traffic_signal.signal = signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_light_group_id] = traffic_signal; + const bool is_unknown_observation = + std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { + return element.color == autoware_perception_msgs::msg::TrafficLightElement::UNKNOWN; + }); + // if the observation is UNKNOWN and past observation is available, only update the timestamp + // and keep the body of the info + const auto old_data = + traffic_light_id_map_last_observed_old.find(signal.traffic_light_group_id); + if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { + // copy last observation + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = + old_data->second; + // update timestamp + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id].stamp = + msg->stamp; + } else { + // if (1)the observation is not UNKNOWN or (2)the very first observation is UNKNOWN + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = + traffic_signal; + } + } +} + +bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) +{ + bool is_ready = true; + const auto & logData = [&clock, this](const std::string & data_type) { + std::string msg = "Waiting for " + data_type + " data"; + RCLCPP_INFO_THROTTLE(get_logger(), clock, logger_throttle_interval, "%s", msg.c_str()); + }; + + const auto & getData = [&logData](auto & dest, auto & sub, const std::string & data_type = "") { + const auto temp = sub.take_data(); + if (temp) { + dest = temp; + return true; + } + if (!data_type.empty()) logData(data_type); + return false; + }; + + is_ready &= getData(planner_data_.current_acceleration, sub_acceleration_, "acceleration"); + is_ready &= getData(planner_data_.predicted_objects, sub_predicted_objects_, "predicted_objects"); + is_ready &= getData(planner_data_.occupancy_grid, sub_occupancy_grid_, "occupancy_grid"); + + const auto odometry = sub_vehicle_odometry_.take_data(); + if (odometry) { + processOdometry(odometry); + } else { + logData("odometry"); + is_ready = false; + } + + const auto no_ground_pointcloud = sub_no_ground_pointcloud_.take_data(); + if (no_ground_pointcloud) { + processNoGroundPointCloud(no_ground_pointcloud); + } else { + logData("pointcloud"); + is_ready = false; + } + + const auto map_data = sub_lanelet_map_.take_data(); + if (map_data) { + planner_data_.route_handler_ = std::make_shared(*map_data); + } + + // planner_data_.external_velocity_limit is std::optional type variable. + const auto external_velocity_limit = sub_external_velocity_limit_.take_data(); + if (external_velocity_limit) { + planner_data_.external_velocity_limit = *external_velocity_limit; + } + + const auto traffic_signals = sub_traffic_signals_.take_data(); + if (traffic_signals) processTrafficSignals(traffic_signals); + + return is_ready; +} + +// NOTE: argument planner_data must not be referenced for multithreading +bool BehaviorVelocityPlannerNode::isDataReady(rclcpp::Clock clock) +{ + if (!planner_data_.velocity_smoother_) { + RCLCPP_INFO_THROTTLE( + get_logger(), clock, logger_throttle_interval, + "Waiting for the initialization of velocity smoother"); + return false; + } + + return processData(clock); +} + +void BehaviorVelocityPlannerNode::onTrigger( + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) +{ + std::unique_lock lk(mutex_); + + if (!isDataReady(*get_clock())) { + return; + } + + // Load map and check route handler + if (!planner_data_.route_handler_) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), logger_throttle_interval, + "Waiting for the initialization of route_handler"); + return; + } + + if (input_path_msg->points.empty()) { + return; + } + + const autoware_planning_msgs::msg::Path output_path_msg = + generatePath(input_path_msg, planner_data_); + + lk.unlock(); + + path_pub_->publish(output_path_msg); + published_time_publisher_->publish_if_subscribed(path_pub_, output_path_msg.header.stamp); + + if (debug_viz_pub_->get_subscription_count() > 0) { + publishDebugMarker(output_path_msg); + } +} + +autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, + const PlannerData & planner_data) +{ + autoware_planning_msgs::msg::Path output_path_msg; + + // TODO(someone): support backward path + const auto is_driving_forward = autoware::motion_utils::isDrivingForward(input_path_msg->points); + is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; + if (!is_driving_forward_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), logger_throttle_interval, + "Backward path is NOT supported. just converting path_with_lane_id to path"); + output_path_msg = to_path(*input_path_msg); + output_path_msg.header.frame_id = "map"; + output_path_msg.header.stamp = input_path_msg->header.stamp; + output_path_msg.left_bound = input_path_msg->left_bound; + output_path_msg.right_bound = input_path_msg->right_bound; + return output_path_msg; + } + + // Plan path velocity + const auto velocity_planned_path = planner_manager_.planPathVelocity( + std::make_shared(planner_data), *input_path_msg); + + // screening + const auto filtered_path = + autoware::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); + + // interpolation + const auto interpolated_path_msg = autoware::behavior_velocity_planner::interpolatePath( + filtered_path, forward_path_length_, behavior_output_path_interval_); + + // check stop point + output_path_msg = autoware::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); + + output_path_msg.header.frame_id = "map"; + output_path_msg.header.stamp = input_path_msg->header.stamp; + + // TODO(someone): This must be updated in each scene module, but copy from input message for now. + output_path_msg.left_bound = input_path_msg->left_bound; + output_path_msg.right_bound = input_path_msg->right_bound; + + return output_path_msg; +} + +void BehaviorVelocityPlannerNode::publishDebugMarker(const autoware_planning_msgs::msg::Path & path) +{ + visualization_msgs::msg::MarkerArray output_msg; + for (size_t i = 0; i < path.points.size(); ++i) { + visualization_msgs::msg::Marker marker; + marker.header = path.header; + marker.id = i; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.pose = path.points.at(i).pose; + marker.scale.y = marker.scale.z = 0.05; + marker.scale.x = 0.25; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + output_msg.markers.push_back(marker); + } + debug_viz_pub_->publish(output_msg); +} +} // namespace autoware::behavior_velocity_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp new file mode 100644 index 0000000000..4f0c673440 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -0,0 +1,91 @@ +// Copyright 2020 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 "autoware/behavior_velocity_planner/planner_manager.hpp" + +#include +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() +: plugin_loader_( + "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") +{ +} + +void BehaviorVelocityPlannerManager::launchScenePlugin( + rclcpp::Node & node, const std::string & name) +{ + if (plugin_loader_.isClassAvailable(name)) { + const auto plugin = plugin_loader_.createSharedInstance(name); + plugin->init(node); + + // Check if the plugin is already registered. + for (const auto & running_plugin : scene_manager_plugins_) { + if (plugin->getModuleName() == running_plugin->getModuleName()) { + RCLCPP_WARN_STREAM(node.get_logger(), "The plugin '" << name << "' is already loaded."); + return; + } + } + + // register + scene_manager_plugins_.push_back(plugin); + RCLCPP_DEBUG_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); + } else { + RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); + } +} + +void BehaviorVelocityPlannerManager::removeScenePlugin( + rclcpp::Node & node, const std::string & name) +{ + auto it = std::remove_if( + scene_manager_plugins_.begin(), scene_manager_plugins_.end(), + [&](const std::shared_ptr plugin) { + return plugin->getModuleName() == name; + }); + + if (it == scene_manager_plugins_.end()) { + RCLCPP_WARN_STREAM( + node.get_logger(), + "The scene plugin '" << name << "' is not found in the registered modules."); + } else { + scene_manager_plugins_.erase(it, scene_manager_plugins_.end()); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is unloaded."); + } +} + +autoware_internal_planning_msgs::msg::PathWithLaneId +BehaviorVelocityPlannerManager::planPathVelocity( + const std::shared_ptr & planner_data, + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path_msg) +{ + autoware_internal_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; + + for (const auto & plugin : scene_manager_plugins_) { + plugin->updateSceneModuleInstances(planner_data, input_path_msg); + plugin->plan(&output_path_msg); + } + + return output_path_msg; +} + +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp new file mode 100644 index 0000000000..a4bdf657b6 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp @@ -0,0 +1,127 @@ +// 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 "autoware/behavior_velocity_planner/test_utils.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_velocity_planner → test_node_ + test_manager->subscribeOutput( + "behavior_velocity_planner_node/output/path"); + + return test_manager; +} + +std::shared_ptr generateNode( + const std::vector & plugin_info_vec) +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto behavior_velocity_planner_common_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common"); + const auto behavior_velocity_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); + + const auto get_behavior_velocity_module_config = [](const std::string & module) { + const auto package_name = "autoware_behavior_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + + std::vector plugin_names; + for (const auto & plugin_info : plugin_info_vec) { + plugin_names.emplace_back(plugin_info.plugin_name); + } + + std::vector params; + params.emplace_back("launch_modules", plugin_names); + params.emplace_back("is_simulation", false); + node_options.parameter_overrides(params); + + auto yaml_files = std::vector{ + autoware_test_utils_dir + "/config/test_common.param.yaml", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml"}; + for (const auto & plugin_info : plugin_info_vec) { + yaml_files.push_back(get_behavior_velocity_module_config(plugin_info.module_name)); + } + + autoware::test_utils::updateNodeOptions(node_options, yaml_files); + + // TODO(Takagi, Isamu): set launch_modules + // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light + // TODO(Kyoichi Sugahara) set to true launch_occlusion_spot + // TODO(Kyoichi Sugahara) set to true launch_run_out + // TODO(Kyoichi Sugahara) set to true launch_speed_bump + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInput( + test_target_node, "/tf", autoware::test_utils::makeTFMsg(test_target_node, "base_link", "map")); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/accel", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/dynamic_objects", + autoware_perception_msgs::msg::PredictedObjects{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud", + sensor_msgs::msg::PointCloud2{}.set__header( + std_msgs::msg::Header{}.set__frame_id("base_link"))); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry", + autoware::test_utils::makeOdometry()); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/accel", + geometry_msgs::msg::AccelWithCovarianceStamped{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/vector_map", + autoware::test_utils::makeMapBinMsg()); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/traffic_signals", + autoware_perception_msgs::msg::TrafficLightGroupArray{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps", + autoware_internal_planning_msgs::msg::VelocityLimit{}); + test_manager->publishInput( + test_target_node, "behavior_velocity_planner_node/input/occupancy_grid", + autoware::test_utils::makeCostMapMsg()); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp new file mode 100644 index 0000000000..d62710f4da --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp @@ -0,0 +1,72 @@ +// Copyright 2023 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 "autoware/behavior_velocity_planner/test_utils.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + const std::string input_path_with_lane_id_topic = + "behavior_velocity_planner_node/input/path_with_lane_id"; + const std::string input_odometry_topic = "behavior_velocity_planner_node/input/vehicle_odometry"; + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithNormalPathWithLaneId(test_target_node, input_path_with_lane_id_topic)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithOffTrackOdometry(test_target_node, input_odometry_topic)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner From 325c9a8f355830ac6d4e9471e1e9636a7b1e096c Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Tue, 25 Mar 2025 22:41:04 +0900 Subject: [PATCH 11/19] fix: apply latest changes from autoware.universe Signed-off-by: Ryohsuke Mitsudome --- .../behavior_velocity_planner_common/planner_data.hpp | 4 ++-- .../package.xml | 3 ++- .../src/scene.cpp | 9 +++++---- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index f8b37999cb..cfc40bfcb8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -20,6 +20,7 @@ #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include #include #include #include @@ -29,7 +30,6 @@ #include #include #include -#include #include #include @@ -62,7 +62,7 @@ struct PlannerData std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; - std::optional external_velocity_limit; + std::optional external_velocity_limit; bool is_simulation = false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 4e44d334cf..e1e462fb80 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -29,8 +29,9 @@ autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_factor_interface + autoware_planning_msgs autoware_route_handler - autoware_universe_utils + autoware_utils autoware_vehicle_info_utils autoware_velocity_smoother diagnostic_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 87b75b7126..6062b01375 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -66,10 +66,11 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) // TODO(soblin): PlanningFactorInterface use trajectory class planning_factor_interface_->add( - path->points, trajectory->compute(*stop_point).point.pose, - planner_data_->current_odometry->pose, planner_data_->current_odometry->pose, - tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, - true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); + path->points, planner_data_->current_odometry->pose, + trajectory->compute(*stop_point).point.pose, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "stopline"); updateStateAndStoppedTime( &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); From 0b60d72e0c8134a15fc152b4682d9e0d4c0b3e32 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Tue, 25 Mar 2025 22:47:15 +0900 Subject: [PATCH 12/19] fix: deadlinks in README Signed-off-by: Ryohsuke Mitsudome --- .../README.md | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md index 26d0e26f79..b6f18f7a81 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md @@ -7,19 +7,19 @@ It loads modules as plugins. Please refer to the links listed below for detail o ![Architecture](./docs/BehaviorVelocityPlanner-Architecture.drawio.svg) -- [Blind Spot](../autoware_behavior_velocity_blind_spot_module/README.md) -- [Crosswalk](../autoware_behavior_velocity_crosswalk_module/README.md) -- [Walkway](../autoware_behavior_velocity_walkway_module/README.md) -- [Detection Area](../autoware_behavior_velocity_detection_area_module/README.md) -- [Intersection](../autoware_behavior_velocity_intersection_module/README.md) -- [MergeFromPrivate](../autoware_behavior_velocity_intersection_module/README.md#merge-from-private) +- [Blind Spot](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/) +- [Crosswalk](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/) +- [Walkway](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/) +- [Detection Area](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/) +- [Intersection](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/) +- [MergeFromPrivate](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/#merge-from-private) - [Stop Line](../autoware_behavior_velocity_stop_line_module/README.md) -- [Virtual Traffic Light](../autoware_behavior_velocity_virtual_traffic_light_module/README.md) -- [Traffic Light](../autoware_behavior_velocity_traffic_light_module/README.md) -- [Occlusion Spot](../autoware_behavior_velocity_occlusion_spot_module/README.md) -- [No Stopping Area](../autoware_behavior_velocity_no_stopping_area_module/README.md) -- [Run Out](../autoware_behavior_velocity_run_out_module/README.md) -- [Speed Bump](../autoware_behavior_velocity_speed_bump_module/README.md) +- [Virtual Traffic Light](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/) +- [Traffic Light](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/) +- [Occlusion Spot](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/) +- [No Stopping Area](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/) +- [Run Out](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/) +- [Speed Bump](https://autowarefoundation.github.io/autoware_universe/main/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/) When each module plans velocity, it considers based on `base_link`(center of rear-wheel axis) pose. So for example, in order to stop at a stop line with the vehicles' front on the stop line, it calculates `base_link` position from the distance between `base_link` to front and modifies path velocity from the `base_link` position. From 4ed955ef4b1ebd96b9fa916599c2631040bb4998 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Wed, 26 Mar 2025 13:35:12 +0900 Subject: [PATCH 13/19] Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp --- .../include/autoware/behavior_velocity_planner/node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 25dac976e1..6404f4f1b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -16,8 +16,8 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ #include "autoware/behavior_velocity_planner/planner_manager.hpp" -#include "autoware_utils/ros/logger_level_configure.hpp" -#include "autoware_utils/ros/polling_subscriber.hpp" +#include +#include #include #include From e168dbf51514011ba0c360f16fb01c76f739a9d2 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Wed, 26 Mar 2025 13:35:19 +0900 Subject: [PATCH 14/19] Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp --- .../behavior_velocity_planner_common/utilization/util.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp index 6535905926..385bc21222 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/util.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#include "autoware_utils/geometry/boost_geometry.hpp" +#include #include #include From 76a426760d0292876d0fb2b364a1a532081a6b76 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Wed, 26 Mar 2025 13:35:27 +0900 Subject: [PATCH 15/19] Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp --- .../src/utilization/util.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index a11ca779b2..b26b9f91ff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -16,8 +16,8 @@ #include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware_lanelet2_extension/utility/query.hpp" -#include "autoware_utils/geometry/geometry.hpp" +#include +#include #include From 2b925ee507fb14f52920f8bdd296a8284a1071a8 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Wed, 26 Mar 2025 13:35:34 +0900 Subject: [PATCH 16/19] Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp --- .../test/src/test_util.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp index bc5abee0ca..0bae486bf3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./utils.hpp" +#include "utils.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" From 60e68f40aa3a8c9a37ec2ef652440b40f783ca15 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Wed, 26 Mar 2025 13:35:40 +0900 Subject: [PATCH 17/19] Update planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp --- .../autoware_behavior_velocity_stop_line_module/src/debug.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp index cb29858bce..925a4ee5b4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" -#include "autoware_utils/geometry/geometry.hpp" +#include #include "scene.hpp" namespace autoware::behavior_velocity_planner From db2746032a1b48cc56549ffa9a3c29e3f687f69e Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Wed, 26 Mar 2025 13:35:47 +0900 Subject: [PATCH 18/19] Update planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp --- .../autoware_behavior_velocity_stop_line_module/src/manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index bd0c446b4e..59d0c3d079 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include "autoware_utils/ros/parameter.hpp" +#include #include From d90881df5fcef6a8632ddc7de36ff82596bb6784 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 26 Mar 2025 04:38:00 +0000 Subject: [PATCH 19/19] style(pre-commit): autofix --- .../include/autoware/behavior_velocity_planner/node.hpp | 4 ++-- .../src/utilization/util.cpp | 1 + .../test/src/test_util.cpp | 2 +- .../autoware_behavior_velocity_stop_line_module/src/debug.cpp | 3 ++- 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 6404f4f1b2..022df4c87b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -16,10 +16,10 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ #include "autoware/behavior_velocity_planner/planner_manager.hpp" -#include -#include #include +#include +#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index b26b9f91ff..b6071159db 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -16,6 +16,7 @@ #include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" + #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp index 0bae486bf3..d7a1714dba 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_util.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" +#include "utils.hpp" #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp index 925a4ee5b4..d529d21571 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp @@ -13,9 +13,10 @@ // limitations under the License. #include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" -#include #include "scene.hpp" +#include + namespace autoware::behavior_velocity_planner {