feat(autoware_control_validator): refactoring & testing (autowarefoun…

…dation#8096) * refactoring Signed-off-by: Y.Hisaki <[email protected]> * updating... Signed-off-by: Y.Hisaki <[email protected]> * update Signed-off-by: Y.Hisaki <[email protected]> * fix Signed-off-by: Y.Hisaki <[email protected]> * fix Signed-off-by: Y.Hisaki <[email protected]> * Update CMakeLists.txt * use yaml to load vehicle info Signed-off-by: Y.Hisaki <[email protected]> --------- Signed-off-by: Y.Hisaki <[email protected]>
go-sakayori · Jul 31, 2024 · 14fa0f3 · 14fa0f3
1 parent ccb3417
commit 14fa0f3
Show file tree

Hide file tree

Showing 9 changed files with 423 additions and 193 deletions.
diff --git a/control/autoware_control_validator/CMakeLists.txt b/control/autoware_control_validator/CMakeLists.txt
@@ -36,8 +36,13 @@ else()
   target_link_libraries(autoware_control_validator_component "${cpp_typesupport_target}")
 endif()
 
-# if(BUILD_TESTING)
-# endif()
+if(BUILD_TESTING)
+  file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
+  ament_add_gtest(test_autoware_control_validator
+    ${TEST_SOURCES}
+  )
+  target_link_libraries(test_autoware_control_validator autoware_control_validator_component)
+endif()
 
 ament_auto_package(
   INSTALL_TO_SHARE

diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp
@@ -17,18 +17,21 @@
 
 #include "autoware/control_validator/debug_marker.hpp"
 #include "autoware/universe_utils/ros/polling_subscriber.hpp"
-#include "autoware_control_validator/msg/control_validator_status.hpp"
-#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
+#include "autoware_vehicle_info_utils/vehicle_info.hpp"
+#include "diagnostic_updater/diagnostic_updater.hpp"
 
-#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <autoware_control_validator/msg/control_validator_status.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <cstdint>
 #include <memory>
 #include <string>
+#include <tuple>
+#include <utility>
 
 namespace autoware::control_validator
 {
@@ -46,58 +49,118 @@ struct ValidationParams
   double max_over_velocity_ratio_threshold;
 };
 
+/**
+ * @class ControlValidator
+ * @brief Validates control commands by comparing predicted trajectories against reference
+ * trajectories.
+ */
 class ControlValidator : public rclcpp::Node
 {
 public:
+  /**
+   * @brief Constructor
+   * @param options Node options
+   */
   explicit ControlValidator(const rclcpp::NodeOptions & options);
 
-  void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg);
-
-  bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory);
-  bool checkValidVelocityDeviation(
-    const Trajectory & reference_trajectory, const Odometry & kinematics);
+  /**
+   * @brief Callback function for the predicted trajectory.
+   * @param msg Predicted trajectory message
+   */
+  void on_predicted_trajectory(const Trajectory::ConstSharedPtr msg);
+
+  /**
+   * @brief Calculate the maximum lateral distance between the reference trajectory and predicted
+   * trajectory.
+   * @param predicted_trajectory Predicted trajectory
+   * @param reference_trajectory Reference trajectory
+   * @return A pair consisting of the maximum lateral deviation and a boolean indicating validity
+   */
+  std::pair<double, bool> calc_lateral_deviation_status(
+    const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const;
+
+  /**
+   * @brief Calculate the velocity deviation between the reference trajectory and the current
+   * vehicle kinematics.
+   * @param reference_trajectory Reference trajectory
+   * @param kinematics Current vehicle kinematics
+   * @return A tuple containing the current velocity, desired velocity, and a boolean indicating
+   * validity
+   */
+  std::tuple<double, double, bool> calc_velocity_deviation_status(
+    const Trajectory & reference_trajectory, const Odometry & kinematics) const;
 
 private:
-  void setupDiag();
-
-  void setupParameters();
-
-  bool isDataReady();
-
+  /**
+   * @brief Setup diagnostic updater
+   */
+  void setup_diag();
+
+  /**
+   * @brief Setup parameters from the parameter server
+   */
+  void setup_parameters();
+
+  /**
+   * @brief Check if all required data is ready for validation
+   * @return Boolean indicating readiness of data
+   */
+  bool is_data_ready();
+
+  /**
+   * @brief Validate the predicted trajectory against the reference trajectory and current
+   * kinematics
+   * @param predicted_trajectory Predicted trajectory
+   * @param reference_trajectory Reference trajectory
+   * @param kinematics Current vehicle kinematics
+   */
   void validate(
     const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory,
     const Odometry & kinematics);
 
-  void publishPredictedTrajectory();
-  void publishDebugInfo();
-  void displayStatus();
-
-  void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg);
-
+  /**
+   * @brief Publish debug information
+   */
+  void publish_debug_info();
+
+  /**
+   * @brief Display validation status on terminal
+   */
+  void display_status();
+
+  /**
+   * @brief Set the diagnostic status
+   * @param stat Diagnostic status wrapper
+   * @param is_ok Boolean indicating if the status is okay
+   * @param msg Status message
+   */
+  void set_status(
+    DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) const;
+
+  // Subscribers and publishers
   rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
-  autoware::universe_utils::InterProcessPollingSubscriber<Odometry> sub_kinematics_{
-    this, "~/input/kinematics"};
-  autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> sub_reference_traj_{
-    this, "~/input/reference_trajectory"};
+  universe_utils::InterProcessPollingSubscriber<Odometry>::SharedPtr sub_kinematics_;
+  universe_utils::InterProcessPollingSubscriber<Trajectory>::SharedPtr sub_reference_traj_;
   rclcpp::Publisher<ControlValidatorStatus>::SharedPtr pub_status_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;
 
   // system parameters
-  int diag_error_count_threshold_ = 0;
+  int64_t diag_error_count_threshold_ = 0;
   bool display_on_terminal_ = true;
 
   Updater diag_updater_{this};
 
   ControlValidatorStatus validation_status_;
   ValidationParams validation_params_;  // for thresholds
 
-  // ego nearest index search
-  double ego_nearest_dist_threshold_;
-  double ego_nearest_yaw_threshold_;
-
-  autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
+  vehicle_info_utils::VehicleInfo vehicle_info_;
 
-  bool isAllValid(const ControlValidatorStatus & status);
+  /**
+   * @brief Check if all validation criteria are met
+   * @param status Validation status
+   * @return Boolean indicating if all criteria are met
+   */
+  static bool is_all_valid(const ControlValidatorStatus & status);
 
   Trajectory::ConstSharedPtr current_reference_trajectory_;
   Trajectory::ConstSharedPtr current_predicted_trajectory_;

diff --git a/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp b/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp
@@ -22,23 +22,41 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <map>
-#include <memory>
 #include <string>
-#include <vector>
 
+/**
+ * @brief Class for publishing debug markers
+ */
 class ControlValidatorDebugMarkerPublisher
 {
 public:
+  /**
+   * @brief Constructor
+   */
   explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node);
 
-  void pushPoseMarker(
-    const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0);
-  void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0);
-  void pushVirtualWall(const geometry_msgs::msg::Pose & pose);
-  void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg);
+  /**
+   * @brief Push a virtual wall
+   * @param pose pose of the virtual wall
+   */
+  void push_virtual_wall(const geometry_msgs::msg::Pose & pose);
+
+  /**
+   * @brief Push a warning message
+   * @param pose pose of the warning message
+   * @param msg warning message
+   */
+  void push_warning_msg(const geometry_msgs::msg::Pose & pose, const std::string & msg);
+
+  /**
+   * @brief Publish markers
+   */
   void publish();
 
