feat: [codecov/refactoring] [simple_pure_pursuit] isolation of core logics, and full revamp of unit tests (#1182)

* init simpupu core logics module with Apache license

* init simpupu core logics module with Apache license

* init simpupu core logics header with Apache license

* defined simpupu params struct

* define public funcs

* defined private funcs and entities for core logics

* implement simple pure pursuit constructor and param setter

* isolate create_control_command's core logics from the mess of ROS2+core logics in original node module

* isolatecalc_longitudinal_control the same way

* isolate calc_lateral_coltrol the same way

* modified calc_lateral_control as for now completely immitates the original func (after discussing with the team)

* refactored the simpupu main header - removing all core logics, leaving only ROS-related

* refactored the simpupu main module - same way as headers - only ROS2 related + core logics inference

* quick fixes based for clang compiler suggests

* adapt cmakelists, package xml and header of core logics, thus wrapping up step 2

* test_simple_pure_pursuit.cpp - first make it work on build

* removed the old test cases in unit test module because the old ones are bundled and obsoleted with new architecture

* added test 1 of perfectly normal case ahppy tracking

* added test 2 of strong terminal brake at trajectory goal

* added test 3 of strong terminal brake when trajectory is wayyyyyyyy too short

* added test 4 of overriding with external target velocity

* adeed test 5 of lateral offset correcting

* added test 6 of lookahead distance clamping (the current calc logic is weird so I had to resort to infinity check)

* added final branch coverage case of lookahead point search exceeding trajectory length case

* some final touchups, all good I guess

* style(pre-commit): autofix

* deal with spell-check-differential

* fixed that cpplink precommit test of you dont need a ; after a } thingy

* style(pre-commit): autofix

* fixed that cpplint precommit of Add #include <memory> for make_unique<>  [build/include_what_you_use]

