Merge pull request #205 from bark-simulator/smaller_changes

Fixed Bug in the map generation and logging

.gitignore

@@ -151,4 +151,7 @@ web/node_modules
 .DS_Store
 .gitkeep
 
-*.scenario
+*.scenario
+
+Town01_copy.xodr
+Town01.xodr

.vscode/launch.json

@@ -11,7 +11,7 @@
             "program": "${file}",
             "console": "integratedTerminal",
             "env": {
-                "PYTHONPATH": "${workspaceFolder}/bazel-bin/modules/models/tests/py_behavior_model_test.runfiles/bark_project/python/:${workspaceFolder}/bazel-bin/modules/models/tests/py_behavior_model_test.runfiles/bark_project"
+                "PYTHONPATH": "${workspaceFolder}/bazel-bin/examples/analyze_map.runfiles/bark_project/python/:${workspaceFolder}/bazel-bin/examples/analyze_map.runfiles/bark_project"
 			   }
         },
         {
@@ -38,7 +38,7 @@
             "request": "attach",
             "program": "${workspaceFolder}/python/venv/bin/python3",
             "cwd" : "${workspaceFolder}",
-            "additionalSOLibSearchPath":"${workspaceFolder}/bazel-bin/modules/models/tests/py_behavior_model_test.runfiles/__main__/python",
+            "additionalSOLibSearchPath":"${workspaceFolder}/bazel-bin/examples/analyze_map.runfiles/__main__/python",
             "processId": "${command:pickProcess}",
             "MIMode": "gdb",
             "sourceFileMap" : {"/proc/self/cwd/": "${workspaceFolder}"}

examples/analyze_map.py

@@ -10,6 +10,7 @@
 import filecmp
 import matplotlib.pyplot as plt
 from bark.world import World
+from bark.geometry import compute_center_line
 from bark.world.map import MapInterface
 from modules.runtime.commons.parameters import ParameterServer
 from modules.runtime.commons.xodr_parser import XodrParser
@@ -19,7 +20,7 @@
 # Name and Output Directory
 # CHANGE THIS #
 map_name = "4way_intersection"
-output_dir = "/home/esterle/map_analysis" + map_name
+output_dir = "/home/hart/Dokumente/2020/" + map_name
 
 # Map Definition
 xodr_parser = XodrParser("modules/runtime/tests/data/" + map_name + ".xodr")
@@ -37,7 +38,8 @@
 
 open_drive_map = world.map.get_open_drive_map()
 
-viewer = MPViewer(params=params, use_world_bounds=True)
+viewer = MPViewer(params=params,
+                  use_world_bounds=True)
 viewer.drawWorld(world)
 viewer.saveFig(output_dir + "/" + "world_plain.png")
 
@@ -48,6 +50,7 @@
   viewer.drawWorld(world)
   viewer.drawRoad(road)
   viewer.saveFig(output_dir + "/" + "open_drive_map_road_" + str(idx_r) + ".png")
+  viewer.show()
   viewer.clear()
 
 for idx_r, road in open_drive_map.get_roads().items():
@@ -56,6 +59,7 @@
     viewer.drawRoad(road, color_triplet_gray)
     viewer.drawLaneSection(lane_section)
     viewer.saveFig(output_dir + "/" + "open_drive_map_road_" + str(idx_r) + "_lane_section" + str(idx_ls) + ".png")
+    viewer.show()
     viewer.clear()
 
 for idx_r, road in open_drive_map.get_roads().items():
@@ -66,6 +70,7 @@
       viewer.drawLaneSection(lane_section, color_triplet_gray)
       viewer.drawLane(lane)
       viewer.saveFig(output_dir + "/" + "open_drive_map_road_" + str(idx_r) + "_lane_section" + str(idx_ls) + "_lane" + str(idx_l) + ".png")
+      viewer.show()
       viewer.clear()
 
 # Lanes of Roadgraph
@@ -75,11 +80,20 @@
 
 for lane_id in lane_ids:
   lane_polygon = roadgraph.get_lane_polygon_by_id(lane_id)
+  # plot plan_view
+  road_id = roadgraph.get_road_by_lane_id(lane_id)
+  road = map_interface.get_open_drive_map().get_road(road_id)
+  plan_view_reference = road.plan_view.get_reference_line()
+  # plot polygon with center line
+  outer, inner = roadgraph.compute_lane_boundaries(lane_id)
+  center_line = compute_center_line(outer.line, inner.line)
   viewer.drawWorld(world)
   color = list(np.random.choice(range(256), size=3)/256)
   viewer.drawPolygon2d(lane_polygon, color, 1.0)
+  viewer.drawLine2d(center_line)
+  viewer.drawLine2d(plan_view_reference, color="red")
   viewer.saveFig(output_dir + "/" + "roadgraph_laneid_" + str(lane_id) + ".png")
-  viewer.clear()
+  viewer.show(block=False)
 
 
 #for rc in all_corridors:

modules/geometry/line.hpp

@@ -222,22 +222,23 @@ inline float get_tangent_angle_at_s(Line l, float s) {
   if (s >= l.s_.back()) {
     Point2d p1 = l.obj_.at(l.obj_.size()-2);
     Point2d p2 = l.obj_.at(l.obj_.size()-1);
-    return atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1));
+    float angle = atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1));
+    return angle;
   } else if (s <= 0.0) {
     Point2d p1 = l.obj_.at(0);
     Point2d p2 = l.obj_.at(1);
     return atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1));
   } else {  // not start or end
     int end_segment_it = get_segment_end_idx(l, s);
     if (check_s_for_segment_intersection(l, s)) {  // check if s is at intersection, if true then calculate the intermediate angle
-    Point2d p1 = l.obj_.at(end_segment_it-2);
-    Point2d p2 = l.obj_.at(end_segment_it-1);
-    Point2d p3 = l.obj_.at(end_segment_it);
-    float sin_mean = 0.5 * (sin(atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1)))
-                      + sin(atan2(bg::get<1>(p3) - bg::get<1>(p2), bg::get<0>(p3) - bg::get<0>(p2))));
-    float cos_mean = 0.5 * (cos(atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1)))
-                      + cos(atan2(bg::get<1>(p3) - bg::get<1>(p2), bg::get<0>(p3) - bg::get<0>(p2))));
-    return atan2(sin_mean, cos_mean);
+      Point2d p1 = l.obj_.at(end_segment_it-2);
+      Point2d p2 = l.obj_.at(end_segment_it-1);
+      Point2d p3 = l.obj_.at(end_segment_it);
+      float sin_mean = 0.5 * (sin(atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1)))
+                        + sin(atan2(bg::get<1>(p3) - bg::get<1>(p2), bg::get<0>(p3) - bg::get<0>(p2))));
+      float cos_mean = 0.5 * (cos(atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1)))
+                        + cos(atan2(bg::get<1>(p3) - bg::get<1>(p2), bg::get<0>(p3) - bg::get<0>(p2))));
+      return atan2(sin_mean, cos_mean);
     } else {  // every s not start, end or intersection
       Point2d p1 = l.obj_.at(end_segment_it-1);
       Point2d p2 = l.obj_.at(end_segment_it);
@@ -247,8 +248,13 @@ inline float get_tangent_angle_at_s(Line l, float s) {
 }
 
 inline Point2d get_normal_at_s(Line l, float s) {
+  // std::cout << "get_normal_at_s() ";
+  // for (auto& ll : l) {
+  //   std::cout << boost::geometry::get<0>(ll) << ", " << boost::geometry::get<1>(ll) << " " << s << std::endl;
+  // }
   float tangent = get_tangent_angle_at_s(l, s);
   Point2d t(cos(tangent+asin(1)), sin(tangent+asin(1)));  // rotate unit vector anti-clockwise with angle = tangent by 1/2 pi
+  // std::cout << "tangent" << tangent << std::endl;
   return t;
 }
 
@@ -385,7 +391,7 @@ inline double signed_distance(const Line &line, const Point2d &p, const float& o
 }
 
 inline Line ComputeCenterLine(const Line& outer_line_,
-                                  const Line& inner_line_) {
+                              const Line& inner_line_) {
   Line center_line_;
   Line line_more_points = outer_line_;
   Line line_less_points = inner_line_;

modules/runtime/commons/xodr_parser.py

@@ -5,10 +5,11 @@
 
 from lxml import etree
 import pprint
+import logging
 from bark.world.opendrive import *
 from bark.world.map import *
 from bark.geometry import *
-
+logger = logging.getLogger()
 
 class XodrParser(object):
     def __init__(self, file_name):
@@ -305,13 +306,13 @@ def create_lane_link(self, link):
             try:
                 new_link.from_position = int(link["predecessor"])
             except:
-                print("No LaneLink.predecessor")
+                logger.info("No LaneLink.predecessor")
             try:
                 new_link.to_position = int(link["successor"])
             except:
-                print("No LaneLink.successor")
+                logger.info("No LaneLink.successor")
         else:
-            print("No LaneLink")
+            logger.info("No LaneLink")
 
         return new_link
 
@@ -393,7 +394,7 @@ def create_cpp_lane_section(self, new_road, road):
                         previous_line = new_lane_section.get_lane_by_position(lane['id']+1).line
                         new_lane_section = self.create_cpp_lane(new_lane_section, new_road, lane, previous_line.length(), previous_line)
                     else:
-                        print("Calculating previous lane does not work well.")
+                        logger.info("Calculating previous lane did not work.")
 
 
             new_road.add_lane_section(new_lane_section)

modules/runtime/runtime.hpp

@@ -36,18 +36,18 @@ inline void EvalRuntime(Runtime r,
                         Eigen::Matrix<float,
                                       Eigen::Dynamic,
                                       Eigen::Dynamic> action) {
-  std::cout << "Received valid runtime." << std::endl;
-  std::cout << "Stepping runtime..." << std::endl;
+  LOG(INFO)<< "Received valid runtime." << std::endl;
+  LOG(INFO) << "Stepping runtime..." << std::endl;
   r.Step(action);
-  std::cout << "Runtime has been successfully stepped." << std::endl;
+  LOG(INFO) << "Runtime has been successfully stepped." << std::endl;
 }
 
 inline void EvalRuntime(Runtime r,
                         int action) {
-  std::cout << "Received valid runtime." << std::endl;
-  std::cout << "Stepping runtime..." << std::endl;
+  LOG(INFO) << "Received valid runtime." << std::endl;
+  LOG(INFO) << "Stepping runtime..." << std::endl;
   r.Step(action);
-  std::cout << "Runtime has been successfully stepped." << std::endl;
+  LOG(INFO) << "Runtime has been successfully stepped." << std::endl;
 }
 
 typedef std::shared_ptr<Runtime> RuntimePtr;

modules/runtime/scenario/scenario.py

@@ -54,9 +54,7 @@ def _build_world_state(self):
 
   def __getstate__(self):
       odict = self.__dict__.copy()
-      print(odict['_map_interface'])
       del odict['_map_interface']
-      print(odict)
       return odict
 
   def __setstate__(self, sdict):

modules/runtime/viewer/viewer.py

@@ -4,13 +4,15 @@
 # https://opensource.org/licenses/MIT
 
 import numpy as np
+import logging
 from bark.viewer import Viewer
 from bark.geometry import *
 from bark.models.dynamic import *
 from bark.world.opendrive import *
 from bark.world.goal_definition import *
 from modules.runtime.commons.parameters import ParameterServer
 
+logger = logging.getLogger()
 
 class BaseViewer(Viewer):
     def __init__(self, params=None, **kwargs):
@@ -249,7 +251,7 @@ def drawDrivingCorridor(self, corridor, color=None):
             self.drawLine2d(corridor.inner, color, 1)
             self.drawLine2d(corridor.outer, color, 1)
         else:
-            print("Cannot draw Driving Corridor, as it is empty")        
+            logger.info("Cannot draw Driving Corridor, as it is empty")        
 
     def drawRoute(self, agent):
         # TODO(@hart): visualize the global as well as the local driving corridor

modules/world/evaluation/BUILD

@@ -98,7 +98,24 @@ cc_library(
     visibility = ["//visibility:public"],
 )
 
+cc_library(
+    name = "evaluator_drivable_area",
+    hdrs = [
+        "evaluator_drivable_area.hpp"
+    ],
+    deps = [
+        "//modules/world/evaluation:base_evaluator",
+        "//modules/geometry",
+        "//modules/commons:commons",
+        "//modules/world/map:map_interface",
+        "//modules/world/map:roadgraph",
+        "//modules/world/opendrive:opendrive",
+        "@boost//:geometry",
+        "//modules/world:world",
 
+    ],
+    visibility = ["//visibility:public"],
+)
 
 cc_library(
     name = "evaluation",
@@ -107,6 +124,7 @@ cc_library(
         ":base_evaluator",
         ":evaluator_collision_agents",
         ":evaluator_collision_driving_corridor",
+        ":evaluator_drivable_area",
         ":evaluator_collision_ego_agent",
         ":evaluator_step_count"
     ],

modules/world/map/local_map.cpp

@@ -17,10 +17,9 @@ using geometry::FindNearestIdx;
 using geometry::distance;
 using models::dynamic::StateDefinition;
 
-LaneId LocalMap::GoalLaneIdFromGoalDefinitionPolygon(
-  const modules::geometry::Polygon& goal_polygon) {
-  modules::geometry::Point2d goal_center(goal_polygon.center_(0),
-                                         goal_polygon.center_(1));
+LaneId LocalMap::GoalLaneIdFromGoalPolygon(const GoalDefinitionPolygon& goal_definition) {
+  modules::geometry::Point2d goal_center(goal_definition.get_shape().center_(0),
+                                         goal_definition.get_shape().center_(1));
   std::vector<opendrive::LanePtr> nearest_lanes;
 
   if (map_interface_->FindNearestLanes(goal_center, 1, nearest_lanes)) {
@@ -52,8 +51,13 @@ bool LocalMap::Generate(Point2d point) {
   }
   driving_corridor_ = DrivingCorridor();
 
-  goal_lane_id_ = GoalLaneIdFromGoalDefinitionPolygon(
-    goal_definition_->get_shape());
+  auto goal_definition_polygon = std::dynamic_pointer_cast<GoalDefinitionPolygon>(goal_definition_);
+  if (!goal_definition_polygon) {
+    return false; //< todo: handle this better
+  }
+
+  goal_lane_id_ = GoalLaneIdFromGoalPolygon(*goal_definition_polygon);
+
   std::vector<LanePtr> lanes;
   map_interface_->FindNearestLanes(point, 1, lanes);
   LanePtr current_lane = lanes.at(0);

modules/world/map/local_map.hpp

@@ -89,7 +89,7 @@ class LocalMap {
                         Line& line_of_corridor,
                         std::vector< std::pair<int, LaneId> >& lane_ids);
 
-  LaneId GoalLaneIdFromGoalDefinitionPolygon(const modules::geometry::Polygon& goal_polygon);
+  LaneId GoalLaneIdFromGoalPolygon(const GoalDefinitionPolygon& goal_definition);
 
   LanePtr FindLane(const Point2d& point) const;
 

modules/world/map/roadgraph.cpp

@@ -432,7 +432,7 @@ void Roadgraph::GeneratePreAndSuccessors(OpenDriveMapPtr map) {
         successor_lane_section = successor_road->get_lane_sections().front();
       }
     } else {
-      std::cout << "Road has no successor road. \n";
+      LOG(INFO) << "Road has no successor road. \n";
     }
 
     if (road_element.second->get_link().get_predecessor().type_ ==
@@ -443,7 +443,7 @@ void Roadgraph::GeneratePreAndSuccessors(OpenDriveMapPtr map) {
         predecessor_lane_section = predecessor_road->get_lane_sections().back();
       }
     } else {
-      std::cout << "Road has no predeseccor road. \n";
+      LOG(INFO) << "Road has no predeseccor road. \n";
     }
 
     // TODO(@hart): there could be mult. lane_sections
@@ -489,7 +489,7 @@ void Roadgraph::GeneratePreAndSuccessors(OpenDriveMapPtr map) {
             }
           }
         } catch (const std::exception &ex) {
-          std::cerr << "Road has no predeseccor road. \n";
+          LOG(INFO) << "Road has no predeseccor road. \n";
         }
       }
     }
@@ -553,7 +553,7 @@ void Roadgraph::GenerateFromJunctions(OpenDriveMapPtr map) {
                   add_successor(pre_lane->get_id(), successor_lane->get_id());
             }
           } catch (...) {
-            std::cerr << "Junction has no connections. \n";
+            LOG(INFO) << "Junction has no connections. \n";
           }
         }
       }
@@ -647,11 +647,17 @@ std::pair<PolygonPtr, bool> Roadgraph::ComputeLanePolygon(
   }
   return std::make_pair(polygon, success);
 }
+
 PolygonPtr Roadgraph::get_lane_polygon_by_id(const LaneId &lane_id) {
   auto v = get_vertex_by_lane_id(lane_id);
   return get_lane_graph().operator[](v.first).polygon;
 }
 
+RoadId Roadgraph::get_road_by_lane_id(const LaneId &lane_id) {
+  auto v = get_vertex_by_lane_id(lane_id);
+  return get_lane_graph().operator[](v.first).road_id;
+}
+
 }  // namespace map
 }  // namespace world
 }  // namespace modules

modules/world/map/roadgraph.hpp

@@ -169,6 +169,7 @@ class Roadgraph {
 
   void Generate(OpenDriveMapPtr map);
 
+  RoadId get_road_by_lane_id(const LaneId &lane_id);
 
  private:
   LaneGraph g_;