Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
Classes | Functions
carma_wm::test Namespace Reference

Classes

struct  MapOptions
 

Functions

lanelet::Point3d getPoint (double x, double y, double z)
 helper function for quickly creating a lanelet::Point3d. random id is assigned More...
 
lanelet::BasicPoint2d getBasicPoint (double x, double y)
 helper function for quickly creating a lanelet::BasicPoint. random id is assigned More...
 
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)
 helper function for quickly creating a lanelet::Lanelet using linestrings. random id is assigned More...
 
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)
 helper function for quickly creating a lanelet::Lanelet using points. random id is assigned More...
 
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)
 helper function for quickly creating a lanelet::Lanelet using linestrings. id is specified More...
 
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)
 helper function for quickly creating a lanelet::Lanelet using points. id is specified More...
 
lanelet::LaneletMapPtr buildGuidanceTestMap (double width, double length, int num=1)
 helper function for creating lanelet map for getGuidanceTestMap More...
 
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)
 adds a roadway object at the specified cartesian coords More...
 
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)
 adds a roadway object at the specified trackpos relative to the given lanelet id More...
 
void setSpeedLimit (lanelet::Velocity speed_limit, std::shared_ptr< carma_wm::CARMAWorldModel > cmw)
 Sets all lanelets in the map the given speed limit. More...
 
void setRouteByLanelets (std::vector< lanelet::ConstLanelet > lanelets, std::shared_ptr< carma_wm::CARMAWorldModel > cmw)
 Sets the route by series of lanelets. More...
 
void setRouteByIds (std::vector< lanelet::Id > lanelet_ids, std::shared_ptr< carma_wm::CARMAWorldModel > cmw)
 Sets the route by series of lanelets id. More...
 
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), std::make_pair< boost::posix_time::ptime, lanelet::CarmaTrafficSignalState >(lanelet::time::timeFromSec(4.0), lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE), std::make_pair< boost::posix_time::ptime, lanelet::CarmaTrafficSignalState >(lanelet::time::timeFromSec(24.0), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN), std::make_pair< boost::posix_time::ptime, lanelet::CarmaTrafficSignalState >(lanelet::time::timeFromSec(44.0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) })
 Method adds a traffic light to the provided world model instance NOTE: The stop line for the light will be located at the end of the owning_lanelets (in order) and formed from their two bound end points. NOTE: Exit lanelet matches the entry lanelets by order.
More...
 
std::shared_ptr< carma_wm::CARMAWorldModel > getGuidanceTestMap (MapOptions map_options)
 Gets the CARMAWorldModel for the guidance test map. More...
 
std::shared_ptr< carma_wm::CARMAWorldModel > getGuidanceTestMap ()
 

Function Documentation

◆ addObstacle() [1/2]

void carma_wm::test::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 
)
inline

adds a roadway object at the specified trackpos relative to the given lanelet id

Parameters
tpTrackPos relative to the given lanelet_id
lanelet_idLanelet Id to place the roadway object relative to
cmwCARMAWorldModel shared ptr
pred_trackpos_listvector of TrackPos predicted coords with 1s interval in the future. This is relative to the given lanelet_id
time_steptime_step interval for each predicted coords (milliseconds)
widthwidth of roadway object, default 3 meters
lengthlength of roadway object, default 3 meters NOTE: This assumes a similar simple shape of the GuidanceTestMap and does not populate cartesian components of the roadway object.

Definition at line 329 of file WMTestLibForGuidance.hpp.

