WMTestLibForGuidance.hpp

Go to the documentation of this file.

#pragma once
/*
 * Copyright (C) 2020-2021 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
 
#include <gtest/gtest.h>
#include <iostream>
#include <carma_wm/CARMAWorldModel.hpp>
#include <lanelet2_core/geometry/LineString.h>
#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
#include <lanelet2_core/Attribute.h>
#include <carma_perception_msgs/msg/roadway_obstacle.hpp>
#include <carma_perception_msgs/msg/predicted_state.hpp>
#include <carma_perception_msgs/msg/connected_vehicle_type.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <tf2/LinearMath/Transform.h>
#include <lanelet2_core/geometry/Polygon.h>
#include <lanelet2_extension/regulatory_elements/DigitalSpeedLimit.h>
#include <carma_wm/Geometry.hpp>
#include <carma_wm/MapConformer.hpp>
#include <rclcpp/rclcpp.hpp>
#include <lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h>
namespace carma_wm
{
namespace test
{
using namespace lanelet::units::literals;
/*
 * Class which is passed into getGuidanceTestMap to configure
 */
struct MapOptions
{
  enum class Obstacle {DEFAULT, NONE};
  enum class SpeedLimit {DEFAULT, NONE};
  MapOptions(double lane_width = 3.7, double lane_length = 25, Obstacle obstacle =  Obstacle::DEFAULT, SpeedLimit speed_limit = SpeedLimit::DEFAULT, int seg_num = 1): 
                            lane_width_(lane_width), lane_length_(lane_length), obstacle_(obstacle), speed_limit_(speed_limit), seg_num_(seg_num){}
  double lane_width_;
  double lane_length_;
  Obstacle obstacle_ ;
  SpeedLimit speed_limit_;
  int seg_num_;
};
inline lanelet::Point3d getPoint(double x, double y, double z)
{
  return lanelet::Point3d(lanelet::utils::getId(), x, y, z);
}
inline lanelet::BasicPoint2d getBasicPoint(double x, double y)
{
  return lanelet::utils::to2D(getPoint(x, y, 0.0)).basicPoint();
}
 
inline lanelet::Lanelet getLanelet(lanelet::LineString3d& left_ls, lanelet::LineString3d& right_ls,
                                   const lanelet::Attribute& left_sub_type = lanelet::AttributeValueString::SolidSolid,
                                   const lanelet::Attribute& right_sub_type = lanelet::AttributeValueString::Solid)
{
  left_ls.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::LineThin;
  left_ls.attributes()[lanelet::AttributeName::Subtype] = left_sub_type;
 
  right_ls.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::LineThin;
  right_ls.attributes()[lanelet::AttributeName::Subtype] = right_sub_type;
 
  lanelet::Lanelet ll;
  ll.setId(lanelet::utils::getId());
  ll.setLeftBound(left_ls);
  ll.setRightBound(right_ls);
 
  ll.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::Lanelet;
  ll.attributes()[lanelet::AttributeName::Subtype] = lanelet::AttributeValueString::Road;
  ll.attributes()[lanelet::AttributeName::Location] = lanelet::AttributeValueString::Urban;
  ll.attributes()[lanelet::AttributeName::OneWay] = "yes";
  ll.attributes()[lanelet::AttributeName::Dynamic] = "no";
 
  return ll;
}
 
inline lanelet::Lanelet getLanelet(std::vector<lanelet::Point3d> left, std::vector<lanelet::Point3d> right,
                                   const lanelet::Attribute& left_sub_type = lanelet::AttributeValueString::SolidSolid,
                                   const lanelet::Attribute& right_sub_type = lanelet::AttributeValueString::Solid)
{
  lanelet::LineString3d left_ls(lanelet::utils::getId(), left);
 
  lanelet::LineString3d right_ls(lanelet::utils::getId(), right);
 
  return getLanelet(left_ls, right_ls, left_sub_type, right_sub_type);
}
 
