Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : #include "ad/rss/map/RssObjectResultListAnalyzer.hpp"
       9                 :            : #include "ad/rss/map/Logging.hpp"
      10                 :            : 
      11                 :            : namespace ad {
      12                 :            : namespace rss {
      13                 :            : namespace map {
      14                 :            : 
      15                 :          0 : std::set<ad::rss::world::ObjectId> RssObjectResultListAnalyzer::getAvailableObjects() const
      16                 :            : {
      17                 :          0 :   return available_objects_;
      18                 :            : }
      19                 :            : 
      20                 :            : ad::rss::map::RssObjectData::ConstPtr
      21                 :          0 : RssObjectResultListAnalyzer::getObjectData(ad::rss::world::ObjectId const &object_id) const
      22                 :            : {
      23         [ #  # ]:          0 :   auto find_result = object_data_map_.find(object_id);
      24         [ #  # ]:          0 :   if (find_result != object_data_map_.end())
      25                 :            :   {
      26                 :          0 :     return find_result->second;
      27                 :            :   }
      28                 :          0 :   return nullptr;
      29                 :            : }
      30                 :            : 
      31                 :          0 : std::set<ad::rss::world::ObjectId> RssObjectResultListAnalyzer::getAvailableEgoObjects() const
      32                 :            : {
      33                 :          0 :   return available_ego_objects_;
      34                 :            : }
      35                 :            : 
      36                 :            : ad::rss::map::RssRouteCheckResult::ConstPtr
      37                 :          0 : RssObjectResultListAnalyzer::getSelectedRouteCheckResult(ad::rss::world::ObjectId const &ego_object_id) const
      38                 :            : {
      39         [ #  # ]:          0 :   auto find_result = selected_route_map_.find(ego_object_id);
      40         [ #  # ]:          0 :   if (find_result != selected_route_map_.end())
      41                 :            :   {
      42                 :          0 :     return find_result->second;
      43                 :            :   }
      44                 :          0 :   return nullptr;
      45                 :            : }
      46                 :            : 
      47                 :            : ad::rss::map::RssRouteId
      48                 :          0 : RssObjectResultListAnalyzer::getCurrentRouteId(ad::rss::world::ObjectId const &ego_object_id) const
      49                 :            : {
      50         [ #  # ]:          0 :   auto route_check_result = getSelectedRouteCheckResult(ego_object_id);
      51         [ #  # ]:          0 :   if (route_check_result)
      52                 :            :   {
      53                 :          0 :     return route_check_result->rss_route.route_id;
      54                 :            :   }
      55                 :          0 :   return 0;
      56                 :          0 : }
      57                 :            : 
      58                 :            : std::set<ad::rss::map::RssRouteId>
      59                 :          0 : RssObjectResultListAnalyzer::getAvailableRoutes(ad::rss::world::ObjectId const &ego_object_id) const
      60                 :            : {
      61                 :          0 :   std::set<ad::rss::map::RssRouteId> route_ids;
      62         [ #  # ]:          0 :   auto find_result = is_safe_map_.find(ego_object_id);
      63         [ #  # ]:          0 :   if (find_result != is_safe_map_.end())
      64                 :            :   {
      65         [ #  # ]:          0 :     for (auto const &route_safe_pair : find_result->second)
      66                 :            :     {
      67         [ #  # ]:          0 :       route_ids.insert(route_safe_pair.first);
      68                 :            :     }
      69                 :            :   }
      70                 :          0 :   return route_ids;
      71                 :          0 : }
      72                 :            : 
      73                 :          0 : bool RssObjectResultListAnalyzer::isRouteAvailable(ad::rss::world::ObjectId const &ego_object_id,
      74                 :            :                                                    ad::rss::map::RssRouteId const &route_id) const
      75                 :            : {
      76         [ #  # ]:          0 :   auto find_result = is_safe_map_.find(ego_object_id);
      77         [ #  # ]:          0 :   if (find_result != is_safe_map_.end())
      78                 :            :   {
      79         [ #  # ]:          0 :     auto find_route_result = find_result->second.find(route_id);
      80                 :          0 :     return find_route_result != find_result->second.end();
      81                 :            :   }
      82                 :          0 :   return false;
      83                 :            : }
      84                 :            : 
      85                 :          0 : void RssObjectResultListAnalyzer::updateRssObjectResultList(
      86                 :            :   std::shared_ptr<ad::rss::map::RssObjectResultList> rss_object_result_list)
      87                 :            : {
      88                 :          0 :   rss_object_result_list_ = rss_object_result_list;
      89                 :            : 
      90                 :          0 :   std::map<ad::rss::world::ObjectId, std::map<ad::rss::map::RssRouteId, bool>> new_is_safe_map;
      91                 :          0 :   std::map<ad::rss::world::ObjectId, ad::rss::map::RssRouteCheckResult::ConstPtr> new_selected_route_map;
      92                 :          0 :   std::map<ad::rss::world::ObjectId, ad::rss::map::RssObjectData::ConstPtr> new_object_data_map;
      93                 :          0 :   std::set<ad::rss::world::ObjectId> new_available_objects;
      94                 :          0 :   std::set<ad::rss::world::ObjectId> new_available_ego_objects;
      95                 :            : 
      96         [ #  # ]:          0 :   if (bool(rss_object_result_list_))
      97                 :            :   {
      98         [ #  # ]:          0 :     for (auto const &object_check_result : *rss_object_result_list_)
      99                 :            :     {
     100                 :          0 :       auto const object_id = object_check_result.object_data.id;
     101         [ #  # ]:          0 :       new_available_objects.insert(object_id);
     102         [ #  # ]:          0 :       new_object_data_map[object_id]
     103                 :          0 :         = ad::rss::map::RssObjectData::ConstPtr(rss_object_result_list_, &object_check_result.object_data);
     104                 :            : 
     105         [ #  # ]:          0 :       if (object_check_result.situation_check_results.size() > 0u)
     106                 :            :       {
     107         [ #  # ]:          0 :         new_available_ego_objects.insert(object_id);
     108         [ #  # ]:          0 :         ad::rss::map::RssRouteId last_selected_route = getCurrentRouteId(object_id);
     109                 :          0 :         ad::rss::map::RssRouteCheckResult const *selected_route = nullptr;
     110                 :          0 :         ad::rss::map::RssRouteId selected_route_id = 0;
     111                 :          0 :         bool first_unsafe_route_found = false;
     112                 :          0 :         bool first_safe_route_found = false;
     113         [ #  # ]:          0 :         for (auto const &route_check_result : object_check_result.situation_check_results)
     114                 :            :         {
     115                 :          0 :           auto const route_id = route_check_result.rss_route.route_id;
     116         [ #  # ]:          0 :           if (route_id == last_selected_route)
     117                 :            :           {
     118                 :          0 :             selected_route = &route_check_result;
     119                 :          0 :             selected_route_id = route_id;
     120                 :            :           }
     121   [ #  #  #  # ]:          0 :           new_is_safe_map[object_id][route_id] = route_check_result.proper_response.is_safe;
     122                 :            : 
     123   [ #  #  #  # ]:          0 :           if ((!first_unsafe_route_found) && (!route_check_result.proper_response.is_safe))
     124                 :            :           {
     125                 :          0 :             first_unsafe_route_found = true;
     126         [ #  # ]:          0 :             if (route_selection_mode_ == RouteSelectionMode::FirstUnsafeRoute)
     127                 :            :             {
     128                 :          0 :               selected_route = &route_check_result;
     129                 :          0 :               selected_route_id = route_id;
     130                 :          0 :               last_selected_route = selected_route_id;
     131                 :            :             }
     132                 :            :           }
     133   [ #  #  #  # ]:          0 :           if ((!first_safe_route_found) && (route_check_result.proper_response.is_safe))
     134                 :            :           {
     135                 :          0 :             first_safe_route_found = true;
     136         [ #  # ]:          0 :             if (route_selection_mode_ == RouteSelectionMode::LastUnsafeRoute)
     137                 :            :             {
     138                 :          0 :               selected_route = &route_check_result;
     139                 :          0 :               selected_route_id = route_id;
     140                 :          0 :               last_selected_route = selected_route_id;
     141                 :            :             }
     142                 :            :           }
     143                 :            : 
     144                 :          0 :           bool route_id_is_new = false;
     145   [ #  #  #  # ]:          0 :           auto find_result = is_safe_map_[object_id].find(route_id);
     146   [ #  #  #  # ]:          0 :           if (find_result == is_safe_map_[object_id].end())
     147                 :            :           {
     148                 :            :             // route not in previous results, try parent route
     149   [ #  #  #  # ]:          0 :             find_result = is_safe_map_[object_id].find(route_check_result.rss_route.parent_route_id);
     150                 :          0 :             route_id_is_new = true;
     151                 :            :           }
     152   [ #  #  #  # ]:          0 :           if (find_result != is_safe_map_[object_id].end())
     153                 :            :           {
     154         [ #  # ]:          0 :             if (route_check_result.proper_response.is_safe != find_result->second)
     155                 :            :             {
     156   [ #  #  #  # ]:          0 :               getLogger()->info("rss_check_result[{}:{}]{} rss safe CHANGED {} -> {}",
     157                 :            :                                 object_id,
     158                 :            :                                 route_id,
     159         [ #  # ]:          0 :                                 route_id_is_new ? "EXT ROUTE" : "",
     160         [ #  # ]:          0 :                                 find_result->second,
     161                 :          0 :                                 route_check_result.proper_response.is_safe);
     162                 :            :             }
     163                 :            :             else
     164                 :            :             {
     165                 :            :               /*
     166                 :            :                 getLogger()->info("rss_check_result[{}:{}]{} rss safe: {}", object_id, route_id,
     167                 :            :                 route_id_is_new?"EXT ROUTE":"", route_check_result.proper_response.is_safe);
     168                 :            :               */
     169                 :            :             }
     170                 :            :           }
     171                 :            :           else
     172                 :            :           {
     173   [ #  #  #  # ]:          0 :             getLogger()->info("rss_check_result[{}:{}] NEW ROUTE: rss safe: {}",
     174                 :            :                               object_id,
     175                 :            :                               route_id,
     176         [ #  # ]:          0 :                               route_check_result.proper_response.is_safe);
     177                 :            :           }
     178                 :            :         }
     179                 :            : 
     180   [ #  #  #  # ]:          0 :         if ((!first_unsafe_route_found) && (route_selection_mode_ == RouteSelectionMode::FirstUnsafeRoute))
     181                 :            :         {
     182         [ #  # ]:          0 :           if (!object_check_result.situation_check_results.empty())
     183                 :            :           {
     184                 :          0 :             selected_route = &object_check_result.situation_check_results[0];
     185                 :          0 :             selected_route_id = object_check_result.situation_check_results[0].rss_route.route_id;
     186                 :          0 :             last_selected_route = selected_route_id;
     187                 :            :           }
     188                 :            :         }
     189                 :            : 
     190         [ #  # ]:          0 :         if (last_selected_route != selected_route_id)
     191                 :            :         {
     192                 :            :           // search if a child route to follow is available
     193         [ #  # ]:          0 :           for (auto const &route_check_result : object_check_result.situation_check_results)
     194                 :            :           {
     195         [ #  # ]:          0 :             if (route_check_result.rss_route.parent_route_id == last_selected_route)
     196                 :            :             {
     197                 :          0 :               selected_route = &route_check_result;
     198                 :          0 :               selected_route_id = route_check_result.rss_route.route_id;
     199                 :          0 :               last_selected_route = selected_route_id;
     200                 :          0 :               break;
     201                 :            :             }
     202                 :            :           }
     203         [ #  # ]:          0 :           if (last_selected_route != selected_route_id)
     204                 :            :           {
     205                 :            :             // fallback to the first route
     206         [ #  # ]:          0 :             if (!object_check_result.situation_check_results.empty())
     207                 :            :             {
     208                 :          0 :               selected_route = &object_check_result.situation_check_results[0];
     209                 :          0 :               selected_route_id = object_check_result.situation_check_results[0].rss_route.route_id;
     210                 :          0 :               last_selected_route = selected_route_id;
     211                 :            :             }
     212                 :            :           }
     213                 :            :         }
     214         [ #  # ]:          0 :         new_selected_route_map[object_id]
     215                 :          0 :           = ad::rss::map::RssRouteCheckResult::ConstPtr(rss_object_result_list_, selected_route);
     216                 :            : 
     217                 :            :         /*
     218                 :            :           RCLCPP_INFO(get_logger(), "rss_check_result[{}] selected route: {}", object_id, selected_route_id);
     219                 :            :         */
     220                 :            :       }
     221                 :            :     }
     222                 :            :   }
     223                 :            : 
     224                 :          0 :   available_ego_objects_.swap(new_available_ego_objects);
     225                 :          0 :   available_objects_.swap(new_available_objects);
     226                 :          0 :   selected_route_map_.swap(new_selected_route_map);
     227                 :          0 :   object_data_map_.swap(new_object_data_map);
     228                 :          0 :   is_safe_map_.swap(new_is_safe_map);
     229                 :          0 : }
     230                 :            : 
     231                 :            : } // namespace map
     232                 :            : } // namespace rss
     233                 :            : } // namespace ad