329 {}, int time_step = 100, double width = 3, double length = 3)
330{
331 //TODO: width & length are not used; if there are no plans to use them soon, remove them from param list
332 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMTestLibForGuidance"), "/// the following args are not used: width = " << width << ", length = " << length << ". Logging to avoid compiler warning.");
333
334 carma_perception_msgs::msg::RoadwayObstacle rwo;
335
336 if (!cmw->getMap() || cmw->getMap()->laneletLayer.size() == 0)
337 {
338 throw std::invalid_argument("Map is not set or does not contain lanelets");
339 }
340 rwo.connected_vehicle_type.type =
341 carma_perception_msgs::msg::ConnectedVehicleType::NOT_CONNECTED; // TODO No clear way to determine automation state at this time
342
343 // get id of specified tp lanelet
344 // it assumes similar shape map with getGuidanceTestMap
345 auto reference_llt = cmw->getMutableMap()->laneletLayer.get(lanelet_id);
346 lanelet::BasicPoint2d object_center = {(reference_llt.leftBound()[0].x() + reference_llt.rightBound()[0].x())/2 + tp.crosstrack,
347 (reference_llt.leftBound()[0].y() + reference_llt.rightBound()[0].y())/2 + tp.downtrack};
348
349 auto nearestLanelet = cmw->getMap()->laneletLayer.nearest(object_center, 1)[0];
350 if (!boost::geometry::within(object_center, nearestLanelet.polygon2d()))
351 {
352 throw std::invalid_argument("Given trackpos from given lanelet id does land on any lanelet in the map");
353 }
354 rwo.lanelet_id = nearestLanelet.id();
355
356 rwo.down_track = tp.downtrack;
357 rwo.cross_track = tp.crosstrack;
358
359 int time_stamp = 0;
360 std::vector<carma_perception_msgs::msg::PredictedState> pred_states;
361 for (auto pred_track_pos : pred_trackpos_list)
362 {
363 // record time intervals
364 carma_perception_msgs::msg::PredictedState ps;
365 time_stamp += (time_step * 1e6);
366 ps.header.stamp.nanosec = time_stamp;
367
368 auto ref_llt_pred = cmw->getMutableMap()->laneletLayer.get(lanelet_id);
369 lanelet::BasicPoint2d object_center_pred = {(ref_llt_pred.leftBound()[0].x() + ref_llt_pred.rightBound()[0].x())/2 + pred_track_pos.crosstrack,
370 (ref_llt_pred.leftBound()[0].y() + ref_llt_pred.rightBound()[0].y())/2 + pred_track_pos.downtrack};
371
372 auto predNearestLanelet = cmw->getMap()->laneletLayer.nearest(object_center_pred, 1)[0];
373 if (!boost::geometry::within(object_center_pred, predNearestLanelet.polygon2d()))
374 {
375 RCLCPP_WARN_STREAM(rclcpp::get_logger("WMTestLibForGuidance"),"Given pred trackpos from given lanelet id does land on any lanelet in the map");
376 continue;
377 }
378 rwo.predicted_lanelet_ids.emplace_back(predNearestLanelet.id());
379 auto tp_pred = carma_wm::geometry::trackPos(predNearestLanelet,object_center_pred);
380 rwo.predicted_cross_tracks.emplace_back(tp_pred.crosstrack);
381 rwo.predicted_down_tracks.emplace_back(tp_pred.downtrack);
382
383 pred_states.push_back(ps);
384 }
385 // add time intervals
386 rwo.object.predictions = pred_states;
387 std::vector<carma_perception_msgs::msg::RoadwayObstacle> rw_objs = cmw->getRoadwayObjects();
388
389 rw_objs.push_back(rwo);
390
391 cmw->setRoadwayObjects(rw_objs);
392}
carma_wm::TrackPos::crosstrack
double crosstrack
Definition: TrackPos.hpp:38
carma_wm::TrackPos::downtrack
double downtrack
Definition: TrackPos.hpp:37
carma_wm::geometry::trackPos
TrackPos trackPos(const lanelet::ConstLanelet &lanelet, const lanelet::BasicPoint2d &point)
Returns the TrackPos, computed in 2d, of the provided point relative to the centerline of the provide...
Definition: Geometry.cpp:120

◆ addObstacle() [2/2]

void carma_wm::test::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 
)
inline

adds a roadway object at the specified cartesian coords

Parameters
xcoord
ycoord
cmwCARMAWorldModel shared ptr
pred_coordsvector of std::pair(x,y) predicted coords with 1s interval in the future
time_steptime_step interval for each predicted coords (milliseconds)
widthwidth of roadway object, default 3 meters
lengthlength of roadway object, default 3 meters NOTE: x,y is the center of your object

Definition at line 266 of file WMTestLibForGuidance.hpp.

