Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix/assorted fix for yield #2200

Merged
merged 22 commits into from
Dec 5, 2023
Merged
Show file tree
Hide file tree
Changes from 14 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 17 additions & 17 deletions carma_wm/src/CARMAWorldModel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -82,10 +82,10 @@ namespace carma_wm
}

lanelet::Id CARMAWorldModel::getTrafficSignalId(uint16_t intersection_id, uint8_t signal_group_id)
{
{
lanelet::Id inter_id = lanelet::InvalId;
lanelet::Id signal_id = lanelet::InvalId;

if (sim_.intersection_id_to_regem_id_.find(intersection_id) != sim_.intersection_id_to_regem_id_.end())
{
inter_id = sim_.intersection_id_to_regem_id_[intersection_id];
Expand All @@ -95,7 +95,7 @@ namespace carma_wm
{
signal_id = sim_.signal_group_to_traffic_light_id_[signal_group_id];
}

return signal_id;
}

Expand Down Expand Up @@ -133,15 +133,15 @@ namespace carma_wm
std::vector<lanelet::BusStopRulePtr> bus_stop_list;
auto curr_downtrack = routeTrackPos(loc).downtrack;
// shortpath is already sorted by distance

for (const auto &ll : route_->shortestPath())
{
auto bus_stops = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs<lanelet::BusStopRule>();
if (bus_stops.empty())
if (bus_stops.empty())
{
continue;
}

for (auto bus_stop : bus_stops)
{
auto stop_line = bus_stop->stopAndWaitLine();
Expand Down Expand Up @@ -335,7 +335,7 @@ namespace carma_wm

if (!bounds_inclusive) // reduce bounds slightly to avoid including exact bounds
{
if (std::max(min.downtrack, start + 0.00001) > std::min(max.downtrack, end - 0.00001)
if (std::max(min.downtrack, start + 0.00001) > std::min(max.downtrack, end - 0.00001)
|| (start == end && (min.downtrack >= start || max.downtrack <= end)))
{ // Check for 1d intersection
// No intersection so continue
Expand All @@ -344,7 +344,7 @@ namespace carma_wm
}
else
{
if (std::max(min.downtrack, start) > std::min(max.downtrack, end)
if (std::max(min.downtrack, start) > std::min(max.downtrack, end)
|| (start == end && (min.downtrack > start || max.downtrack < end)))
{ // Check for 1d intersection
// No intersection so continue
Expand Down Expand Up @@ -699,7 +699,7 @@ namespace carma_wm
// Append distance to current centerline
size_t offset =
lineStrings.size() == 0 || lineStrings.back().size() == 0 ?
0 :
0 :
1; // Prevent duplicate points when concatenating. Not clear if causes an issue at lane changes
if ((nextCenterline.size() <= 1) || (nextCenterline.size() <= 2 && offset == 1))
{
Expand Down Expand Up @@ -1222,15 +1222,15 @@ namespace carma_wm
std::vector<lanelet::CarmaTrafficSignalPtr> light_list;
auto curr_downtrack = routeTrackPos(loc).downtrack;
// shortpath is already sorted by distance

for (const auto &ll : route_->shortestPath())
{
auto lights = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
if (lights.empty())
{
continue;
}

for (auto light : lights)
{
auto stop_line = light->getStopLine(ll);
Expand Down Expand Up @@ -1380,7 +1380,7 @@ namespace carma_wm
}

lanelet::CarmaTrafficSignalPtr CARMAWorldModel::getTrafficSignal(const lanelet::Id& id) const
{
{
auto general_regem = semantic_map_->regulatoryElementLayer.get(id);

auto lanelets_general = semantic_map_->laneletLayer.findUsages(general_regem);
Expand Down Expand Up @@ -1464,9 +1464,9 @@ namespace carma_wm
RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"), "No intersection_state_list in the newly received SPAT msg. Returning...");
return;
}

for (const auto& curr_intersection : spat_msg.intersection_state_list)
{
{

for (const auto& current_movement_state : curr_intersection.movement_list)
{
Expand All @@ -1478,7 +1478,7 @@ namespace carma_wm
}

lanelet::CarmaTrafficSignalPtr curr_light = getTrafficSignal(curr_light_id);

if (curr_light == nullptr)
{
continue;
Expand Down Expand Up @@ -1518,11 +1518,11 @@ namespace carma_wm
start_time_dynamic=min_end_time_converter_minute_of_year(start_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time); // Accounting minute of the year

auto received_state_dynamic = static_cast<lanelet::CarmaTrafficSignalState>(current_movement_event.event_state.movement_phase_state);

sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time_dynamic, received_state_dynamic));
sim_.traffic_signal_start_times_[curr_intersection.id.id][current_movement_state.signal_group].push_back(
start_time_dynamic);

RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "intersection id: " << (int)curr_intersection.id.id << ", signal: " << (int)current_movement_state.signal_group
<< ", start_time: " << std::to_string(lanelet::time::toSec(start_time_dynamic))
<< ", end_time: " << std::to_string(lanelet::time::toSec(min_end_time_dynamic))
Expand Down
93 changes: 47 additions & 46 deletions carma_wm/src/WMListenerWorker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
namespace carma_wm
{

enum class GeofenceType{ INVALID, DIGITAL_SPEED_LIMIT, PASSING_CONTROL_LINE, REGION_ACCESS_RULE, DIGITAL_MINIMUM_GAP,
enum class GeofenceType{ INVALID, DIGITAL_SPEED_LIMIT, PASSING_CONTROL_LINE, REGION_ACCESS_RULE, DIGITAL_MINIMUM_GAP,
DIRECTION_OF_TRAVEL, STOP_RULE, CARMA_TRAFFIC_LIGHT, SIGNALIZED_INTERSECTION/* ... others */ };
// helper function that return geofence type as an enum, which makes it cleaner by allowing switch statement
GeofenceType resolveGeofenceType(const std::string& rule_name)
Expand Down Expand Up @@ -66,7 +66,7 @@ void WMListenerWorker::mapCallback(const autoware_lanelet2_msgs::msg::MapBin::Un
// After setting map evaluate the current update queue to apply any updates that arrived before the map
bool more_updates_to_apply = true;
while(!map_update_queue_.empty() && more_updates_to_apply) {

auto update = std::move(map_update_queue_.front()); // Get first update
map_update_queue_.pop(); // Remove update from queue

Expand Down Expand Up @@ -119,34 +119,35 @@ void WMListenerWorker::enableUpdatesWithoutRoute()
// helper function to log SignalizedIntersectionManager content
void logSignalizedIntersectionManager(const carma_wm::SignalizedIntersectionManager& sim)
{
for (auto pair : sim.intersection_id_to_regem_id_)
for (auto const &[intersection_id, regulatory_element_id] : sim.intersection_id_to_regem_id_)
{
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "inter id: " << (int)pair.first << ", regem id: " << pair.second);
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),
"inter id: " << intersection_id << ", regem id: " << regulatory_element_id);
}
for (auto pair : sim.signal_group_to_entry_lanelet_ids_)
for (auto const &[signal_id, entry_llt_ids] : sim.signal_group_to_entry_lanelet_ids_)
{
for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++)
for (auto iter = entry_llt_ids.begin(); iter != entry_llt_ids.end(); iter++)
MishkaMN marked this conversation as resolved.
Show resolved Hide resolved
{
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", entry llt id: " << *iter);
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", entry llt id: " << *iter);
}
}
for (auto pair : sim.signal_group_to_exit_lanelet_ids_)
for (auto const &[signal_id, exit_llt_ids] : sim.signal_group_to_exit_lanelet_ids_)
{
for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++)
for (auto iter = exit_llt_ids.begin(); iter != exit_llt_ids.end(); iter++)
{
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", exit llt id: " << *iter);
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", exit llt id: " << *iter);
}
}
for (auto pair : sim.signal_group_to_traffic_light_id_)
for (auto const &[signal_id, regem_id] : sim.signal_group_to_traffic_light_id_)
{
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", regem id: " << pair.second);
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", regem id: " << regem_id);
}
}

void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::UniquePtr geofence_msg)
{
RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Map Update Being Evaluated. SeqNum: " << geofence_msg->seq_id);
if (rerouting_flag_) // no update should be applied if rerouting
if (rerouting_flag_) // no update should be applied if rerouting
{
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_);
map_update_queue_.emplace(std::move(geofence_msg));
Expand All @@ -170,7 +171,7 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un


if(geofence_msg->invalidates_route==true && world_model_->getRoute())
{
{
rerouting_flag_=true;
recompute_route_flag_ = true;

Expand All @@ -187,21 +188,21 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un
most_recent_update_msg_seq_ = geofence_msg->seq_id; // Update current sequence count

auto gf_ptr = std::shared_ptr<carma_wm::TrafficControl>(new carma_wm::TrafficControl);

// convert ros msg to geofence object
carma_wm::fromBinMsg(*geofence_msg, gf_ptr, world_model_->getMutableMap());

RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Processing Map Update with Geofence Id:" << gf_ptr->id_);

RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests addition of lanelets size: " << gf_ptr->lanelet_additions_.size());
for (auto llt : gf_ptr->lanelet_additions_)
{
// world model here should blindly accept the map update received
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new lanelet with id: " << llt.id());
auto left = llt.leftBound3d(); //new lanelet coming in

// updating incoming points' memory addresses with local ones of same ids
// so that lanelet library can recognize they are same objects
// so that lanelet library can recognize they are same objects
for (size_t i = 0; i < left.size(); i ++)
{
if (world_model_->getMutableMap()->pointLayer.exists(left[i].id())) //rewrite the memory address of new pts with that of local
Expand All @@ -222,10 +223,10 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un
}

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());
for (auto pair : gf_ptr->traffic_light_id_lookup_)
for (auto const &[traffic_light_id, lanelet_id] : gf_ptr->traffic_light_id_lookup_)
{
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new pair for traffic light ids: " << pair.first << ", and lanelet::Id: " << pair.second);
world_model_->setTrafficLightIds(pair.first, pair.second);
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new pair for traffic light ids: " << traffic_light_id << ", and lanelet::Id: " << lanelet_id);
world_model_->setTrafficLightIds(traffic_light_id, lanelet_id);
}

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());
Expand All @@ -236,48 +237,48 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un
}

RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests removal of size: " << gf_ptr->remove_list_.size());
for (auto pair : gf_ptr->remove_list_)
for (auto const &[lanelet_id, lanelet_to_remove] : gf_ptr->remove_list_)
{
auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(pair.first);
auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(lanelet_id);
// we can only check by id, if the element is there
// this is only for speed optimization, as world model here should blindly accept the map update received
auto regems_copy_to_check = parent_llt.regulatoryElements(); // save local copy as the regem can be deleted during iteration
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Regems found in lanelet: " << regems_copy_to_check.size());
for (auto regem: regems_copy_to_check)
{
// we can't use the deserialized element as its data address conflicts the one in this node
if (pair.second->id() == regem->id()) world_model_->getMutableMap()->remove(parent_llt, regem);
if (lanelet_to_remove->id() == regem->id()) world_model_->getMutableMap()->remove(parent_llt, regem);
}
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Regems left in lanelet after removal: " << parent_llt.regulatoryElements().size());

}

RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests update of size: " << gf_ptr->update_list_.size());

// we should extract general regem to specific type of regem the geofence specifies
for (auto pair : gf_ptr->update_list_)
for (auto const &[lanelet_id, lanelet_to_update]: gf_ptr->update_list_)
{

auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(pair.first);
auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(lanelet_id);

auto regemptr_it = world_model_->getMutableMap()->regulatoryElementLayer.find(pair.second->id());
auto regemptr_it = world_model_->getMutableMap()->regulatoryElementLayer.find(lanelet_to_update->id());

// if this regem is already in the map.
// This section is expected to be called to add back regulations which were previously removed by expired geofences.
if (regemptr_it != world_model_->getMutableMap()->regulatoryElementLayer.end())
if (regemptr_it != world_model_->getMutableMap()->regulatoryElementLayer.end())
{

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());
// again we should use the element with correct data address to be consistent
world_model_->getMutableMap()->update(parent_llt, *regemptr_it);
}
else // Updates are treated as new regulations after the old value was removed. In both cases we enter this block.
else // Updates are treated as new regulations after the old value was removed. In both cases we enter this block.
{
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "New regulatory element at lanelet: " << parent_llt.id() << ", and id: " << pair.second->id());
newRegemUpdateHelper(parent_llt, pair.second.get());
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "New regulatory element at lanelet: " << parent_llt.id() << ", and id: " << lanelet_to_update->id());
newRegemUpdateHelper(parent_llt, lanelet_to_update.get());
}
}

// set the Map to trigger a new route graph construction if rerouting was required by the updates and a new graph was not provided
world_model_->setMap(world_model_->getMutableMap(), current_map_version_, recompute_route_flag_ && !geofence_msg->has_routing_graph );

Expand All @@ -298,9 +299,9 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un
// no need to reroute again unless received invalidated msg again
if (recompute_route_flag_)
recompute_route_flag_ = false;


RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_);

RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_);

// Call user defined map callback
if (map_callback_)
Expand Down Expand Up @@ -365,7 +366,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet
{
std::invalid_argument("Dynamic Pointer cast failed on getting valid region access rule");
}

break;
}
case GeofenceType::DIGITAL_MINIMUM_GAP:
Expand All @@ -380,7 +381,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet
{
std::invalid_argument("Dynamic Pointer cast failed on getting valid minimum gap rule");
}

break;
}
case GeofenceType::DIRECTION_OF_TRAVEL:
Expand All @@ -395,7 +396,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet
{
std::invalid_argument("Dynamic Pointer cast failed on getting valid direction of travel");
}

break;
}
case GeofenceType::STOP_RULE:
Expand Down Expand Up @@ -436,7 +437,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet
{
std::invalid_argument("Dynamic Pointer cast failed on getting valid signalized intersection");
}

break;
}
default:
Expand All @@ -449,8 +450,8 @@ LaneletRoutingGraphPtr WMListenerWorker::routingGraphFromMsg(const autoware_lane

if (msg.participant_type.compare(getVehicleParticipationType()) != 0) {

RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph does not have matching vehicle type for world model. WM Type: "
<< getVehicleParticipationType()
RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph does not have matching vehicle type for world model. WM Type: "
<< getVehicleParticipationType()
<< " graph type: " << msg.participant_type
);

Expand Down Expand Up @@ -543,14 +544,14 @@ LaneletRoutingGraphPtr WMListenerWorker::routingGraphFromMsg(const autoware_lane

// Create edge
graph->addEdge(
lanelet,
map->laneletLayer.get(vertex.lanelet_or_area_ids[j]),
lanelet,
map->laneletLayer.get(vertex.lanelet_or_area_ids[j]),
lanelet::routing::internal::EdgeInfo{vertex.edge_routing_costs[j], vertex.edge_routing_cost_source_ids[j], relation}
);

} catch(const lanelet::NoSuchPrimitiveError& e) {

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: "
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: "
<< vertex.lanelet_or_area_ids[j] << " Actual exception: " << e.what());

return nullptr;
Expand Down Expand Up @@ -601,7 +602,7 @@ void WMListenerWorker::routeCallback(const carma_planning_msgs::msg::Route::Uniq
while(!map_update_queue_.empty() && more_updates_to_apply) {

auto update = std::move(map_update_queue_.front()); // Get first update
map_update_queue_.pop(); // Remove update from queue
map_update_queue_.pop(); // Remove update from queue

if (update->map_version < current_map_version_) { // Drop any so far unapplied updates for the current map
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.");
Expand Down Expand Up @@ -701,7 +702,7 @@ double WMListenerWorker::getConfigSpeedLimit() const
}

void WMListenerWorker::setVehicleParticipationType(std::string participant)
{
{
//Function to load participation type into CarmaWorldModel
world_model_->setVehicleParticipationType(participant);
}
Expand Down
Loading