Merge pull request #525 from bark-simulator/safe_distance_label_lateral

Safe distance label lateral

Author: juloberno

bark/benchmark/tests/py_benchmark_runner_tests.py

@@ -49,7 +49,7 @@ def test_database_runner(self):
         safe_dist_params = ParameterServer(log_if_default=True)
         test_python_params = ParameterServer(log_if_default=True)
         evaluators = {"success" : "EvaluatorGoalReached", "collision" : "EvaluatorCollisionEgoAgent",
-                      "max_steps": "EvaluatorStepCount", "safe_dist_lon" : {"type" : "EvaluatorDynamicSafeDistLong", "params" : safe_dist_params},
+                      "max_steps": "EvaluatorStepCount", "safe_dist_lon" : {"type" : "EvaluatorDynamicSafeDist", "params" : safe_dist_params},
                       "safe_dist_lat" : {"type" : "EvaluatorStaticSafeDist", "params" : safe_dist_params},
                       "test_evaluator" : {"type" : "TestPythonEvaluator", "params" : test_python_params},
                       "test_evaluator_serializable" : TestPythonEvaluatorSerializable()}
@@ -173,7 +173,7 @@ def test_database_runner_python_behavior(self):
           db = BenchmarkDatabase(database_root=local_release_filename)
           safe_dist_params = ParameterServer(log_if_default=True)
           evaluators = {"success" : "EvaluatorGoalReached", "collision" : "EvaluatorCollisionEgoAgent",
-                        "max_steps": "EvaluatorStepCount", "safe_dist_lon" : {"type" : "EvaluatorDynamicSafeDistLong", "params" : safe_dist_params},
+                        "max_steps": "EvaluatorStepCount", "safe_dist_lon" : {"type" : "EvaluatorDynamicSafeDist", "params" : safe_dist_params},
                         "safe_dist_lat" : {"type" : "EvaluatorStaticSafeDist", "params" : safe_dist_params}}
           terminal_when = {"collision" :lambda x: x, "max_steps": lambda x : x>2, "safe_dist_lon" : lambda x: x }
           params = ParameterServer() # only for evaluated agents not passed to scenario!

bark/commons/tests/frenet_test.cc

@@ -221,6 +221,29 @@ TEST(calculate_state_diff, minus_pi2_minuspi4_oriented_no_overlap) {
   EXPECT_NEAR(diff.angle, B_PI_2*2/3, 0.001);
 }
 