* change all  to  (yeah lol right, tbf I've used English for too long to the point of not thinking twice about plural single forms of these words

* style(pre-commit): autofix

* refactoring the file/class names as Ishikawa-san suggested

* style(pre-commit): autofix

* bring trajectory fallback to core logics

* prevent node to return early upon false validation

* dropping set_params() for ummitability

* successful build

* rebased, gonna add the precommit lint locally later

* style(pre-commit): autofix

* rebase

* style(pre-commit): autofix

* fixed spellcheck differentials

* unified namespaces

* Update control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp

Co-authored-by: Takayuki AKAMINE <38586589+takam5f2@users.noreply.github.com>

* [akamine] redundant function set_params() at header

* [akamine] remove branch redundancy in clongitudinal control command core logic

* [akamine] name the magic number -10.0 as terminal_brake_accel at simple_pure_pursuit logic header and apply it to whole node

* [akamine] fix typo EXCEPT=>EXPECT

* [akamine] fix class name in docstring

* [akamine] node name inconsistency fix

---------

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Takayuki AKAMINE <38586589+takam5f2@users.noreply.github.com>

Author: 3 people

.gitignore

@@ -16,3 +16,5 @@ log/
 
 #prettier
 node_modules/
+.cache/
+lcov/

control/autoware_simple_pure_pursuit/CMakeLists.txt

@@ -15,6 +15,9 @@ rclcpp_components_register_node(${PROJECT_NAME}_lib
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
+  find_package(autoware_testing REQUIRED)
+  find_package(autoware_test_utils REQUIRED)
+
   ament_add_ros_isolated_gtest(test_simple_pure_pursuit_integration
     test/test_simple_pure_pursuit_integration.cpp
   )

control/autoware_simple_pure_pursuit/package.xml

@@ -27,6 +27,8 @@
   <test_depend>ament_index_cpp</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
+  <test_depend>autoware_test_utils</test_depend>
+  <test_depend>autoware_testing</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp

@@ -21,53 +21,20 @@
 
 namespace autoware::control::simple_pure_pursuit
 {
-using autoware::motion_utils::findNearestIndex;
 
-SimplePurePursuitNode::SimplePurePursuitNode(const rclcpp::NodeOptions & node_options)
-: Node("simple_pure_pursuit", node_options),
-  pub_control_command_(
-    create_publisher<autoware_control_msgs::msg::Control>(
-      "~/output/control_command", rclcpp::QoS(1).transient_local())),
-  vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()),
-  lookahead_gain_(declare_parameter<float>("lookahead_gain")),
-  lookahead_min_distance_(declare_parameter<float>("lookahead_min_distance")),
-  speed_proportional_gain_(declare_parameter<float>("speed_proportional_gain")),
-  use_external_target_vel_(declare_parameter<bool>("use_external_target_vel")),
-  external_target_vel_(declare_parameter<float>("external_target_vel"))
-{
-  using namespace std::literals::chrono_literals;
-  timer_ = rclcpp::create_timer(
-    this, get_clock(), 30ms, std::bind(&SimplePurePursuitNode::on_timer, this));
-}
+using autoware::motion_utils::findNearestIndex;
 
-void SimplePurePursuitNode::on_timer()
+// Constructor
+SimplePurePursuit::SimplePurePursuit(const SimplePurePursuitParameters & params) : params_(params)
 {
-  // 1. subscribe data
-  const auto odom_ptr = odom_sub_.take_data();
-  const auto traj_ptr = traj_sub_.take_data();
-  if (!odom_ptr || !traj_ptr) {
-    return;
-  }
-
-  // 2. extract subscribed data
-  const auto odom = *odom_ptr;
-  const auto traj = *traj_ptr;
-
-  // 3. create control command
-  const auto control_command = create_control_command(odom, traj);
-
-  // 4. publish control command
-  pub_control_command_->publish(control_command);
 }
 
-autoware_control_msgs::msg::Control SimplePurePursuitNode::create_control_command(
-  const Odometry & odom, const Trajectory & traj)
+// Core logic for creating control command based on current odom and traj
+autoware_control_msgs::msg::Control SimplePurePursuit::create_control_command(
+  const nav_msgs::msg::Odometry & odom, const autoware_planning_msgs::msg::Trajectory & traj) const
 {
-  // Check for empty trajectory
+  // Trajectory fallback now at core logic too
   if (traj.points.empty()) {
-    RCLCPP_WARN_THROTTLE(
-      get_logger(), *get_clock(), 5000,
-      "Received empty trajectory, returning zero velocity command.");
     autoware_control_msgs::msg::Control cmd;
     cmd.stamp = odom.header.stamp;
     cmd.longitudinal.velocity = 0.0;
@@ -77,83 +44,87 @@ autoware_control_msgs::msg::Control SimplePurePursuitNode::create_control_comman
 
   const size_t closest_traj_point_idx = findNearestIndex(traj.points, odom.pose.pose.position);
 
-  // when the ego reaches the goal
-  if (closest_traj_point_idx == traj.points.size() - 1 || traj.points.size() <= 5) {
+  // When ego reaches goal or traj is too short, stop vehicle
+  if ((closest_traj_point_idx == traj.points.size() - 1) || (traj.points.size() <= 5)) {
     autoware_control_msgs::msg::Control control_command;
     control_command.stamp = odom.header.stamp;
     control_command.longitudinal.velocity = 0.0;
-    control_command.longitudinal.acceleration = -10.0;
+    control_command.longitudinal.acceleration = terminal_brake_accel;
     control_command.longitudinal.is_defined_acceleration = true;
-    RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "reached to the goal");
+
     return control_command;
   }
 
-  // calculate target longitudinal velocity
+  // Calculate target longitudinal velocity
   const double target_longitudinal_vel =
-    use_external_target_vel_ ? external_target_vel_
-                             : traj.points.at(closest_traj_point_idx).longitudinal_velocity_mps;
+    params_.use_external_target_vel
+      ? params_.external_target_vel
+      : traj.points.at(closest_traj_point_idx).longitudinal_velocity_mps;
 
-  // calculate control command
+  // Calculate control command
   autoware_control_msgs::msg::Control control_command;
   control_command.stamp = odom.header.stamp;
   control_command.longitudinal = calc_longitudinal_control(odom, target_longitudinal_vel);
   control_command.lateral =
     calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx);
 
   return control_command;
-}
+};
 
-autoware_control_msgs::msg::Longitudinal SimplePurePursuitNode::calc_longitudinal_control(
-  const Odometry & odom, const double target_longitudinal_vel) const
+// Core logic for calculating longitudinal control command
+autoware_control_msgs::msg::Longitudinal SimplePurePursuit::calc_longitudinal_control(
+  const nav_msgs::msg::Odometry & odom, const double target_longitudinal_vel) const
 {
-  const double current_longitudinal_vel = odom.twist.twist.linear.x;
-
   autoware_control_msgs::msg::Longitudinal longitudinal_control_command;
+  const double current_velocity = odom.twist.twist.linear.x;
+
   longitudinal_control_command.velocity = static_cast<float>(target_longitudinal_vel);
   longitudinal_control_command.acceleration = static_cast<float>(
-    speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel));
+    (target_longitudinal_vel - current_velocity) * params_.speed_proportional_gain);
 
   return longitudinal_control_command;
