Merge pull request #549 from bark-simulator/road_from_csv

Road from CSV

bark/python_wrapper/world/map.cpp

@@ -30,6 +30,7 @@ void python_map(py::module m) {
                     &MapInterface::GetFullJunctionArea,
                     &MapInterface::SetFullJunctionArea)
       .def("SetOpenDriveMap", &MapInterface::SetOpenDriveMap)
+      .def("SetCsvMap", &MapInterface::SetCsvMap)
       .def("find_nearest_lanes",
            [](const MapInterface& m, const Point2d& point,
               const unsigned& num_lanes) {

bark/world/map/map_interface.cpp

@@ -8,6 +8,7 @@
 
 #include "bark/world/map/map_interface.hpp"
 #include <math.h>
+#include <boost/tokenizer.hpp>
 #include <memory>
 #include <random>
 
@@ -40,6 +41,169 @@ bool MapInterface::interface_from_opendrive(
   return true;
 }
 
+bool MapInterface::interface_from_csvtable(const std::string csvfile) {
+  // Read map data
+  std::ifstream file(csvfile);
+  if (!file.is_open()) {
+    LOG(ERROR) << "Error reading mapfile " << csvfile;
+    return false;
+  }
+  std::vector<double> cx, cy, lx, ly, rx, ry;
+  typedef boost::tokenizer<boost::escaped_list_separator<char>> Tokenizer;
+  std::string line;
+  std::vector<std::string> row;
+  bool toprow = true;
+  while (getline(file, line)) {
+    if (!toprow) {
+      Tokenizer tok(line);
+      row.assign(tok.begin(), tok.end());
+      cx.push_back(stod(row[1]));
+      cy.push_back(stod(row[2]));
+      rx.push_back(stod(row[3]));
+      ry.push_back(stod(row[4]));
+      lx.push_back(stod(row[5]));
+      ly.push_back(stod(row[6]));
+    } else {
+      toprow = false;
+    }
+  }
+  const int nr_points = cx.size();
+
+  // Generate centerline
+  using bark::geometry::Line;
+  using bark::geometry::Point2d;
+  Line centerline;
+  for (int idx = 0; idx < nr_points; ++idx) {
+    centerline.AddPoint(Point2d(cx[idx], cy[idx]));
+  }
+
+  // Generate road polygon
+  using bark::geometry::Polygon;
+  Polygon lanepoly;
+  for (int idx = 0; idx < nr_points; ++idx) {
+    lanepoly.AddPoint(Point2d(lx[idx], ly[idx]));
+  }
+  for (int idx = nr_points-1; idx >= 0; --idx) {  // reverse
+    lanepoly.AddPoint(Point2d(rx[idx], ry[idx]));
+  }
+  // lanepoly.correct(); //@todo do we need this?
+
+  // Generate Lane
+  double speed = 30 / 3.6;
+  int laneid = 0;
+  XodrLanePtr xodrlane = std::make_shared<XodrLane>();
+  xodrlane->SetId(laneid);
+  xodrlane->SetLanePosition(-1);  //@todo how to assign? Type: XodrLanePosition
+  // xodrlane->link_; //not needed
+  xodrlane->SetLine(centerline);
+  // xodrlane->junction_id_ //not needed
+  xodrlane->SetIsInJunction(false);
+  xodrlane->SetLaneType(XodrLaneType::DRIVING);
+  xodrlane->SetDrivingDirection(XodrDrivingDirection::FORWARD);
+  // xodrlane->road_mark_ // @todo do we need this to be assigned= Type:
+  // XodrRoadMark
+  xodrlane->SetSpeed(speed);
+  // xodrlane->lane_count//@todo how to assign?
+
+  LanePtr lane = std::make_shared<Lane>(xodrlane);
+  // lane->left_lane_ = nullptr; //@todo do we have to assign?
+  // lane->right_lane_ = nullptr; //@todo do we have to assign?
+  // lane->next_lane_ = nullptr; //@todo do we have to assign?
+  lane->center_line_ = centerline;
+  lane->polygon_ = lanepoly;
+  // Boundary left_boundary_; //@todo do we need this?
+  // Boundary right_boundary_; //@todo do we need this?
+
+  // Generate LaneCorridorPtr!
+  LaneCorridorPtr lanecorridor = std::make_shared<LaneCorridor>();
+  lanecorridor->lanes_[0] = lane;  // s_end: @todo what is the key? is zero ok??
+  lanecorridor->center_line_ = centerline;
+  lanecorridor->fine_center_line_ = centerline;
+  lanecorridor->merged_polygon_ = lanepoly;
+  // Line left_boundary_; //@todo do we need this?
+  // Line right_boundary_; //@todo do we need this?
+
+  // Generate PlanView
+  // @todo if we need the planview make a constructor from line!
+  PlanViewPtr xodrplanview = std::make_shared<PlanView>();
+
+  // Generate XodrLaneSection
+  double s = 0;
+  XodrLaneSectionPtr xodrlanesection = std::make_shared<XodrLaneSection>(s);
+  xodrlanesection->AddLane(lane);
+
+  // Generate XodrRoad
+  int roadid = 0;
+  XodrRoadPtr xodrroad = std::make_shared<XodrRoad>();
+  xodrroad->SetId(roadid);
+  xodrroad->SetName("dummy_name");
+  // xodrroad->SetLink(); //not needed
+  xodrroad->SetPlanView(xodrplanview);
+  xodrroad->AddLaneSection({xodrlanesection});
+
+  // Generate Road
+  RoadPtr road = std::make_shared<Road>(xodrroad);
+  road->next_road_ = nullptr;
+  road->lanes_[laneid] = lane;
+
+  // Generate road corridor
+  RoadCorridorPtr rc = std::make_shared<RoadCorridor>();
+  rc->roads_[roadid] = road;
+  rc->road_polygon_ = lanepoly;
+  rc->unique_lane_corridors_ = {lanecorridor};
+  rc->road_ids_ = {roadid};
+  rc->driving_direction_ = XodrDrivingDirection::FORWARD;
+  rc->lane_corridors_[laneid] = lanecorridor;
+
+  // Generate roadgraph
+  RoadgraphPtr roadgraph(new Roadgraph());
+  roadgraph->AddLane(roadid, xodrlane);
+  bark::world::map::PolygonPtr lanepolyptr =
+      std::make_shared<Polygon>(lanepoly);
+  bool lanepoly_set = roadgraph->SetPolygonForVertexFromId(laneid, lanepolyptr);
+  if (!lanepoly_set) {
+    return false;
+  }
+
+  // Generate lane rtree
+  rtree_lane_.clear();
+  using LineSegment = boost::geometry::model::segment<Point2d>;
+  LineSegment lane_segment(
+      *centerline.begin(),
+      *(centerline.end() - 1));  //@todo ERROR! should be the boundary here, but
+                                 // which? I did not model the planview
+  rtree_lane_.insert(std::make_pair(lane_segment, lane));
+
+  // Generate bounding box
+  boost::geometry::model::box<Point2d> box;
+  boost::geometry::envelope(lanepoly.obj_, box);
+  boost::geometry::correct(box);
+  bounding_box_ = std::make_pair(
+      Point2d(boost::geometry::get<boost::geometry::min_corner, 0>(box),
+              bg::get<bg::min_corner, 1>(box)),
+      Point2d(boost::geometry::get<boost::geometry::max_corner, 0>(box),
+              bg::get<bg::max_corner, 1>(box)));
+
+  // Generate (dummy) Open Drive Map
+  OpenDriveMapPtr open_drive_map = std::make_shared<OpenDriveMap>();
+  open_drive_map->AddRoad(xodrroad);
+
+  // Generate map interface
+  open_drive_map_ = open_drive_map;
+  road_from_csvtable_ = true;
+  roadgraph_ = roadgraph;
+  // rtree_lane_ assigned above
+
+  // TODO THIS IS A HACK!!!
+  std::vector<XodrRoadId> road_ids = {static_cast<XodrRoadId>(roadid)};
+  XodrDrivingDirection driving_direction = XodrDrivingDirection::FORWARD;
+  std::size_t road_corridor_hash = RoadCorridor::GetHash(driving_direction, road_ids);
+
+  road_corridors_[road_corridor_hash] = rc;
+  // bounding_box_ assigned above
+  return true;
+}
+
 bool MapInterface::FindNearestXodrLanes(const Point2d& point,
                                         const unsigned& num_lanes,
                                         std::vector<XodrLanePtr>& lanes,

bark/world/map/map_interface.hpp

@@ -28,6 +28,7 @@ using bark::world::opendrive::OpenDriveMapPtr;
 using bark::world::opendrive::XodrDrivingDirection;
 using bark::world::opendrive::XodrLaneId;
 using bark::world::opendrive::XodrLanePtr;
+using bark::world::map::Road;
 
 using rtree_lane_model = boost::geometry::model::segment<Point2d>;
 using rtree_lane_id = XodrLanePtr;
@@ -42,10 +43,13 @@ class MapInterface {
   MapInterface()
       : num_points_nearest_lane_(20),
         max_simplification_dist_(0.4),
-        full_junction_area_(false) {}
+        full_junction_area_(false),
+        road_from_csvtable_(false) {}
 
   bool interface_from_opendrive(const OpenDriveMapPtr& open_drive_map);
 
+  bool interface_from_csvtable(const std::string csvfile);
+
   bool FindNearestXodrLanes(const Point2d& point, const unsigned& num_lanes,
                             std::vector<opendrive::XodrLanePtr>& lanes,
                             bool type_driving_only = true) const;
@@ -70,6 +74,10 @@ class MapInterface {
     return true;
   }
 
+  bool SetCsvMap(const std::string csvfile) {
+    return interface_from_csvtable(csvfile);
+  }
+
   bool SetRoadgraph(RoadgraphPtr roadgraph) {
     roadgraph_ = roadgraph;
     return true;
@@ -119,6 +127,9 @@ class MapInterface {
   void SetFullJunctionArea(bool in) { full_junction_area_ = in; }
   bool GetFullJunctionArea() { return full_junction_area_; }
 
+  void SetRoadFromCsvTable(bool in) { road_from_csvtable_ = in; }
+  bool GetRoadFromCsvTable() { return road_from_csvtable_; }
+
  private:
   OpenDriveMapPtr open_drive_map_;
   RoadgraphPtr roadgraph_;
@@ -130,6 +141,7 @@ class MapInterface {
   unsigned num_points_nearest_lane_;
   double max_simplification_dist_;
   bool full_junction_area_;
+  bool road_from_csvtable_;
 
   static bool IsLaneType(rtree_lane_value const& m) {
     return (m.second->GetLaneType() == XodrLaneType::DRIVING);

bark/world/map/roadgraph.cpp

@@ -603,6 +603,19 @@ void Roadgraph::GeneratePolygonsForVertices() {
   }
 }
 
+bool Roadgraph::SetPolygonForVertexFromId(const XodrLaneId& lane_id,
+                                          PolygonPtr polygon) {
+  bool exists;
+  vertex_t v;
+  std::tie(v, exists) = GetVertexByLaneId(lane_id);
+  if (!exists) {
+    return false;
+  } else {
+    g_[v].polygon = polygon;
+    return true;
+  }
+}
+
 void Roadgraph::Generate(OpenDriveMapPtr map) {
   GenerateVertices(map);
 

bark/world/map/roadgraph.hpp

@@ -55,7 +55,7 @@ enum XodrLaneEdgeType {
 struct XodrLaneEdge {
   XodrLaneEdgeType edge_type;
   double weight;  //! @todo tobias: for shortest path calculation: a very basic
-                 //! implementation!
+                  //! implementation!
   XodrLaneEdgeType GetEdgeType() const { return edge_type; }
   XodrLaneEdge() : edge_type(LANE_SUCCESSOR_EDGE), weight(1) {}
   explicit XodrLaneEdge(XodrLaneEdgeType edge_type_in)
@@ -253,6 +253,8 @@ class Roadgraph {
       const XodrLaneId& lane_id, const XodrDrivingDirection& driving_direction);
   PolygonPtr ComputeJunctionArea(uint32_t junction_id);
 
+  bool SetPolygonForVertexFromId(const XodrLaneId& lane_id, PolygonPtr polygon);
+
  private:
   XodrLaneGraph g_;
 

bark/world/objects/agent.cpp

@@ -72,7 +72,8 @@ Agent::Agent(const Agent& other_agent)
       first_valid_timestamp_(other_agent.first_valid_timestamp_),
       goal_definition_(other_agent.goal_definition_),
       road_corridor_road_ids_(other_agent.road_corridor_road_ids_),
-      road_corridor_driving_direction_(other_agent.road_corridor_driving_direction_),
+      road_corridor_driving_direction_(
+          other_agent.road_corridor_driving_direction_),
       sensed_world_(other_agent.sensed_world_) {}
 
 void Agent::PlanBehavior(const double& min_planning_dt,
@@ -97,24 +98,34 @@ void Agent::UpdateStateAction() {
 }
 
 bool Agent::GenerateRoadCorridor(const MapInterfacePtr& map_interface) {
-   if (goal_definition_ && road_corridor_road_ids_.empty()) {
-    road_corridor_ = map_interface->GenerateRoadCorridor(
-    GetCurrentPosition(),
-    goal_definition_->GetShape());
-    road_corridor_road_ids_ = road_corridor_->GetRoadIds();
-    road_corridor_driving_direction_ = road_corridor_->GetDrivingDirection();
-  } else if(!road_corridor_road_ids_.empty()) {
-    VLOG(6) << "Road corridor from ids" << road_corridor_road_ids_;
-    map_interface->GenerateRoadCorridor(road_corridor_road_ids_,
-                                  road_corridor_driving_direction_);
-    road_corridor_ = map_interface->GetRoadCorridor(road_corridor_road_ids_, 
-                                              road_corridor_driving_direction_);
-  } else {
-    LOG(INFO) << "Agent has map interface but no information to generate road corridor.";
-    return false;
+  if (!map_interface->GetRoadFromCsvTable()) {  // default xodr
+    LOG(INFO) << "Map Interface from XODR";
+    if (goal_definition_ && road_corridor_road_ids_.empty()) {
+      road_corridor_ = map_interface->GenerateRoadCorridor(
+          GetCurrentPosition(), goal_definition_->GetShape());
+      road_corridor_road_ids_ = road_corridor_->GetRoadIds();
+      road_corridor_driving_direction_ = road_corridor_->GetDrivingDirection();
+    } else if (!road_corridor_road_ids_.empty()) {
+      VLOG(6) << "Road corridor from ids" << road_corridor_road_ids_;
+      map_interface->GenerateRoadCorridor(road_corridor_road_ids_,
+                                          road_corridor_driving_direction_);
+      road_corridor_ = map_interface->GetRoadCorridor(
+          road_corridor_road_ids_, road_corridor_driving_direction_);
+    } else {
+      LOG(INFO) << "Agent has map interface but no information to generate "
+                   "road corridor.";
+      return false;
+    }
+  } else {  // road from csv
+    LOG(INFO) << "Map Interface from CSV";
+    road_corridor_road_ids_ = {static_cast<world::map::XodrRoadId>(0)};
+    road_corridor_driving_direction_ = bark::world::opendrive::
+      XodrDrivingDirection::FORWARD;
+    road_corridor_ = map_interface->GetRoadCorridor(
+      road_corridor_road_ids_, road_corridor_driving_direction_);
   }
 
-  if(!road_corridor_) {
+  if (!road_corridor_) {
     LOG(INFO) << "No corridor for agent found.";
     return false;
   }
@@ -181,7 +192,7 @@ std::shared_ptr<Object> Agent::Clone() const {
   if (execution_model_) {
     new_agent->execution_model_ = execution_model_->Clone();
   }
-  if(goal_definition_) {
+  if (goal_definition_) {
     new_agent->goal_definition_ = goal_definition_->Clone();
   }
   return std::dynamic_pointer_cast<Object>(new_agent);

bark/world/tests/BUILD

@@ -62,7 +62,8 @@ cc_test(
         "//bark/world/evaluation/rss:_rss": ["-D RSS"],
         "//conditions:default": [],
     }),
-    data = ["//bark/runtime/tests:xodr_data"],
+    data = ["//bark/runtime/tests:xodr_data",
+            "//bark/world/tests/map:csv_map_data"],
     deps = [
         "//bark/geometry",
         "//bark/world:world",
@@ -107,7 +108,8 @@ cc_test(
 py_test(
   name = "py_world_tests",
   srcs = ["py_world_tests.py"],
-  data = ["//bark:generate_core"],
+  data = ["//bark:generate_core",
+          "//bark/world/tests/map:csv_map_data"],
   imports = ['../../../python/'],
   deps = ["//bark/runtime/commons:parameters",
           "//bark/runtime:runtime"],

bark/world/tests/map/BUILD

@@ -1,3 +1,9 @@
+filegroup(
+   name="csv_map_data",
+   srcs=glob(["data/**"]),
+   visibility = ["//visibility:public"],
+)
+
 cc_test(
     name = "opendrive_tests",
     srcs = [
@@ -53,6 +59,7 @@ cc_test(
         "//bark/world/tests:make_test_xodr_map",
         "@gtest//:gtest_main",
     ],
+    data = [":csv_map_data"]
 )
 
 py_test(