266 {},
267 int time_step = 100, double width = 3, double length = 3)
268{
269 carma_perception_msgs::msg::RoadwayObstacle rwo;
270
271 tf2::Quaternion tf_orientation;
272 tf_orientation.setRPY(0, 0, 1.5708); // 90 deg
273
274 rwo.object.pose.pose.position.x = x;
275 rwo.object.pose.pose.position.y = y;
276 rwo.object.pose.pose.position.z = 0;
277
278 rwo.object.pose.pose.orientation.x = tf_orientation.getX();
279 rwo.object.pose.pose.orientation.y = tf_orientation.getY();
280 rwo.object.pose.pose.orientation.z = tf_orientation.getZ();
281 rwo.object.pose.pose.orientation.w = tf_orientation.getW();
282
283 rwo.object.size.x = width;
284 rwo.object.size.y = length;
285 rwo.object.size.z = 1;
286
287 int time_stamp = 0;
288 std::vector<carma_perception_msgs::msg::PredictedState> pred_states;
289 for (auto pred : pred_coords)
290 {
291 carma_perception_msgs::msg::PredictedState ps;
292 time_stamp += (time_step * 1e6);
293 ps.header.stamp.nanosec = time_stamp;
294
295 ps.predicted_position.position.x = pred.first;
296 ps.predicted_position.position.y = pred.second;
297 ps.predicted_position.position.z = 0;
298
299 ps.predicted_position.orientation.x = tf_orientation.getX();
300 ps.predicted_position.orientation.y = tf_orientation.getY();
301 ps.predicted_position.orientation.z = tf_orientation.getZ();
302 ps.predicted_position.orientation.w = tf_orientation.getW();
303 pred_states.push_back(ps);
304 }
305
306 rwo.object.predictions = pred_states;
307
308 // populate current and predicted lanelet_id, cross_track, downtracks
309 rwo = cmw->toRoadwayObstacle(rwo.object).get();
310
311 std::vector<carma_perception_msgs::msg::RoadwayObstacle> rw_objs = cmw->getRoadwayObjects();
312
313 rw_objs.push_back(rwo);
314
315 cmw->setRoadwayObjects(rw_objs);
316}
process_traj_logs.y
y
Definition: process_traj_logs.py:129
process_traj_logs.x
x
Definition: process_traj_logs.py:128

Referenced by getGuidanceTestMap().

Here is the caller graph for this function:

◆ addTrafficLight()

void carma_wm::test::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),  std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(4.0), lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE),  std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(24.0), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN),  std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(44.0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)  } 
)
inline

Method adds a traffic light to the provided world model instance NOTE: The stop line for the light will be located at the end of the owning_lanelets (in order) and formed from their two bound end points. NOTE: Exit lanelet matches the entry lanelets by order.

Parameters
cmwThe world model instance to update
light_idThe lanelet id to use for the generated traffic light regulatory element. This id should NOT be present in the map prior to this method call
entry_lanelet_idsThe ids of the lanelet which will own the traffic light element. These ids MUST be present in the map prior to this method being called
exit_lanelet_idsThe ids of the exit lanelet of this traffic light. These ids MUST be present in the map prior to this method being called
timeing_planOptional parameter that is the timing plan to use for the light. The specifications match those of CarmaTrafficSignalState.setStates() The default timing plan is 4sec yewllow, 20sec red, 20sec green
Exceptions
ifany of the lanelet is not in map, or entry/exit lanelet sizes do not match

Definition at line 502 of file WMTestLibForGuidance.hpp.

504{
505 std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED), // Just ended green
506 std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(4.0), lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE), // 4 sec yellow
507 std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(24.0), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN), // 20 sec red
508 std::make_pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>(lanelet::time::timeFromSec(44.0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) // 20 sec green
509})
510{
511 if (entry_lanelet_ids.size() != exit_lanelet_ids.size())
512 {
513 throw std::invalid_argument("Provided entry and exit lanelets size do not match!");
514 }
515 lanelet::Lanelets entry_lanelets;
516
517 for (auto id : entry_lanelet_ids) {
518 auto iterator = cmw->getMutableMap()->laneletLayer.find(id);
519
520 if (iterator == cmw->getMutableMap()->laneletLayer.end())
521 throw std::invalid_argument("Provided with lanelet id not in map: " + std::to_string(id));
522
523 entry_lanelets.push_back(*iterator);
524 }
525
526 lanelet::Lanelets exit_lanelets;
527
528 for (auto id : exit_lanelet_ids) {
529 auto iterator = cmw->getMutableMap()->laneletLayer.find(id);
530
531 if (iterator == cmw->getMutableMap()->laneletLayer.end())
532 throw std::invalid_argument("Provided with lanelet id not in map: " + std::to_string(id));
533
534 exit_lanelets.push_back(*iterator);
535
536 }
537
538 // Create stop line at end of owning lanelet
539 std::vector<lanelet::LineString3d> stop_lines;
540 for (auto llt: entry_lanelets)
541 {
542 lanelet::LineString3d virtual_stop_line(lanelet::utils::getId(), { llt.leftBound().back(), llt.rightBound().back() });
543 stop_lines.push_back(virtual_stop_line);
544 }
545
546 // Build traffic light
547 std::shared_ptr<lanelet::CarmaTrafficSignal> traffic_light(new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(light_id, stop_lines, entry_lanelets, exit_lanelets )));
548
549 // Set the timing plan
550 traffic_light->setStates(timing_plan,0);
551
552 // Ensure map lookup tables are updated
553 for (auto llt: entry_lanelets)
554 {
555 cmw->getMutableMap()->update(llt, traffic_light);
556 }
557
558}
carma_cooperative_perception::to_string
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21