-}
+};
 
-autoware_control_msgs::msg::Lateral SimplePurePursuitNode::calc_lateral_control(
-  const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel,
-  const size_t closest_traj_point_idx) const
+// Core logic for calculating lateral control command
+autoware_control_msgs::msg::Lateral SimplePurePursuit::calc_lateral_control(
+  const nav_msgs::msg::Odometry & odom, const autoware_planning_msgs::msg::Trajectory & traj,
+  const double target_longitudinal_vel, const size_t closest_traj_point_idx) const
 {
-  // calculate lookahead distance
+  // Calculate lookahead distance
   const double lookahead_distance =
-    lookahead_gain_ * target_longitudinal_vel + lookahead_min_distance_;
+    params_.lookahead_gain * target_longitudinal_vel + params_.lookahead_min_distance;
 
-  // calculate center coordinate of rear wheel
+  // Calculate center coordinate of rear wheel
   const double vehicle_heading = tf2::getYaw(odom.pose.pose.orientation);
   const double rear_x =
-    odom.pose.pose.position.x - vehicle_info_.wheel_base_m / 2.0 * std::cos(vehicle_heading);
+    odom.pose.pose.position.x - params_.wheel_base_m / 2.0 * std::cos(vehicle_heading);
   const double rear_y =
-    odom.pose.pose.position.y - vehicle_info_.wheel_base_m / 2.0 * std::sin(vehicle_heading);
+    odom.pose.pose.position.y - params_.wheel_base_m / 2.0 * std::sin(vehicle_heading);
 
-  // search lookahead point
+  // Search for the lookahead point on the trajectory
   auto lookahead_point_itr = std::find_if(
     traj.points.begin() + static_cast<std::ptrdiff_t>(closest_traj_point_idx), traj.points.end(),
-    [&](const TrajectoryPoint & point) {
+    [&](const autoware_planning_msgs::msg::TrajectoryPoint & point) {
       return std::hypot(point.pose.position.x - rear_x, point.pose.position.y - rear_y) >=
              lookahead_distance;
     });
+
   if (lookahead_point_itr == traj.points.end()) {
     lookahead_point_itr = traj.points.end() - 1;
   }
+
   const double lookahead_point_x = lookahead_point_itr->pose.position.x;
   const double lookahead_point_y = lookahead_point_itr->pose.position.y;
 
-  // calculate steering angle
+  // Calculate pure pursuit steering angle
   autoware_control_msgs::msg::Lateral lateral_control_command;
   const double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) -
                        tf2::getYaw(odom.pose.pose.orientation);
+
   lateral_control_command.steering_tire_angle = static_cast<float>(
-    std::atan2(2.0 * vehicle_info_.wheel_base_m * std::sin(alpha), lookahead_distance));
+    std::atan2(2.0 * params_.wheel_base_m * std::sin(alpha), lookahead_distance));
 
   return lateral_control_command;
 }
-}  // namespace autoware::control::simple_pure_pursuit
 
-#include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(autoware::control::simple_pure_pursuit::SimplePurePursuitNode)
+};  // namespace autoware::control::simple_pure_pursuit

control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp

@@ -15,63 +15,83 @@
 #ifndef SIMPLE_PURE_PURSUIT_HPP_
 #define SIMPLE_PURE_PURSUIT_HPP_
 
-#include <autoware_utils_rclcpp/polling_subscriber.hpp>
-#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
-#include <rclcpp/rclcpp.hpp>
-
 #include <autoware_control_msgs/msg/control.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
