LCOV - code coverage report
Current view: top level - src/mapping/had_map/lanelet2_map_provider/src - lanelet2_map_visualizer.cpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 53 54 98.1 %
Date: 2023-03-03 05:44:19 Functions: 3 3 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 72 202 35.6 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2020 The Autoware Foundation
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : 
      15                 :            : /// \brief main function for lanelet2_map_provider
      16                 :            : 
      17                 :            : 
      18                 :            : #include <rclcpp/rclcpp.hpp>
      19                 :            : #include <rclcpp_components/register_node_macro.hpp>
      20                 :            : 
      21                 :            : #include <visualization_msgs/msg/marker.hpp>
      22                 :            : #include <visualization_msgs/msg/marker_array.hpp>
      23                 :            : #include <geometry_msgs/msg/point.hpp>
      24                 :            : #include <common/types.hpp>
      25                 :            : 
      26                 :            : #include <chrono>
      27                 :            : #include <memory>
      28                 :            : #include <unordered_set>
      29                 :            : 
      30                 :            : #include "lanelet2_map_provider/lanelet2_map_provider.hpp"
      31                 :            : #include "autoware_auto_mapping_msgs/srv/had_map_service.hpp"
      32                 :            : #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
      33                 :            : 
      34                 :            : #include "had_map_utils/had_map_conversion.hpp"
      35                 :            : #include "had_map_utils/had_map_query.hpp"
      36                 :            : #include "had_map_utils/had_map_visualization.hpp"
      37                 :            : 
      38                 :            : #include "lanelet2_map_provider/lanelet2_map_visualizer.hpp"
      39                 :            : 
      40                 :            : using autoware::common::types::bool8_t;
      41                 :            : using autoware::common::types::float64_t;
      42                 :            : 
      43                 :            : using namespace std::chrono_literals;
      44                 :            : 
      45                 :            : namespace autoware
      46                 :            : {
      47                 :            : namespace lanelet2_map_provider
      48                 :            : {
      49                 :          6 : void insertMarkerArray(
      50                 :            :   visualization_msgs::msg::MarkerArray & a1,
      51                 :            :   const visualization_msgs::msg::MarkerArray & a2)
      52                 :            : {
      53         [ +  - ]:          6 :   if (a2.markers.size() > 0) {
      54                 :          6 :     a1.markers.insert(a1.markers.end(), a2.markers.begin(), a2.markers.end());
      55                 :            :   }
      56                 :          6 : }
      57                 :            : 
      58                 :          1 : void Lanelet2MapVisualizer::visualize_map_callback(
      59                 :            :   rclcpp::Client<autoware_auto_mapping_msgs::srv::HADMapService>::SharedFuture response)
      60                 :            : {
      61                 :          2 :   auto msg = response.get()->map;
      62                 :            : 
      63                 :            :   std::shared_ptr<lanelet::LaneletMap> sub_map = std::make_shared<lanelet::LaneletMap>();
      64         [ +  - ]:          1 :   autoware::common::had_map_utils::fromBinaryMsg(msg, sub_map);
      65                 :            : 
      66         [ +  - ]:          2 :   auto lls = autoware::common::had_map_utils::getConstLaneletLayer(sub_map);
      67                 :            : 
      68                 :            :   std_msgs::msg::ColorRGBA color_lane_bounds, color_parking_bounds,
      69                 :            :     color_parking_access_bounds, color_geom_bounds,
      70                 :            :     color_lanelets, color_parking, color_parking_access,
      71                 :            :     color_pickup_dropoff;
      72                 :          1 :   autoware::common::had_map_utils::setColor(
      73         [ +  - ]:          1 :     &color_lane_bounds, 1.0f, 1.0f, 1.0f, 1.0f);
      74                 :          1 :   autoware::common::had_map_utils::setColor(
      75         [ +  - ]:          1 :     &color_parking_bounds, 1.0f, 1.0f, 1.0f, 1.0f);
      76                 :          1 :   autoware::common::had_map_utils::setColor(
      77         [ +  - ]:          1 :     &color_parking_access_bounds, 1.0f, 1.0f, 1.0f, 1.0f);
      78                 :          1 :   autoware::common::had_map_utils::setColor(
      79         [ +  - ]:          1 :     &color_geom_bounds, 0.0f, 0.0f, 1.0f, 1.0f);
      80                 :          1 :   autoware::common::had_map_utils::setColor(
      81         [ +  - ]:          1 :     &color_lanelets, 0.2f, 0.5f, 0.6f, 0.6f);
      82                 :          1 :   autoware::common::had_map_utils::setColor(
      83         [ +  - ]:          1 :     &color_parking, 0.3f, 0.3f, 0.7f, 0.5f);
      84                 :          1 :   autoware::common::had_map_utils::setColor(
      85         [ +  - ]:          1 :     &color_parking_access, 0.3f, 0.7f, 0.3f, 0.5f);
      86                 :          1 :   autoware::common::had_map_utils::setColor(
      87   [ +  -  +  - ]:          1 :     &color_pickup_dropoff, 0.9f, 0.2f, 0.1f, 0.7f);
      88                 :            : 
      89                 :            :   visualization_msgs::msg::MarkerArray map_marker_array;
      90                 :            : 
      91         [ +  - ]:          2 :   rclcpp::Time marker_t = rclcpp::Time(0);
      92         [ +  - ]:          1 :   insertMarkerArray(
      93                 :            :     map_marker_array,
      94                 :          2 :     autoware::common::had_map_utils::laneletsBoundaryAsMarkerArray(
      95                 :            :       marker_t, lls,
      96         [ +  - ]:          1 :       color_lane_bounds, true));
      97         [ +  - ]:          1 :   insertMarkerArray(
      98                 :            :     map_marker_array,
      99   [ +  -  +  -  :          3 :     autoware::common::had_map_utils::laneletsAsTriangleMarkerArray(
             +  -  -  - ]
     100                 :            :       marker_t,
     101                 :            :       "lanelet_triangles", lls, color_lanelets));
     102                 :            : 
     103                 :            :   // for parking spots defined as areas (LaneletOSM definition)
     104         [ +  - ]:          2 :   auto ll_areas = autoware::common::had_map_utils::getAreaLayer(sub_map);
     105   [ +  -  +  - ]:          2 :   auto ll_parking_areas = autoware::common::had_map_utils::subtypeAreas(ll_areas, "parking_spot");
     106                 :            :   auto ll_parking_access_areas = autoware::common::had_map_utils::subtypeAreas(
     107                 :            :     ll_areas,
     108   [ +  -  +  - ]:          2 :     "parking_access");
     109                 :            : 
     110         [ +  - ]:          1 :   insertMarkerArray(
     111                 :            :     map_marker_array,
     112   [ +  -  +  -  :          2 :     autoware::common::had_map_utils::areasBoundaryAsMarkerArray(
             +  -  -  - ]
     113                 :            :       marker_t, "parking_area_bounds",
     114                 :            :       ll_parking_areas, color_parking_bounds));
     115         [ +  - ]:          1 :   insertMarkerArray(
     116                 :            :     map_marker_array,
     117   [ +  -  +  -  :          2 :     autoware::common::had_map_utils::areasBoundaryAsMarkerArray(
             +  -  -  - ]
     118                 :            :       marker_t, "parking_access_area_bounds",
     119                 :            :       ll_parking_access_areas, color_parking_bounds));
     120         [ +  - ]:          1 :   insertMarkerArray(
     121                 :            :     map_marker_array,
     122   [ +  -  +  -  :          2 :     autoware::common::had_map_utils::areasAsTriangleMarkerArray(
             +  -  -  - ]
     123                 :            :       marker_t, "parking_area_triangles",
     124                 :            :       ll_parking_areas, color_parking));
     125         [ +  - ]:          1 :   insertMarkerArray(
     126                 :            :     map_marker_array,
     127   [ +  -  +  -  :          3 :     autoware::common::had_map_utils::areasAsTriangleMarkerArray(
          +  -  +  -  -  
                      - ]
     128                 :            :       marker_t, "parking_access_area_triangles",
     129                 :            :       ll_parking_access_areas, color_parking_access));
     130                 :            : 
     131         [ +  - ]:          1 :   m_viz_pub->publish(map_marker_array);
     132                 :          1 : }
     133                 :            : 
     134                 :          2 : Lanelet2MapVisualizer::Lanelet2MapVisualizer(const rclcpp::NodeOptions & options)
     135   [ +  -  +  -  :          4 : : Node("lanelet2_map_visualizer", options)
                   +  - ]
     136                 :            : {
     137                 :            :   m_client =
     138   [ +  -  +  -  :          4 :     this->create_client<autoware_auto_mapping_msgs::srv::HADMapService>("HAD_Map_Service");
          -  +  -  +  +  
                      - ]
     139                 :            : 
     140   [ +  -  +  -  :          6 :   m_viz_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>(
          -  +  +  -  -  
                      - ]
     141                 :            :     "viz_had_map",
     142   [ +  -  +  -  :          4 :     rclcpp::QoS(rclcpp::KeepLast(5U)).transient_local());
                   +  - ]
     143                 :            : 
     144                 :            :   auto request = std::make_shared<autoware_auto_mapping_msgs::srv::HADMapService::Request>();
     145                 :            :   bool8_t use_geom_bounds = false;
     146                 :            : 
     147                 :            :   float64_t d = 20.0;
     148                 :            :   float64_t pos[] = {25.0, 25.0, 0.0};
     149                 :            :   float64_t upper[] = {pos[0] + d, pos[1] + d, 0.0};
     150                 :            :   float64_t lower[] = {pos[0] - d, pos[1] - d, 0.0};
     151                 :            :   request->geom_upper_bound.clear();
     152                 :            :   request->geom_lower_bound.clear();
     153                 :            :   if (!use_geom_bounds) {
     154         [ +  - ]:          2 :     request->requested_primitives.push_back(
     155                 :            :       autoware_auto_mapping_msgs::srv::HADMapService::Request::FULL_MAP);
     156                 :            :   } else {
     157                 :            :     request->requested_primitives.push_back(
     158                 :            :       autoware_auto_mapping_msgs::srv::HADMapService::Request::DRIVEABLE_GEOMETRY);
     159                 :            :     for (size_t i = 0; i < 3; i++) {
     160                 :            :       request->geom_upper_bound.push_back(upper[i]);
     161                 :            :       request->geom_lower_bound.push_back(lower[i]);
     162                 :            :     }
     163                 :            :   }
     164   [ +  -  +  + ]:          2 :   while (!m_client->wait_for_service(1s)) {
     165   [ +  -  +  - ]:          2 :     if (!rclcpp::ok()) {
     166   [ -  +  -  -  :          5 :       RCLCPP_INFO(this->get_logger(), "Interrupted while waiting for the service. Exiting");
          -  -  -  -  -  
          -  -  -  -  -  
          -  -  -  -  +  
          -  +  -  +  -  
          +  -  +  -  +  
                      - ]
     167                 :            :       return;
     168                 :            :     }
     169   [ #  #  #  #  :          0 :     RCLCPP_INFO(this->get_logger(), "Service not available, waiting again...");
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
                      # ]
     170                 :            :   }
     171                 :            : 
     172                 :            :   auto result = m_client->async_send_request(
     173                 :            :     request,
     174   [ +  -  +  -  :          2 :     std::bind(&Lanelet2MapVisualizer::visualize_map_callback, this, std::placeholders::_1));
                   -  - ]
     175                 :            : }
     176                 :            : 
     177                 :            : }  // namespace lanelet2_map_provider
     178                 :            : 
     179                 :            : }  // namespace autoware
     180                 :            : 
     181                 :            : RCLCPP_COMPONENTS_REGISTER_NODE(
     182                 :            :   autoware::lanelet2_map_provider::Lanelet2MapVisualizer)

Generated by: LCOV version 1.14