◆ buildGuidanceTestMap()

lanelet::LaneletMapPtr carma_wm::test::buildGuidanceTestMap ( double  width,
double  length,
int  num = 1 
)
inline

helper function for creating lanelet map for getGuidanceTestMap

Parameters
widthwidth of single lanelet, default is 3.7 meters which is US standard
lengthlength of a single lanelet, default is 25 meters to accomplish 100 meters of full lane
numhow many number of segments should linestrings of the lanelet have. a.k.a num + 1 points in each linestring

Definition at line 184 of file WMTestLibForGuidance.hpp.

185{
186 std::vector<lanelet::Lanelet> all_lanelets;
187 double step_length = length / num; // 26 points in one lanelet's lanelent
188
189 std::vector<lanelet::Point3d> pts0;
190 std::vector<lanelet::Point3d> pts1;
191 std::vector<lanelet::Point3d> pts2;
192 std::vector<lanelet::Point3d> pts3;
193
194 for (int i = 0; i < num * 4 + 1; i ++)
195 {
196 pts0.push_back(carma_wm::test::getPoint(0.0, i * step_length, 0));
197 }
198 for (int i = 0; i < num * 4 + 1; i ++)
199 {
200 pts1.push_back(carma_wm::test::getPoint(1 * width, i * step_length, 0));
201 }
202 for (int i = 0; i < num * 4 + 1; i ++)
203 {
204 pts2.push_back(carma_wm::test::getPoint(2 * width, i * step_length, 0));
205 }
206 for (int i = 0; i < num * 4 + 1; i ++)
207 {
208 pts3.push_back(carma_wm::test::getPoint(3 * width, i * step_length, 0));
209 }
210
211 lanelet::LineString3d ls00(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts0.begin(), pts0.begin() + num + 1));
212 lanelet::LineString3d ls01(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts0.begin() + num, pts0.begin() + 2 * num + 1));
213 lanelet::LineString3d ls02(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts0.begin() + 2 * num, pts0.begin() + 3 * num + 1)) ;
214 lanelet::LineString3d ls03(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts0.begin() + 3 * num, pts0.begin() + 4 * num + 1));
215
216 lanelet::LineString3d ls10(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts1.begin(), pts1.begin() + num + 1));
217 lanelet::LineString3d ls11(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts1.begin() + num, pts1.begin() + 2 * num + 1));
218 lanelet::LineString3d ls12(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts1.begin() + 2 * num, pts1.begin() + 3 * num + 1));
219 lanelet::LineString3d ls13(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts1.begin() + 3 * num, pts1.begin() + 4 * num + 1));
220
221 lanelet::LineString3d ls20(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts2.begin(), pts2.begin() + num + 1));
222 lanelet::LineString3d ls21(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts2.begin() + num, pts2.begin() + 2 * num + 1));
223 lanelet::LineString3d ls22(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts2.begin() + 2 * num, pts2.begin() + 3 * num + 1));
224 lanelet::LineString3d ls23(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts2.begin() + 3 * num, pts2.begin() + 4 * num + 1));
225
226 lanelet::LineString3d ls30(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts3.begin(), pts3.begin() + num + 1));
227 lanelet::LineString3d ls31(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts3.begin() + num, pts3.begin() + 2 * num + 1));
228 lanelet::LineString3d ls32(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts3.begin() + 2 * num, pts3.begin() + 3 * num + 1));
229 lanelet::LineString3d ls33(lanelet::utils::getId(), std::vector<lanelet::Point3d>(pts3.begin() + 3 * num, pts3.begin() + 4 * num + 1));
230
231 all_lanelets.push_back(getLanelet(1200, ls00,ls10,lanelet::AttributeValueString::Solid, lanelet::AttributeValueString::Dashed));
232 all_lanelets.push_back(getLanelet(1201, ls01,ls11,lanelet::AttributeValueString::Solid, lanelet::AttributeValueString::Dashed));
233 all_lanelets.push_back(getLanelet(1202, ls02,ls12,lanelet::AttributeValueString::Solid, lanelet::AttributeValueString::Dashed));
234 all_lanelets.push_back(getLanelet(1203, ls03,ls13,lanelet::AttributeValueString::Solid, lanelet::AttributeValueString::Dashed));
235
236 all_lanelets.push_back(getLanelet(1210, ls10,ls20,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Dashed));
237 all_lanelets.push_back(getLanelet(1211, ls11,ls21,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Dashed));
238 all_lanelets.push_back(getLanelet(1212, ls12,ls22,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Dashed));
239 all_lanelets.push_back(getLanelet(1213, ls13,ls23,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Dashed));
240
241 all_lanelets.push_back(getLanelet(1220, ls20,ls30,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Solid));
242 all_lanelets.push_back(getLanelet(1221, ls21,ls31,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Solid));
243 all_lanelets.push_back(getLanelet(1222, ls22,ls32,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Solid));
244 all_lanelets.push_back(getLanelet(1223, ls23,ls33,lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Solid));
245
246 // Create basic map and verify that the map and routing graph can be build, but the route remains false
247 lanelet::LaneletMapPtr map = lanelet::utils::createMap(all_lanelets, {});
248
249
250 using namespace lanelet::units::literals;
251 lanelet::MapConformer::ensureCompliance(map, 0_mph);
252 return map;
253}
carma_wm::test::getLanelet
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)
helper function for quickly creating a lanelet::Lanelet using points. id is specified
Definition: WMTestLibForGuidance.hpp:170
carma_wm::test::getPoint
lanelet::Point3d getPoint(double x, double y, double z)
helper function for quickly creating a lanelet::Point3d. random id is assigned
Definition: WMTestLibForGuidance.hpp:83
lanelet::MapConformer::ensureCompliance
void ensureCompliance(lanelet::LaneletMapPtr map, lanelet::Velocity config_limit=80_mph)
Function modifies an existing map to make a best effort attempt at ensuring the map confroms to the e...
Definition: MapConformer.cpp:550
process_bag.i
int i
Definition: process_bag.py:59