-  void clearMarkers();
+  /**
+   * @brief Clear markers
+   */
+  void clear_markers();
 
 private:
   rclcpp::Node * node_;
@@ -47,14 +65,6 @@ class ControlValidatorDebugMarkerPublisher
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_pub_;
   std::map<std::string, int> marker_id_;
-
-  int getMarkerId(const std::string & ns)
-  {
-    if (marker_id_.count(ns) == 0) {
-      marker_id_[ns] = 0.0;
-    }
-    return marker_id_[ns]++;
-  }
 };
 
 #endif  // AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_
diff --git a/control/autoware_control_validator/include/autoware/control_validator/utils.hpp b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp
@@ -15,41 +15,26 @@
 #ifndef AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_
 #define AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_
 
-#include <autoware/motion_utils/trajectory/conversion.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 
-#include <string>
-#include <utility>
-#include <vector>
-
 namespace autoware::control_validator
 {
-using autoware::motion_utils::convertToTrajectory;
-using autoware::motion_utils::convertToTrajectoryPointArray;
-using autoware_planning_msgs::msg::Trajectory;
-using autoware_planning_msgs::msg::TrajectoryPoint;
-using geometry_msgs::msg::Pose;
-using TrajectoryPoints = std::vector<TrajectoryPoint>;
-
-void shiftPose(Pose & pose, double longitudinal);
-
-void insertPointInPredictedTrajectory(
-  TrajectoryPoints & modified_trajectory, const geometry_msgs::msg::Pose & reference_pose,
-  const TrajectoryPoints & predicted_trajectory);
-
-TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory);
-
-bool removeFrontTrajectoryPoint(
-  const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points,
-  const TrajectoryPoints & predicted_trajectory_points);
-
-Trajectory alignTrajectoryWithReferenceTrajectory(
-  const Trajectory & trajectory, const Trajectory & predicted_trajectory);
-
-double calcMaxLateralDistance(
-  const Trajectory & trajectory, const Trajectory & predicted_trajectory);
+/**
+ * @brief Shift pose along the yaw direction
+ */
+void shift_pose(geometry_msgs::msg::Pose & pose, double longitudinal);
+
+/**
+ * @brief Calculate the maximum lateral distance between the reference trajectory and the predicted
+ * trajectory
+ * @param reference_trajectory reference trajectory
+ * @param predicted_trajectory predicted trajectory
+ */
+double calc_max_lateral_distance(
+  const autoware_planning_msgs::msg::Trajectory & reference_trajectory,
+  const autoware_planning_msgs::msg::Trajectory & predicted_trajectory);
 }  // namespace autoware::control_validator
 
 #endif  // AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_
diff --git a/control/autoware_control_validator/package.xml b/control/autoware_control_validator/package.xml
@@ -34,6 +34,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>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>