inline lanelet::Lanelet getLanelet(lanelet::Id id, lanelet::LineString3d left_ls, lanelet::LineString3d right_ls,
                                   const lanelet::Attribute& left_sub_type = lanelet::AttributeValueString::SolidSolid,
                                   const lanelet::Attribute& right_sub_type = lanelet::AttributeValueString::Solid)
{
  auto ll = getLanelet(left_ls, right_ls, left_sub_type, right_sub_type);
  ll.setId(id);
  return ll;
}
inline lanelet::Lanelet getLanelet(lanelet::Id id, std::vector<lanelet::Point3d> left, std::vector<lanelet::Point3d> right,
                                   const lanelet::Attribute& left_sub_type = lanelet::AttributeValueString::SolidSolid,
                                   const lanelet::Attribute& right_sub_type = lanelet::AttributeValueString::Solid)
{
  auto ll = getLanelet(left, right, left_sub_type, right_sub_type);
  ll.setId(id);
  return ll;
}
inline lanelet::LaneletMapPtr buildGuidanceTestMap(double width, double length, int num = 1)
{
  std::vector<lanelet::Lanelet> all_lanelets;
  double step_length = length / num;  // 26 points in one lanelet's lanelent
 
  std::vector<lanelet::Point3d> pts0;
  std::vector<lanelet::Point3d> pts1;
  std::vector<lanelet::Point3d> pts2;
  std::vector<lanelet::Point3d> pts3;
 
  for (int i = 0; i < num * 4 + 1; i ++)
  {
    pts0.push_back(carma_wm::test::getPoint(0.0, i * step_length, 0));
  } 
  for (int i = 0; i < num * 4 + 1; i ++)
  {
    pts1.push_back(carma_wm::test::getPoint(1 * width, i * step_length, 0));
  } 
  for (int i = 0; i < num * 4 + 1; i ++)
  {
    pts2.push_back(carma_wm::test::getPoint(2 * width, i * step_length, 0));
  } 
  for (int i = 0; i < num * 4 + 1; i ++)
  {
    pts3.push_back(carma_wm::test::getPoint(3 * width, i * step_length, 0));
  } 
 
  lanelet::LineString3d ls00(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts0.begin(), pts0.begin() + num + 1));
  lanelet::LineString3d ls01(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts0.begin() + num, pts0.begin() + 2 * num + 1));
  lanelet::LineString3d ls02(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts0.begin() + 2 * num, pts0.begin() + 3 * num + 1)) ;
  lanelet::LineString3d ls03(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts0.begin() + 3 * num, pts0.begin() + 4 * num + 1));
 
  lanelet::LineString3d ls10(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts1.begin(), pts1.begin() + num + 1));
  lanelet::LineString3d ls11(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts1.begin() + num, pts1.begin() + 2 * num + 1));
  lanelet::LineString3d ls12(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts1.begin() + 2 * num, pts1.begin() + 3 * num + 1));
  lanelet::LineString3d ls13(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts1.begin() + 3 * num, pts1.begin() + 4 * num + 1));
 
  lanelet::LineString3d ls20(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts2.begin(), pts2.begin() + num + 1));
  lanelet::LineString3d ls21(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts2.begin() + num, pts2.begin() + 2 * num + 1));
  lanelet::LineString3d ls22(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts2.begin() + 2 * num, pts2.begin() + 3 * num + 1));
  lanelet::LineString3d ls23(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts2.begin() + 3 * num, pts2.begin() + 4 * num + 1));
 
  lanelet::LineString3d ls30(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts3.begin(), pts3.begin() + num + 1));
  lanelet::LineString3d ls31(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts3.begin() + num, pts3.begin() + 2 * num + 1));
  lanelet::LineString3d ls32(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts3.begin() + 2 * num, pts3.begin() + 3 * num + 1));
  lanelet::LineString3d ls33(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts3.begin() + 3 * num, pts3.begin() + 4 * num + 1));
 
  all_lanelets.push_back(getLanelet(1200, ls00,ls10,lanelet::AttributeValueString::Solid, lanelet::AttributeValueString::Dashed));
  all_lanelets.push_back(getLanelet(1201, ls01,ls11,lanelet::AttributeValueString::Solid, lanelet::AttributeValueString::Dashed));
  all_lanelets.push_back(getLanelet(1202, ls02,ls12,lanelet::AttributeValueString::Solid, lanelet::AttributeValueString::Dashed));
  all_lanelets.push_back(getLanelet(1203, ls03,ls13,lanelet::AttributeValueString::Solid, lanelet::AttributeValueString::Dashed));
 
  all_lanelets.push_back(getLanelet(1210, ls10,ls20,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Dashed));
  all_lanelets.push_back(getLanelet(1211, ls11,ls21,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Dashed));
  all_lanelets.push_back(getLanelet(1212, ls12,ls22,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Dashed));
  all_lanelets.push_back(getLanelet(1213, ls13,ls23,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Dashed));
  
  all_lanelets.push_back(getLanelet(1220, ls20,ls30,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Solid));
  all_lanelets.push_back(getLanelet(1221, ls21,ls31,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Solid));
  all_lanelets.push_back(getLanelet(1222, ls22,ls32,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Solid));
  all_lanelets.push_back(getLanelet(1223, ls23,ls33,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Solid));
  
  // Create basic map and verify that the map and routing graph can be build, but the route remains false   
  lanelet::LaneletMapPtr map = lanelet::utils::createMap(all_lanelets, {});
 
  
  using namespace lanelet::units::literals;
  lanelet::MapConformer::ensureCompliance(map, 0_mph);
  return map;
}
 