References lanelet::MapConformer::ensureCompliance(), getLanelet(), getPoint(), and process_bag::i.

Referenced by getGuidanceTestMap().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getBasicPoint()

lanelet::BasicPoint2d carma_wm::test::getBasicPoint ( double  x,
double  y 
)
inline

helper function for quickly creating a lanelet::BasicPoint. random id is assigned

Parameters
xcoord
ycoord

Definition at line 92 of file WMTestLibForGuidance.hpp.

93{
94 return lanelet::utils::to2D(getPoint(x, y, 0.0)).basicPoint();
95}

References getPoint(), process_traj_logs::x, and process_traj_logs::y.

Here is the call graph for this function:

◆ getGuidanceTestMap() [1/2]

std::shared_ptr< carma_wm::CARMAWorldModel > carma_wm::test::getGuidanceTestMap ( )
inline

Definition at line 596 of file WMTestLibForGuidance.hpp.

597{
598 MapOptions map_options;
599 return getGuidanceTestMap(map_options);
600}
carma_wm::test::getGuidanceTestMap
std::shared_ptr< carma_wm::CARMAWorldModel > getGuidanceTestMap()
Definition: WMTestLibForGuidance.hpp:596
carma_wm::test::MapOptions
Definition: WMTestLibForGuidance.hpp:66

References getGuidanceTestMap().

Here is the call graph for this function:

◆ getGuidanceTestMap() [2/2]

std::shared_ptr< carma_wm::CARMAWorldModel > carma_wm::test::getGuidanceTestMap ( MapOptions  map_options)
inline

Gets the CARMAWorldModel for the guidance test map.

