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)
|