+TEST(calculate_state_diff, zero_minus_3pi4_oriented_no_overlap) {
+  // some line with three points from x=1 to x=10, y=0
+  FrenetState state1{2.0, 0.0, 3.0, 1.0, 0.0, 0.0, 0.0};
+  FrenetState state2{11.0, -8.0, 1.0, 0.0, -3*B_PI/4, 0.0, 0.0};
+
+  double width1 = 2.0, width2 = 3.0, length1 = 5.0, length2 = 4.0;
+  const auto shape1 = Polygon(
+      Pose(0.5, width1/2, 0),
+      {Point2d(0, 0), Point2d(0, width1), Point2d(length1, width1),
+                           Point2d(length1, 0), Point2d(0, 0)});
+  const auto shape2 = Polygon(
+      Pose(0.5, width2/2, 0),
+      {Point2d(0, 0), Point2d(0, width2), Point2d(length2, width2),
+                           Point2d(length2, 0), Point2d(0, 0)});
+  FrenetStateDifference diff(state1, shape1, state2, shape2);
+
+  EXPECT_NEAR(diff.lon, 11.0 - 2.0 - 4.5 + cos(3*B_PI/4)*3.5 - sin(3*B_PI/4)*1.5, 0.001);
+  EXPECT_NEAR(diff.lat, - (8.0 -1.0 - sin(3*B_PI/4)*3.5 + cos(3*B_PI/4)*1.5), 0.001);
+  EXPECT_NEAR(diff.vlat, -1.0, 0.001);
+  EXPECT_NEAR(diff.vlon, -2.0, 0.001);
+  EXPECT_NEAR(diff.angle, -3*B_PI/4, 0.001);
+}
+
 TEST(calculate_state_diff, lateral_zero_oriented_no_overlap) {
   // some line with three points from x=1 to x=10, y=0
   FrenetState state1{1.0, 0.0, 3.0, 5.0, 0.0, 0.0, 0.0};

bark/commons/transformation/frenet_state.cpp

@@ -92,7 +92,7 @@ State FrenetStateToDynamicState(const FrenetState& frenet_state,
  * @param polygon 
  * @return A struct containing rear_dist, front_dist, left_dist, side_dist of rotated shape
  */
-auto ShapeExtensionAtTangentAngle(const double& tangent_angle, const Polygon& polygon) { 
+ShapeExtension ShapeExtensionAtTangentAngle(const double& tangent_angle, const Polygon& polygon) { 
   // Assumes equal extension to sides for polygon
   BARK_EXPECT_TRUE(std::abs(polygon.right_dist_ - polygon.left_dist_) < 0.01);
   const double polygon_side_extend = polygon.left_dist_;
@@ -111,8 +111,7 @@ auto ShapeExtensionAtTangentAngle(const double& tangent_angle, const Polygon& po
   } else {
     std::swap(left_dist, right_dist);
   }
-  const struct{double front_dist; double rear_dist; double left_dist; double right_dist;}
-         shape_extension{front_dist, rear_dist, left_dist, right_dist};
+  ShapeExtension shape_extension{front_dist, rear_dist, left_dist, right_dist};
   return shape_extension;
 }
 
@@ -140,10 +139,12 @@ FrenetStateDifference::FrenetStateDifference(const FrenetState& frenet_from, con
   if(from.lon <= to.lon) {
     double diff_lon = to.lon - shape_extend_at_tangent2.rear_dist -
                         (from.lon + shape_extend_at_tangent1.front_dist);
+    lon_zeroed = diff_lon <= 0;
     lon = diff_lon > 0 ? diff_lon : to.lon - from.lon;
   } else {
     double diff_lon = to.lon + shape_extend_at_tangent2.front_dist -
                       (from.lon - shape_extend_at_tangent1.rear_dist);
+    lon_zeroed = diff_lon >= 0;
     lon = diff_lon < 0 ? diff_lon : to.lon - from.lon;
   }
 
@@ -154,11 +155,13 @@ FrenetStateDifference::FrenetStateDifference(const FrenetState& frenet_from, con
     double diff_lat = to.lat - shape_extend_at_tangent2.right_dist -
                       (from.lat + shape_extend_at_tangent1.left_dist);
     // if shape consideration leads to negative distance use only pure lateral difference
+    lat_zeroed = diff_lat <= 0;
     lat = diff_lat > 0 ? diff_lat : to.lat - from.lat;
   } else {
     // lateral difference is negative
-    double diff_lat = to.lat - shape_extend_at_tangent1.right_dist -
-                      (from.lat + shape_extend_at_tangent2.left_dist);
+    double diff_lat = to.lat + shape_extend_at_tangent1.right_dist -
+                      (from.lat - shape_extend_at_tangent2.right_dist);
+    lat_zeroed = diff_lat >= 0;
     lat = diff_lat < 0 ? diff_lat : to.lat - from.lat;
   }
 

bark/commons/transformation/frenet_state.hpp

@@ -51,17 +51,22 @@ 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
-  FrenetStateDifference() : FrenetState(), from(), to() {}
+  // - 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);
   FrenetState from;
   FrenetState to;
+  bool lat_zeroed;
+  bool lon_zeroed;
 };
 
 bark::models::dynamic::State FrenetStateToDynamicState(
     const FrenetState& frenet_state, const bark::geometry::Line& path);
 
-auto ShapeExtensionAtTangentAngle(const double& tangent_angle, const bark::geometry::Polygon& polygon);
+typedef struct{double front_dist; double rear_dist; double left_dist; double right_dist;} ShapeExtension;
+ShapeExtension ShapeExtensionAtTangentAngle(const double& tangent_angle, const bark::geometry::Polygon& polygon);
 
 double LatAccStreetToVehicleCs(
     double acc_lat_street, double acc_lon, double delta_time,

bark/models/behavior/idm/idm_lane_tracking.hpp

@@ -27,7 +27,8 @@ class BehaviorIDMLaneTracking : public BaseIDM {
   explicit BehaviorIDMLaneTracking(const commons::ParamsPtr& params) :
     BehaviorModel(params),
     BaseIDM(params),
-    limit_steering_rate_(true) {
+    limit_steering_rate_(params->GetBool("BehaviorIDMLaneTracking::LimitSteeringRate",
+                        "Bool if steering limited according to dynamics model", true)) {
     crosstrack_error_gain_ =
         params->GetReal("BehaviorIDMLaneTracking::CrosstrackErrorGain",
                         "Tuning factor of stanley controller", 1.0);

bark/models/behavior/motion_primitives/primitives/primitive_const_acc_stay_lane.hpp

@@ -32,6 +32,8 @@ class PrimitiveConstAccStayLane : public Primitive,
 
   std::string GetName() const override;
 
+  double GetAcceleration() const { return acceleration_; }
+
   LaneCorridorPtr SelectTargetCorridor(
       const ObservedWorld& observed_world,
       const AdjacentLaneCorridors& adjacent_corridors) override;

bark/models/behavior/rule_based/lane_change_behavior.cpp

@@ -50,7 +50,11 @@ BehaviorLaneChangeRuleBased::FrontRearAgents(
     const ObservedWorld& observed_world,
     const LaneCorridorPtr& lane_corr) const {
   AgentInformation front_info, rear_info;
-  const auto& front_rear = observed_world.GetAgentFrontRear(lane_corr);
+  double lateral_difference_threshold = 100; // do not restrict regarding lateral diff
+  const auto& front_rear = observed_world.GetAgentFrontRearForId(observed_world.GetEgoAgentId(),
+                                         lane_corr,
+                                         lateral_difference_threshold,
+                                         true); // require to be in this lane corridor
   const auto& ego_agent = observed_world.GetEgoAgent();
   if (front_rear.front.first) {
     // front info

bark/models/dynamic/single_track.hpp

@@ -88,6 +88,39 @@ class SingleTrackModel : public DynamicModel {
   double GetLatAccelerationMax() const {
     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_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));
+    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_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);
+  }
+
   double GetLonAccelerationMax(const State& x) const {
     return acceleration_limits_.lon_acc_max;
   }
@@ -158,7 +191,7 @@ inline double CalculateSteeringAngle(const SingleTrackModelPtr& model,
 
   FrenetState f_state = FrenetState(state_front, ref_line);
   double vel = state(StateDefinition::VEL_POSITION);
-  double delta = -NormToPI(f_state.angle) + atan2(-gain * f_state.lat, vel);
+  double delta = +NormToPI(f_state.angle) + atan2(-gain * f_state.lat, vel);
 
   VLOG(5) << "del=" << delta << ", fa=" << f_state.angle << ", na=" << NormToPI(f_state.angle) <<
               ", g=" << gain << ", lat=" << f_state.lat << ", v=" << vel;

bark/python_wrapper/models/behavior.cpp

@@ -213,7 +213,8 @@ void python_behavior(py::module m) {
   py::class_<Primitive, PyPrimitive, PrimitivePtr>(m, "Primitive")
       .def(py::init<const bark::commons::ParamsPtr&>())
       .def("Plan", &Primitive::Plan)
-      .def("IsPreConditionSatisfied", &Primitive::IsPreConditionSatisfied);
+      .def("IsPreConditionSatisfied", &Primitive::IsPreConditionSatisfied)
+      .def_property_readonly("name", &Primitive::GetName);
 
   py::class_<BehaviorMPMacroActions, BehaviorModel,
              shared_ptr<BehaviorMPMacroActions>>(m, "BehaviorMPMacroActions")
@@ -292,13 +293,14 @@ void python_behavior(py::module m) {
       .def(py::init<const bark::commons::ParamsPtr&>())
       .def(py::pickle(
           [](const PrimitiveConstAccStayLane& b) {
-            return py::make_tuple(ParamsToPython(b.Primitive::GetParams()));
+            return py::make_tuple(ParamsToPython(b.Primitive::GetParams()), 
+                                    b.GetAcceleration());
           },
           [](py::tuple t) {
-            if (t.size() != 1)
+            if (t.size() != 2)
               throw std::runtime_error("Invalid behavior model state!");
             return new PrimitiveConstAccStayLane(
-                PythonToParams(t[0].cast<py::tuple>()));
+                PythonToParams(t[0].cast<py::tuple>()), t[1].cast<double>());
           }));
 
   py::class_<PrimitiveConstAccChangeToLeft, Primitive,

bark/python_wrapper/polymorphic_conversion.cpp

@@ -68,12 +68,14 @@
 #ifdef PLANNER_UCT
 #include "bark_mcts/models/behavior/behavior_uct_hypothesis.hpp"
 #include "bark_mcts/models/behavior/behavior_uct_risk_constraint.hpp"
+#include "bark_mcts/models/behavior/behavior_uct_nheuristic_risk_constraint.hpp"
 #include "bark_mcts/models/behavior/behavior_uct_cooperative.hpp"
 #include "bark_mcts/models/behavior/hypothesis/idm/hypothesis_idm.hpp"
 using bark::models::behavior::BehaviorHypothesisIDM;
 using bark::models::behavior::BehaviorUCTHypothesis;
 using bark::models::behavior::BehaviorUCTRiskConstraint;
 using bark::models::behavior::BehaviorUCTCooperative;
+using bark::models::behavior::BehaviorUCTNHeuristicRiskConstraint;
 #endif
 
 #ifdef PLANNER_MIQP
@@ -177,6 +179,8 @@ py::tuple BehaviorModelToPython(BehaviorModelPtr behavior_model) {
 #ifdef PLANNER_UCT
   else if (typeid(*behavior_model) == typeid(BehaviorUCTCooperative)) {
     behavior_model_name = "BehaviorUCTCooperative";
+  } else if (typeid(*behavior_model) == typeid(BehaviorUCTNHeuristicRiskConstraint)) {
+    behavior_model_name = "BehaviorUCTNHeuristicRiskConstraint";
   } else if (typeid(*behavior_model) == typeid(BehaviorUCTRiskConstraint)) {
     behavior_model_name = "BehaviorUCTRiskConstraint";
   } else if (typeid(*behavior_model) == typeid(BehaviorUCTHypothesis)) {
@@ -256,6 +260,9 @@ BehaviorModelPtr PythonToBehaviorModel(py::tuple t) {
   else if (behavior_model_name.compare("BehaviorUCTCooperative") == 0) {
     return std::make_shared<BehaviorUCTCooperative>(
         t[0].cast<BehaviorUCTCooperative>());
+  } else if (behavior_model_name.compare("BehaviorUCTNHeuristicRiskConstraint") == 0) {
+    return std::make_shared<BehaviorUCTNHeuristicRiskConstraint>(
+        t[0].cast<BehaviorUCTNHeuristicRiskConstraint>());
   } else if (behavior_model_name.compare("BehaviorUCTRiskConstraint") == 0) {
     return std::make_shared<BehaviorUCTRiskConstraint>(
         t[0].cast<BehaviorUCTRiskConstraint>());