Parameters
widthwidth of single lanelet, default is 3.7 meters which is US standard
lengthlength of a single lanelet, default is 25 meters to accomplish 100 meters of full lane
map_optionsvector of enum map_options. No input is DEFAULT options, but empty vector is NO_OBSTACLE, NO_SPEED_LIMIT options. if mix and match, provide each one explicitly e.g. NO_OBSTACLE, DEFAULT_SPEED_LIMIT NOTE: Input 1/4th of full lane length you want to accomplish in length.

Definition at line 569 of file WMTestLibForGuidance.hpp.

570{
571 std::shared_ptr<carma_wm::CARMAWorldModel> cmw = std::make_shared<carma_wm::CARMAWorldModel>();
572 // create the semantic map
573
574 auto map = buildGuidanceTestMap(map_options.lane_width_, map_options.lane_length_, map_options.seg_num_);
575
576 // set the map, with default routingGraph
577
578 cmw->setMap(map);
579
580 // set default route, from 1200 to 1203 (it will automatically pick the shortest)
581 setRouteByIds({1200,1203}, cmw);
582
583 // set the obstacle
584
585 if (map_options.obstacle_ == MapOptions::Obstacle::DEFAULT)
586 {
587 addObstacle(1.5*map_options.lane_width_, 2.5*map_options.lane_length_, cmw); // lanelet_id at 1212
588 }
589 if (map_options.speed_limit_ == MapOptions::SpeedLimit::DEFAULT)
590 {
591 setSpeedLimit(25_mph, cmw); // change speed limit of all lanelets in the map to 25mph
592 }
593
594 return cmw;
595}
carma_wm::test::setSpeedLimit
void setSpeedLimit(lanelet::Velocity speed_limit, std::shared_ptr< carma_wm::CARMAWorldModel > cmw)
Sets all lanelets in the map the given speed limit.
Definition: WMTestLibForGuidance.hpp:402
carma_wm::test::addObstacle
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)
adds a roadway object at the specified trackpos relative to the given lanelet id
Definition: WMTestLibForGuidance.hpp:329
carma_wm::test::buildGuidanceTestMap
lanelet::LaneletMapPtr buildGuidanceTestMap(double width, double length, int num=1)
helper function for creating lanelet map for getGuidanceTestMap
Definition: WMTestLibForGuidance.hpp:184
carma_wm::test::setRouteByIds
void setRouteByIds(std::vector< lanelet::Id > lanelet_ids, std::shared_ptr< carma_wm::CARMAWorldModel > cmw)
Sets the route by series of lanelets id.
Definition: WMTestLibForGuidance.hpp:476
carma_wm::test::MapOptions::lane_length_
double lane_length_
Definition: WMTestLibForGuidance.hpp:72
carma_wm::test::MapOptions::speed_limit_
SpeedLimit speed_limit_
Definition: WMTestLibForGuidance.hpp:74
carma_wm::test::MapOptions::obstacle_
Obstacle obstacle_
Definition: WMTestLibForGuidance.hpp:73
carma_wm::test::MapOptions::seg_num_
int seg_num_
Definition: WMTestLibForGuidance.hpp:75
carma_wm::test::MapOptions::lane_width_
double lane_width_
Definition: WMTestLibForGuidance.hpp:71

References addObstacle(), buildGuidanceTestMap(), carma_wm::test::MapOptions::DEFAULT, carma_wm::test::MapOptions::lane_length_, carma_wm::test::MapOptions::lane_width_, carma_wm::test::MapOptions::obstacle_, carma_wm::test::MapOptions::seg_num_, setRouteByIds(), setSpeedLimit(), and carma_wm::test::MapOptions::speed_limit_.

Referenced by getGuidanceTestMap().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getLanelet() [1/4]

lanelet::Lanelet carma_wm::test::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 
)
inline

helper function for quickly creating a lanelet::Lanelet using linestrings. id is specified

Parameters
idlanelet_id to use. it must be never used anywhere before
left_lsleft linestring to use
right_lsright linestring to use
left_sub_typeleft linestring's line type. defaults to double solid line
right_sub_typeright linestring's line type. defaults to solid line

Definition at line 154 of file WMTestLibForGuidance.hpp.

157{
158 auto ll = getLanelet(left_ls, right_ls, left_sub_type, right_sub_type);
159 ll.setId(id);
160 return ll;
161}

