py_test for collide and intersection fix viewer bug

bark/geometry/tests/py_geometry_tests.py

@@ -246,6 +246,109 @@ def test_bounding_box_polygon(self):
 
         bbox_poly = CalculateBoundingBoxPolygon(shape)
         self.assertTrue(Within(shape, bbox_poly))
+
+    def test_poly_geometry_collide(self):
+        poly1 = Polygon2d()
+        poly1.AddPoint([0, 0])
+        poly1.AddPoint([0, 2])
+        poly1.AddPoint([4, 2])
+        poly1.AddPoint([4, 0])
+        poly1.AddPoint([0, 0])
+        self.assertTrue(poly1.Valid())
+
+        p1 = Point2d(2, 1)
+        p2 = Point2d(1, 5)
+        self.assertTrue(Collide(poly1,p1))
+        self.assertFalse(Collide(poly1,p2))
+
+        p3 = Point2d(4, 5)
+        lin1 = Line2d()
+        lin1.AddPoint(p1)
+        lin1.AddPoint(p2)
+        self.assertTrue(Collide(poly1,lin1))
+        lin2 = Line2d()
+        lin2.AddPoint(p3)
+        lin2.AddPoint(p2)
+        self.assertFalse(Collide(poly1,lin2))
+
+        poly2 = Polygon2d()
+        poly2.AddPoint([3, 0])
+        poly2.AddPoint([3, 2])
+        poly2.AddPoint([8, 2])
+        poly2.AddPoint([8, 0])
+        poly2.AddPoint([3, 0])
+        self.assertTrue(Collide(poly1,poly2))
+        poly3 = Polygon2d()
+        poly3.AddPoint([5, 0])
+        poly3.AddPoint([5, 2])
+        poly3.AddPoint([8, 2])
+        poly3.AddPoint([8, 0])
+        poly3.AddPoint([5, 0])
+        self.assertFalse(Collide(poly1,poly3))
+        # a2 = poly2.CalculateArea()
+
+    def test_poly_geometry_intersesction(self):
+        poly1 = Polygon2d()
+        poly1.AddPoint([0, 0])
+        poly1.AddPoint([0, 2])
+        poly1.AddPoint([4, 2])
+        poly1.AddPoint([4, 0])
+        poly1.AddPoint([0, 0])
+        self.assertTrue(poly1.Valid())
+
+        p0 = Point2d(2, 2)
+        p1 = Point2d(2, 1)
+        p2 = Point2d(2, 5)
+        lin1 = Line2d()
+        lin1.AddPoint(p1)
+        lin1.AddPoint(p2)
+        inte_pts = Intersection(poly1,lin1)
+        self.assertEqual(Distance(p0, inte_pts[0]), 0)
+        self.assertEqual(len(inte_pts), 1)
+
+        lin2 = Line2d()
+        lin2.AddPoint([0, 2])
+        lin2.AddPoint([4, 2])
+        inte_pts = Intersection(lin2,lin1)
+        self.assertEqual(Distance(p0, inte_pts[0]), 0)
+        self.assertEqual(len(inte_pts), 1)
+
+        poly2 = Polygon2d()
+        poly2.AddPoint([3, 0])
+        poly2.AddPoint([3, 2])
+        poly2.AddPoint([8, 2])
+        poly2.AddPoint([8, 0])
+        poly2.AddPoint([3, 0])
+        inte_pts = Intersection(poly1,poly2)
+        poly_int = Polygon2d()
+        for pt in inte_pts:
+          poly_int.AddPoint(pt)
+        poly_int.AddPoint(inte_pts[0])
+        self.assertTrue(poly_int.Valid())
+        a_int = poly_int.CalculateArea()
+        self.assertEqual(a_int, 2)
+
+        poly_int2 = Polygon2d([0, 0, 0], inte_pts)
+        self.assertTrue(poly_int2.Valid())
+        a_int2 = poly_int2.CalculateArea()
+        self.assertEqual(a_int2, 2)
+
+        poly3 = Polygon2d()
+        poly3.AddPoint([5, 0])
+        poly3.AddPoint([5, 2])
+        poly3.AddPoint([8, 2])
+        poly3.AddPoint([8, 0])
+        poly3.AddPoint([5, 0])
+        inte_pts = Intersection(poly1,poly3)
+        self.assertEqual(len(inte_pts), 0)
+
+        arr = np.array([[0, 0], [0, 2], [4, 2], [4, 0], [0, 0]])
+        poly4 = Polygon2d([1, 3, 0], arr)
+        poly5 = Polygon2d([0, 0, 0], arr)
+        inte_pts45 = Intersection(poly4,poly5)
+        poly_int45 = Polygon2d([0, 0, 0], inte_pts45)
+        self.assertEqual(poly_int45.CalculateArea(), 8)
+
 
 if __name__ == '__main__':
     unittest.main()