inline void addObstacle(double x, double y, std::shared_ptr<carma_wm::CARMAWorldModel> cmw, std::vector<std::pair<double,double>> pred_coords = {}, 
  int time_step = 100, double width = 3, double length = 3)
{
    carma_perception_msgs::msg::RoadwayObstacle rwo;    
 
    tf2::Quaternion tf_orientation; 
    tf_orientation.setRPY(0, 0, 1.5708); // 90 deg
 
    rwo.object.pose.pose.position.x = x;    
    rwo.object.pose.pose.position.y = y;    
    rwo.object.pose.pose.position.z = 0;    
 
    rwo.object.pose.pose.orientation.x = tf_orientation.getX(); 
    rwo.object.pose.pose.orientation.y = tf_orientation.getY(); 
    rwo.object.pose.pose.orientation.z = tf_orientation.getZ(); 
    rwo.object.pose.pose.orientation.w = tf_orientation.getW(); 
 
    rwo.object.size.x = width;  
    rwo.object.size.y = length; 
    rwo.object.size.z = 1;  
 
    int time_stamp = 0;
    std::vector<carma_perception_msgs::msg::PredictedState> pred_states;
    for (auto pred : pred_coords)
    {
        carma_perception_msgs::msg::PredictedState ps;
        time_stamp += (time_step * 1e6);
        ps.header.stamp.nanosec = time_stamp;
 
        ps.predicted_position.position.x = pred.first;  
        ps.predicted_position.position.y = pred.second; 
        ps.predicted_position.position.z = 0;   
 
        ps.predicted_position.orientation.x = tf_orientation.getX();    
        ps.predicted_position.orientation.y = tf_orientation.getY();    
        ps.predicted_position.orientation.z = tf_orientation.getZ();    
        ps.predicted_position.orientation.w = tf_orientation.getW();    
        pred_states.push_back(ps);
    }
 
    rwo.object.predictions = pred_states;
 
    // populate current and predicted lanelet_id, cross_track, downtracks 
    rwo = cmw->toRoadwayObstacle(rwo.object).get();
 
    std::vector<carma_perception_msgs::msg::RoadwayObstacle> rw_objs = cmw->getRoadwayObjects();    
 
    rw_objs.push_back(rwo); 
 
    cmw->setRoadwayObjects(rw_objs);    
}
 