References getLanelet().

Here is the call graph for this function:

◆ getLanelet() [2/4]

lanelet::Lanelet carma_wm::test::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 
)
inline

helper function for quickly creating a lanelet::Lanelet using points. id is specified

Parameters
idlanelet_id to use. it must be never used anywhere before
leftvector of points to use for left linestring
rightvector of points to use for right linestring
left_sub_typeleft linestring's line type. defaults to double solid line
right_sub_typeright linestring's line type. defaults to solid line

Definition at line 170 of file WMTestLibForGuidance.hpp.

173{
174 auto ll = getLanelet(left, right, left_sub_type, right_sub_type);
175 ll.setId(id);
176 return ll;
177}

References getLanelet().

Here is the call graph for this function:

◆ getLanelet() [3/4]

lanelet::Lanelet carma_wm::test::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 
)
inline

helper function for quickly creating a lanelet::Lanelet using linestrings. random id is assigned

Parameters
left_lsleft linestring to use
right_lsright linestring to use
left_sub_typeleft linestring's line type. defaults to double solid line
right_sub_typeright linestring's line type. defaults to solid line

Definition at line 104 of file WMTestLibForGuidance.hpp.

107{
108 left_ls.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::LineThin;
109 left_ls.attributes()[lanelet::AttributeName::Subtype] = left_sub_type;
110
111 right_ls.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::LineThin;
112 right_ls.attributes()[lanelet::AttributeName::Subtype] = right_sub_type;
113
114 lanelet::Lanelet ll;
115 ll.setId(lanelet::utils::getId());
116 ll.setLeftBound(left_ls);
117 ll.setRightBound(right_ls);
118
119 ll.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::Lanelet;
120 ll.attributes()[lanelet::AttributeName::Subtype] = lanelet::AttributeValueString::Road;
121 ll.attributes()[lanelet::AttributeName::Location] = lanelet::AttributeValueString::Urban;
122 ll.attributes()[lanelet::AttributeName::OneWay] = "yes";
123 ll.attributes()[lanelet::AttributeName::Dynamic] = "no";
124
125 return ll;
126}

Referenced by buildGuidanceTestMap(), and getLanelet().

Here is the caller graph for this function:

◆ getLanelet() [4/4]

lanelet::Lanelet carma_wm::test::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 
)
inline

helper function for quickly creating a lanelet::Lanelet using points. random id is assigned

Parameters
leftvector of points to use for left linestring
rightvector of points to use for right linestring
left_sub_typeleft linestring's line type. defaults to double solid line
right_sub_typeright linestring's line type. defaults to solid line

Definition at line 135 of file WMTestLibForGuidance.hpp.

138{
139 lanelet::LineString3d left_ls(lanelet::utils::getId(), left);
140
141 lanelet::LineString3d right_ls(lanelet::utils::getId(), right);
142
143 return getLanelet(left_ls, right_ls, left_sub_type, right_sub_type);
144}

References getLanelet().

Here is the call graph for this function:

◆ getPoint()

lanelet::Point3d carma_wm::test::getPoint ( double  x,
double  y,
double  z 
)
inline

helper function for quickly creating a lanelet::Point3d. random id is assigned

Parameters
xcoord
ycoord
zcoord

Definition at line 83 of file WMTestLibForGuidance.hpp.

84{
85 return lanelet::Point3d(lanelet::utils::getId(), x, y, z);
86}

References process_traj_logs::x, and process_traj_logs::y.

Referenced by buildGuidanceTestMap(), and getBasicPoint().

Here is the caller graph for this function:

◆ setRouteByIds()

void carma_wm::test::setRouteByIds ( std::vector< lanelet::Id >  lanelet_ids,
std::shared_ptr< carma_wm::CARMAWorldModel >  cmw 
)
inline

Sets the route by series of lanelets id.

Parameters
laneletslanelets that you want the route to follow
cmwCARMAWorldModel shared_ptr
Exceptions
std::invalid_argumentif fewer than 2 lanelets passed, or the given lanelets are not routable

Definition at line 476 of file WMTestLibForGuidance.hpp.

