comment on lateral acc calculation function

Author: juloberno

bark/commons/transformation/frenet_state.hpp

@@ -51,6 +51,8 @@ struct FrenetStateDifference : public FrenetState {
   // - positive lateral difference if frenet_state2 is in "left" of frenet_state1
   // - positive long and velocities diff if frenet state 2 is "faster" than frenet state1
   // - positive angle difference if frenet state2 is turned more left than frenet state 1
+  // - boolean flags lat_zeroed and lon_zeroed indicate if the lateral/longitudinal shape-based distance is zero
+  //                  giving that the calculated distances actually represent a state-based distance
   FrenetStateDifference() : FrenetState(), from(), to(), lat_zeroed(false), lon_zeroed(false) {}
   FrenetStateDifference(const FrenetState& frenet_state1, const bark::geometry::Polygon& polygon1,
                                         const FrenetState& frenet_state2, const bark::geometry::Polygon& polygon2);

bark/models/dynamic/single_track.hpp

@@ -89,23 +89,34 @@ class SingleTrackModel : public DynamicModel {
     return acceleration_limits_.lat_acc_max;
   }
 
+  /**
+   * @brief Calculates maximum possible lateral evasive acceleration away from center line of a lane
+   *        assuming single track vehicle dynamics
+   * 
+   * @param v velocity of vehicle in global coordinate system
+   * @param theta orientation of vehicle in global coordinate system
+   * @param tangent_angle angle of center line in global coordinate system, assumed to be constant over time
+   * @param a_total_max maximum possible acceleration of vehicle into direction of orientation
+   * @param on_left_of_center_line to calculate evasive lateral maximum acceleration, flag indicates on which side
+   *                              of center line the vehicle is
+   * @return double maximum possible lateral evasive acceleration
+   */
   double CalculateLatAccelerationMaxAtFrenetAngle(const double &v, const double& theta,
                                                  const double& tangent_angle,
-                                                 const double a_long_max,
+                                                 const double a_total_max,
                                                  bool on_left_of_center_line) const {
     const auto max_acc_lat_dyn = GetLatAccelerationMax();
     const double delta_max = std::atan2(max_acc_lat_dyn * wheel_base_, v * v);
     double delta_max_evasive;
     const double abs_angle_diff = std::abs(bark::geometry::SignedAngleDiff(theta, tangent_angle));
-    const double evasive_sign = abs_angle_diff < bark::geometry::B_PI_2;
     if ((on_left_of_center_line && abs_angle_diff < bark::geometry::B_PI_2) ||
         (!on_left_of_center_line && abs_angle_diff > bark::geometry::B_PI_2)) {
       delta_max_evasive = delta_max;
     } else {
       delta_max_evasive = delta_max;
     }
     const double max_lat_acc_from_steering = v*v*sin(delta_max_evasive)/wheel_base_*cos(tangent_angle+theta);
-    const double max_lat_acc_from_long_acc = std::abs(a_long_max)*(sin(tangent_angle-theta));
+    const double max_lat_acc_from_long_acc = std::abs(a_total_max)*(sin(tangent_angle-theta));
     const double max_lat_acc = max_lat_acc_from_long_acc-max_lat_acc_from_steering;
     return std::abs(max_lat_acc);
   }

bark/world/evaluation/ltl/label_functions/safe_distance_label_function.cpp

@@ -213,7 +213,7 @@ double SafeDistanceLabelFunction::CalcSafeDistance3(const double v_r,
 bool SafeDistanceLabelFunction::CheckSafeDistanceLateral(FrontRearAgents& fr_agents, const AgentPtr& ego_agent) const {  
   auto GetMaxAccLat = [](const AgentPtr& agent,
                            const double& road_angle,
-                           const double& max_long_acc,
+                           const double& max_total_acc,
                            const bool& on_left_side_of_center_line) {
     auto single_track = std::dynamic_pointer_cast<models::dynamic::SingleTrackModel>(
       agent->GetDynamicModel());
@@ -222,7 +222,7 @@ bool SafeDistanceLabelFunction::CheckSafeDistanceLateral(FrontRearAgents& fr_age
     const double a_max_lat = single_track->CalculateLatAccelerationMaxAtFrenetAngle(
                         state(StateDefinition::VEL_POSITION), 
                         state(StateDefinition::THETA_POSITION), 
-                        road_angle, max_long_acc,
+                        road_angle, max_total_acc,
                         on_left_side_of_center_line);
     return a_max_lat;
   };