MapConformer.cpp

Go to the documentation of this file.

/*
 * 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 <rclcpp/rclcpp.hpp>
#include <carma_wm/WorldModel.hpp>
#include <lanelet2_core/Attribute.h>
#include <lanelet2_core/utility/Units.h>
#include <boost/algorithm/string.hpp>
#include <carma_wm/MapConformer.hpp>
 
 
namespace lanelet
{
namespace MapConformer
{
namespace
{  // Private namespace
// Enum defining types of lane change
enum class LaneChangeType
{
  ToRight,
  ToLeft,
  Both,
  None
};
 
// List of supported participants. Should exactly match elements of lanelet::Participants struct
constexpr size_t PARTICIPANT_COUNT = 12;
constexpr const char* participant_types[PARTICIPANT_COUNT] = { lanelet::Participants::Vehicle,  //
                                                               lanelet::Participants::VehicleBus,
                                                               lanelet::Participants::VehicleCar,
                                                               lanelet::Participants::VehicleCarElectric,
                                                               lanelet::Participants::VehicleCarCombustion,
                                                               lanelet::Participants::VehicleTruck,
                                                               lanelet::Participants::VehicleMotorcycle,
                                                               lanelet::Participants::VehicleTaxi,
                                                               lanelet::Participants::VehicleEmergency,
                                                               lanelet::Participants::Pedestrian,  //
                                                               lanelet::Participants::Bicycle,     //
                                                               lanelet::Participants::Train };
 
std::vector<lanelet::traffic_rules::TrafficRulesUPtr> getAllGermanTrafficRules()
{
  std::vector<lanelet::traffic_rules::TrafficRulesUPtr> german_traffic_rules_set;
  german_traffic_rules_set.reserve(PARTICIPANT_COUNT);
 
  for (size_t i = 0; i < PARTICIPANT_COUNT; i++)
  {
    // This loop is designed to identify the set of currently supported participants for german traffic rules
    // Using exceptions as logic like this is not usually a good practice, but since this is an operation performed only
    // at startup on a small number of elements efficiency should not be an issue
    try
    {
      lanelet::traffic_rules::TrafficRulesUPtr traffic_rules =
          lanelet::traffic_rules::TrafficRulesFactory::create(lanelet::Locations::Germany, participant_types[i]);
      german_traffic_rules_set.emplace_back(std::move(traffic_rules));
    }
    catch (const lanelet::InvalidInputError& e)
    {
      // Ignore participants which there is no generic rules for
      RCLCPP_INFO_STREAM ( rclcpp::get_logger("MapConformer"), "Ignoring participant: " << participant_types[i] <<  ", which there is no generic rule for...");
    }
  }
 
  return german_traffic_rules_set;
}
 
template <typename Map, typename Key, typename Value>
Value getMapOrDefault(const Map& map, Key key, Value defaultVal)
{
  auto elem = map.find(key);
  if (elem == map.end())
  {
    return defaultVal;
  }
  return elem->second;
}
 
bool startswith(const std::string& str, const std::string& substr)
{
  return str.compare(0, substr.size(), substr) == 0;
}
 
LaneChangeType getChangeType(const std::string& type, const std::string& subtype, const std::string& participant)
{
  using LaneChangeMap = std::map<std::pair<std::string, std::string>, LaneChangeType>;
  const static LaneChangeMap VehicleChangeType{
    { { AttributeValueString::LineThin, AttributeValueString::Dashed }, LaneChangeType::Both },
    { { AttributeValueString::LineThick, AttributeValueString::Dashed }, LaneChangeType::Both },
    { { AttributeValueString::LineThin, AttributeValueString::DashedSolid }, LaneChangeType::ToRight },
    { { AttributeValueString::LineThick, AttributeValueString::DashedSolid }, LaneChangeType::ToRight },
    { { AttributeValueString::LineThin, AttributeValueString::SolidDashed }, LaneChangeType::ToLeft },
    { { AttributeValueString::LineThick, AttributeValueString::SolidDashed }, LaneChangeType::ToLeft }
  };
  const static LaneChangeMap PedestrianChangeType{ { { AttributeValueString::Curbstone, AttributeValueString::Low },
                                                     LaneChangeType::Both } };
 
  if (startswith(participant, Participants::Vehicle))
  {
    return getMapOrDefault(VehicleChangeType, std::make_pair(type, subtype), LaneChangeType::None);
  }
  if (participant == Participants::Pedestrian)
  {
    return getMapOrDefault(PedestrianChangeType, std::make_pair(type, subtype), LaneChangeType::None);
  }
  if (participant == Participants::Bicycle)
  {
    auto asVehicle = getMapOrDefault(VehicleChangeType, std::make_pair(type, subtype), LaneChangeType::None);
    if (asVehicle != LaneChangeType::None)
    {
      return asVehicle;
    }
    return getMapOrDefault(PedestrianChangeType, std::make_pair(type, subtype), LaneChangeType::None);
  }
  return LaneChangeType::None;
}
 
PassingControlLinePtr buildControlLine(LineString3d& bound, const LaneChangeType type, const std::string& participant)
{
  if (bound.inverted())
  {
    switch (type)
    {
      case LaneChangeType::ToRight:
        return std::shared_ptr<PassingControlLine>(new PassingControlLine(
            PassingControlLine::buildData(lanelet::utils::getId(), { bound.invert() }, {}, { participant })));
      case LaneChangeType::ToLeft:
        return std::shared_ptr<PassingControlLine>(new PassingControlLine(
            PassingControlLine::buildData(lanelet::utils::getId(), { bound.invert() }, { participant }, {})));
      case LaneChangeType::Both:
        return std::shared_ptr<PassingControlLine>(new PassingControlLine(PassingControlLine::buildData(
            lanelet::utils::getId(), { bound.invert() }, { participant }, { participant })));
      default:  // LaneChangeType::None
        return std::shared_ptr<PassingControlLine>(
            new PassingControlLine(PassingControlLine::buildData(lanelet::utils::getId(), { bound.invert() }, {}, {})));
    }
  }
  else
  {
    switch (type)
    {
      case LaneChangeType::ToRight:
        return std::shared_ptr<PassingControlLine>(new PassingControlLine(
            PassingControlLine::buildData(lanelet::utils::getId(), { bound }, { participant }, {})));
      case LaneChangeType::ToLeft:
        return std::shared_ptr<PassingControlLine>(new PassingControlLine(
            PassingControlLine::buildData(lanelet::utils::getId(), { bound }, {}, { participant })));
      case LaneChangeType::Both:
        return std::shared_ptr<PassingControlLine>(new PassingControlLine(
            PassingControlLine::buildData(lanelet::utils::getId(), { bound }, { participant }, { participant })));
      default:  // LaneChangeType::None
        return std::shared_ptr<PassingControlLine>(
            new PassingControlLine(PassingControlLine::buildData(lanelet::utils::getId(), { bound }, {}, {})));
    }
  }
}
 
void addInferredAccessRule(Lanelet& lanelet, lanelet::LaneletMapPtr map,
                           const std::vector<lanelet::traffic_rules::TrafficRulesUPtr>& default_traffic_rules)
{
  auto access_rules = lanelet.regulatoryElementsAs<RegionAccessRule>();
  // If the lanelet does not have an access rule then add one based on the generic traffic rules
  if (access_rules.size() == 0)
  {  // No access rule detected so add one
 
    // We want to check for all participants which are currently supported
    std::vector<std::string> allowed_participants;
 
    for (const auto& rules : default_traffic_rules)
    {
      if (rules->canPass(lanelet))
      {
        allowed_participants.emplace_back(rules->participant());
      }
    }
 
    std::shared_ptr<RegionAccessRule> rar(new RegionAccessRule(
        RegionAccessRule::buildData(lanelet::utils::getId(), { lanelet }, {}, allowed_participants)));
    lanelet.addRegulatoryElement(rar);
    map->add(rar);
  }
}
 
void addInferredAccessRule(Area& area, lanelet::LaneletMapPtr map,
                           const std::vector<lanelet::traffic_rules::TrafficRulesUPtr>& default_traffic_rules)
{
  auto access_rules = area.regulatoryElementsAs<RegionAccessRule>();
  // If the lanelet does not have an access rule then add one based on the generic traffic rules
  if (access_rules.size() == 0)
  {  // No access rule detected so add one
 
    // We want to check for all participants which are currently supported
    std::vector<std::string> allowed_participants;
 
    for (const auto& rules : default_traffic_rules)
    {
      if (rules->canPass(area))
      {
        allowed_participants.emplace_back(rules->participant());
      }
    }
 
    std::shared_ptr<RegionAccessRule> rar(
        new RegionAccessRule(RegionAccessRule::buildData(lanelet::utils::getId(), {}, { area }, allowed_participants)));
    area.addRegulatoryElement(rar);
    map->add(rar);
  }
}
 
void addInferredPassingControlLine(Lanelet& lanelet, lanelet::LaneletMapPtr map)
{
  // Since this class is only designed to add passing control lines based on lane changes
  // we will always assume the participant is a vehicle
  std::string participant(lanelet::Participants::Vehicle);
 
  LineString3d left_bound = lanelet.leftBound();
  LineString3d right_bound = lanelet.rightBound();
 
  // Determine possibility of lane change for left and right bounds
  LaneChangeType left_type = getChangeType(left_bound.attribute(AttributeName::Type).value(),
                                           left_bound.attribute(AttributeName::Subtype).value(), participant);
  LaneChangeType right_type = getChangeType(right_bound.attribute(AttributeName::Type).value(),
                                            right_bound.attribute(AttributeName::Subtype).value(), participant);
 
  auto local_control_lines = lanelet.regulatoryElementsAs<PassingControlLine>();
 
  bool foundLeft = false;
  bool foundRight = false;
 
  // Iterate over all regulatory elements in the map to determine if there is an existing regulation for this
  // lanelet's bounds
  for (auto reg_elem : map->regulatoryElementLayer)
  {
    if (reg_elem->attribute(AttributeName::Subtype).value() != PassingControlLine::RuleName)
    {
      continue;
    }
 
    auto pcl = std::static_pointer_cast<PassingControlLine>(reg_elem);
    for (auto sub_line : pcl->controlLine())
    {
      bool shouldAdd = false;
      
      if (left_bound.id() == sub_line.id() && !foundLeft)
      {
        foundLeft = true;
        shouldAdd = !lanelet::utils::contains(local_control_lines, pcl);
      }
      else if (right_bound.id() == sub_line.id() && !foundRight)
      {
        foundRight = true;
        shouldAdd = !lanelet::utils::contains(local_control_lines, pcl);
      }
      // Check if our lanelet contains this control line
      // If it does not then add it
      if (shouldAdd)
      {
        lanelet.addRegulatoryElement(pcl);
      }
    }
    
  }
 
  // If no existing regulation was found for this lanelet's right or left bound then create a new one and add it to
  // the lanelet and the map
  if (!foundLeft)
  {
    PassingControlLinePtr pcl_left = buildControlLine(left_bound, left_type, participant);
    lanelet.addRegulatoryElement(pcl_left);
    map->add(pcl_left);
  }
  if (!foundRight)
  {
    PassingControlLinePtr pcl_right = buildControlLine(right_bound, right_type, participant);
    lanelet.addRegulatoryElement(pcl_right);
    map->add(pcl_right);
  }
}
 
void addInferredPassingControlLine(Area& area, lanelet::LaneletMapPtr map)
{
  // Since this class is only designed to add passing control lines based on lane changes
  // we will always assume the participant is a vehicle
  std::string participant(lanelet::Participants::Vehicle);
 
  LineStrings3d outerBounds = area.outerBound();
 
  auto local_control_lines = area.regulatoryElementsAs<PassingControlLine>();
 
  std::vector<size_t> found_indices;
 
  // Iterate over all regulatory elements in the map to determine if there is an existing regulation for this area's
  // bounds
  for (auto reg_elem : map->regulatoryElementLayer)
  {
    if (reg_elem->attribute(AttributeName::Subtype).value() == PassingControlLine::RuleName)
    {
      auto pcl = std::static_pointer_cast<PassingControlLine>(reg_elem);
      for (auto sub_line : pcl->controlLine())
      {
        size_t i = 0;
        for (auto sub_bound : outerBounds)
        {
          if (sub_bound.id() == sub_line.id())
          {
            found_indices.push_back(i);
            // If an existing regulation applies to this area's bounds then add it to the area
            bool alreadyAdded = lanelet::utils::contains(local_control_lines, pcl);
            if (!alreadyAdded)
            {
              area.addRegulatoryElement(pcl);
            }
          }
        }
      }
    }
  }
 
  // For all the bounds which did not have an existing regulation create a new one
  size_t i = 0;
  for (auto sub_bound : outerBounds)
  {
    if (lanelet::utils::contains(found_indices, i))
    {
      continue;  // Continue if this sub_bound is already accounted for
    }
 
    LaneChangeType change_type = getChangeType(sub_bound.attribute(AttributeName::Type).value(),
                                               sub_bound.attribute(AttributeName::Subtype).value(), participant);
    PassingControlLinePtr control_line = buildControlLine(sub_bound, change_type, participant);
    area.addRegulatoryElement(control_line);
    map->add(control_line);
 
    i++;
  }
}
 
void addInferredDirectionOfTravel(Lanelet& lanelet, lanelet::LaneletMapPtr map,
                                  const std::vector<lanelet::traffic_rules::TrafficRulesUPtr>& default_traffic_rules)
{
  auto direction_of_travel = lanelet.regulatoryElementsAs<DirectionOfTravel>();
  // If the lanelet does not have an access rule then add one based on the generic traffic rules
  if (direction_of_travel.size() == 0)
  {  // No direction detected so need to check if one is required
 
    // We want to check for all participants which are currently supported
    std::vector<std::string> allowed_participants;
 
    for (const auto& rules : default_traffic_rules)
    {
      if (!rules->isOneWay(lanelet))
      {  // Check if this lanelet is not oneway
        allowed_participants.emplace_back(rules->participant());
      }
    }
 
    if (allowed_participants.size() > 0)
    {  // Only add bi-directional regulations
      std::shared_ptr<DirectionOfTravel> rar(new DirectionOfTravel(DirectionOfTravel::buildData(
          lanelet::utils::getId(), { lanelet }, DirectionOfTravel::BiDirectional, allowed_participants)));
      lanelet.addRegulatoryElement(rar);
      map->add(rar);
    }
  }
}
 
void addValidSpeedLimit(Lanelet& lanelet, lanelet::LaneletMapPtr map, lanelet::Velocity config_limit,
    const std::vector<lanelet::traffic_rules::TrafficRulesUPtr>& default_traffic_rules )
{
  lanelet::Velocity max_speed;
    auto speed_limit = lanelet.regulatoryElementsAs<DigitalSpeedLimit>();
     if(config_limit < 80_mph && config_limit > 0_mph)//Accounting for the configured speed limit, input zero when not in use
      {  
        max_speed = config_limit;
      }
      else
      {
        max_speed = 80_mph;
      }
      
    // If the lanelet does not have a digital speed limit then add one with the maximum value of 80
    std::vector<std::string> allowed_participants;
     //Maximum speed limit is 80
   
 
    if (speed_limit.empty())//If there is no assigned speed limit value
    {
 
         for(const auto& rules : default_traffic_rules)
         {
            if (rules->canPass(lanelet))
             {
              allowed_participants.emplace_back(rules->participant());
             }
        }
     if (!allowed_participants.empty())
     {
 
      auto rar = std::make_shared<DigitalSpeedLimit>(DigitalSpeedLimit::buildData(lanelet::utils::getId(), max_speed, {lanelet},
      {}, allowed_participants));
 
      lanelet.addRegulatoryElement(rar);
      map->add(rar);//Add DigitalSpeedLimit data to the map
 
     }
  }
  else  //If the speed limit value already exists 
  {
      for(const auto& rules : default_traffic_rules)
         {
            if (rules->canPass(lanelet))
             {
              allowed_participants.emplace_back(rules->participant());
             }
        }
    if(speed_limit.back().get()->speed_limit_ > max_speed)//Check that speed limit value does not exceed the maximum value
    {
    
      RCLCPP_DEBUG_STREAM( rclcpp::get_logger("lanelet::MapConformer"), "Invalid speed limit value. Value reset to maximum speed limit.");
      auto rar = std::make_shared<DigitalSpeedLimit>(DigitalSpeedLimit::buildData(lanelet::utils::getId(), max_speed, {lanelet},
      {}, allowed_participants));
      lanelet.removeRegulatoryElement(speed_limit.back());
      lanelet.addRegulatoryElement(rar);
      map->update(lanelet, rar);//Add DigitalSpeedLimit data to the map
      RCLCPP_INFO_STREAM( rclcpp::get_logger("lanelet::MapConformer"), "Number of Regulatory Elements: "<< map->regulatoryElementLayer.size());
 
 
    }
    
  }
}
 
 
}  // namespace
 
void ensureCompliance(lanelet::LaneletMapPtr map, lanelet::Velocity config_limit)
{
  
  auto default_traffic_rules = getAllGermanTrafficRules();  // Use german traffic rules as default as they most closely
                                                            // match the generic traffic rules
  // Handle lanelets
  for (auto lanelet : map->laneletLayer)
  {
    addInferredAccessRule(lanelet, map, default_traffic_rules);
    addInferredPassingControlLine(lanelet, map);
    addInferredDirectionOfTravel(lanelet, map, default_traffic_rules);
    addValidSpeedLimit(lanelet, map, config_limit, default_traffic_rules);// 0_mph can be changed with the config_limit
 
 
  }
  // Handle areas
  for (auto area : map->areaLayer)
  {
    addInferredAccessRule(area, map, default_traffic_rules);
    addInferredPassingControlLine(area, map);
  }
}
}  // namespace MapConformer
}  // namespace lanelet