477{
478 std::vector<lanelet::ConstLanelet> lanelets;
479 for (auto id : lanelet_ids)
480 {
481 lanelets.push_back(cmw->getMap()->laneletLayer.get(id));
482 }
483 setRouteByLanelets(lanelets, cmw);
484}
carma_wm::test::setRouteByLanelets
void setRouteByLanelets(std::vector< lanelet::ConstLanelet > lanelets, std::shared_ptr< carma_wm::CARMAWorldModel > cmw)
Sets the route by series of lanelets.
Definition: WMTestLibForGuidance.hpp:433

References setRouteByLanelets().

Referenced by getGuidanceTestMap().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setRouteByLanelets()

void carma_wm::test::setRouteByLanelets ( std::vector< lanelet::ConstLanelet >  lanelets,
std::shared_ptr< carma_wm::CARMAWorldModel >  cmw 
)
inline

Sets the route by series of lanelets.

Parameters
laneletslanelets that you want the route to follow
cmwCARMAWorldModel shared_ptr
Exceptions
std::invalid_argumentif fewer than 2 lanelets passed, or the given lanelets are not routable

Definition at line 433 of file WMTestLibForGuidance.hpp.

434{
435 // Build routing graph from map
436 auto traffic_rules = cmw->getTrafficRules();
437 lanelet::routing::RoutingGraphUPtr map_graph = lanelet::routing::RoutingGraph::build(*cmw->getMap(), *traffic_rules.get());
438
439 // Generate route
440 lanelet::Optional<lanelet::routing::Route> optional_route;
441 std::vector<lanelet::ConstLanelet> middle_lanelets;
442 if (lanelets.size() > 2)
443 {
444 middle_lanelets = std::vector<lanelet::ConstLanelet>(lanelets.begin() + 1, lanelets.end() - 1);
445 optional_route = map_graph->getRouteVia(lanelets[0], middle_lanelets, lanelets[lanelets.size() - 1]);
446 }
447 else if (lanelets.size() == 2)
448 {
449 optional_route = map_graph->getRoute(lanelets[0], lanelets[1]);
450 }
451 else
452 {
453 throw lanelet::InvalidInputError("Fewer than 2 lanelets have been passed to setRouteByLanelets, which cannot be a valid route");
454 }
455
456 if (!optional_route)
457 {
458 throw lanelet::InvalidInputError("There was an error in routing using the given lanelets in setRouteByLanelets");
459 }
460
461 lanelet::routing::Route route = std::move(*optional_route);
462 carma_wm::LaneletRoutePtr route_ptr = std::make_shared<lanelet::routing::Route>(std::move(route));
463 cmw->setRoute(route_ptr);
464
465 RCLCPP_INFO_STREAM(rclcpp::get_logger("WMTestLibForGuidance"),"New route has been set successfully!");
466}
carma_wm::LaneletRoutePtr
std::shared_ptr< lanelet::routing::Route > LaneletRoutePtr
Definition: WorldModel.hpp:45

Referenced by setRouteByIds().

Here is the caller graph for this function:

◆ setSpeedLimit()

void carma_wm::test::setSpeedLimit ( lanelet::Velocity  speed_limit,
std::shared_ptr< carma_wm::CARMAWorldModel >  cmw 
)
inline

Sets all lanelets in the map the given speed limit.

Parameters
speed_limitspeed limit value to set
cmwCARMAWorldModel
Exceptions
std::invalid_argumentif the map is not set, contains no lanelets NOTE: this overwrites existing speed limit regulatory elements in the map

Definition at line 402 of file WMTestLibForGuidance.hpp.

403{
404 if (!cmw->getMap() || cmw->getMap()->laneletLayer.size() == 0)
405 {
406 throw std::invalid_argument("setSpeedLimit: Map is not set or does not contain lanelets");
407 }
408 // remove all speed limit regulatory element and set the new speed limit
409 for (auto llt : cmw->getMutableMap()->laneletLayer)
410 {
411 for (auto regem : llt.regulatoryElements())
412 {
413 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0)
414 {
415 cmw->getMutableMap()->remove(llt, regem);
416 }
417 }
418 lanelet::DigitalSpeedLimitPtr sl = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(), speed_limit, {llt}, {},
419 { lanelet::Participants::Vehicle }));
420 cmw->getMutableMap()->update(llt, sl);
421 }
422 RCLCPP_INFO_STREAM(rclcpp::get_logger("WMTestLibForGuidance"),"Set the new speed limit! Value: " << speed_limit.value());
423}

Referenced by getGuidanceTestMap().

Here is the caller graph for this function: