Xodr offset transformation

docs/source/profiling.md

@@ -24,7 +24,23 @@ include easy_profiler using `\#include <easy/profiler.h>`
 
 in every function you want to profile, write `EASY_FUNCTION();` at the beginning
 
+define the functions `void profiler_startup()` and void `profiler_finish()`, for example in some utitly function
 
+```
+void profiler_startup() {
+  EASY_PROFILER_ENABLE;
+//  profiler::startListen();
+}
+
+void profiler_finish() {
+  auto blocks_written = profiler::dumpBlocksToFile("/tmp/<some example>.prof");
+  LOG(INFO) << "Easy profiler blocks written: " << blocks_written;
+}
+```
+
+and wrap them to python
+
+in the python runtime, you then have to call them before and after the code you want to profile.
 
 ### Step 3: Run BARK
 

examples/analyze_map.py

@@ -19,8 +19,8 @@
 
 # Name and Output Directory
 # CHANGE THIS #
-map_name = "4way_intersection"
-output_dir = "/home/hart/Dokumente/2020/" + map_name
+map_name = "DR_DEU_Merging_MT_with_offset"
+output_dir = "/home/esterle/" + map_name
 
 # Map Definition
 xodr_parser = XodrParser("modules/runtime/tests/data/" + map_name + ".xodr")

modules/geometry/line.hpp

@@ -177,6 +177,23 @@ inline T length(const Line &line) {
   return bg::length<T>(line.obj_);
 }
 
