From 63cfff8419e62c4e9cb1df2edb439b9324b89629 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Fri, 6 Dec 2024 07:31:46 +0900 Subject: [PATCH] test(autoware_behavior_velocity_planner_common): refactor and test autoware_behavior_velocity_planner_common (#9551) * test(autoware_behavior_velocity_planner_common): refactor and test autoware_behavior_velocity_planner_common Signed-off-by: Y.Hisaki * remove nodiscard Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki * fix Signed-off-by: Y.Hisaki * fix * update Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- .../CMakeLists.txt | 8 +- .../planner_data.hpp | 82 +---- .../scene_module_interface.hpp | 6 +- .../utilization/arc_lane_util.hpp | 8 +- .../utilization/debug.hpp | 8 +- .../utilization/path_utilization.hpp | 4 +- .../utilization/state_machine.hpp | 7 +- .../utilization/util.hpp | 46 ++- .../package.xml | 1 + .../src/planner_data.cpp | 72 ++++ .../src/utilization/arc_lane_util.cpp | 9 - .../src/utilization/boost_geometry_helper.cpp | 10 +- .../src/utilization/debug.cpp | 33 +- .../src/utilization/path_utilization.cpp | 9 +- .../src/utilization/trajectory_utils.cpp | 6 +- .../src/utilization/util.cpp | 83 +++-- ...lization.cpp => test_path_utilization.cpp} | 67 ++++ .../test/src/test_trajectory_utils.cpp | 113 +++++++ .../test/src/test_util.cpp | 308 ++++++++++++++++++ 19 files changed, 680 insertions(+), 200 deletions(-) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp rename planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/{test_utilization.cpp => test_path_utilization.cpp} (70%) 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 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 index f76fcfa31c240..4c06c427a026d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -6,6 +6,7 @@ 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 @@ -15,11 +16,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/src/test_state_machine.cpp - test/src/test_arc_lane_util.cpp - test/src/test_utilization.cpp - ) + 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} 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 4075f356c187a..309ba33a3498a 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 @@ -15,11 +15,10 @@ #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 -#include -#include +#include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include #include @@ -38,32 +37,19 @@ #include #include -#include #include #include #include #include -#include namespace autoware::behavior_velocity_planner { -class BehaviorVelocityPlannerNode; struct PlannerData { - explicit 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"); // TODO(someone): read min_acc in velocity_controller.param.yaml? - 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"); - } + explicit PlannerData(rclcpp::Node & node); rclcpp::Clock::SharedPtr clock_; - // msgs from callbacks that are used for data-ready geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; @@ -71,85 +57,33 @@ struct PlannerData std::deque velocity_buffer; autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; - // occupancy grid + nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; - // nearest search double ego_nearest_dist_threshold; double ego_nearest_yaw_threshold; - // other internal data - // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the - // last observed infomation for UNKNOWN std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; std::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; - // This variable is used when the Autoware's behavior has to depend on whether it's simulation or - // not. bool is_simulation = false; - // velocity smoother std::shared_ptr velocity_smoother_; - // route handler std::shared_ptr route_handler_; - // parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; - // additional parameters 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 - { - if (velocity_buffer.empty()) { - return false; - } - - // Get velocities within stop_duration - 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; - } - } - - // Check all velocities - constexpr double stop_velocity = 1e-3; - for (const auto & v : vs) { - if (v >= stop_velocity) { - return false; - } - } - - return true; - } - - /** - *@fn - *@brief queries the traffic signal information of given Id. if keep_last_observation = true, - *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation - */ + bool isVehicleStopped(const double stop_duration = 0.0) const; + std::optional getTrafficSignal( - const lanelet::Id id, const bool keep_last_observation = false) 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)); - } + const lanelet::Id id, const bool keep_last_observation = false) const; }; } // namespace autoware::behavior_velocity_planner 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 5725ebe51b4a5..fadda66f12562 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 @@ -39,6 +39,7 @@ #include #include #include +#include #include // Debug @@ -69,9 +70,9 @@ struct ObjectOfInterest autoware_perception_msgs::msg::Shape shape; ColorName color; ObjectOfInterest( - const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, autoware_perception_msgs::msg::Shape shape, const ColorName & color_name) - : pose(pose), shape(shape), color(color_name) + : pose(pose), shape(std::move(shape)), color(color_name) { } }; @@ -89,6 +90,7 @@ class SceneModuleInterface virtual std::vector createVirtualWalls() = 0; int64_t getModuleId() const { return module_id_; } + void setPlannerData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; 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 56154879ea938..fae13db2822e7 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 @@ -20,7 +20,6 @@ #include -#include #include #include @@ -29,9 +28,8 @@ namespace autoware::behavior_velocity_planner { -namespace -{ -geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p) + +inline geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -40,8 +38,6 @@ geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Poi return geom_p; } -} // namespace - namespace arc_lane_utils { using PathIndexWithPose = std::pair; // front index, pose 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 index 3a1c8fdeb7a0d..5ac91cdaf1369 100644 --- 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 @@ -26,9 +26,7 @@ #include #include -namespace autoware::behavior_velocity_planner -{ -namespace debug +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, @@ -46,6 +44,6 @@ 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 debug -} // namespace autoware::behavior_velocity_planner +} // 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 index 1e45e5655f12f..2d4dab018fb18 100644 --- 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 @@ -20,13 +20,11 @@ #include #include -#include - namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, - tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); + tier4_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( 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 index 4d88f2bfecce8..dfe70d376621e 100644 --- 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 @@ -19,7 +19,6 @@ #include #include -#include namespace autoware::behavior_velocity_planner { @@ -37,11 +36,11 @@ class StateMachine { if (state == State::STOP) { return "STOP"; - } else if (state == State::GO) { + } + if (state == State::GO) { return "GO"; - } else { - return ""; } + return ""; } StateMachine() { 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 4cbf820268df5..fc4f852ef82f7 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 +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include #include @@ -36,23 +36,29 @@ namespace autoware::behavior_velocity_planner { +/** + * @brief Represents detection range parameters. + */ struct DetectionRange { - bool use_right = true; - bool use_left = true; - double interval; - double min_longitudinal_distance; - double max_longitudinal_distance; - double max_lateral_distance; - double wheel_tread; - double right_overhang; - double left_overhang; + 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; - autoware_perception_msgs::msg::TrafficLightGroup signal; + builtin_interfaces::msg::Time stamp; ///< Timestamp of the signal. + autoware_perception_msgs::msg::TrafficLightGroup signal; ///< Traffic light group. }; using Pose = geometry_msgs::msg::Pose; @@ -70,21 +76,27 @@ namespace planning_utils size_t calcSegmentIndexFromPointIndex( const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx); -// create detection area from given range return false if creation failure + 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 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); @@ -96,6 +108,7 @@ geometry_msgs::msg::Pose getAheadPose( const tier4_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); @@ -210,6 +223,7 @@ bool isOverLine( 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); @@ -218,11 +232,11 @@ std::optional insertStopPoint( or lane-changeable parent lanes with `lane` and has same turn_direction value. */ std::set getAssociativeIntersectionLanelets( - lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map, + const lanelet::ConstLanelet & lane, const lanelet::LaneletMapPtr lanelet_map, const lanelet::routing::RoutingGraphPtr routing_graph); lanelet::ConstLanelets getConstLaneletsFromIds( - lanelet::LaneletMapConstPtr map, const std::set & ids); + const lanelet::LaneletMapConstPtr & map, const std::set & ids); } // namespace planning_utils } // namespace autoware::behavior_velocity_planner 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 6692358f1b21f..105b0528940bf 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 @@ -52,6 +52,7 @@ 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/src/planner_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp new file mode 100644 index 0000000000000..df2a183e3bfb3 --- /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/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index da6ef7262de74..9b02b374b0a6c 100644 --- 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 @@ -26,10 +26,7 @@ #include #endif -#include -#include #include -#include namespace { @@ -54,12 +51,6 @@ geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & p, const d return multiplied_p; } -/* -geometry_msgs::msg::Point operator*(const double v, const geometry_msgs::msg::Point & p) -{ -return p * v; -} -*/ } // namespace 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 index 33cf428e80808..d9162419ec33f 100644 --- 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 @@ -16,8 +16,6 @@ #include -#include -#include namespace autoware::behavior_velocity_planner { @@ -27,8 +25,8 @@ Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & rig polygon.outer().push_back(left_line.front()); - for (auto itr = right_line.begin(); itr != right_line.end(); ++itr) { - polygon.outer().push_back(*itr); + for (const auto & itr : right_line) { + polygon.outer().push_back(itr); } for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) { @@ -57,8 +55,8 @@ 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 = p.x(); - point_msg.y = p.y(); + point_msg.x = static_cast(p.x()); + point_msg.y = static_cast(p.y()); polygon_msg.points.push_back(point_msg); } return polygon_msg; 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 7fe87bd285309..2ddf62bb584ff 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 @@ -18,14 +18,10 @@ #include #include -namespace autoware::behavior_velocity_planner +namespace autoware::behavior_velocity_planner::debug { -namespace debug -{ -using autoware::universe_utils::appendMarkerArray; using autoware::universe_utils::createDefaultMarker; using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerOrientation; using autoware::universe_utils::createMarkerScale; visualization_msgs::msg::MarkerArray createPolygonMarkerArray( @@ -36,8 +32,9 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( visualization_msgs::msg::MarkerArray msg; { auto marker = createDefaultMarker( - "map", now, ns.c_str(), module_id, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(x, y, z), createMarkerColor(r, g, b, 0.8)); + "map", now, ns, static_cast(module_id), visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(static_cast(x), static_cast(y), static_cast(z)), + createMarkerColor(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) { @@ -67,15 +64,18 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( const auto & p = path.points.at(i); auto marker = createDefaultMarker( - "map", now, ns.c_str(), planning_utils::bitShift(lane_id) + i, - visualization_msgs::msg::Marker::ARROW, createMarkerScale(x, y, z), - createMarkerColor(r, g, b, 0.999)); + "map", now, ns, static_cast(planning_utils::bitShift(lane_id) + i), + visualization_msgs::msg::Marker::ARROW, + createMarkerScale(static_cast(x), static_cast(y), static_cast(z)), + createMarkerColor( + 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 = createMarkerColor(r, g, b, 0.999); + marker.color = createMarkerColor( + static_cast(r), static_cast(g), static_cast(b), 0.999f); } else { marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.999); } @@ -93,13 +93,13 @@ visualization_msgs::msg::MarkerArray createObjectsMarkerArray( auto marker = createDefaultMarker( "map", now, ns, 0, visualization_msgs::msg::Marker::CUBE, createMarkerScale(3.0, 1.0, 1.0), - createMarkerColor(r, g, b, 0.8)); + createMarkerColor(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 = planning_utils::bitShift(module_id) + 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); } @@ -116,15 +116,14 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( auto marker = createDefaultMarker( "map", now, ns, 0, visualization_msgs::msg::Marker::SPHERE, createMarkerScale(x, y, z), - createMarkerColor(r, g, b, 0.999)); + createMarkerColor(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 = i + planning_utils::bitShift(module_id); + marker.id = static_cast(i + planning_utils::bitShift(module_id)); marker.pose.position = points.at(i); msg.markers.push_back(marker); } return msg; } -} // namespace debug -} // namespace autoware::behavior_velocity_planner +} // 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 index 17ea92022ea13..b8e6e28bae7c2 100644 --- 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 @@ -18,14 +18,13 @@ #include #include -#include #include namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, - tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger) + tier4_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."); @@ -156,12 +155,12 @@ autoware_planning_msgs::msg::Path filterStopPathPoint( { autoware_planning_msgs::msg::Path filtered_path = path; bool found_stop = false; - for (size_t i = 0; i < filtered_path.points.size(); ++i) { - if (std::fabs(filtered_path.points.at(i).longitudinal_velocity_mps) < 0.01) { + for (auto & point : filtered_path.points) { + if (std::fabs(point.longitudinal_velocity_mps) < 0.01) { found_stop = true; } if (found_stop) { - filtered_path.points.at(i).longitudinal_velocity_mps = 0.0; + point.longitudinal_velocity_mps = 0.0; } } return filtered_path; 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 index 72a6d29e7ba06..cb9b58ec48924 100644 --- 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 @@ -79,13 +79,15 @@ bool smoothPath( TrajectoryPoints clipped; TrajectoryPoints traj_smoothed; clipped.insert( - clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + 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() + traj_resampled_closest); + traj_smoothed.begin(), traj_resampled.begin(), + traj_resampled.begin() + static_cast(traj_resampled_closest)); if (external_v_limit) { autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( 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 1b1c47dc6fbb7..a8f909201ac7f 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 @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include +#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_lanelet2_extension/utility/query.hpp" #include @@ -25,9 +26,6 @@ #include #include -#include -#include - #ifdef ROS_DISTRO_GALACTIC #include #else @@ -35,7 +33,9 @@ #endif #include -#include +#include +#include +#include #include #include @@ -67,7 +67,7 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con PathPoint p; p.pose = autoware::universe_utils::calcInterpolatedPose(p0, p1, ratio); const double v = lerp(p0.longitudinal_velocity_mps, p1.longitudinal_velocity_mps, ratio); - p.longitudinal_velocity_mps = v; + p.longitudinal_velocity_mps = static_cast(v); return p; } @@ -94,19 +94,11 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( } // namespace -namespace autoware::behavior_velocity_planner -{ -namespace planning_utils +namespace autoware::behavior_velocity_planner::planning_utils { -using autoware::motion_utils::calcLongitudinalOffsetToSegment; using autoware::motion_utils::calcSignedArcLength; -using autoware::motion_utils::validateNonEmpty; -using autoware::universe_utils::calcAzimuthAngle; using autoware::universe_utils::calcDistance2d; using autoware::universe_utils::calcOffsetPose; -using autoware::universe_utils::calcSquaredDistance2d; -using autoware::universe_utils::createQuaternionFromYaw; -using autoware::universe_utils::getPoint; using autoware_planning_msgs::msg::PathPoint; size_t calcSegmentIndexFromPointIndex( @@ -157,7 +149,7 @@ bool createDetectionAreaPolygons( const double offset_right = (da_range.wheel_tread / 2.0) + da_range.right_overhang; //! max index is the last index of path point - const size_t max_index = static_cast(path.points.size() - 1); + const auto max_index = static_cast(path.points.size() - 1); //! avoid bug with same point polygon const double eps = 1e-3; auto nearest_idx = @@ -249,10 +241,9 @@ void extractClosePartition( close_partition.emplace_back(p); } } - return; } -void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys) +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr & ll, BasicPolygons2d & polys) { const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll); for (const auto & partition : partitions) { @@ -262,7 +253,7 @@ void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons } // correct line to calculate distance in accurate boost::geometry::correct(line); - polys.emplace_back(lanelet::BasicPolygon2d(line)); + polys.emplace_back(line); } } @@ -272,7 +263,6 @@ void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLane input->points.at(i).point.longitudinal_velocity_mps = std::min(static_cast(vel), input->points.at(i).point.longitudinal_velocity_mps); } - return; } void insertVelocity( @@ -312,7 +302,7 @@ geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, const tier4_planning_msgs::msg::PathWithLaneId & path) { - if (path.points.size() == 0) { + if (path.points.empty()) { return geometry_msgs::msg::Pose{}; } @@ -330,7 +320,8 @@ geometry_msgs::msg::Pose getAheadPose( 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, q1_tf; + 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)); @@ -427,15 +418,16 @@ double findReachTime( const int warn_iter = 100; double lower = min; double upper = max; - double t; + double t = NAN; int iter = 0; - for (int i = 0;; i++) { + while (true) { t = 0.5 * (lower + upper); const double fx = f(t, j, a, v, d); // std::cout<<"fx: "< 0.0) { + } + if (fx > 0.0) { upper = t; } else { lower = t; @@ -478,19 +470,18 @@ double calcDecelerationVelocityFromDistanceToTarget( 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; - } else { - 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); } - return current_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) @@ -549,6 +540,7 @@ std::vector getLaneletsOnPath( } 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)); } @@ -590,7 +582,7 @@ std::vector getSubsequentLaneIdsSetOnPath( // cannot find base_index in all_lane_ids if (base_index == all_lane_ids.end()) { - return std::vector(); + return {}; } std::vector subsequent_lane_ids; @@ -664,11 +656,11 @@ std::optional insertStopPoint( } std::set getAssociativeIntersectionLanelets( - lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map, + 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.compare("else") == 0) { + if (turn_direction == "else") { return {}; } @@ -693,7 +685,7 @@ std::set getAssociativeIntersectionLanelets( } lanelet::ConstLanelets getConstLaneletsFromIds( - lanelet::LaneletMapConstPtr map, const std::set & ids) + const lanelet::LaneletMapConstPtr & map, const std::set & ids) { lanelet::ConstLanelets ret{}; for (const auto & id : ids) { @@ -703,5 +695,4 @@ lanelet::ConstLanelets getConstLaneletsFromIds( return ret; } -} // namespace planning_utils -} // namespace autoware::behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner::planning_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp similarity index 70% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp index d3664f2b46ffc..51c6b5b9dda04 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_path_utilization.cpp @@ -176,3 +176,70 @@ TEST(specialInterpolation, specialInterpolation) 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_trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_trajectory_utils.cpp new file mode 100644 index 0000000000000..5ed490ab755a9 --- /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 tier4_planning_msgs::msg::PathPointWithLaneId; + using tier4_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 0000000000000..8c8ef575b5a9a --- /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; + tier4_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) { + tier4_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); + tier4_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) +{ + tier4_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathPointWithLaneId point1; + tier4_planning_msgs::msg::PathPointWithLaneId point2; + tier4_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); +}