bark/models/behavior/dynamic_model/dynamic_model.hpp

@@ -6,9 +6,10 @@
 // This work is licensed under the terms of the MIT license.
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
-#ifndef BARK_MODELS_BEHAVIOR_DYNAMIC_MODEL_HPP_
-#define BARK_MODELS_BEHAVIOR_DYNAMIC_MODEL_HPP_
+#ifndef BARK_MODELS_BEHAVIOR_DYNAMIC_MODEL_DYNAMIC_MODEL_HPP_
+#define BARK_MODELS_BEHAVIOR_DYNAMIC_MODEL_DYNAMIC_MODEL_HPP_
 
+#include <memory>
 #include "bark/models/behavior/behavior_model.hpp"
 #include "bark/models/dynamic/dynamic_model.hpp"
 #include "bark/world/world.hpp"
@@ -37,7 +38,12 @@ class BehaviorDynamicModel : public BehaviorModel {
 
   virtual std::shared_ptr<BehaviorModel> Clone() const;
 
-  virtual void ActionToBehavior(const Action& action) { action_ = action; }
+  virtual void ActionToBehavior(const Action& action) {
+    action_ = action;
+    BehaviorModel::ActionToBehavior(action);
+    BehaviorModel::SetLastAction(action);
+    }
+  Action GetAction() const { return action_; }
 
  private:
   double integration_time_delta_;
@@ -54,4 +60,4 @@ inline std::shared_ptr<BehaviorModel> BehaviorDynamicModel::Clone() const {
 }  // namespace models
 }  // namespace bark
 
-#endif  // BARK_MODELS_BEHAVIOR_DYNAMIC_MODEL_HPP_
+#endif  // BARK_MODELS_BEHAVIOR_DYNAMIC_MODEL_DYNAMIC_MODEL_HPP_

bark/python_wrapper/geometry/geometry.cpp

@@ -246,6 +246,8 @@ void python_geometry(py::module m) {
         "return intersection points");
   m.def("Intersection", &bark::geometry::Intersection<Line, Line>,
         "return intersection points");
+  m.def("Intersection", &bark::geometry::Intersection<Polygon, Polygon>,
+        "return intersection points");
 
   python_standard_shapes(
       m.def_submodule("standard_shapes",

bark/python_wrapper/models/behavior.cpp

@@ -344,7 +344,8 @@ void python_behavior(py::module m) {
              shared_ptr<BehaviorDynamicModel>>(m, "BehaviorDynamicModel")
       .def(py::init<const bark::commons::ParamsPtr&>())
       .def("SetLastAction", &BehaviorModel::SetLastAction)
-      .def("GetAction", &BehaviorModel::GetAction)
+      .def("GetAction", &BehaviorDynamicModel::GetAction)
+      .def("ActionToBehavior", &BehaviorDynamicModel::ActionToBehavior)
       .def("__repr__",
            [](const BehaviorDynamicModel& b) {
              return "bark.behavior.BehaviorDynamicModel";

bark/python_wrapper/world/map.cpp

@@ -89,6 +89,7 @@ void python_map(py::module m) {
       .def_property_readonly("lanes", &RoadCorridor::GetLanes)
       .def_property_readonly("road_ids", &RoadCorridor::GetRoadIds)
       .def("GetRoad", &RoadCorridor::GetRoad)
+      .def("GetHash", &RoadCorridor::GetHash)
       .def_property_readonly("polygon", &RoadCorridor::GetPolygon)
       .def("GetLaneCorridor", &RoadCorridor::GetLaneCorridor)
       .def("GetCurrentLaneCorridor", &RoadCorridor::GetCurrentLaneCorridor)

bark/runtime/viewer/viewer.py

@@ -392,6 +392,19 @@ def drawWorldImplementation(self, world, eval_agent_ids=None, filename=None, sce
                 scenario_idx), fontsize=14)
             self.drawText(position=(0.1, 0.95),
                           text="Time: {:.2f}".format(world.time), fontsize=14)
+            if eval_agent_ids:
+                cur_state = world.agents[eval_agent_ids[0]].state
+                if isinstance(cur_state,np.ndarray):
+                    self.drawText(position=(0.1, 0.85), text="Velocity: {:.2f} m/s".format(
+                    cur_state[int(StateDefinition.VEL_POSITION)]), fontsize=14)
+
+                curr_action = world.agents[eval_agent_ids[0]].behavior_model.GetLastAction()
+                if isinstance(curr_action,np.ndarray):
+                    self.drawText(position=(0.1, 0.8), text="Acceleration: {:.2f} m/s²".format(
+                    curr_action[0]), fontsize=14)
+                    self.drawText(position=(0.1, 0.75), text="SteeringRate: {:.2f} rad/s".format(
+                    curr_action[1]), fontsize=14)
+
 
         if self.draw_ltl_debug_info:
             self.drawLTLDebugInfomation(world, eval_agent_ids[0])