-#include <autoware_planning_msgs/msg/trajectory_point.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
 namespace autoware::control::simple_pure_pursuit
 {
-using autoware_planning_msgs::msg::Trajectory;
-using autoware_planning_msgs::msg::TrajectoryPoint;
-using nav_msgs::msg::Odometry;
 
-class SimplePurePursuitNode : public rclcpp::Node
+/**
+@ brief Params for SimplePurePursuit class
+*
+* @param lookahead_gain Gain for calculating lookahead distance
+* @param lookahead_min_distance Minimum lookahead distance
+* @param speed_proportional_gain Proportional gain for longitudinal control
+* @param use_external_target_vel Whether to use external target velocity or not
+* @param external_target_vel External target velocity to use when use_external_target_vel is true
+* @param wheel_base_m Wheel base of vehicle in meters
+*/
+struct SimplePurePursuitParameters
 {
-public:
-  explicit SimplePurePursuitNode(const rclcpp::NodeOptions & node_options);
-
-private:
-  // subscribers
-  autoware_utils_rclcpp::InterProcessPollingSubscriber<Odometry> odom_sub_{
-    this, "~/input/odometry"};
-  autoware_utils_rclcpp::InterProcessPollingSubscriber<Trajectory> traj_sub_{
-    this, "~/input/trajectory"};
+  double lookahead_gain;
+  double lookahead_min_distance;
+  double speed_proportional_gain;
+  bool use_external_target_vel;
+  double external_target_vel;
+  double wheel_base_m;
+};
 
-  // publishers
-  rclcpp::Publisher<autoware_control_msgs::msg::Control>::SharedPtr pub_control_command_;
+class SimplePurePursuit
+{
+public:
+  // Constructor
+  explicit SimplePurePursuit(const SimplePurePursuitParameters & params);
 
-  // timer
-  rclcpp::TimerBase::SharedPtr timer_;
+  /**
+   * @brief Create control command based on current odom and traj
+   *
+   * @param odom Current odometry of vehicle
+   * @param traj Current trajectory to follow
+   *
+   * @return Control command to be published
+   */
+  [[nodiscard]] autoware_control_msgs::msg::Control create_control_command(
+    const nav_msgs::msg::Odometry & odom,
+    const autoware_planning_msgs::msg::Trajectory & traj) const;
 
-  // vehicle info
-  const autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
+  // Hard-coded acceleration for terminal brake
+  static constexpr double terminal_brake_accel = -10.0;
 
-  // pure pursuit parameters
-  const double lookahead_gain_;
-  const double lookahead_min_distance_;
-  const double speed_proportional_gain_;
-  const bool use_external_target_vel_;
-  const double external_target_vel_;
+private:
+  const SimplePurePursuitParameters params_;
 
-  // functions
-  void on_timer();
-  autoware_control_msgs::msg::Control create_control_command(
-    const Odometry & odom, const Trajectory & traj);
-  autoware_control_msgs::msg::Longitudinal calc_longitudinal_control(
-    const Odometry & odom, const double target_longitudinal_vel) const;
-  autoware_control_msgs::msg::Lateral calc_lateral_control(
-    const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel,
-    const size_t closest_traj_point_idx) const;
+  /**
+   * @brief Calculate longitudinal control command
+   *
+   * @param odom Current odometry of vehicle
+   * @param target_longitudinal_vel Target longitudinal velocity to achieve
+   *
+   * @return Longitudinal control command
+   */
+  [[nodiscard]] autoware_control_msgs::msg::Longitudinal calc_longitudinal_control(
+    const nav_msgs::msg::Odometry & odom, const double target_longitudinal_vel) const;
 
-public:
-  friend class SimplePurePursuitNodeTest;
+  /**
+   * @brief Calculate lateral control command
+   *
+   * @param odom Current odometry of vehicle
+   * @param traj Current trajectory to follow
+   * @param target_longitudinal_vel Target longitudinal velocity to achieve
+   * @param closest_traj_point_idx Index of closest trajectory point to current vehicle position
+   *
+   * @return Lateral control command
+   */
+  [[nodiscard]] autoware_control_msgs::msg::Lateral calc_lateral_control(
+    const nav_msgs::msg::Odometry & odom, const autoware_planning_msgs::msg::Trajectory & traj,
+    const double target_longitudinal_vel, const size_t closest_traj_point_idx) const;
 };
 
-}  // namespace autoware::control::simple_pure_pursuit
+};  // namespace autoware::control::simple_pure_pursuit
 
 #endif  // SIMPLE_PURE_PURSUIT_HPP_