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 <yhisaki31@gmail.com>

* remove nodiscard

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* update

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* fix

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* fix

* update

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

---------

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Author: yhisaki

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}

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 <autoware/behavior_velocity_planner_common/utilization/util.hpp>
-#include <autoware/velocity_smoother/smoother/smoother_base.hpp>
-#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+#include "autoware/velocity_smoother/smoother/smoother_base.hpp"
+#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
@@ -38,118 +37,53 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
-#include <algorithm>
 #include <deque>
 #include <map>
 #include <memory>
 #include <optional>
-#include <vector>
 
 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<double>(
-      "max_accel");  // TODO(someone): read min_acc in velocity_controller.param.yaml?
-    max_stop_jerk_threshold = node.declare_parameter<double>("max_jerk");
-    system_delay = node.declare_parameter<double>("system_delay");
-    delay_response_time = node.declare_parameter<double>("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;
   static constexpr double velocity_buffer_time_sec = 10.0;
   std::deque<geometry_msgs::msg::TwistStamped> velocity_buffer;
   autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects;
   pcl::PointCloud<pcl::PointXYZ>::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<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_raw_;
   std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_last_observed_;
   std::optional<tier4_planning_msgs::msg::VelocityLimit> 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<autoware::velocity_smoother::SmootherBase> velocity_smoother_;
-  // route handler
   std::shared_ptr<autoware::route_handler::RouteHandler> 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<double> 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<TrafficSignalStamped> 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<TrafficSignalStamped>(traffic_light_id_map.at(id));
-  }
+    const lanelet::Id id, const bool keep_last_observation = false) const;
 };
 }  // namespace autoware::behavior_velocity_planner
 

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -39,6 +39,7 @@
 #include <set>
 #include <string>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 // 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<autoware::motion_utils::VirtualWall> createVirtualWalls() = 0;
 
   int64_t getModuleId() const { return module_id_; }
+
   void setPlannerData(const std::shared_ptr<const PlannerData> & planner_data)
   {
     planner_data_ = planner_data;

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 <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
-#include <algorithm>
 #include <optional>
 #include <utility>
 
@@ -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<size_t, geometry_msgs::msg::Pose>;  // front index, pose

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/debug.hpp

@@ -26,9 +26,7 @@
 #include <string>
 #include <vector>
 
-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<geometry_msgs::msg::Point> & 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_

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/path_utilization.hpp

@@ -20,13 +20,11 @@
 #include <autoware_planning_msgs/msg/path.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
-#include <vector>
-
 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(

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/state_machine.hpp

@@ -19,7 +19,6 @@
 
 #include <memory>
 #include <string>
-#include <utility>
 
 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()
   {

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/universe_utils/geometry/boost_geometry.hpp>
+#include "autoware/universe_utils/geometry/boost_geometry.hpp"
 
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_perception_msgs/msg/traffic_light_group.hpp>
@@ -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<tier4_planning_msgs::msg::PathPointWithLaneId> & 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<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output);
+
 std::optional<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output);
 
@@ -218,11 +232,11 @@ std::optional<geometry_msgs::msg::Pose> insertStopPoint(
   or lane-changeable parent lanes with `lane` and has same turn_direction value.
  */
 std::set<lanelet::Id> 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<lanelet::Id> & ids);
+  const lanelet::LaneletMapConstPtr & map, const std::set<lanelet::Id> & ids);
 
 }  // namespace planning_utils
 }  // namespace autoware::behavior_velocity_planner

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml

@@ -52,6 +52,7 @@
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
+  <test_depend>autoware_test_utils</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>