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.
WMListenerWorker.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2022 LEIDOS.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
5 * use this file except in compliance with the License. You may obtain a copy of
6 * the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
12 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
13 * License for the specific language governing permissions and limitations under
14 * the License.
15 */
16
17#include <autoware_lanelet2_ros2_interface/utility/message_conversion.hpp>
18#include <lanelet2_extension/regulatory_elements/DirectionOfTravel.h>
19#include <lanelet2_extension/regulatory_elements/StopRule.h>
20#include <lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h>
21#include <lanelet2_extension/regulatory_elements/SignalizedIntersection.h>
22#include <lanelet2_routing/internal/Graph.h>
23#include "WMListenerWorker.hpp"
24
25namespace carma_wm
26{
27
30// helper function that return geofence type as an enum, which makes it cleaner by allowing switch statement
31GeofenceType resolveGeofenceType(const std::string& rule_name)
32{
33 if (rule_name.compare(lanelet::PassingControlLine::RuleName) == 0) return GeofenceType::PASSING_CONTROL_LINE;
34 if (rule_name.compare(lanelet::DigitalSpeedLimit::RuleName) == 0) return GeofenceType::DIGITAL_SPEED_LIMIT;
35 if (rule_name.compare(lanelet::RegionAccessRule::RuleName) == 0) return GeofenceType::REGION_ACCESS_RULE;
36 if (rule_name.compare(lanelet::DigitalMinimumGap::RuleName) == 0) return GeofenceType::DIGITAL_MINIMUM_GAP;
37 if (rule_name.compare(lanelet::DirectionOfTravel::RuleName) == 0) return GeofenceType::DIRECTION_OF_TRAVEL;
38 if (rule_name.compare(lanelet::StopRule::RuleName) == 0) return GeofenceType::STOP_RULE;
39 if (rule_name.compare(lanelet::CarmaTrafficSignal::RuleName) == 0) return GeofenceType::CARMA_TRAFFIC_LIGHT;
40 if (rule_name.compare(lanelet::SignalizedIntersection::RuleName) == 0) return GeofenceType::SIGNALIZED_INTERSECTION;
41
43}
44
46{
48}
49
51{
52 return std::static_pointer_cast<const WorldModel>(world_model_); // Cast pointer to const variant
53}
54
55
56void WMListenerWorker::mapCallback(const autoware_lanelet2_msgs::msg::MapBin::SharedPtr map_msg)
57{
58 current_map_version_ = map_msg->map_version;
59
60 lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap);
61
63
64 world_model_->setMap(new_map, current_map_version_);
65
66 // After setting map evaluate the current update queue to apply any updates that arrived before the map
67 bool more_updates_to_apply = true;
68 while(!map_update_queue_.empty() && more_updates_to_apply) {
69
70 auto update = map_update_queue_.front(); // Get first update
71 map_update_queue_.pop(); // Remove update from queue
72
73 if (update->map_version < current_map_version_) { // Drop any so far unapplied updates for the previous map
74 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "There were unapplied updates in carma_wm when a new map was received.");
75 continue;
76 }
77 if (update->map_version == current_map_version_) { // Current update goes with current map
78 mapUpdateCallback(update); // Apply the update
79 } else {
80 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Done applying updates for new map. However, more updates are waiting for a future map.");
81 more_updates_to_apply = false; // If there is more updates queued that are not for this map version assume they are for a future map version
82 }
83
84 }
85
86 // Call user defined map callback
87 if (map_callback_)
88 {
90 }
91
93 if (delayed_route_msg_.get().map_version == current_map_version_) { // If there is a delayed route message to apply then do so
94 routeCallback(std::make_unique<carma_planning_msgs::msg::Route>(delayed_route_msg_.get()));
95 } else if (delayed_route_msg_.get().map_version < current_map_version_) {
96 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Dropping delayed route message which was never applied as updated map was not recieved");
97 delayed_route_msg_ = boost::none;
98 } else {
99 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "There is a delayed route message still waiting to be applied in carma_wm");
100 }
101 }
102}
103
104void WMListenerWorker::incomingSpatCallback(const carma_v2x_msgs::msg::SPAT::SharedPtr spat_msg)
105{
106 world_model_->processSpatFromMsg(*spat_msg, use_sim_time_, is_spat_wall_time_);
107}
108
110{
111 return rerouting_flag_;
112}
113
115{
116 route_node_flag_=true;
117}
118
119// helper function to log SignalizedIntersectionManager content
121{
122 for (auto const &[intersection_id, regulatory_element_id] : sim.intersection_id_to_regem_id_)
123 {
124 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),
125 "inter id: " << intersection_id << ", regem id: " << regulatory_element_id);
126 }
127 for (auto const &[signal_id, entry_llt_ids] : sim.signal_group_to_entry_lanelet_ids_)
128 {
129 for (const auto & entry_llt_id : entry_llt_ids)
130 {
131 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", entry llt id: " << entry_llt_id);
132 }
133 }
134 for (auto const &[signal_id, exit_llt_ids] : sim.signal_group_to_exit_lanelet_ids_)
135 {
136 for (const auto & exit_llt_id : exit_llt_ids)
137 {
138 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", exit llt id: " << exit_llt_id);
139 }
140 }
141 for (auto const &[signal_id, regem_id] : sim.signal_group_to_traffic_light_id_)
142 {
143 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", regem id: " << regem_id);
144 }
145}
146
147void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::SharedPtr geofence_msg)
148{
149 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Map Update Being Evaluated. SeqNum: " << geofence_msg->seq_id);
150 if (rerouting_flag_) // no update should be applied if rerouting
151 {
152 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Currently new route is being processed. Queueing this update. Received seq: " << geofence_msg->seq_id << " prev seq: " << most_recent_update_msg_seq_);
153 map_update_queue_.emplace(geofence_msg);
154 return;
155 }
156 if (geofence_msg->seq_id <= most_recent_update_msg_seq_) {
157 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Dropping map update which has already been processed. Received seq: " << geofence_msg->seq_id << " prev seq: " << most_recent_update_msg_seq_);
158 return;
159 } else if(!world_model_->getMap() || current_map_version_ < geofence_msg->map_version) { // If our current map version is older than the version target by this update
160 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Update received for newer map version than available. Queueing update until map is available.");
161 map_update_queue_.emplace(geofence_msg);
162 return;
163 } else if (current_map_version_ > geofence_msg->map_version) { // If this update is for an older map
164 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Dropping old map update as newer map is already available.");
165 return;
166 } else if (most_recent_update_msg_seq_ + 1 < geofence_msg->seq_id) {
167 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Queuing map update as we are waiting on an earlier update to be applied. most_recent_update_msg_seq_: " << most_recent_update_msg_seq_ << "geofence_msg->seq_id: " << geofence_msg->seq_id);
168 map_update_queue_.emplace(geofence_msg);
169 return;
170 }
171
172
173 if(geofence_msg->invalidates_route==true && world_model_->getRoute())
174 {
175 rerouting_flag_=true;
177
178 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Received notice that route has been invalidated in mapUpdateCallback");
179
180 if(route_node_flag_!=true)
181 {
182 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Route is not yet available. Therefore queueing the update");
183 map_update_queue_.emplace(geofence_msg);
184 return;
185 }
186 }
187
188 most_recent_update_msg_seq_ = geofence_msg->seq_id; // Update current sequence count
189
190 auto gf_ptr = std::shared_ptr<carma_wm::TrafficControl>(new carma_wm::TrafficControl);
191
192 // convert ros msg to geofence object
193 carma_wm::fromBinMsg(*geofence_msg, gf_ptr, world_model_->getMutableMap());
194
195 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Processing Map Update with Geofence Id:" << gf_ptr->id_);
196
197 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests addition of lanelets size: " << gf_ptr->lanelet_additions_.size());
198 for (auto llt : gf_ptr->lanelet_additions_)
199 {
200 // world model here should blindly accept the map update received
201 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new lanelet with id: " << llt.id());
202 auto left = llt.leftBound3d(); //new lanelet coming in
203
204 // updating incoming points' memory addresses with local ones of same ids
205 // so that lanelet library can recognize they are same objects
206 for (size_t i = 0; i < left.size(); i ++)
207 {
208 if (world_model_->getMutableMap()->pointLayer.exists(left[i].id())) //rewrite the memory address of new pts with that of local
209 {
210 llt.leftBound3d()[i] = world_model_->getMutableMap()->pointLayer.get(left[i].id());
211 }
212 }
213 auto right = llt.rightBound3d(); //new lanelet coming in
214 for (size_t i = 0; i < right.size(); i ++)
215 {
216 if (world_model_->getMutableMap()->pointLayer.exists(right[i].id())) //rewrite the memory address of new pts with that of local
217 {
218 llt.rightBound3d()[i] = world_model_->getMutableMap()->pointLayer.get(right[i].id());
219 }
220 }
221
222 world_model_->getMutableMap()->add(llt);
223 }
224
225 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " sends record of traffic_lights_id size: " << gf_ptr->traffic_light_id_lookup_.size());
226 for (auto const &[traffic_light_id, lanelet_id] : gf_ptr->traffic_light_id_lookup_)
227 {
228 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new pair for traffic light ids: " << traffic_light_id << ", and lanelet::Id: " << lanelet_id);
229 world_model_->setTrafficLightIds(traffic_light_id, lanelet_id);
230 }
231
232 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " sends record of intersections size: " << gf_ptr->sim_.intersection_id_to_regem_id_.size());
233 if (gf_ptr->sim_.intersection_id_to_regem_id_.size() > 0)
234 {
235 world_model_->sim_ = gf_ptr->sim_;
237 }
238
239 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests removal of size: " << gf_ptr->remove_list_.size());
240 for (auto const &[lanelet_id, lanelet_to_remove] : gf_ptr->remove_list_)
241 {
242 auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(lanelet_id);
243 // we can only check by id, if the element is there
244 // this is only for speed optimization, as world model here should blindly accept the map update received
245 auto regems_copy_to_check = parent_llt.regulatoryElements(); // save local copy as the regem can be deleted during iteration
246 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Regems found in lanelet: " << regems_copy_to_check.size());
247 for (auto regem: regems_copy_to_check)
248 {
249 // we can't use the deserialized element as its data address conflicts the one in this node
250 if (lanelet_to_remove->id() == regem->id()) world_model_->getMutableMap()->remove(parent_llt, regem);
251 }
252 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Regems left in lanelet after removal: " << parent_llt.regulatoryElements().size());
253
254 }
255
256 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests update of size: " << gf_ptr->update_list_.size());
257
258 // we should extract general regem to specific type of regem the geofence specifies
259 for (auto const &[lanelet_id, lanelet_to_update]: gf_ptr->update_list_)
260 {
261
262 auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(lanelet_id);
263
264 auto regemptr_it = world_model_->getMutableMap()->regulatoryElementLayer.find(lanelet_to_update->id());
265
266 // if this regem is already in the map.
267 // This section is expected to be called to add back regulations which were previously removed by expired geofences.
268 if (regemptr_it != world_model_->getMutableMap()->regulatoryElementLayer.end())
269 {
270
271 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Reapplying previously existing element for lanelet id:" << parent_llt.id() << ", and regem id: " << regemptr_it->get()->id());
272 // again we should use the element with correct data address to be consistent
273 world_model_->getMutableMap()->update(parent_llt, *regemptr_it);
274 }
275 else // Updates are treated as new regulations after the old value was removed. In both cases we enter this block.
276 {
277 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "New regulatory element at lanelet: " << parent_llt.id() << ", and id: " << lanelet_to_update->id());
278 newRegemUpdateHelper(parent_llt, lanelet_to_update.get());
279 }
280 }
281
282 // set the Map to trigger a new route graph construction if rerouting was required by the updates and a new graph was not provided
283 world_model_->setMap(world_model_->getMutableMap(), current_map_version_, recompute_route_flag_ && !geofence_msg->has_routing_graph );
284
285 // If a new graph was provided then set that graph
286 // recompute_route_flag_ not checked here to support the case of the first map or map version changing
287 if (geofence_msg->has_routing_graph) {
288
289 LaneletRoutingGraphPtr graph = routingGraphFromMsg(geofence_msg->routing_graph, world_model_->getMutableMap());
290
291 if (!graph) {
292 throw std::invalid_argument("Map updated provided routing graph which could not be applied to the current map.");
293 }
294
295 world_model_->setRoutingGraph(graph);
296
297 }
298
299 // no need to reroute again unless received invalidated msg again
301 recompute_route_flag_ = false;
302
303
304 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_);
305
306 // Call user defined map callback
307 if (map_callback_)
308 {
309 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Calling user defined map update callback");
311 }
312}
313
314
322void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet::RegulatoryElement* regem) const
323{
324 auto factory_regem = lanelet::RegulatoryElementFactory::create(regem->attribute(lanelet::AttributeName::Subtype).value(),
325 std::const_pointer_cast<lanelet::RegulatoryElementData>(regem->constData()));
326
327 // we should extract general regem to specific type of regem the geofence specifies
328 switch(resolveGeofenceType(regem->attribute(lanelet::AttributeName::Subtype).value()))
329 {
331 {
332 lanelet::PassingControlLinePtr control_line = std::dynamic_pointer_cast<lanelet::PassingControlLine>(factory_regem);
333 if (control_line)
334 {
335 world_model_->getMutableMap()->update(parent_llt, control_line);
336 }
337 else
338 {
339 std::invalid_argument("Dynamic Pointer cast failed on getting valid control line");
340 }
341
342 break;
343 }
345 {
346 lanelet::DigitalSpeedLimitPtr speed = std::dynamic_pointer_cast<lanelet::DigitalSpeedLimit>(factory_regem);
347 if (speed)
348 {
349 world_model_->getMutableMap()->update(parent_llt, speed);
350 }
351 else
352 {
353 std::invalid_argument("Dynamic Pointer cast failed on getting valid speed limit");
354 }
355 break;
356 }
358 {
359
360 lanelet::RegionAccessRulePtr rar = std::dynamic_pointer_cast<lanelet::RegionAccessRule>(factory_regem);
361 if (rar)
362 {
363 world_model_->getMutableMap()->update(parent_llt, rar);
364 }
365 else
366 {
367 std::invalid_argument("Dynamic Pointer cast failed on getting valid region access rule");
368 }
369
370 break;
371 }
373 {
374
375 lanelet::DigitalMinimumGapPtr min_gap = std::dynamic_pointer_cast<lanelet::DigitalMinimumGap>(factory_regem);
376 if (min_gap)
377 {
378 world_model_->getMutableMap()->update(parent_llt, min_gap);
379 }
380 else
381 {
382 std::invalid_argument("Dynamic Pointer cast failed on getting valid minimum gap rule");
383 }
384
385 break;
386 }
388 {
389
390 lanelet::DirectionOfTravelPtr dot = std::dynamic_pointer_cast<lanelet::DirectionOfTravel>(factory_regem);
391 if (dot)
392 {
393 world_model_->getMutableMap()->update(parent_llt, dot);
394 }
395 else
396 {
397 std::invalid_argument("Dynamic Pointer cast failed on getting valid direction of travel");
398 }
399
400 break;
401 }
403 {
404
405 lanelet::StopRulePtr sr = std::dynamic_pointer_cast<lanelet::StopRule>(factory_regem);
406 if (sr)
407 {
408 world_model_->getMutableMap()->update(parent_llt, sr);
409 }
410 else
411 {
412 std::invalid_argument("Dynamic Pointer cast failed on getting valid stop rule");
413 }
414 break;
415 }
417 {
418 lanelet::CarmaTrafficSignalPtr ctl = std::dynamic_pointer_cast<lanelet::CarmaTrafficSignal>(factory_regem);
419 if (ctl)
420 {
421 world_model_->getMutableMap()->update(parent_llt, ctl);
422 }
423 else
424 {
425 std::invalid_argument("Dynamic Pointer cast failed on getting valid carma traffic signal");
426 }
427 break;
428 }
430 {
431 lanelet::SignalizedIntersectionPtr si = std::dynamic_pointer_cast<lanelet::SignalizedIntersection>(factory_regem);
432 if (si)
433 {
434 world_model_->getMutableMap()->update(parent_llt, si);
435 }
436 else
437 {
438 std::invalid_argument("Dynamic Pointer cast failed on getting valid signalized intersection");
439 }
440
441 break;
442 }
443 default:
444 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "World Model instance received an unsupported geofence type in its map update callback!");
445 break;
446 }
447}
448
449LaneletRoutingGraphPtr WMListenerWorker::routingGraphFromMsg(const autoware_lanelet2_msgs::msg::RoutingGraph& msg, lanelet::LaneletMapPtr map) const {
450
451 if (msg.participant_type.compare(getVehicleParticipationType()) != 0) {
452
453 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph does not have matching vehicle type for world model. WM Type: "
455 << " graph type: " << msg.participant_type
456 );
457
458 return nullptr;
459 }
460
461 // Get the lists of passable lanelets and areas
462 // Both these lists must be populated in the same order as the message to support later logic
463 lanelet::ConstLanelets passable_lanelets;
464 lanelet::ConstAreas passable_areas;
465
466 passable_lanelets.reserve(msg.lanelet_vertices.size());
467 passable_areas.reserve(msg.area_vertices.size());
468
469 try {
470
471 // All the passable lanelets and areas should be included as a vertext so just iterate over each and store
472 for (auto vertex : msg.lanelet_vertices) {
473 passable_lanelets.emplace_back(map->laneletLayer.get(vertex.lanelet_or_area));
474 }
475
476 for (auto vertex : msg.area_vertices) {
477 passable_areas.emplace_back(map->areaLayer.get(vertex.lanelet_or_area));
478 }
479
480 } catch(const lanelet::NoSuchPrimitiveError& e) {
481
482 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Received routing graph specifies lanelets which do not match the current map version. Actual exception: " << e.what());
483
484 return nullptr;
485 }
486
487
488 // Build the submap
489 // This operation does increase in time as the number of lanelets and areas increase
490 // however testing shows it to less than 1% of the total routing graph build time so this is a reasonable operation to keep
491 auto passable_map = lanelet::utils::createConstSubmap(passable_lanelets, passable_areas);
492
493 // This is the actual graph object which is used to initialize a RoutingGraph
494 auto graph = std::make_unique<lanelet::routing::internal::RoutingGraphGraph>(msg.num_unique_routing_cost_ids);
495
496 // Vertex must be added first then the edge can be added
497 for (auto ll : passable_lanelets) {
498 graph->addVertex(lanelet::routing::internal::VertexInfo{ll});
499 }
500
501 for (auto area : passable_areas) {
502 graph->addVertex(lanelet::routing::internal::VertexInfo{area});
503 }
504
505 // Now we can add edges
506 for (size_t i = 0; i < msg.lanelet_vertices.size(); ++i) {
507
508 auto vertex = msg.lanelet_vertices[i];
509 auto lanelet = passable_lanelets[i]; // passable_lanelets should be in the same order based on how its constructed
510
511 for (size_t j = 0; j < vertex.lanelet_or_area_ids.size(); ++j) {
512
513 lanelet::routing::RelationType relation;
514
515 // Get relation
516 switch (vertex.edge_relations[j])
517 {
518 case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_SUCCESSOR:
519 relation = lanelet::routing::RelationType::Successor; break;
520
521 case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_LEFT:
522 relation = lanelet::routing::RelationType::Left; break;
523
524 case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_RIGHT:
526
527 case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_ADJACENT_LEFT:
528 relation = lanelet::routing::RelationType::AdjacentLeft; break;
529
530 case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_ADJACENT_RIGHT:
531 relation = lanelet::routing::RelationType::AdjacentRight; break;
532
533 case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_CONFLICTING:
534 relation = lanelet::routing::RelationType::Conflicting; break;
535
536 case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_AREA:
537 relation = lanelet::routing::RelationType::Area; break;
538
539 default: // Treat default as RELATION_NONE
540 relation = lanelet::routing::RelationType::None; break;
541 }
542
543 try {
544
545 // Create edge
546 graph->addEdge(
547 lanelet,
548 map->laneletLayer.get(vertex.lanelet_or_area_ids[j]),
549 lanelet::routing::internal::EdgeInfo{vertex.edge_routing_costs[j], vertex.edge_routing_cost_source_ids[j], relation}
550 );
551
552 } catch(const lanelet::NoSuchPrimitiveError& e) {
553
554 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph specifies lanelets which do not match the current map version. Not found lanelet or area: "
555 << vertex.lanelet_or_area_ids[j] << " Actual exception: " << e.what());
556
557 return nullptr;
558 }
559
560 }
561 }
562
563 // Build and return the final initialized routing graph
564 return std::make_shared<lanelet::routing::RoutingGraph>(std::move(graph), std::move(passable_map));
565
566}
567
569{
570 return world_model_->getVehicleParticipationType();
571}
572
573void WMListenerWorker::roadwayObjectListCallback(const carma_perception_msgs::msg::RoadwayObstacleList::SharedPtr msg)
574{
575 // this topic publishes only the objects that are on the road
576 world_model_->setRoadwayObjects(msg->roadway_obstacles);
577}
578
579void WMListenerWorker::ros1ClockCallback(const rosgraph_msgs::msg::Clock::SharedPtr clock_msg)
580{
581 world_model_->setRos1Clock(rclcpp::Time(clock_msg->clock));
582}
583
584void WMListenerWorker::simClockCallback(const rosgraph_msgs::msg::Clock::SharedPtr clock_msg)
585{
586 world_model_->setSimulationClock(rclcpp::Time(clock_msg->clock));
587}
588
589void WMListenerWorker::routeCallback(const carma_planning_msgs::msg::Route::SharedPtr route_msg)
590{
591 if (route_msg->map_version < current_map_version_) {
592 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Route message rejected as it is for an older map");
593 rerouting_flag_ = false; // Clear any blockers on map updates as the route we were waiting for is no longer valid
594 return;
595 }
596
597 if (route_msg->map_version > current_map_version_) {
598 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Route message received for future map. Delaying application until map is recieved");
599 delayed_route_msg_ = *route_msg;
600 return;
601 }
602
603 bool route_invalidated_by_queued_map_update = false; // Flag to indicate whether this new route has been invalidated due to one of the applied queued map updates
604 if(rerouting_flag_==true && route_msg->is_rerouted )
605
606 {
607
608 rerouting_flag_ = false; // Reset flag since the route node has finished re-routing
609
610 // After setting map evaluate the current update queue to apply any updates that arrived before the map
611 bool more_updates_to_apply = true;
612 while(!map_update_queue_.empty() && more_updates_to_apply) {
613
614 auto update = map_update_queue_.front(); // Get first update
615 map_update_queue_.pop(); // Remove update from queue
616
617 if (update->map_version < current_map_version_) { // Drop any so far unapplied updates for the current map
618 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Apply from reroute: There were unapplied updates in carma_wm when a new map was recieved.");
619 continue;
620 }
621 if (update->map_version == current_map_version_) { // Current update goes with current map which is also the map used by this route
622 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Applying queued update after route was recieved. ");
623
624 if (update->invalidates_route == true) {
625 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Applied queued map update has invalidated the route.");
626 route_invalidated_by_queued_map_update = true;
627 }
628
629 update->invalidates_route=false; // Do not trigger recomputation of routing graph in mapUpdateCallback; recomputation of routing graph will occur outside of this loop
630
631 mapUpdateCallback(update); // Apply the update
632 } else {
633 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Apply from reroute: Done applying updates for new map. However, more updates are waiting for a future map.");
634 more_updates_to_apply = false; // If there is more updates queued that are not for this map version assume they are for a future map version
635 }
636
637 }
638
639 }
640
641 if (!world_model_->getMap()) { // This check is a bit redundant but still useful from a debugging perspective as the alternative is a segfault
642 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "WMListener received a route before a map was available. Dropping route message.");
643 return;
644 }
645
646 // If one of the applied queued map updates invalidated the route, then the routing graph must be updated again for the route node
647 if (route_invalidated_by_queued_map_update && route_node_flag_){
648 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"At least one applied queued map update has invalidated the route. Routing graph will be recomputed.");
649 world_model_->setMap(world_model_->getMutableMap(), current_map_version_, route_invalidated_by_queued_map_update);
650 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Finished recomputing the routing graph for the applied queued map update(s)");
651
652 rerouting_flag_ = true; // Set flag to trigger a route update by the route node due to the updated routing graph
653
654 return;
655 }
656 else {
657 rerouting_flag_ = false; // Reset flag since no applied queued map updates invalidated the route for the route node
658
659 auto path = lanelet::ConstLanelets();
660 for(auto id : route_msg->shortest_path_lanelet_ids)
661 {
662 auto ll = world_model_->getMap()->laneletLayer.get(id);
663 path.push_back(ll);
664 }
665
666 auto route_opt = path.size() == 1 ? world_model_->getMapRoutingGraph()->getRoute(path.front(), path.back())
667 : world_model_->getMapRoutingGraph()->getRouteVia(path.front(), lanelet::ConstLanelets(path.begin() + 1, path.end() - 1), path.back());
668 if(route_opt.is_initialized()) {
669 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Setting route in world model");
670 auto ptr = std::make_shared<lanelet::routing::Route>(std::move(route_opt.get()));
671 world_model_->setRoute(ptr);
672 }
673
674 world_model_->setRouteEndPoint({route_msg->end_point.x,route_msg->end_point.y,route_msg->end_point.z});
675 world_model_->setRouteName(route_msg->route_name);
676
677 // Call route_callback_
678 if (route_callback_)
679 {
681 }
682
683 return;
684 }
685}
686
688{
690}
691
693{
695}
696
698{
699 config_speed_limit_ = config_lim;
700 //Function to load config_limit into CarmaWorldModel
701 world_model_->setConfigSpeedLimit(config_speed_limit_);
702}
703
704void WMListenerWorker::isUsingSimTime(bool use_sim_time)
705{
706 use_sim_time_ = use_sim_time;
707}
708void WMListenerWorker::isSpatWallTime(bool is_spat_wall_time)
709{
710 is_spat_wall_time_ = is_spat_wall_time;
711}
712
714{
715 return config_speed_limit_;
716}
717
719{
720 //Function to load participation type into CarmaWorldModel
721 world_model_->setVehicleParticipationType(participant);
722}
723
724
725} // namespace carma_wm
Class which implements the WorldModel interface. In addition this class provides write access to the ...
This class manages and keeps track of all signalized intersections in the map. All of the SPAT and MA...
std::unordered_map< uint8_t, std::unordered_set< lanelet::Id > > signal_group_to_entry_lanelet_ids_
std::unordered_map< uint16_t, lanelet::Id > intersection_id_to_regem_id_
std::unordered_map< uint8_t, std::unordered_set< lanelet::Id > > signal_group_to_exit_lanelet_ids_
std::unordered_map< uint8_t, lanelet::Id > signal_group_to_traffic_light_id_
boost::optional< carma_planning_msgs::msg::Route > delayed_route_msg_
void setConfigSpeedLimit(double config_lim)
Allows user to set a callback to be triggered when a map update is received.
std::function< void()> map_callback_
void isSpatWallTime(bool is_spat_wall_time)
set true if incoming spat is based on wall clock
void mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::SharedPtr geofence_msg)
Callback for new map update messages (geofence). Updates the underlying map.
double getConfigSpeedLimit() const
Returns the current configured speed limit value.
void enableUpdatesWithoutRoute()
Enable updates without route and set route_node_flag_ as true.
std::shared_ptr< CARMAWorldModel > world_model_
void setRouteCallback(std::function< void()> callback)
Allows user to set a callback to be triggered when a route update is received.
void newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet::RegulatoryElement *regem) const
This is a helper function updates the parent_llt with specified regem. This function is needed as we ...
LaneletRoutingGraphPtr routingGraphFromMsg(const autoware_lanelet2_msgs::msg::RoutingGraph &msg, lanelet::LaneletMapPtr map) const
Helper function to convert a routing graph message into a actual RoutingGraph object.
void routeCallback(const carma_planning_msgs::msg::Route::SharedPtr route_msg)
Callback for route message.
void setMapCallback(std::function< void()> callback)
Allows user to set a callback to be triggered when a map update is received.
bool checkIfReRoutingNeeded() const
Check if re-routing is needed and returns re-routing flag.
void incomingSpatCallback(const carma_v2x_msgs::msg::SPAT::SharedPtr spat_msg)
incoming spat message
void roadwayObjectListCallback(const carma_perception_msgs::msg::RoadwayObstacleList::SharedPtr msg)
Callback for roadway objects msg.
std::function< void()> route_callback_
void mapCallback(const autoware_lanelet2_msgs::msg::MapBin::SharedPtr map_msg)
Callback for new map messages. Updates the underlying map.
void simClockCallback(const rosgraph_msgs::msg::Clock::SharedPtr clock_msg)
Callback for Simulation clock message (used in Simulation runs)
void isUsingSimTime(bool use_sim_time)
set true if simulation_mode is on
std::string getVehicleParticipationType() const
Returns the Vehicle Participation Type value.
WorldModelConstPtr getWorldModel() const
Constructor.
void ros1ClockCallback(const rosgraph_msgs::msg::Clock::SharedPtr clock_msg)
Callback for ROS1 clock message (used in Simulation runs)
std::queue< autoware_lanelet2_msgs::msg::MapBin::SharedPtr > map_update_queue_
void setVehicleParticipationType(std::string participant)
Allows user to set a callback to be triggered when a map update is received.
bool callback(cav_srvs::PlanTrajectory::Request &req, cav_srvs::PlanTrajectory::Response &res)
Definition: helper.cpp:21
std::shared_ptr< lanelet::routing::RoutingGraph > LaneletRoutingGraphPtr
Definition: WorldModel.hpp:50
std::shared_ptr< const WorldModel > WorldModelConstPtr
Definition: WorldModel.hpp:452
void fromBinMsg(const autoware_lanelet2_msgs::msg::MapBin &msg, std::shared_ptr< carma_wm::TrafficControl > gf_ptr, lanelet::LaneletMapPtr lanelet_map=nullptr)
GeofenceType resolveGeofenceType(const std::string &rule_name)
void logSignalizedIntersectionManager(const carma_wm::SignalizedIntersectionManager &sim)