inline void addObstacle(carma_wm::TrackPos tp, lanelet::Id lanelet_id, std::shared_ptr<carma_wm::CARMAWorldModel> cmw, std::vector<carma_wm::TrackPos> pred_trackpos_list = {}, int time_step = 100, double width = 3, double length = 3)
{
  //TODO: width & length are not used; if there are no plans to use them soon, remove them from param list
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMTestLibForGuidance"), "/// the following args are not used: width = " << width << ", length = " << length << ". Logging to avoid compiler warning.");
 
  carma_perception_msgs::msg::RoadwayObstacle rwo;  
 
  if (!cmw->getMap() || cmw->getMap()->laneletLayer.size() == 0)
  {
    throw std::invalid_argument("Map is not set or does not contain lanelets");
  }
  rwo.connected_vehicle_type.type =
      carma_perception_msgs::msg::ConnectedVehicleType::NOT_CONNECTED;  // TODO No clear way to determine automation state at this time
  
  // get id of specified tp lanelet
  // it assumes similar shape map with getGuidanceTestMap
  auto reference_llt = cmw->getMutableMap()->laneletLayer.get(lanelet_id);
  lanelet::BasicPoint2d object_center = {(reference_llt.leftBound()[0].x() + reference_llt.rightBound()[0].x())/2 + tp.crosstrack,
                                        (reference_llt.leftBound()[0].y() + reference_llt.rightBound()[0].y())/2 + tp.downtrack};    
 
  auto nearestLanelet = cmw->getMap()->laneletLayer.nearest(object_center, 1)[0]; 
  if (!boost::geometry::within(object_center, nearestLanelet.polygon2d()))
  {
    throw std::invalid_argument("Given trackpos from given lanelet id does land on any lanelet in the map");
  }
  rwo.lanelet_id = nearestLanelet.id();
 
  rwo.down_track = tp.downtrack;
  rwo.cross_track = tp.crosstrack;
 
  int time_stamp = 0;
  std::vector<carma_perception_msgs::msg::PredictedState> pred_states;
  for (auto pred_track_pos : pred_trackpos_list)
  {
    // record time intervals
    carma_perception_msgs::msg::PredictedState ps;
    time_stamp += (time_step * 1e6);
    ps.header.stamp.nanosec = time_stamp;
 
    auto ref_llt_pred = cmw->getMutableMap()->laneletLayer.get(lanelet_id);
    lanelet::BasicPoint2d object_center_pred = {(ref_llt_pred.leftBound()[0].x() + ref_llt_pred.rightBound()[0].x())/2 + pred_track_pos.crosstrack,
                                          (ref_llt_pred.leftBound()[0].y() + ref_llt_pred.rightBound()[0].y())/2 + pred_track_pos.downtrack};    
 
    auto predNearestLanelet = cmw->getMap()->laneletLayer.nearest(object_center_pred, 1)[0]; 
    if (!boost::geometry::within(object_center_pred, predNearestLanelet.polygon2d()))
    {
      RCLCPP_WARN_STREAM(rclcpp::get_logger("WMTestLibForGuidance"),"Given pred trackpos from given lanelet id does land on any lanelet in the map");
      continue;
    }
    rwo.predicted_lanelet_ids.emplace_back(predNearestLanelet.id());
    auto tp_pred = carma_wm::geometry::trackPos(predNearestLanelet,object_center_pred);
    rwo.predicted_cross_tracks.emplace_back(tp_pred.crosstrack);
    rwo.predicted_down_tracks.emplace_back(tp_pred.downtrack);
 
    pred_states.push_back(ps);
  }
  // add time intervals
  rwo.object.predictions = pred_states;
  std::vector<carma_perception_msgs::msg::RoadwayObstacle> rw_objs = cmw->getRoadwayObjects();  
 
  rw_objs.push_back(rwo);   
 
  cmw->setRoadwayObjects(rw_objs);  
}
inline void setSpeedLimit (lanelet::Velocity speed_limit, std::shared_ptr<carma_wm::CARMAWorldModel> cmw)
{
  if (!cmw->getMap() || cmw->getMap()->laneletLayer.size() == 0)
  {
    throw std::invalid_argument("setSpeedLimit: Map is not set or does not contain lanelets");
  }
  // remove all speed limit regulatory element and set the new speed limit
  for (auto llt : cmw->getMutableMap()->laneletLayer)
  {
    for (auto regem : llt.regulatoryElements())
    {
      if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0)
      {
        cmw->getMutableMap()->remove(llt, regem);
      }
    }
    lanelet::DigitalSpeedLimitPtr sl = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(), speed_limit, {llt}, {},
                                                     { lanelet::Participants::Vehicle }));
    cmw->getMutableMap()->update(llt, sl);
  }
  RCLCPP_INFO_STREAM(rclcpp::get_logger("WMTestLibForGuidance"),"Set the new speed limit! Value: " << speed_limit.value());
}
 
inline void setRouteByLanelets (std::vector<lanelet::ConstLanelet> lanelets, std::shared_ptr<carma_wm::CARMAWorldModel> cmw)
{
  // Build routing graph from map   
  auto traffic_rules = cmw->getTrafficRules();
  lanelet::routing::RoutingGraphUPtr map_graph = lanelet::routing::RoutingGraph::build(*cmw->getMap(), *traffic_rules.get());
 
  // Generate route
  lanelet::Optional<lanelet::routing::Route> optional_route;
  std::vector<lanelet::ConstLanelet> middle_lanelets;
  if (lanelets.size() > 2)
  {
    middle_lanelets = std::vector<lanelet::ConstLanelet>(lanelets.begin() + 1, lanelets.end() - 1);
    optional_route = map_graph->getRouteVia(lanelets[0], middle_lanelets, lanelets[lanelets.size() - 1]);   
  }
  else if (lanelets.size() == 2)
  {
    optional_route = map_graph->getRoute(lanelets[0], lanelets[1]);
  }
  else
  {
    throw lanelet::InvalidInputError("Fewer than 2 lanelets have been passed to setRouteByLanelets, which cannot be a valid route");
  }
  
  if (!optional_route)
  {
    throw lanelet::InvalidInputError("There was an error in routing using the given lanelets in setRouteByLanelets");
  }
 
  lanelet::routing::Route route = std::move(*optional_route);
  carma_wm::LaneletRoutePtr route_ptr = std::make_shared<lanelet::routing::Route>(std::move(route));
  cmw->setRoute(route_ptr);
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("WMTestLibForGuidance"),"New route has been set successfully!");
}
 
inline void setRouteByIds (std::vector<lanelet::Id> lanelet_ids, std::shared_ptr<carma_wm::CARMAWorldModel> cmw)
{
  std::vector<lanelet::ConstLanelet> lanelets;
  for (auto id : lanelet_ids)
  {
      lanelets.push_back(cmw->getMap()->laneletLayer.get(id));
  }
  setRouteByLanelets(lanelets, cmw);
}
 
 
 
 
inline void addTrafficLight(std::shared_ptr<carma_wm::CARMAWorldModel> cmw, lanelet::Id light_id, std::vector<lanelet::Id> entry_lanelet_ids, std::vector<lanelet::Id> exit_lanelet_ids, 
std::vector<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>> timing_plan =
{
  std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED), // Just ended green
  std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(4.0), lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE), // 4 sec yellow
  std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(24.0), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN), // 20 sec red
  std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(44.0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) // 20 sec green
})
{
  if (entry_lanelet_ids.size() != exit_lanelet_ids.size())
  {
    throw std::invalid_argument("Provided entry and exit lanelets size do not match!");
  }
  lanelet::Lanelets entry_lanelets;
 
  for (auto id : entry_lanelet_ids) {
    auto iterator = cmw->getMutableMap()->laneletLayer.find(id);
    
    if (iterator == cmw->getMutableMap()->laneletLayer.end())
      throw std::invalid_argument("Provided with lanelet id not in map: " + std::to_string(id));
 
    entry_lanelets.push_back(*iterator);
  }
 
  lanelet::Lanelets exit_lanelets;
 
  for (auto id : exit_lanelet_ids) {
    auto iterator = cmw->getMutableMap()->laneletLayer.find(id);
    
    if (iterator == cmw->getMutableMap()->laneletLayer.end())
      throw std::invalid_argument("Provided with lanelet id not in map: " + std::to_string(id));
 
    exit_lanelets.push_back(*iterator);
  
  }
 
    // Create stop line at end of owning lanelet
  std::vector<lanelet::LineString3d> stop_lines;
  for (auto llt: entry_lanelets)
  {
    lanelet::LineString3d virtual_stop_line(lanelet::utils::getId(), { llt.leftBound().back(), llt.rightBound().back() });
    stop_lines.push_back(virtual_stop_line);
  }
 
  // Build traffic light
  std::shared_ptr<lanelet::CarmaTrafficSignal> traffic_light(new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(light_id, stop_lines, entry_lanelets, exit_lanelets )));
  
  // Set the timing plan
  traffic_light->setStates(timing_plan,0);
 
  // Ensure map lookup tables are updated
  for (auto llt: entry_lanelets)
  {
    cmw->getMutableMap()->update(llt, traffic_light);
  }
 
}
 
inline std::shared_ptr<carma_wm::CARMAWorldModel> getGuidanceTestMap(MapOptions map_options)
{
  std::shared_ptr<carma_wm::CARMAWorldModel> cmw = std::make_shared<carma_wm::CARMAWorldModel>();   
  // create the semantic map
  
  auto map = buildGuidanceTestMap(map_options.lane_width_, map_options.lane_length_, map_options.seg_num_);
 
  // set the map, with default routingGraph
  
  cmw->setMap(map);
 
  // set default route, from 1200 to 1203 (it will automatically pick the shortest)
  setRouteByIds({1200,1203}, cmw);
 
  // set the obstacle
 
  if (map_options.obstacle_ == MapOptions::Obstacle::DEFAULT)
  {
    addObstacle(1.5*map_options.lane_width_, 2.5*map_options.lane_length_, cmw);  // lanelet_id at 1212
  }
  if (map_options.speed_limit_ == MapOptions::SpeedLimit::DEFAULT)
  {
    setSpeedLimit(25_mph, cmw);   // change speed limit of all lanelets in the map to 25mph
  }
 
  return cmw;
}
inline std::shared_ptr<carma_wm::CARMAWorldModel> getGuidanceTestMap()
{
  MapOptions map_options;
  return getGuidanceTestMap(map_options);
}
}
  //namespace test
} //namespace carma_wm