test(obstacle_collision_checker): refactor and add tests (autowarefoundation#8989)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp

@@ -1,4 +1,4 @@
-// Copyright 2020 Tier IV, Inc. All rights reserved.
+// Copyright 2020-2024 Tier IV, Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -15,7 +15,6 @@
 #ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_
 #define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_
 
-#include <autoware/universe_utils/geometry/boost_geometry.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory.hpp>
@@ -24,8 +23,6 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
-#include <boost/optional.hpp>
-
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
@@ -54,6 +51,8 @@ struct Input
   geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform;
   autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory;
   autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory;
+  autoware::vehicle_info_utils::VehicleInfo vehicle_info;
+  Param param;
 };
 
 struct Output
@@ -65,43 +64,31 @@ struct Output
   std::vector<LinearRing2d> vehicle_passing_areas;
 };
 
-class ObstacleCollisionChecker
-{
-public:
-  explicit ObstacleCollisionChecker(rclcpp::Node & node);
-  Output update(const Input & input);
-
-  void setParam(const Param & param) { param_ = param; }
+Output check_for_collisions(const Input & input);
 
-private:
-  Param param_;
-  autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
+//! This function assumes the input trajectory is sampled dense enough
+autoware_planning_msgs::msg::Trajectory resample_trajectory(
+  const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval);
 
-  //! This function assumes the input trajectory is sampled dense enough
-  static autoware_planning_msgs::msg::Trajectory resampleTrajectory(
-    const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval);
+autoware_planning_msgs::msg::Trajectory cut_trajectory(
+  const autoware_planning_msgs::msg::Trajectory & trajectory, const double length);
 
-  static autoware_planning_msgs::msg::Trajectory cutTrajectory(
-    const autoware_planning_msgs::msg::Trajectory & trajectory, const double length);
+std::vector<LinearRing2d> create_vehicle_footprints(
+  const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param,
+  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info);
 
-  static std::vector<LinearRing2d> createVehicleFootprints(
-    const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param,
-    const autoware::vehicle_info_utils::VehicleInfo & vehicle_info);
+std::vector<LinearRing2d> create_vehicle_passing_areas(
+  const std::vector<LinearRing2d> & vehicle_footprints);
 
-  static std::vector<LinearRing2d> createVehiclePassingAreas(
-    const std::vector<LinearRing2d> & vehicle_footprints);
+LinearRing2d create_hull_from_footprints(const LinearRing2d & area1, const LinearRing2d & area2);
 
-  static LinearRing2d createHullFromFootprints(
-    const LinearRing2d & area1, const LinearRing2d & area2);
+bool will_collide(
+  const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
+  const std::vector<LinearRing2d> & vehicle_footprints);
 
-  static bool willCollide(
-    const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
-    const std::vector<LinearRing2d> & vehicle_footprints);
-
-  static bool hasCollision(
-    const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
-    const LinearRing2d & vehicle_footprint);
-};
+bool has_collision(
+  const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
+  const LinearRing2d & vehicle_footprint);
 }  // namespace obstacle_collision_checker
 
 #endif  // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_

control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp

@@ -22,6 +22,7 @@
 #include <autoware/universe_utils/ros/processing_time_publisher.hpp>
 #include <autoware/universe_utils/ros/self_pose_listener.hpp>
 #include <autoware/universe_utils/ros/transform_listener.hpp>
+#include <autoware_vehicle_info_utils/vehicle_info.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -38,7 +39,7 @@ namespace obstacle_collision_checker
 {
 struct NodeParam
 {
-  double update_rate;
+  double update_rate{};
 };
 
 class ObstacleCollisionCheckerNode : public rclcpp::Node
@@ -64,46 +65,45 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node
   geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_;
   autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_;
   autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_;
+  autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
 
   // Callback
-  void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
-  void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg);
-  void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg);
-  void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void on_obstacle_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
+  void on_reference_trajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg);
+  void on_predicted_trajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg);
+  void on_odom(const nav_msgs::msg::Odometry::SharedPtr msg);
 
   // Publisher
   std::shared_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;
   std::shared_ptr<autoware::universe_utils::ProcessingTimePublisher> time_publisher_;
 
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;
-  void initTimer(double period_s);
+  void init_timer(double period_s);
 
-  bool isDataReady();
-  bool isDataTimeout();
-  void onTimer();
+  bool is_data_ready();
+  bool is_data_timeout();
+  void on_timer();
 
   // Parameter
   NodeParam node_param_;