+inline Line rotate(const Line& line, float hdg) {
+  using boost::geometry::strategy::transform::rotate_transformer;
+  rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(hdg);
+  Line line_rotated;
+  boost::geometry::transform(line.obj_, line_rotated.obj_, rotate);
+  line_rotated.recompute_s();
+  return line_rotated;
+}
+
+inline Line translate(const Line& line, float x, float y) {
+  using boost::geometry::strategy::transform::translate_transformer;
+  translate_transformer<double, 2, 2> translate(x, y);
+  Line line_translated;
+  boost::geometry::transform(line.obj_, line_translated.obj_, translate);
+  return line_translated;
+}
+
 inline int get_segment_end_idx(Line l, float s) {
   std::vector<float>::iterator up = std::upper_bound(l.s_.begin(), l.s_.end(), s);
   if(up != l.s_.end()) {

modules/geometry/tests/geometry_test.cc

@@ -130,6 +130,36 @@ TEST(geometry, line) {
   EXPECT_NEAR(get_nearest_s(l3, Point2d(5, 5)), 0.5 * sqrt(200), 0.1f);
 }
 
+TEST(geometry, line_transform) {
+  using modules::geometry::Line;
+  using modules::geometry::Point2d;
+  namespace bg = boost::geometry;
+
+  Line line_in;  // vertical
+  line_in.add_point(Point2d(0.0f, 0.0f));
+  line_in.add_point(Point2d(0.0f, 10.0f));
+
+  float hdg = 3.14159265359;
+  float offset_x = 1;
+  float offset_y = 2;
+
+  Line obj_rotated = rotate(line_in, hdg);
+  EXPECT_NEAR(line_in.length(), obj_rotated.length(), 0.01);
+
+  EXPECT_NEAR(bg::get<0>(obj_rotated.obj_.at(0)), 0, 0);
+  EXPECT_NEAR(bg::get<1>(obj_rotated.obj_.at(0)), 0, 0);
+  EXPECT_NEAR(bg::get<0>(obj_rotated.obj_.at(1)), 0, 0.1);
+  EXPECT_NEAR(bg::get<1>(obj_rotated.obj_.at(1)), -10, 0.1);
+
+  Line obj_transformed = translate(obj_rotated, offset_x, offset_y);
+  EXPECT_NEAR(line_in.length(), obj_transformed.length(), 0.01);
+
+  EXPECT_NEAR(bg::get<0>(obj_transformed.obj_.at(0)), 1, 0);
+  EXPECT_NEAR(bg::get<1>(obj_transformed.obj_.at(0)), 2, 0);
+  EXPECT_NEAR(bg::get<0>(obj_transformed.obj_.at(1)), 1, 0.1);
+  EXPECT_NEAR(bg::get<1>(obj_transformed.obj_.at(1)), -8, 0.1);
+}
+
 TEST(geometry, polygon) {
   using modules::geometry::Point2d;
   using modules::geometry::Polygon;

modules/runtime/commons/xodr_parser.py

@@ -115,12 +115,23 @@ def parse_lanes_from_lane_sections(self, lanes, lane_section):
             lane_section["lanes"].append(new_lane)
         return lane_section
 
+    def parse_offset(self, header):
+      # in compiliance with 5.2.2 of specification
+      offset = {}
+      offset["x"] = float(header.find("offset").get("x"))
+      offset["y"] = float(header.find("offset").get("y"))
+      offset["z"] = float(header.find("offset").get("z"))
+      offset["hdg"] = float(header.find("offset").get("hdg"))
+      return offset
+
     def parse_header(self, header):
         new_header = {}
         new_header["north"] = header.get("north")
         new_header["south"] = header.get("south")
         new_header["east"] = header.get("east")
         new_header["west"] = header.get("west")
+        if header.find("offset") is not None:
+          new_header["offset"] = self.parse_offset(header)
         self.python_map["header"] = new_header
 
     def parse_lane_sections_from_road(self, lane_sections, road):
@@ -247,7 +258,8 @@ def print_python_map(self):
         pp = pprint.PrettyPrinter(indent=2)
         pp.pprint(self.python_map)
 
-    def create_cpp_plan_view(self, plan_view):
+    def create_cpp_plan_view(self, plan_view, header):
+
         new_plan_view = PlanView()
         # create plan view..
         for geometry in plan_view["geometries"]:
@@ -267,6 +279,15 @@ def create_cpp_plan_view(self, plan_view):
                     float(geometry["length"]),
                     float(geometry["geometry"]["curv_start"]),
                     float(geometry["geometry"]["curv_end"]), 2) # TODO: s_inc
+
+        # now use header/ offset to modify plan view
+        if "offset" in header:
+          off_x = header["offset"]["x"]
+          off_y = header["offset"]["y"]
+          off_hdg = header["offset"]["hdg"]
+          print("Transforming PlanView with given offset", header["offset"])
+          new_plan_view.apply_offset_transform(off_x, off_y, off_hdg)
+
         return new_plan_view
 
     def create_cpp_road_link(self, link):
@@ -289,11 +310,11 @@ def create_cpp_road_link(self, link):
             pass
         return new_link
 
-    def create_cpp_road(self, road):
+    def create_cpp_road(self, road, header):
         new_road = Road()
         new_road.id = int(road["id"])
         new_road.name = road["name"]
-        new_road.plan_view = self.create_cpp_plan_view(road["plan_view"])
+        new_road.plan_view = self.create_cpp_plan_view(road["plan_view"], header)
         new_road = self.create_cpp_lane_section(new_road, road)
         new_road.link = self.create_cpp_road_link(road["link"])
 
@@ -431,7 +452,7 @@ def convert_to_map(self, python_map):
       CPP Map -- Map for usage with CPP
     """
         for road in self.python_map["roads"]:
-            new_road = self.create_cpp_road(road)
+            new_road = self.create_cpp_road(road, self.python_map["header"])
             self.map.add_road(new_road)
 
         for junction in self.python_map["junctions"]:

modules/world/opendrive/plan_view.cpp

@@ -92,6 +92,14 @@ bool PlanView::add_arc(
   return true;
 }
 
+bool PlanView::apply_offset_transform(float x, float y, float hdg) {
+  geometry::Line rotated_line = rotate(reference_line_, hdg);
+  geometry::Line transformed_line = translate(rotated_line, x, y);
+  reference_line_ = transformed_line;
+
+  return true;
+}
+
 }  // namespace opendrive
 }  // namespace world
 }  // namespace modules

modules/world/opendrive/plan_view.hpp

@@ -37,6 +37,8 @@ class PlanView {
 
   geometry::Point2d test(geometry::Point2d p) { return p; }
 
+  bool apply_offset_transform(float x, float y, float hdg);
+
   float get_length() const { return length_; }
   float get_distance( const geometry::Point2d &p) const { return boost::geometry::distance(reference_line_.obj_, p); }
 

python/world/opendrive.cpp

@@ -21,7 +21,8 @@ void python_opendrive(py::module m) {
       .def("add_line", &PlanView::add_line, "Add line to planview")
       .def("add_spiral", &PlanView::add_spiral, "Add spiral to planview")
       .def("add_arc", &PlanView::add_arc, "Add arc to planview")
-      .def("get_reference_line", &PlanView::get_reference_line, "Return as numpy array");
+      .def("get_reference_line", &PlanView::get_reference_line, "Return as numpy array")
+      .def("apply_offset_transform", &PlanView::apply_offset_transform, "Apply offset to planview");
 
   py::class_<RoadLinkInfo>(m, "RoadLinkInfo")
       .def(py::init<>())