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 index 402eb5e20aa24..f4528f0d13cf4 100644 --- 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 @@ -11,4 +11,15 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_scene.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/config/stop_line.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml index 7b1d82a46f652..f5e00fa7087ae 100644 --- 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 @@ -3,8 +3,4 @@ stop_line: stop_margin: 0.0 stop_duration_sec: 1.0 - use_initialization_stop_line_state: true hold_stop_margin_distance: 2.0 - - debug: - show_stop_line_collision_check: false # [-] whether to show stopline collision 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 e24f167eb2058..c5e3690dd620d 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 @@ -33,6 +33,7 @@ tf2_geometry_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common 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 573a260138679..fa94bfbaa361b 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 @@ -12,89 +12,12 @@ // 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/motion_utils/marker/virtual_wall_marker_creator.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" #include "scene.hpp" -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::appendMarkerArray; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; - -namespace -{ -using DebugData = StopLineModule::DebugData; - -visualization_msgs::msg::MarkerArray createStopLineCollisionCheck( - const DebugData & debug_data, const int64_t module_id) -{ - visualization_msgs::msg::MarkerArray msg; - - // Search Segments - { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.ns = "search_segments"; - marker.id = module_id; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - marker.action = visualization_msgs::msg::Marker::ADD; - for (const auto & e : debug_data.search_segments) { - marker.points.push_back( - geometry_msgs::build().x(e.at(0).x()).y(e.at(0).y()).z(0.0)); - marker.points.push_back( - geometry_msgs::build().x(e.at(1).x()).y(e.at(1).y()).z(0.0)); - } - marker.scale = createMarkerScale(0.1, 0.1, 0.1); - marker.color = createMarkerColor(0.0, 0.0, 1.0, 0.999); - msg.markers.push_back(marker); - } - - // Search stopline - { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.ns = "search_stopline"; - marker.id = module_id; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - const auto p0 = debug_data.search_stopline.at(0); - marker.points.push_back( - geometry_msgs::build().x(p0.x()).y(p0.y()).z(0.0)); - const auto p1 = debug_data.search_stopline.at(1); - marker.points.push_back( - geometry_msgs::build().x(p1.x()).y(p1.y()).z(0.0)); - - marker.scale = createMarkerScale(0.1, 0.1, 0.1); - marker.color = createMarkerColor(1.0, 0.0, 0.0, 0.999); - msg.markers.push_back(marker); - } - - return msg; -} - -} // namespace - -visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray() -{ - visualization_msgs::msg::MarkerArray debug_marker_array; - if (planner_param_.show_stop_line_collision_check) { - appendMarkerArray( - createStopLineCollisionCheck(debug_data_, module_id_), &debug_marker_array, - this->clock_->now()); - } - return debug_marker_array; -} autoware::motion_utils::VirtualWalls StopLineModule::createVirtualWalls() { 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 ffdcea16b45b5..b305a1d2aa404 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 @@ -15,7 +15,8 @@ #include "manager.hpp" #include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware_lanelet2_extension/utility/query.hpp" + +#include #include #include @@ -28,7 +29,7 @@ using autoware::universe_utils::getOrDeclareParameter; using lanelet::TrafficSign; StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterface(node, getModuleName()) +: SceneModuleManagerInterface(node, getModuleName()), planner_param_() { const std::string ns(StopLineModuleManager::getModuleName()); auto & p = planner_param_; @@ -36,11 +37,6 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) p.hold_stop_margin_distance = getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); p.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); - p.use_initialization_stop_line_state = - getOrDeclareParameter(node, ns + ".use_initialization_stop_line_state"); - // debug - p.show_stop_line_collision_check = - getOrDeclareParameter(node, ns + ".debug.show_stop_line_collision_check"); } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( @@ -82,10 +78,9 @@ void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pat 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(); - const auto lane_id = stop_line_with_lane_id.second; if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, lane_id, stop_line_with_lane_id.first, planner_param_, + module_id, stop_line_with_lane_id.first, planner_param_, logger_.get_child("stop_line_module"), clock_)); } } 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 index b83a4f94e9a1f..bef8a5eef4ac0 100644 --- 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 @@ -15,7 +15,6 @@ #ifndef MANAGER_HPP_ #define MANAGER_HPP_ -#include "autoware/behavior_velocity_planner_common/plugin_interface.hpp" #include "autoware/behavior_velocity_planner_common/plugin_wrapper.hpp" #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" #include "scene.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 index 6a4b85cd6926c..b83a274f99513 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 @@ -15,22 +15,32 @@ #include "scene.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" -#include "autoware/trajectory/path_point_with_lane_id.hpp" -#include +#include + +#include + +#include namespace autoware::behavior_velocity_planner { +geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line) +{ + geometry_msgs::msg::Point center_point; + center_point.x = (stop_line[0].x() + stop_line[1].x()) / 2.0; + center_point.y = (stop_line[0].y() + stop_line[1].y()) / 2.0; + center_point.z = (stop_line[0].z() + stop_line[1].z()) / 2.0; + return center_point; +} + StopLineModule::StopLineModule( - const int64_t module_id, const size_t lane_id, lanelet::ConstLineString3d stop_line, - const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) + const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), - lane_id_(lane_id), stop_line_(std::move(stop_line)), - state_(State::APPROACH), planner_param_(planner_param), + state_(State::APPROACH), debug_data_() { velocity_factor_.init(PlanningBehavior::STOP_SIGN); @@ -46,132 +56,144 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop return true; } - debug_data_ = DebugData(); - const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - debug_data_.base_link2front = base_link2front; - first_stop_path_point_distance_ = trajectory->length(); - *stop_reason = planning_utils::initializeStopReason(StopReason::STOP_LINE); - - 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 = - trajectory->crossed(stop_line.front(), stop_line.back()); + auto [ego_s, stop_point] = + getEgoAndStopPoint(*trajectory, planner_data_->current_odometry->pose, state_); - // If no collision found, do nothing - if (!trajectory_stop_line_intersection) { - RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision"); + if (!stop_point) { return true; } - const double stop_point_s = - *trajectory_stop_line_intersection - - (base_link2front + planner_param_.stop_margin); // consider vehicle length and stop margin + trajectory->longitudinal_velocity_mps.range(*stop_point, trajectory->length()).set(0.0); - if (stop_point_s < 0.0) { - return true; - } + path->points = trajectory->restore(); - const auto stop_pose = trajectory->compute(stop_point_s); + updateVelocityFactor(&velocity_factor_, state_, *stop_point - ego_s); - /** - * @brief : calculate signed arc length consider stop margin from stop line - * - * |----------------------------| - * s---ego----------x--|--------g - */ - const double ego_on_trajectory_s = - trajectory->closest(planner_data_->current_odometry->pose.position); - const double signed_arc_dist_to_stop_point = stop_point_s - ego_on_trajectory_s; + updateStateAndStoppedTime( + &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); - switch (state_) { - case State::APPROACH: { - // Insert stop pose - trajectory->longitudinal_velocity_mps.range(stop_point_s, trajectory->length()).set(0.0); - - // Update first stop path point distance - first_stop_path_point_distance_ = stop_point_s; - debug_data_.stop_pose = stop_pose.point.pose; - - // Get stop point and stop factor - { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_pose.point.pose; - stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); - planning_utils::appendStopReason(stop_factor, stop_reason); - velocity_factor_.set(signed_arc_dist_to_stop_point, VelocityFactor::APPROACHING); - } + geometry_msgs::msg::Pose stop_pose = trajectory->compute(*stop_point).point.pose; - // Move to stopped state if stopped - if ( - signed_arc_dist_to_stop_point < planner_param_.hold_stop_margin_distance && - planner_data_->isVehicleStopped()) { - RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); + updateStopReason(stop_reason, stop_pose); - state_ = State::STOPPED; - stopped_time_ = std::make_shared(clock_->now()); + updateDebugData(&debug_data_, stop_pose, state_); - if (signed_arc_dist_to_stop_point < -planner_param_.hold_stop_margin_distance) { - RCLCPP_ERROR( - logger_, "Failed to stop near stop line but ego stopped. Change state to STOPPED"); - } - } + return true; +} - break; - } +std::pair> StopLineModule::getEgoAndStopPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & ego_pose, + const State & state) const +{ + const double ego_s = trajectory.closest(ego_pose.position); + std::optional stop_point_s; - case State::STOPPED: { - // Insert stop pose - trajectory->longitudinal_velocity_mps.range(ego_on_trajectory_s, trajectory->length()) - .set(0.0); - const auto ego_pos_on_path = trajectory->compute(ego_on_trajectory_s).point.pose; - debug_data_.stop_pose = ego_pos_on_path; - - // Get stop point and stop factor - { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = ego_pos_on_path; - stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); - planning_utils::appendStopReason(stop_factor, stop_reason); - velocity_factor_.set(signed_arc_dist_to_stop_point, VelocityFactor::STOPPED); + 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 = + trajectory.crossed(stop_line.front(), stop_line.back()); + + // If no collision found, do nothing + if (!trajectory_stop_line_intersection) { + stop_point_s = std::nullopt; + break; } - const double elapsed_time = (clock_->now() - *stopped_time_).seconds(); + stop_point_s = + *trajectory_stop_line_intersection - + (base_link2front + planner_param_.stop_margin); // consider vehicle length and stop margin - if (planner_param_.stop_duration_sec < elapsed_time) { - RCLCPP_INFO(logger_, "STOPPED -> START"); - state_ = State::START; + if (*stop_point_s < 0.0) { + stop_point_s = std::nullopt; } + break; + } + case State::STOPPED: { + stop_point_s = ego_s; break; } case State::START: { - // Initialize if vehicle is far from stop_line - if (planner_param_.use_initialization_stop_line_state) { - if (signed_arc_dist_to_stop_point > planner_param_.hold_stop_margin_distance) { - RCLCPP_INFO(logger_, "START -> APPROACH"); - state_ = State::APPROACH; + 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; } } +} - path->points = trajectory->restore(); +void StopLineModule::updateVelocityFactor( + autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, + const double & distance_to_stop_point) +{ + switch (state) { + case State::APPROACH: { + velocity_factor->set(distance_to_stop_point, VelocityFactor::APPROACHING); + break; + } + case State::STOPPED: { + velocity_factor->set(distance_to_stop_point, VelocityFactor::STOPPED); + break; + } + case State::START: + break; + } +} - return true; +void StopLineModule::updateStopReason( + StopReason * stop_reason, const geometry_msgs::msg::Pose & stop_pose) const +{ + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); + planning_utils::appendStopReason(stop_factor, stop_reason); } -geometry_msgs::msg::Point StopLineModule::getCenterOfStopLine( - const lanelet::ConstLineString3d & stop_line) +void StopLineModule::updateDebugData( + DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const { - geometry_msgs::msg::Point center_point; - center_point.x = (stop_line[0].x() + stop_line[1].x()) / 2.0; - center_point.y = (stop_line[0].y() + stop_line[1].y()) / 2.0; - center_point.z = (stop_line[0].z() + stop_line[1].z()) / 2.0; - return center_point; + 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 index cb48aabe57c1a..cb5bf339f8846 100644 --- 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 @@ -14,75 +14,108 @@ #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/boost_geometry_helper.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/motion_utils/factor/velocity_factor_interface.hpp" +#include "autoware/trajectory/path_point_with_lane_id.hpp" -#include -#include -#include - -#define EIGEN_MPL2_ONLY #include #include #include +#include + +#include #include +#include +#include + namespace autoware::behavior_velocity_planner { class StopLineModule : public SceneModuleInterface { - using StopLineWithLaneId = std::pair; - public: + using StopLineWithLaneId = std::pair; + using Trajectory = + autoware::trajectory::Trajectory; enum class State { APPROACH, STOPPED, START }; struct DebugData { - double base_link2front; - boost::optional stop_pose; - std::vector search_segments; - LineString2d search_stopline; + 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; - double stop_duration_sec; - double hold_stop_margin_distance; - bool use_initialization_stop_line_state; - bool show_stop_line_collision_check; + 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. + */ StopLineModule( - const int64_t module_id, const size_t lane_id, lanelet::ConstLineString3d stop_line, + const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; - visualization_msgs::msg::MarkerArray createDebugMarkerArray() 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; + + static void updateVelocityFactor( + autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, + const double & distance_to_stop_point); + + void updateStopReason(StopReason * stop_reason, const geometry_msgs::msg::Pose & stop_pose) 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: - std::shared_ptr stopped_time_; - - geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line); - - int64_t lane_id_; - - lanelet::ConstLineString3d stop_line_; - - // State machine - State state_; - - // Parameter - PlannerParam planner_param_; - - // Debug - DebugData debug_data_; + 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 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 0000000000000..1eafb71eb403c --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -0,0 +1,143 @@ +// 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; + +tier4_planning_msgs::msg::PathPointWithLaneId path_point(double x, double y) +{ + tier4_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_); + + 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()); +}