-  Param param_;
 
   // Dynamic Reconfigure
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
-  rcl_interfaces::msg::SetParametersResult paramCallback(
+  rcl_interfaces::msg::SetParametersResult param_callback(
     const std::vector<rclcpp::Parameter> & parameters);
 
   // Core
   Input input_;
   Output output_;
-  std::unique_ptr<ObstacleCollisionChecker> obstacle_collision_checker_;
 
   // Diagnostic Updater
   diagnostic_updater::Updater updater_;
 
-  void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void check_lane_departure(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
   // Visualization
-  visualization_msgs::msg::MarkerArray createMarkerArray() const;
+  visualization_msgs::msg::MarkerArray create_marker_array() const;
 };
 }  // namespace obstacle_collision_checker
 

control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp

@@ -20,24 +20,18 @@
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
 
 #include <boost/geometry.hpp>
 
 #include <pcl_conversions/pcl_conversions.h>
 #include <tf2/utils.h>
 
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_eigen/tf2_eigen.h>
-#else
-#include <tf2_eigen/tf2_eigen.hpp>
-#endif
-
-#include <iostream>
 #include <vector>
 
 namespace
 {
-pcl::PointCloud<pcl::PointXYZ> getTransformedPointCloud(
+pcl::PointCloud<pcl::PointXYZ> get_transformed_point_cloud(
   const sensor_msgs::msg::PointCloud2 & pointcloud_msg,
   const geometry_msgs::msg::Transform & transform)
 {
@@ -52,7 +46,7 @@ pcl::PointCloud<pcl::PointXYZ> getTransformedPointCloud(
   return transformed_pointcloud;
 }
 
-pcl::PointCloud<pcl::PointXYZ> filterPointCloudByTrajectory(
+pcl::PointCloud<pcl::PointXYZ> filter_point_cloud_by_trajectory(
   const pcl::PointCloud<pcl::PointXYZ> & pointcloud,
   const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius)
 {
@@ -70,7 +64,7 @@ pcl::PointCloud<pcl::PointXYZ> filterPointCloudByTrajectory(
   return filtered_pointcloud;
 }
 
-double calcBrakingDistance(
+double calc_braking_distance(
   const double abs_velocity, const double max_deceleration, const double delay_time)
 {
   const double idling_distance = abs_velocity * delay_time;
@@ -82,12 +76,7 @@ double calcBrakingDistance(
 
 namespace obstacle_collision_checker
 {
-ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node)
-: vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo())
-{
-}
-
-Output ObstacleCollisionChecker::update(const Input & input)
+Output check_for_collisions(const Input & input)
 {
   Output output;
   autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
@@ -97,31 +86,32 @@ Output ObstacleCollisionChecker::update(const Input & input)
   const auto & raw_abs_velocity = std::abs(input.current_twist->linear.x);
   const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity;
   const auto braking_distance =
-    calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time);
-  output.resampled_trajectory = cutTrajectory(
-    resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance);
+    calc_braking_distance(abs_velocity, input.param.max_deceleration, input.param.delay_time);
+  output.resampled_trajectory = cut_trajectory(
+    resample_trajectory(*input.predicted_trajectory, input.param.resample_interval),
+    braking_distance);
   output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true);
 
   // resample pointcloud
   const auto obstacle_pointcloud =
-    getTransformedPointCloud(*input.obstacle_pointcloud, input.obstacle_transform->transform);
-  const auto filtered_obstacle_pointcloud = filterPointCloudByTrajectory(
-    obstacle_pointcloud, output.resampled_trajectory, param_.search_radius);
+    get_transformed_point_cloud(*input.obstacle_pointcloud, input.obstacle_transform->transform);
+  const auto filtered_obstacle_pointcloud = filter_point_cloud_by_trajectory(
+    obstacle_pointcloud, output.resampled_trajectory, input.param.search_radius);
 
   output.vehicle_footprints =
-    createVehicleFootprints(output.resampled_trajectory, param_, vehicle_info_);
+    create_vehicle_footprints(output.resampled_trajectory, input.param, input.vehicle_info);
   output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true);
 
-  output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints);
+  output.vehicle_passing_areas = create_vehicle_passing_areas(output.vehicle_footprints);
   output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true);
 
-  output.will_collide = willCollide(filtered_obstacle_pointcloud, output.vehicle_passing_areas);
+  output.will_collide = will_collide(filtered_obstacle_pointcloud, output.vehicle_passing_areas);
   output.processing_time_map["willCollide"] = stop_watch.toc(true);
 
   return output;
 }
 
-autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory(
+autoware_planning_msgs::msg::Trajectory resample_trajectory(
   const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval)
 {
   autoware_planning_msgs::msg::Trajectory resampled;
@@ -131,11 +121,9 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec
   for (size_t i = 1; i < trajectory.points.size() - 1; ++i) {
     const auto & point = trajectory.points.at(i);
 
-    const auto p1 =
-      autoware::universe_utils::fromMsg(resampled.points.back().pose.position).to_2d();
-    const auto p2 = autoware::universe_utils::fromMsg(point.pose.position).to_2d();
-
-    if (boost::geometry::distance(p1, p2) > interval) {
+    const auto distance =
+      autoware::universe_utils::calcDistance2d(resampled.points.back(), point.pose.position);
+    if (distance > interval) {
       resampled.points.push_back(point);
     }
   }
@@ -144,7 +132,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec
   return resampled;
 }
 
-autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory(
+autoware_planning_msgs::msg::Trajectory cut_trajectory(
   const autoware_planning_msgs::msg::Trajectory & trajectory, const double length)
 {
   autoware_planning_msgs::msg::Trajectory cut;
@@ -157,8 +145,8 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory(
 
     const auto p1 = autoware::universe_utils::fromMsg(cut.points.back().pose.position);
     const auto p2 = autoware::universe_utils::fromMsg(point.pose.position);
-    const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d());
 
+    const auto points_distance = boost::geometry::distance(p1, p2);
     const auto remain_distance = length - total_length;
 
     // Over length
@@ -187,7 +175,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory(
   return cut;
 }
 
-std::vector<LinearRing2d> ObstacleCollisionChecker::createVehicleFootprints(
+std::vector<LinearRing2d> create_vehicle_footprints(
   const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param,
   const autoware::vehicle_info_utils::VehicleInfo & vehicle_info)
 {
@@ -205,22 +193,21 @@ std::vector<LinearRing2d> ObstacleCollisionChecker::createVehicleFootprints(
   return vehicle_footprints;
 }
 
-std::vector<LinearRing2d> ObstacleCollisionChecker::createVehiclePassingAreas(
+std::vector<LinearRing2d> create_vehicle_passing_areas(
   const std::vector<LinearRing2d> & vehicle_footprints)
 {
   // Create hull from two adjacent vehicle footprints
   std::vector<LinearRing2d> areas;
   for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) {
     const auto & footprint1 = vehicle_footprints.at(i);
     const auto & footprint2 = vehicle_footprints.at(i + 1);
-    areas.push_back(createHullFromFootprints(footprint1, footprint2));
+    areas.push_back(create_hull_from_footprints(footprint1, footprint2));
   }
 
   return areas;
 }
 
-LinearRing2d ObstacleCollisionChecker::createHullFromFootprints(
-  const LinearRing2d & area1, const LinearRing2d & area2)
+LinearRing2d create_hull_from_footprints(const LinearRing2d & area1, const LinearRing2d & area2)
 {
   autoware::universe_utils::MultiPoint2d combined;
   for (const auto & p : area1) {
@@ -234,33 +221,32 @@ LinearRing2d ObstacleCollisionChecker::createHullFromFootprints(
   return hull;
 }
 
-bool ObstacleCollisionChecker::willCollide(
+bool will_collide(
   const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
   const std::vector<LinearRing2d> & vehicle_footprints)
 {
   for (size_t i = 1; i < vehicle_footprints.size(); i++) {
     // skip first footprint because surround obstacle checker handle it
     const auto & vehicle_footprint = vehicle_footprints.at(i);
-    if (hasCollision(obstacle_pointcloud, vehicle_footprint)) {
-      RCLCPP_WARN(
-        rclcpp::get_logger("obstacle_collision_checker"), "ObstacleCollisionChecker::willCollide");
+    if (has_collision(obstacle_pointcloud, vehicle_footprint)) {
+      RCLCPP_WARN(rclcpp::get_logger("obstacle_collision_checker"), "willCollide");
       return true;
     }
   }
 
   return false;
 }
 
-bool ObstacleCollisionChecker::hasCollision(
+bool has_collision(
   const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
   const LinearRing2d & vehicle_footprint)
 {
   for (const auto & point : obstacle_pointcloud.points) {
     if (boost::geometry::within(
           autoware::universe_utils::Point2d{point.x, point.y}, vehicle_footprint)) {
       RCLCPP_WARN(
-        rclcpp::get_logger("obstacle_collision_checker"),
-        "[ObstacleCollisionChecker] Collide to Point x: %f y: %f", point.x, point.y);
+        rclcpp::get_logger("obstacle_collision_checker"), "Collide to Point x: %f y: %f", point.x,
+        point.y);
       return true;
     }
   }