diff --git a/map/lanelet2_extension/CMakeLists.txt b/map/lanelet2_extension/CMakeLists.txt new file mode 100644 index 0000000000000..c755d45eee3e0 --- /dev/null +++ b/map/lanelet2_extension/CMakeLists.txt @@ -0,0 +1,90 @@ +cmake_minimum_required(VERSION 3.5) +project(lanelet2_extension) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) + +find_library(GeographicLib_LIBRARIES + NAMES Geographic +) + +find_library(PUGIXML_LIBRARIES + NAMES pugixml +) + +find_path(PUGIXML_INCLUDE_DIRS + NAMES pugixml.hpp + PATH_SUFFIXES pugixml +) + +include_directories( + ${GeographicLib_INCLUDE_DIRS} + ${PUGIXML_INCLUDE_DIRS} +) + +add_definitions(${GeographicLib_DEFINITIONS}) + +ament_auto_add_library(lanelet2_extension_lib SHARED + lib/autoware_osm_parser.cpp + lib/autoware_traffic_light.cpp + lib/detection_area.cpp + lib/no_stopping_area.cpp + lib/message_conversion.cpp + lib/mgrs_projector.cpp + lib/query.cpp + lib/road_marking.cpp + lib/utilities.cpp + lib/virtual_traffic_light.cpp + lib/visualization.cpp +) +target_link_libraries(lanelet2_extension_lib + ${GeographicLib_LIBRARIES} +) + +ament_auto_add_executable(lanelet2_extension_sample src/sample_code.cpp) +add_dependencies(lanelet2_extension_sample lanelet2_extension_lib) +target_link_libraries(lanelet2_extension_sample + lanelet2_extension_lib +) + +ament_auto_add_executable(autoware_lanelet2_validation src/validation.cpp) +add_dependencies(autoware_lanelet2_validation lanelet2_extension_lib) +target_link_libraries(autoware_lanelet2_validation + ${catkin_LIBRARIES} + ${PUGIXML_LIBRARIES} + lanelet2_extension_lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(message_conversion-test test/src/test_message_conversion.cpp) + target_link_libraries(message_conversion-test lanelet2_extension_lib) + ament_add_gtest(projector-test test/src/test_projector.cpp) + target_link_libraries(projector-test lanelet2_extension_lib) + ament_add_gtest(query-test test/src/test_query.cpp) + target_link_libraries(query-test lanelet2_extension_lib) + ament_add_gtest(regulatory_elements-test test/src/test_regulatory_elements.cpp) + target_link_libraries(regulatory_elements-test lanelet2_extension_lib) + ament_add_gtest(utilities-test test/src/test_utilities.cpp) + target_link_libraries(utilities-test lanelet2_extension_lib) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/map/lanelet2_extension/README.md b/map/lanelet2_extension/README.md new file mode 100644 index 0000000000000..6da971b782db9 --- /dev/null +++ b/map/lanelet2_extension/README.md @@ -0,0 +1,87 @@ +# lanelet2_extension package + +This package contains external library for Lanelet2 and is meant to ease the use of Lanelet2 in Autoware. + +## Lanelet Format for Autoware + +Autoware uses extended Lanelet2 Format for Autoware, which means you need to add some tags to default OSM file if you want to fully use Lanelet2 maps. For details about custom tags, please refer to this [document](./docs/lanelet2_format_extension.md). + +## Code API + +### IO + +#### Autoware OSM Parser + +Autoware Lanelet2 Format uses .osm extension as original Lanelet2. +However, there are some custom tags that is used by the parser. + +Currently, this includes: + +- overwriting x,y values with `local_x` and `local_y` tags. +- reading `` tag which contains information about map format version and map version. + +The parser is registered as "autoware_osm_handler" as lanelet parser + +### Projection + +#### MGRS Projector + +MGRS projector projects latitude longitude into MGRS Coordinates. + +### Regulatory Elements + +#### Autoware Traffic Light + +Autoware Traffic Light class allows you to retrieve information about traffic lights. +Autoware Traffic Light class contains following members: + +- traffic light shape +- light bulbs information of traffic lights +- stopline associated to traffic light + +### Utility + +#### Message Conversion + +This contains functions to convert lanelet map objects into ROS messages. +Currently it contains following conversions: + +- lanelet::LaneletMapPtr to/from autoware_auto_mapping_msgs::msg::HADMapBin +- lanelet::Point3d to geometry_msgs::Point +- lanelet::Point2d to geometry_msgs::Point +- lanelet::BasicPoint3d to geometry_msgs::Point + +#### Query + +This module contains functions to retrieve various information from maps. +e.g. crosswalks, trafficlights, stoplines + +#### Utilities + +This module contains other useful functions related to Lanelet. +e.g. matching waypoint with lanelets + +### Visualization + +Visualization contains functions to convert lanelet objects into visualization marker messages. +Currently it contains following conversions: + +- lanelet::Lanelet to Triangle Markers +- lanelet::LineString to LineStrip Markers +- TrafficLights to Triangle Markers + +## Nodes + +### lanelet2_extension_sample + +Code for this explains how this lanelet2_extension library is used. +The executable is not meant to do anything. + +### autoware_lanelet2_extension + +This node checks if an .osm file follows the Autoware version of Lanelet2 format. +You can check by running: + +```sh +ros2 run lanelet2_extension autoware_lanelet2_validation _map_file:= +``` diff --git a/map/lanelet2_extension/docs/detection_area.png b/map/lanelet2_extension/docs/detection_area.png new file mode 100644 index 0000000000000..0ae10872cb01f Binary files /dev/null and b/map/lanelet2_extension/docs/detection_area.png differ diff --git a/map/lanelet2_extension/docs/extra_lanelet_subtypes.md b/map/lanelet2_extension/docs/extra_lanelet_subtypes.md new file mode 100644 index 0000000000000..3632b07f74e8f --- /dev/null +++ b/map/lanelet2_extension/docs/extra_lanelet_subtypes.md @@ -0,0 +1,45 @@ +# Extra Lanelet Subtypes + +## Roadside Lane + +The subtypes for this lanelet classify the outer lanes adjacent to the driving lane.Since the list of lanelet subtypes defined in this [link](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletAndAreaTagging.md) cannot represent the shoulder lane and pedestrian lane described below, two new subtypes are defined.When parking on the street, it is necessary to distinguish between a shoulder lane which can be used by vehicles, and a pedestrian lane which can be used by pedestrians and bicycles.If you park in a shoulder lane, you can use the entire lane for temporary parking, but if you park in a pedestrian lane, you must leave a space of at least 75cm. + +### Road shoulder subtype + +- refers: lanelet with subtype attribute. Subtype explains what the type of roadside it represents. If there is an area outside of this roadside lane that is open to traffic, such as a sidewalk or bike lane, select the road_shoulder subtype. + +![Road shoulder](road_shoulder.svg) + +Sample road shoulder in .osm format is shown below: + +```xml + + + + + + + + + +``` + +### Pedestrian lane subtype + +- refers: lanelet with subtype attribute. Subtype explains what the type of roadside it represents. If there are no passable areas outside of this roadside lane, select the pedestrian_lane subtype. + +![Pedestrian lane](pedestrian_lane.svg) + +Sample pedestrian lane in .osm format is shown below: + +```xml + + + + + + + + + +``` diff --git a/map/lanelet2_extension/docs/extra_regulatory_elements.md b/map/lanelet2_extension/docs/extra_regulatory_elements.md new file mode 100644 index 0000000000000..041ec0361c476 --- /dev/null +++ b/map/lanelet2_extension/docs/extra_regulatory_elements.md @@ -0,0 +1,63 @@ +# Extra Regulatory Elements + +## Detection Area + +This regulatory element specifies region of interest which vehicle must pay attention whenever it is driving along the associated lanelet. When there are any obstacle in the detection area, vehicle must stop at specified stopline. + +- refers: refers to detection area polygon. There could be multiple detection areas registered to a single regulatory element. +- refline: refers to stop line of the detection area + +![Detection area](detection_area.png) + +Sample detection area in .osm format is shown below: + +```xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +``` + +## Road Marking + +This regulatory element specifies related road markings to a lanelet as shown below. + +\* Note that the stopline in the image is for stoplines that are for reference, and normal stoplines should be expressed using TrafficSign regulatory element. + +refers: linestring with type attribute. Type explains what road marking it represents (e.g. stopline). + +![Road marking](road_mark.png) diff --git a/map/lanelet2_extension/docs/lanelet2_format_extension.md b/map/lanelet2_extension/docs/lanelet2_format_extension.md new file mode 100644 index 0000000000000..6a206d22820e2 --- /dev/null +++ b/map/lanelet2_extension/docs/lanelet2_format_extension.md @@ -0,0 +1,186 @@ +# Modifying Lanelet2 format for Autoware + +## Overview + +About the basics of the default format, please refer to main [Lanelet2 repository](https://github.com/fzi-forschungszentrum-informatik/Lanelet2). (see [here](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletPrimitives.md) about primitives) + +In addition to default Lanelet2 Format, users should add following mandatory/optional tags to their osm lanelet files as explained in reset of this document. +Users may use `autoware_lanelet2_validation` [node](../README.md#nodes) to check if their maps are valid. + +The following is the extra format added for Autoware: + +- [extra regulatory elements](extra_regulatory_elements.md) + - Detection Area + - Road Marking +- [extra lanelet subtype](extra_lanelet_subtypes.md) + - Roadside Lane + +## Mandatory Tags + +### Elevation Tags + +Elevation("ele") information for points(`node`) is optional in default Lanelet2 format. +However, some of Autoware packages(e.g. trafficlight_recognizer) need elevation to be included in HD map. Therefore, users must make sure that all points in their osm maps contain elevation tags. + +Here is an example osm syntax for node object. + +```xml + + + +``` + +### TrafficLights + +Default Lanelet2 format uses LineString(`way`) or Polygon class to represent the shape of a traffic light. For Autoware, traffic light objects must be represented only by LineString to avoid confusion, where start point is at bottom left edge and end point is at bottom right edge. Also, "height" tag must be added in order to represent the size in vertical direction (not the position). + +The Following image illustrates how LineString is used to represent shape of Traffic Light in Autoware. +![How LineString is used to represent shape of Traffic Light in Autoware](traffic_light.png) + +Here is an example osm syntax for traffic light object. + +```xml + + + + + + + +``` + +### Turn Directions + +Users must add "turn_direction" tags to lanelets within intersections to indicate vehicle's turning direction. You do not need this tags for lanelets that are not in intersections. If you do not have this tag, Autoware will not be able to light up turning indicators. +This tags only take following values: + +- left +- right +- straight + +Following image illustrates how lanelets should be tagged. + +![Turn Directions: How lanelets should be tagged](turn_direction.png) + +Here is an example of osm syntax for lanelets in intersections. + +```xml + + + + + + + + + + +``` + +## Optional Taggings + +Following tags are optional tags that you may want to add depending on how you want to use your map in Autoware. + +### Meta Info + +Users may add the `MetaInfo` element to their OSM file to indicate format version and map version of their OSM file. This information is not meant to influence Autoware vehicle's behavior, but is published as ROS message so that developers could know which map was used from ROSBAG log files. MetaInfo elements exists in the same hierarchy with `node`, `way`, and `relation` elements, otherwise JOSM wouldn't be able to load the file correctly. + +Here is an example of MetaInfo in osm file: + +```xml + + + + ... + ... + ... + +``` + +### Local Coordinate Expression + +Sometimes users might want to create Lanelet2 maps that are not georeferenced. +In such a case, users may use "local_x", "local_y" taggings to express local positions instead of latitude and longitude. +Autoware Osm Parser will overwrite x,y positions with these tags when they are present. +For z values, use "ele" tags as default Lanelet2 Format. +You would still need to fill in lat and lon attributes so that parser does not crush, but their values could be anything. + +Here is example `node` element in osm with "local_x", "local_y" taggings: + +```xml + + + + + + +``` + +### Light Bulbs in Traffic Lights + +Default Lanelet format can only express shape (base + height) of traffic lights. +However, region_tlr node in Autoware uses positions of each light bulbs to recognize color of traffic light. If users may wish to use this node, "light_bulbs"(`way`) element must be registered to traffic_light regulatory_element object define position and color of each light bulb in traffic lights. If you are using other trafficlight_recognizer nodes(e.g. tlr_mxnet), which only uses bounding box of traffic light, then you do not need to add this object. + +"light_bulbs" object is defined using LineString(`way`), and each node of line string is placed at the center of each light bulb. Also, each node should have "color" and optionally "arrow" tags to describe its type. Also, "traffic_light_id" tag is used to indicate which ID of relevant traffic_light element. + +"color" tag is used to express the color of the light bulb. Currently only three values are used: + +- red +- yellow +- green + +"arrow" tag is used to express the direction of the arrow of light bulb: + +- up +- right +- left +- up_right +- up_left + +Following image illustrates how "light_bulbs" LineString should be created. + +![How "light_bulbs" LineString should be created](light_bulbs.png) + +Here is an example of osm syntax for light_bulb object: + +```xml + + + + + + + + + + + + + + + + + + + + + + + + +``` + +After creating "light_bulbs" elements, you have to register them to traffic_light regulatory element as role "light_bulbs". +The following illustrates how light_bulbs are registered to traffic_light regulatory elements. + +![How light_bulbs are registered to traffic_light regulatory elements](traffic_light_regulatory_element.png) + +```xml + + + + + + + +``` diff --git a/map/lanelet2_extension/docs/light_bulbs.png b/map/lanelet2_extension/docs/light_bulbs.png new file mode 100644 index 0000000000000..dc1e6e14d7b38 Binary files /dev/null and b/map/lanelet2_extension/docs/light_bulbs.png differ diff --git a/map/lanelet2_extension/docs/pedestrian_lane.svg b/map/lanelet2_extension/docs/pedestrian_lane.svg new file mode 100644 index 0000000000000..526506ad2c067 --- /dev/null +++ b/map/lanelet2_extension/docs/pedestrian_lane.svg @@ -0,0 +1 @@ +
type: lanelet
subtype: pedestrian_lane
type: lanelet subtype: p...
Unaccessible area
(e.g. fences, plants, trees, etc.)
Unaccessible area...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/map/lanelet2_extension/docs/road_mark.png b/map/lanelet2_extension/docs/road_mark.png new file mode 100644 index 0000000000000..98b41025d5d2b Binary files /dev/null and b/map/lanelet2_extension/docs/road_mark.png differ diff --git a/map/lanelet2_extension/docs/road_shoulder.svg b/map/lanelet2_extension/docs/road_shoulder.svg new file mode 100644 index 0000000000000..40231247b50e1 --- /dev/null +++ b/map/lanelet2_extension/docs/road_shoulder.svg @@ -0,0 +1 @@ +
type: lanelet
subtype: road_shoulder
type: lanelet subtype: r...
Accessible area
(e.g. sidewalks, bike lanes, etc.)
Accessible area...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/map/lanelet2_extension/docs/traffic_light.png b/map/lanelet2_extension/docs/traffic_light.png new file mode 100644 index 0000000000000..aa97c7bc61c7b Binary files /dev/null and b/map/lanelet2_extension/docs/traffic_light.png differ diff --git a/map/lanelet2_extension/docs/traffic_light_regulatory_element.png b/map/lanelet2_extension/docs/traffic_light_regulatory_element.png new file mode 100644 index 0000000000000..17b75677e81c6 Binary files /dev/null and b/map/lanelet2_extension/docs/traffic_light_regulatory_element.png differ diff --git a/map/lanelet2_extension/docs/turn_direction.png b/map/lanelet2_extension/docs/turn_direction.png new file mode 100644 index 0000000000000..3fd63585e10fe Binary files /dev/null and b/map/lanelet2_extension/docs/turn_direction.png differ diff --git a/map/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp b/map/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp new file mode 100644 index 0000000000000..3dd7ce63f0e6f --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp @@ -0,0 +1,62 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__IO__AUTOWARE_OSM_PARSER_HPP_ +#define LANELET2_EXTENSION__IO__AUTOWARE_OSM_PARSER_HPP_ + +#include + +#include +#include + +namespace lanelet +{ +namespace io_handlers +{ +class AutowareOsmParser : public OsmParser +{ +public: + using OsmParser::OsmParser; + + /** + * [parse parse osm file to laneletMap. It is generally same as default + * OsmParser, but it will overwrite x and y value with local_x and local_y + * tags if present] + * @param filename [path to osm file] + * @param errors [any errors caught during parsing] + * @return [returns LaneletMap] + */ + std::unique_ptr parse( + const std::string & filename, ErrorMessages & errors) const; // NOLINT + + /** + * [parseVersions parses MetaInfo tags from osm file] + * @param filename [path to osm file] + * @param format_version [parsed information about map format version] + * @param map_version [parsed information about map version] + */ + static void parseVersions( + const std::string & filename, std::string * format_version, std::string * map_version); + + static constexpr const char * extension() { return ".osm"; } + + static constexpr const char * name() { return "autoware_osm_handler"; } +}; + +} // namespace io_handlers +} // namespace lanelet + +#endif // LANELET2_EXTENSION__IO__AUTOWARE_OSM_PARSER_HPP_ diff --git a/map/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp b/map/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp new file mode 100644 index 0000000000000..402162b8b8538 --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp @@ -0,0 +1,115 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__PROJECTION__MGRS_PROJECTOR_HPP_ +#define LANELET2_EXTENSION__PROJECTION__MGRS_PROJECTOR_HPP_ + +#include +#include + +#include +#include + +#include +#include + +namespace lanelet +{ +namespace projection +{ +class MGRSProjector : public Projector +{ +public: + explicit MGRSProjector(Origin origin = Origin({0.0, 0.0})); // NOLINT + + /** + * [MGRSProjector::forward projects gps lat/lon to MGRS 100km grid] + * @param gps [point with latitude longitude information] + * @return [projected point in MGRS coordinate] + */ + BasicPoint3d forward(const GPSPoint & gps) const override; + + /** + * [MGRSProjector::forward projects gpgs lat/lon to MGRS xyz coordinate] + * @param gps [point with latitude longitude information] + * @param precision [resolution of MGRS Grid 0=100km, 1=10km, 2=1km, 3=100m, + * 4=10m, 5=1m] + * @return [projected point in MGRS coordinate] + */ + BasicPoint3d forward(const GPSPoint & gps, const int precision) const; + + /** + * [MGRSProjector::reverse projects point within MGRS 100km grid into gps + * lat/lon (WGS84)] + * @param mgrs_point [3d point in MGRS 100km grid] + * @return [projected point in WGS84] + */ + GPSPoint reverse(const BasicPoint3d & mgrs) const override; + + /** + * [MGRSProjector::reverse projects point within MGRS grid into gps lat/lon + * (WGS84)] + * @param mgrs_point [3d point in MGRS grid] + * @param mgrs_code [MGRS grid code] + * @return [projected point in WGS84] + */ + GPSPoint reverse(const BasicPoint3d & mgrs_point, const std::string & mgrs_code) const; + + /** + * [MGRSProjector::setMGRSCode sets MGRS code used for reverse projection] + * @param mgrs_code [MGRS code. Minimum requirement is GZD and 100 km Grid + * Square ID. e.g. "4QFJ"] + */ + void setMGRSCode(const std::string & mgrs_code); + + /** + * [MGRSProjector::setMGRSCode sets MGRS code used for reverse projection from + * gps lat/lon values] + * @param gps [gps point used to find GMRS Grid] + * @param precision [resolution of MGRS Grid 0=100km, 1=10km, 2=1km, 3=100m, + * 4=10m, 5=1m] + */ + void setMGRSCode(const GPSPoint & gps, const int precision = 0); + + /** + * [getProjectedMGRSGrid returns mgrs] + * @return [description] + */ + std::string getProjectedMGRSGrid() const { return projected_grid_; } + + /** + * [isMGRSCodeSet checks if mgrs code is set for reverse projection] + * @return [true if mgrs_code member is set] + */ + bool isMGRSCodeSet() const { return !mgrs_code_.empty(); } + +private: + /** + * mgrs grid code used for reverse function + */ + std::string mgrs_code_; + + /** + * mgrs grid code that was last projected in previous forward function. + * reverse function will use this if isMGRSCodeSet() returns false. + */ + mutable std::string projected_grid_; +}; + +} // namespace projection +} // namespace lanelet + +#endif // LANELET2_EXTENSION__PROJECTION__MGRS_PROJECTOR_HPP_ diff --git a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp new file mode 100644 index 0000000000000..6b385663639b5 --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp @@ -0,0 +1,96 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__AUTOWARE_TRAFFIC_LIGHT_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__AUTOWARE_TRAFFIC_LIGHT_HPP_ + +#include +#include + +#include +#include + +namespace lanelet +{ +namespace autoware +{ +struct AutowareRoleNameString +{ + static constexpr const char LightBulbs[] = "light_bulbs"; +}; + +class AutowareTrafficLight : public lanelet::TrafficLight +{ +public: + using Ptr = std::shared_ptr; + static constexpr char RuleName[] = "traffic_light"; + + //! Directly construct a stop line from its required rule parameters. + //! Might modify the input data in oder to get correct tags. + static Ptr make( + Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights, + const Optional & stopLine = {}, const LineStrings3d & lightBulbs = {}) + { + return Ptr{new AutowareTrafficLight(id, attributes, trafficLights, stopLine, lightBulbs)}; + } + + /** + * @brief get the relevant traffic light bulbs + * @return the traffic light bulbs + * + * There might be multiple traffic light bulbs but they are required to show + * the same signal. + */ + ConstLineStrings3d lightBulbs() const; + + /** + * @brief add a new traffic light bulb + * @param primitive the traffic light bulb to add + * + * Traffic light bulbs are represented as linestrings with each point + * expressing position of each light bulb (lamp). + */ + void addLightBulbs(const LineStringOrPolygon3d & primitive); + + /** + * @brief remove a traffic light bulb + * @param primitive the primitive + * @return true if the traffic light bulb existed and was removed + */ + bool removeLightBulbs(const LineStringOrPolygon3d & primitive); + +private: + // the following lines are required so that lanelet2 can create this object + // when loading a map with this regulatory element + friend class lanelet::RegisterRegulatoryElement; + AutowareTrafficLight( + Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights, + const Optional & stopLine, const LineStrings3d & lightBulbs); + explicit AutowareTrafficLight(const lanelet::RegulatoryElementDataPtr & data); +}; +static lanelet::RegisterRegulatoryElement regAutowareTraffic; + +// moved to lanelet2_extension/lib/autoware_traffic_light.cpp to avoid multiple +// definition errors +/* +#if __cplusplus < 201703L +constexpr char AutowareTrafficLight::RuleName[]; // instantiate string in +cpp file #endif +*/ +} // namespace autoware +} // namespace lanelet + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__AUTOWARE_TRAFFIC_LIGHT_HPP_ diff --git a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp new file mode 100644 index 0000000000000..e73ce32fa9889 --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp @@ -0,0 +1,95 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__DETECTION_AREA_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__DETECTION_AREA_HPP_ + +#include +#include + +#include +#include + +namespace lanelet +{ +namespace autoware +{ +class DetectionArea : public lanelet::RegulatoryElement +{ +public: + using Ptr = std::shared_ptr; + static constexpr char RuleName[] = "detection_area"; + + //! Directly construct a stop line from its required rule parameters. + //! Might modify the input data in oder to get correct tags. + static Ptr make( + Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas, + const LineString3d & stopLine) + { + return Ptr{new DetectionArea(id, attributes, detectionAreas, stopLine)}; + } + + /** + * @brief get the relevant detection_areas + * @return detection_areas + */ + ConstPolygons3d detectionAreas() const; + Polygons3d detectionAreas(); + + /** + * @brief add a new detection area + * @param primitive detection area to add + */ + void addDetectionArea(const Polygon3d & primitive); + + /** + * @brief remove a detection area + * @param primitive the primitive + * @return true if the detection area existed and was removed + */ + bool removeDetectionArea(const Polygon3d & primitive); + + /** + * @brief get the stop line for the detection area + * @return the stop line as LineString + */ + ConstLineString3d stopLine() const; + LineString3d stopLine(); + + /** + * @brief set a new stop line, overwrite the old one + * @param stopLine new stop line + */ + void setStopLine(const LineString3d & stopLine); + + //! Deletes the stop line + void removeStopLine(); + +private: + // the following lines are required so that lanelet2 can create this object + // when loading a map with this regulatory element + friend class lanelet::RegisterRegulatoryElement; + DetectionArea( + Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas, + const LineString3d & stopLine); + explicit DetectionArea(const lanelet::RegulatoryElementDataPtr & data); +}; +static lanelet::RegisterRegulatoryElement regDetectionArea; + +} // namespace autoware +} // namespace lanelet + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__DETECTION_AREA_HPP_ diff --git a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp new file mode 100644 index 0000000000000..9e2ca0c72b277 --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp @@ -0,0 +1,93 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_STOPPING_AREA_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_STOPPING_AREA_HPP_ + +#include +#include + +#include +#include + +namespace lanelet +{ +namespace autoware +{ +class NoStoppingArea : public lanelet::RegulatoryElement +{ +public: + using Ptr = std::shared_ptr; + static constexpr char RuleName[] = "no_stopping_area"; + + //! Directly construct a stop line from its required rule parameters. + //! Might modify the input data in oder to get correct tags. + static Ptr make( + Id id, const AttributeMap & attributes, const Polygons3d & no_stopping_areas, + const Optional & stopLine = {}) + { + return Ptr{new NoStoppingArea(id, attributes, no_stopping_areas, stopLine)}; + } + + /** + * @brief get the relevant no stopping area + * @return no stopping area + */ + ConstPolygons3d noStoppingAreas() const; + Polygons3d noStoppingAreas(); + + /** + * @brief add a new no stopping area + * @param primitive no stopping area to add + */ + void addNoStoppingArea(const Polygon3d & primitive); + + /** + * @brief remove a no stopping area + * @param primitive the primitive + * @return true if the no stopping area existed and was removed + */ + bool removeNoStoppingArea(const Polygon3d & primitive); + + /** + * @brief get the stop line for the no stopping area + * @return the stop line as LineString + */ + Optional stopLine() const; + Optional stopLine(); + + /** + * @brief set a new stop line, overwrite the old one + * @param stopLine new stop line + */ + void setStopLine(const LineString3d & stopLine); + + //! Deletes the stop line + void removeStopLine(); + +private: + // the following lines are required so that lanelet2 can create this object + // when loading a map with this regulatory element + friend class lanelet::RegisterRegulatoryElement; + NoStoppingArea( + Id id, const AttributeMap & attributes, const Polygons3d & no_stopping_area, + const Optional & stopLine); + explicit NoStoppingArea(const lanelet::RegulatoryElementDataPtr & data); +}; +static lanelet::RegisterRegulatoryElement regNoStoppingArea; + +} // namespace autoware +} // namespace lanelet + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_STOPPING_AREA_HPP_ diff --git a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp new file mode 100644 index 0000000000000..62b719d249fbd --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp @@ -0,0 +1,71 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__ROAD_MARKING_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__ROAD_MARKING_HPP_ + +#include +#include + +#include +#include + +namespace lanelet +{ +namespace autoware +{ +class RoadMarking : public lanelet::RegulatoryElement +{ +public: + using Ptr = std::shared_ptr; + static constexpr char RuleName[] = "road_marking"; + + //! Directly construct a stop line from its required rule parameters. + //! Might modify the input data in oder to get correct tags. + static Ptr make(Id id, const AttributeMap & attributes, const LineString3d & road_marking) + { + return Ptr{new RoadMarking(id, attributes, road_marking)}; + } + + /** + * @brief get the relevant road marking + * @return road marking + */ + ConstLineString3d roadMarking() const; + LineString3d roadMarking(); + + /** + * @brief add a new road marking + * @param primitive road marking to add + */ + void setRoadMarking(const LineString3d & primitive); + + /** + * @brief remove a road marking + */ + void removeRoadMarking(); + +private: + // the following lines are required so that lanelet2 can create this object + // when loading a map with this regulatory element + friend class lanelet::RegisterRegulatoryElement; + RoadMarking(Id id, const AttributeMap & attributes, const LineString3d & roadMarking); + explicit RoadMarking(const lanelet::RegulatoryElementDataPtr & data); +}; +static lanelet::RegisterRegulatoryElement regRoadMarking; + +} // namespace autoware +} // namespace lanelet + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__ROAD_MARKING_HPP_ diff --git a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp new file mode 100644 index 0000000000000..d4049953730aa --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp @@ -0,0 +1,78 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__VIRTUAL_TRAFFIC_LIGHT_HPP_ +#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__VIRTUAL_TRAFFIC_LIGHT_HPP_ + +#include +#include + +#include +#include + +namespace lanelet +{ +namespace autoware +{ +class VirtualTrafficLight : public lanelet::RegulatoryElement +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + static constexpr char RuleName[] = "virtual_traffic_light"; + + static Ptr make( + const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light) + { + return Ptr{new VirtualTrafficLight(id, attributes, virtual_traffic_light)}; + } + + ConstLineString3d getVirtualTrafficLight() const + { + return getParameters(RoleName::Refers).front(); + } + + Optional getStopLine() const + { + const auto vec = getParameters(RoleName::RefLine); + if (vec.empty()) { + return {}; + } + return vec.front(); + } + + ConstLineString3d getStartLine() const + { + return getParameters("start_line").front(); + } + + ConstLineStrings3d getEndLines() const { return getParameters("end_line"); } + +private: + // the following lines are required so that lanelet2 can create this object + // when loading a map with this regulatory element + friend class lanelet::RegisterRegulatoryElement; + + VirtualTrafficLight( + const Id id, const AttributeMap & attributes, const LineString3d & virtualTrafficLight); + + explicit VirtualTrafficLight(const lanelet::RegulatoryElementDataPtr & data); +}; + +static lanelet::RegisterRegulatoryElement regVirtualTrafficLight; + +} // namespace autoware +} // namespace lanelet + +#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__VIRTUAL_TRAFFIC_LIGHT_HPP_ diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.hpp b/map/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.hpp new file mode 100644 index 0000000000000..a1886b862e573 --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.hpp @@ -0,0 +1,100 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__UTILITY__MESSAGE_CONVERSION_HPP_ +#define LANELET2_EXTENSION__UTILITY__MESSAGE_CONVERSION_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +namespace lanelet +{ +namespace utils +{ +namespace conversion +{ +/** + * [toBinMsg converts lanelet2 map to ROS message. Similar implementation to + * lanelet::io_handlers::BinHandler::write()] + * @param map [lanelet map data] + * @param msg [converted ROS message. Only "data" field is filled] + */ +void toBinMsg(const lanelet::LaneletMapPtr & map, autoware_auto_mapping_msgs::msg::HADMapBin * msg); + +/** + * [fromBinMsg converts ROS message into lanelet2 data. Similar implementation + * to lanelet::io_handlers::BinHandler::parse()] + * @param msg [ROS message for lanelet map] + * @param map [Converted lanelet2 data] + */ +void fromBinMsg(const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map); +void fromBinMsg( + const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map, + lanelet::traffic_rules::TrafficRulesPtr * traffic_rules, + lanelet::routing::RoutingGraphPtr * routing_graph); + +/** + * [toGeomMsgPt converts various point types to geometry_msgs point] + * @param src [input point(geometry_msgs::msg::Point3, + * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ] + * @param dst [converted geometry_msgs point] + */ +void toGeomMsgPt(const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst); +void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst); +void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst); +void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst); + +/** + * [toGeomMsgPt converts various point types to geometry_msgs point] + * @param src [input point(geometry_msgs::msg::Point3, + * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ] + * @return [converted geometry_msgs point] + */ +geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src); +geometry_msgs::msg::Point toGeomMsgPt(const Eigen::Vector3d & src); +geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint3d & src); +geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint2d & src); + +lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src); +void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst); + +/** + * [toGeomMsgPoly converts lanelet polygon to geometry_msgs polygon] + * @param ll_poly [input polygon] + * @param geom_poly [converted geometry_msgs point] + */ +void toGeomMsgPoly( + const lanelet::ConstPolygon3d & ll_poly, geometry_msgs::msg::Polygon * geom_poly); + +/** + * [toGeomMsgPt32 converts Eigen::Vector3d(lanelet:BasicPoint3d to + * geometry_msgs::msg::Point32)] + * @param src [input point] + * @param dst [converted point] + */ +void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst); + +} // namespace conversion +} // namespace utils +} // namespace lanelet + +#endif // LANELET2_EXTENSION__UTILITY__MESSAGE_CONVERSION_HPP_ diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/map/lanelet2_extension/include/lanelet2_extension/utility/query.hpp new file mode 100644 index 0000000000000..2984525c9786d --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -0,0 +1,252 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__UTILITY__QUERY_HPP_ +#define LANELET2_EXTENSION__UTILITY__QUERY_HPP_ + +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" +#include "lanelet2_extension/regulatory_elements/detection_area.hpp" +#include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace lanelet +{ +using TrafficSignConstPtr = std::shared_ptr; +using TrafficLightConstPtr = std::shared_ptr; +using AutowareTrafficLightConstPtr = std::shared_ptr; +using DetectionAreaConstPtr = std::shared_ptr; +using NoStoppingAreaConstPtr = std::shared_ptr; +} // namespace lanelet + +namespace lanelet +{ +namespace utils +{ +namespace query +{ +/** + * [laneletLayer converts laneletLayer into lanelet vector] + * @param ll_Map [input lanelet map] + * @return [all lanelets in the map] + */ +lanelet::ConstLanelets laneletLayer(const lanelet::LaneletMapConstPtr & ll_Map); + +/** + * [subtypeLanelets extracts Lanelet that has given subtype attribute] + * @param lls [input lanelets with various subtypes] + * @param subtype [subtype of lanelets to be retrieved (e.g. + * lanelet::AttributeValueString::Road)] + * @return [lanelets with given subtype] + */ +lanelet::ConstLanelets subtypeLanelets(const lanelet::ConstLanelets lls, const char subtype[]); + +/** + * [crosswalkLanelets extracts crosswalk lanelets] + * @param lls [input lanelets with various subtypes] + * @return [crosswalk lanelets] + */ +lanelet::ConstLanelets crosswalkLanelets(const lanelet::ConstLanelets lls); +lanelet::ConstLanelets walkwayLanelets(const lanelet::ConstLanelets lls); + +/** + * [roadLanelets extracts road lanelets] + * @param lls [input lanelets with subtype road] + * @return [road lanelets] + */ +lanelet::ConstLanelets roadLanelets(const lanelet::ConstLanelets lls); + +/** + * [shoulderLanelets extracts shoulder lanelets] + * @param lls [input lanelets with subtype shoulder] + * @return [shoulder lanelets] + */ +lanelet::ConstLanelets shoulderLanelets(const lanelet::ConstLanelets lls); +/** + * [trafficLights extracts Traffic Light regulatory element from lanelets] + * @param lanelets [input lanelets] + * @return [traffic light that are associated with input lanelets] + */ +std::vector trafficLights(const lanelet::ConstLanelets lanelets); + +/** + * [autowareTrafficLights extracts Autoware Traffic Light regulatory element + * from lanelets] + * @param lanelets [input lanelets] + * @return [autoware traffic light that are associated with input + * lanelets] + */ +std::vector autowareTrafficLights( + const lanelet::ConstLanelets lanelets); + +/** + * [detectionAreas extracts Detection Area regulatory elements from lanelets] + * @param lanelets [input lanelets] + * @return [detection areas that are associated with input lanelets] + */ +std::vector detectionAreas(const lanelet::ConstLanelets & lanelets); + +/** + * [noStoppingArea extracts NoStopping Area regulatory elements from lanelets] + * @param lanelets [input lanelets] + * @return [no stopping areas that are associated with input lanelets] + */ +std::vector noStoppingAreas( + const lanelet::ConstLanelets & lanelets); + +// query all obstacle polygons in lanelet2 map +lanelet::ConstPolygons3d getAllObstaclePolygons( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// query all parking lots in lanelet2 map +lanelet::ConstPolygons3d getAllParkingLots(const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// query all pedestrian markings in lanelet2 map +lanelet::ConstLineStrings3d getAllPedestrianMarkings( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// query all parking spaces in lanelet2 map +lanelet::ConstLineStrings3d getAllParkingSpaces( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// query linked parking spaces from lanelet +lanelet::ConstLineStrings3d getLinkedParkingSpaces( + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr & lanelet_map_ptr); +lanelet::ConstLineStrings3d getLinkedParkingSpaces( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, + const lanelet::ConstPolygons3d & all_parking_lots); +// query linked lanelets from parking space +bool getLinkedLanelet( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstLanelets & all_road_lanelets, + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet); +bool getLinkedLanelet( + const lanelet::ConstLineString3d & parking_space, + const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet); +lanelet::ConstLanelets getLinkedLanelets( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstLanelets & all_road_lanelets, + const lanelet::ConstPolygons3d & all_parking_lots); +lanelet::ConstLanelets getLinkedLanelets( + const lanelet::ConstLineString3d & parking_space, + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); + +// get linked parking lot from lanelet +bool getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, + lanelet::ConstPolygon3d * linked_parking_lot); +bool getLinkedParkingLot( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot); + +// query linked parking space from parking lot +lanelet::ConstLineStrings3d getLinkedParkingSpaces( + const lanelet::ConstPolygon3d & parking_lot, + const lanelet::ConstLineStrings3d & all_parking_spaces); +// query linked lanelets from parking lot +lanelet::ConstLanelets getLinkedLanelets( + const lanelet::ConstPolygon3d & parking_lot, const lanelet::ConstLanelets & all_road_lanelets); + +/** + * [stopLinesLanelets extracts stoplines that are associated to lanelets] + * @param lanelets [input lanelets] + * @return [stop lines that are associated with input lanelets] + */ +std::vector stopLinesLanelets(const lanelet::ConstLanelets lanelets); + +/** + * [stopLinesLanelet extracts stop lines that are associated with a given + * lanelet] + * @param ll [input lanelet] + * @return [stop lines that are associated with input lanelet] + */ +std::vector stopLinesLanelet(const lanelet::ConstLanelet ll); + +/** + * [stopSignStopLines extracts stoplines that are associated with stop signs] + * @param lanelets [input lanelets] + * @param stop_sign_id [sign id of stop sign] + * @return [array of stoplines] + */ +std::vector stopSignStopLines( + const lanelet::ConstLanelets lanelets, const std::string & stop_sign_id = "stop_sign"); + +ConstLanelets getLaneletsWithinRange( + const lanelet::ConstLanelets & lanelets, const lanelet::BasicPoint2d & search_point, + const double range); +ConstLanelets getLaneletsWithinRange( + const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point, + const double range); + +ConstLanelets getLaneChangeableNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet); +ConstLanelets getLaneChangeableNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, + const geometry_msgs::msg::Point & search_point); + +ConstLanelets getAllNeighbors(const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet); +ConstLanelets getAllNeighborsLeft( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet); +ConstLanelets getAllNeighborsRight( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet); +ConstLanelets getAllNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, + const geometry_msgs::msg::Point & search_point); + +bool getClosestLanelet( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelet * closest_lanelet_ptr); + +/** + * [getSucceedingLaneletSequences retrieves a sequence of lanelets after the given lanelet. + * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence + * does not include input lanelet.] + * @param graph [input lanelet routing graph] + * @param lanelet [input lanelet] + * @param length [minimum length of retrieved lanelet sequence] + * @return [lanelet sequence that follows given lanelet] + */ +std::vector getSucceedingLaneletSequences( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length); + +/** + * [getPrecedingLaneletSequences retrieves a sequence of lanelets before the given lanelet. + * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence + * does not include input lanelet.] + * @param graph [input lanelet routing graph] + * @param lanelet [input lanelet] + * @param length [minimum length of retrieved lanelet sequence] + * @return [lanelet sequence that leads to given lanelet] + */ +std::vector getPrecedingLaneletSequences( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length, const lanelet::ConstLanelets & exclude_lanelets = {}); + +} // namespace query +} // namespace utils +} // namespace lanelet + +#endif // LANELET2_EXTENSION__UTILITY__QUERY_HPP_ diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp new file mode 100644 index 0000000000000..2a5d48deee549 --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp @@ -0,0 +1,84 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Kenji Miyake, Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__UTILITY__UTILITIES_HPP_ +#define LANELET2_EXTENSION__UTILITY__UTILITIES_HPP_ + +#include + +#include +#include + +#include +#include + +#include + +namespace lanelet +{ +namespace utils +{ +lanelet::LineString3d generateFineCenterline( + const lanelet::ConstLanelet & lanelet_obj, const double resolution = 5.0); +lanelet::ConstLineString3d getCenterlineWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0); + +lanelet::ConstLanelet getExpandedLanelet( + const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset); + +lanelet::ConstLanelets getExpandedLanelets( + const lanelet::ConstLanelets & lanelet_obj, const double left_offset, const double right_offset); + +/** + * @brief Apply a patch for centerline because the original implementation + * doesn't have enough quality + */ +void overwriteLaneletsCenterline( + lanelet::LaneletMapPtr lanelet_map, const double resolution = 5.0, + const bool force_overwrite = false); + +lanelet::ConstLanelets getConflictingLanelets( + const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet); + +bool lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon); + +bool lineStringToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon); + +double getLaneletLength2d(const lanelet::ConstLanelet & lanelet); +double getLaneletLength3d(const lanelet::ConstLanelet & lanelet); +double getLaneletLength2d(const lanelet::ConstLanelets & lanelet_sequence); +double getLaneletLength3d(const lanelet::ConstLanelets & lanelet_sequence); + +lanelet::ArcCoordinates getArcCoordinates( + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose); + +lanelet::ConstLineString3d getClosestSegment( + const lanelet::BasicPoint2d & search_pt, const lanelet::ConstLineString3d & linestring); + +lanelet::CompoundPolygon3d getPolygonFromArcLength( + const lanelet::ConstLanelets & lanelets, const double s1, const double s2); +double getLaneletAngle( + const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point); +bool isInLanelet( + const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet, + const double radius = 0.0); + +} // namespace utils +} // namespace lanelet + +#endif // LANELET2_EXTENSION__UTILITY__UTILITIES_HPP_ diff --git a/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp new file mode 100644 index 0000000000000..9d21ac69bd2f3 --- /dev/null +++ b/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp @@ -0,0 +1,253 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#ifndef LANELET2_EXTENSION__VISUALIZATION__VISUALIZATION_HPP_ +#define LANELET2_EXTENSION__VISUALIZATION__VISUALIZATION_HPP_ + +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" +#include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" +#include "lanelet2_extension/utility/query.hpp" + +#include + +#include +#include + +#include +#include + +#include +#include + +namespace lanelet +{ +namespace visualization +{ +/** + * [lanelet2Triangle converts lanelet into vector of triangles. Used for + * triangulation] + * @param ll [input lanelet] + * @param triangles [array of polygon message, each containing 3 vertices] + */ +void lanelet2Triangle( + const lanelet::ConstLanelet & ll, std::vector * triangles); + +/** + * [polygon2Triangle converts polygon into vector of triangles] + * @param polygon [input polygon] + * @param triangles [array of polygon message, each containing 3 vertices] + */ +void polygon2Triangle( + const geometry_msgs::msg::Polygon & polygon, + std::vector * triangles); + +/** + * [lanelet2Polygon converts lanelet into a polygon] + * @param ll [input lanelet] + * @param polygon [polygon message containing shape of the input lanelet.] + */ +void lanelet2Polygon(const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon); + +/** + * [initLineStringMarker initializes marker to visualize shape of linestring] + * @param marker [output marker message] + * @param frame_id [frame id of the marker] + * @param ns [namespace of the marker] + * @param c [color of the marker] + */ +void initLineStringMarker( + visualization_msgs::msg::Marker * marker, const std::string frame_id, const std::string ns, + const std_msgs::msg::ColorRGBA c); + +/** + * [pushLineStringMarker pushes marker vertices to visualize shape of linestring] + * @param marker [output marker message] + * @param ls [input linestring] + * @param c [color of the marker] + * @param lss [thickness of the marker] + */ +void pushLineStringMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA c, const float lss = 0.1); + +/** + * [initTrafficLightTriangleMarker initializes marker to visualize shape of traffic + * lights] + * @param marker [created marker] + * @param ns [namespace of the marker] + * @param duration [lifetime of the marker] + */ +void initTrafficLightTriangleMarker( + visualization_msgs::msg::Marker * marker, const std::string ns, + const rclcpp::Duration duration = rclcpp::Duration(0, 0)); + +/** + * [pushTrafficLightTriangleMarker pushes marker vertices to visualize shape of traffic + * lights] + * @param marker [created marker] + * @param ls [linestring that represents traffic light shape] + * @param cl [color of the marker] + * @param scale [scale of the marker] + */ +void pushTrafficLightTriangleMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d ls, + const std_msgs::msg::ColorRGBA cl, const double scale = 1.0); + +/** + * [laneletsBoundaryAsMarkerArray create marker array to visualize shape of + * boundaries of lanelets] + * @param lanelets [input lanelets] + * @param c [color of the boundary] + * @param viz_centerline [flag to visualize centerline or not] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray( + const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA c, + const bool viz_centerline, const std::string & additional_namespace = ""); +/** + * [laneletsAsTriangleMarkerArray create marker array to visualize shape of the + * lanelet] + * @param ns [namespace of the marker] + * @param lanelets [input lanelets] + * @param c [color of the marker] + * @return [created marker] + */ +visualization_msgs::msg::MarkerArray laneletsAsTriangleMarkerArray( + const std::string ns, const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA c); + +/** + * [laneletDirectionAsMarkerArray create marker array to visualize direction of + * the lanelet] + * @param lanelets [input lanelets] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray laneletDirectionAsMarkerArray( + const lanelet::ConstLanelets lanelets, const std::string & additional_namespace = ""); + +/** + * [lineStringsAsMarkerArray creates marker array to visualize shape of + * linestrings] + * @param line_strings [input linestrings] + * @param name_space [namespace of the marker] + * @param c [color of the marker] + * @param lss [thickness of the marker] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray lineStringsAsMarkerArray( + const std::vector line_strings, const std::string name_space, + const std_msgs::msg::ColorRGBA c, const double lss); + +/** + * [autowareTrafficLightsAsMarkerArray creates marker array to visualize traffic + * lights] + * @param tl_reg_elems [traffic light regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray autowareTrafficLightsAsMarkerArray( + const std::vector tl_reg_elems, + const std_msgs::msg::ColorRGBA c, const rclcpp::Duration duration = rclcpp::Duration(0, 0), + const double scale = 1.0); + +/** + * [generateTrafficLightIdMaker creates marker array to visualize traffic id + * lights] + * @param tl_reg_elems [traffic light regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray generateTrafficLightIdMaker( + const std::vector tl_reg_elems, + const std_msgs::msg::ColorRGBA c, const rclcpp::Duration duration = rclcpp::Duration(0, 0), + const double scale = 1.0); + +/** + * [trafficLightsAsTriangleMarkerArray creates marker array to visualize shape + * of traffic lights] + * @param tl_reg_elems [traffic light regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray trafficLightsAsTriangleMarkerArray( + const std::vector tl_reg_elems, const std_msgs::msg::ColorRGBA c, + const rclcpp::Duration duration = rclcpp::Duration(0, 0), const double scale = 1.0); + +/** + * [detectionAreasAsMarkerArray creates marker array to visualize detection areas] + * @param da_reg_elems [detection area regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + */ +visualization_msgs::msg::MarkerArray detectionAreasAsMarkerArray( + const std::vector & da_reg_elems, + const std_msgs::msg::ColorRGBA c, const rclcpp::Duration duration = rclcpp::Duration(0, 0)); + +/** + * [noStoppingAreasAsMarkerArray creates marker array to visualize detection areas] + * @param no_reg_elems [mp stopping area regulatory elements] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + */ +visualization_msgs::msg::MarkerArray noStoppingAreasAsMarkerArray( + const std::vector & no_reg_elems, + const std_msgs::msg::ColorRGBA c, const rclcpp::Duration duration = rclcpp::Duration(0, 0)); + +/** + * [pedestrianMarkingsAsMarkerArray creates marker array to visualize pedestrian markings] + * @param pedestrian_markings [pedestrian marking polygon] + * @param c [color of the marker] + */ +visualization_msgs::msg::MarkerArray pedestrianMarkingsAsMarkerArray( + const lanelet::ConstLineStrings3d & pedestrian_markings, const std_msgs::msg::ColorRGBA & c); + +/** + * [parkingLotsAsMarkerArray creates marker array to visualize parking lots] + * @param parking_lots [parking lot polygon] + * @param c [color of the marker] + */ +visualization_msgs::msg::MarkerArray parkingLotsAsMarkerArray( + const lanelet::ConstPolygons3d & parking_lots, const std_msgs::msg::ColorRGBA & c); + +/** + * [parkingSpacesAsMarkerArray creates marker array to visualize parking spaces] + * @param parking_spaces [parking space line string] + * @param c [color of the marker] + */ +visualization_msgs::msg::MarkerArray parkingSpacesAsMarkerArray( + const lanelet::ConstLineStrings3d & parking_spaces, const std_msgs::msg::ColorRGBA & c); + +/** + * [detectionAreasAsMarkerArray creates marker array to visualize lanelet_id] + * @param road_lanelets [road lanelets] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + * @param scale [scale of the marker] + * @return visualization_msgs::msg::MarkerArray + */ +visualization_msgs::msg::MarkerArray generateLaneletIdMarker( + const lanelet::ConstLanelets road_lanelets, const std_msgs::msg::ColorRGBA c, + const double scale = 0.5); + +visualization_msgs::msg::MarkerArray obstaclePolygonsAsMarkerArray( + const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c); + +} // namespace visualization +} // namespace lanelet + +#endif // LANELET2_EXTENSION__VISUALIZATION__VISUALIZATION_HPP_ diff --git a/map/lanelet2_extension/lib/autoware_osm_parser.cpp b/map/lanelet2_extension/lib/autoware_osm_parser.cpp new file mode 100644 index 0000000000000..984147a4554c5 --- /dev/null +++ b/map/lanelet2_extension/lib/autoware_osm_parser.cpp @@ -0,0 +1,88 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Ryohsuke Mitsudome + +#include "lanelet2_extension/io/autoware_osm_parser.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace lanelet +{ +namespace io_handlers +{ +std::unique_ptr AutowareOsmParser::parse( + const std::string & filename, ErrorMessages & errors) const +{ + auto map = OsmParser::parse(filename, errors); + + // overwrite x and y values if there are local_x, local_y tags + for (Point3d point : map->pointLayer) { + if (point.hasAttribute("local_x")) { + point.x() = point.attribute("local_x").asDouble().value(); + } + if (point.hasAttribute("local_y")) { + point.y() = point.attribute("local_y").asDouble().value(); + } + } + + // rerun align function in just in case + for (Lanelet & lanelet : map->laneletLayer) { + LineString3d new_left, new_right; + std::tie(new_left, new_right) = geometry::align(lanelet.leftBound(), lanelet.rightBound()); + lanelet.setLeftBound(new_left); + lanelet.setRightBound(new_right); + } + + return map; +} + +namespace +{ +RegisterParser regParser; +} + +void AutowareOsmParser::parseVersions( + const std::string & filename, std::string * format_version, std::string * map_version) +{ + if (format_version == nullptr || map_version == nullptr) { + std::cerr << __FUNCTION__ << ": either format_version or map_version is null pointer!"; + return; + } + + pugi::xml_document doc; + auto result = doc.load_file(filename.c_str()); + if (!result) { + throw lanelet::ParseError( + std::string("Errors occurred while parsing osm file: ") + result.description()); + } + + auto osmNode = doc.child("osm"); + auto metainfo = osmNode.child("MetaInfo"); + if (metainfo.attribute("format_version")) { + *format_version = metainfo.attribute("format_version").value(); + } + if (metainfo.attribute("map_version")) { + *map_version = metainfo.attribute("map_version").value(); + } +} + +} // namespace io_handlers +} // namespace lanelet diff --git a/map/lanelet2_extension/lib/autoware_traffic_light.cpp b/map/lanelet2_extension/lib/autoware_traffic_light.cpp new file mode 100644 index 0000000000000..d7c7b5033f253 --- /dev/null +++ b/map/lanelet2_extension/lib/autoware_traffic_light.cpp @@ -0,0 +1,151 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Ryohsuke Mitsudome + +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace lanelet +{ +namespace autoware +{ +namespace +{ +template +bool findAndErase(const T & primitive, RuleParameters * member) +{ + if (member == nullptr) { + std::cerr << __FUNCTION__ << ": member is null pointer"; + return false; + } + auto it = std::find(member->begin(), member->end(), RuleParameter(primitive)); + if (it == member->end()) { + return false; + } + member->erase(it); + return true; +} + +template +RuleParameters toRuleParameters(const std::vector & primitives) +{ + auto cast_func = [](const auto & elem) { return static_cast(elem); }; + return utils::transform(primitives, cast_func); +} + +template <> +RuleParameters toRuleParameters(const std::vector & primitives) +{ + auto cast_func = [](const auto & elem) { return elem.asRuleParameter(); }; + return utils::transform(primitives, cast_func); +} + +LineStringsOrPolygons3d getLsOrPoly(const RuleParameterMap & paramsMap, RoleName role) +{ + auto params = paramsMap.find(role); + if (params == paramsMap.end()) { + return {}; + } + LineStringsOrPolygons3d result; + for (auto & param : params->second) { + auto l = boost::get(¶m); + if (l != nullptr) { + result.push_back(*l); + } + auto p = boost::get(¶m); + if (p != nullptr) { + result.push_back(*p); + } + } + return result; +} + +[[maybe_unused]] ConstLineStringsOrPolygons3d getConstLsOrPoly( + const RuleParameterMap & params, RoleName role) +{ + auto cast_func = [](auto & lsOrPoly) { + return static_cast(lsOrPoly); + }; + return utils::transform(getLsOrPoly(params, role), cast_func); +} + +[[maybe_unused]] RegulatoryElementDataPtr constructAutowareTrafficLightData( + Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights, + const Optional & stopLine, const LineStrings3d & lightBulbs) +{ + RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(trafficLights)}}; + + if (!!stopLine) { + RuleParameters rule_parameters = {*stopLine}; + rpm.insert(std::make_pair(RoleNameString::RefLine, rule_parameters)); + } + if (!lightBulbs.empty()) { + rpm.insert(std::make_pair(AutowareRoleNameString::LightBulbs, toRuleParameters(lightBulbs))); + } + + auto data = std::make_shared(id, rpm, attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = AttributeValueString::TrafficLight; + return data; +} +} // namespace + +constexpr const char AutowareRoleNameString::LightBulbs[]; + +AutowareTrafficLight::AutowareTrafficLight(const RegulatoryElementDataPtr & data) +: TrafficLight(data) +{ +} + +AutowareTrafficLight::AutowareTrafficLight( + Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights, + const Optional & stopLine, const LineStrings3d & lightBulbs) +: TrafficLight(id, attributes, trafficLights, stopLine) +{ + for (const auto & lightBulb : lightBulbs) { + addLightBulbs(lightBulb); + } +} + +ConstLineStrings3d AutowareTrafficLight::lightBulbs() const +{ + return getParameters(AutowareRoleNameString::LightBulbs); +} + +void AutowareTrafficLight::addLightBulbs(const LineStringOrPolygon3d & primitive) +{ + parameters()[AutowareRoleNameString::LightBulbs].emplace_back(primitive.asRuleParameter()); +} + +bool AutowareTrafficLight::removeLightBulbs(const LineStringOrPolygon3d & primitive) +{ + return findAndErase( + primitive.asRuleParameter(), ¶meters().find(AutowareRoleNameString::LightBulbs)->second); +} + +#if __cplusplus < 201703L +constexpr char AutowareTrafficLight::RuleName[]; // instantiate string in cpp file +#endif + +} // namespace autoware +} // namespace lanelet diff --git a/map/lanelet2_extension/lib/detection_area.cpp b/map/lanelet2_extension/lib/detection_area.cpp new file mode 100644 index 0000000000000..b9cf5650a4ae0 --- /dev/null +++ b/map/lanelet2_extension/lib/detection_area.cpp @@ -0,0 +1,157 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Ryohsuke Mitsudome + +#include "lanelet2_extension/regulatory_elements/detection_area.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace lanelet +{ +namespace autoware +{ +namespace +{ +template +bool findAndErase(const T & primitive, RuleParameters * member) +{ + if (member == nullptr) { + std::cerr << __FUNCTION__ << ": member is null pointer"; + return false; + } + auto it = std::find(member->begin(), member->end(), RuleParameter(primitive)); + if (it == member->end()) { + return false; + } + member->erase(it); + return true; +} + +template +RuleParameters toRuleParameters(const std::vector & primitives) +{ + auto cast_func = [](const auto & elem) { return static_cast(elem); }; + return utils::transform(primitives, cast_func); +} + +// template <> +// RuleParameters toRuleParameters(const std::vector& primitives) +// { +// auto cast_func = [](const auto& elem) { return elem.asRuleParameter(); }; +// return utils::transform(primitives, cast_func); +// } + +Polygons3d getPoly(const RuleParameterMap & paramsMap, RoleName role) +{ + auto params = paramsMap.find(role); + if (params == paramsMap.end()) { + return {}; + } + + Polygons3d result; + for (auto & param : params->second) { + auto p = boost::get(¶m); + if (p != nullptr) { + result.push_back(*p); + } + } + return result; +} + +ConstPolygons3d getConstPoly(const RuleParameterMap & params, RoleName role) +{ + auto cast_func = [](auto & poly) { return static_cast(poly); }; + return utils::transform(getPoly(params, role), cast_func); +} + +RegulatoryElementDataPtr constructDetectionAreaData( + Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas, + const LineString3d & stopLine) +{ + RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(detectionAreas)}}; + + RuleParameters rule_parameters = {stopLine}; + rpm.insert(std::make_pair(RoleNameString::RefLine, rule_parameters)); + + auto data = std::make_shared(id, rpm, attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = "detection_area"; + return data; +} +} // namespace + +DetectionArea::DetectionArea(const RegulatoryElementDataPtr & data) : RegulatoryElement(data) +{ + if (getConstPoly(data->parameters, RoleName::Refers).empty()) { + throw InvalidInputError("No detection area defined!"); + } + if (getParameters(RoleName::RefLine).size() != 1) { + throw InvalidInputError("There must be exactly one stopline defined!"); + } +} + +DetectionArea::DetectionArea( + Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas, + const LineString3d & stopLine) +: DetectionArea(constructDetectionAreaData(id, attributes, detectionAreas, stopLine)) +{ +} + +ConstPolygons3d DetectionArea::detectionAreas() const +{ + return getConstPoly(parameters(), RoleName::Refers); +} +Polygons3d DetectionArea::detectionAreas() { return getPoly(parameters(), RoleName::Refers); } + +void DetectionArea::addDetectionArea(const Polygon3d & primitive) +{ + parameters()["detection_area"].emplace_back(primitive); +} + +bool DetectionArea::removeDetectionArea(const Polygon3d & primitive) +{ + return findAndErase(primitive, ¶meters().find("detection_area")->second); +} + +ConstLineString3d DetectionArea::stopLine() const +{ + return getParameters(RoleName::RefLine).front(); +} + +LineString3d DetectionArea::stopLine() +{ + return getParameters(RoleName::RefLine).front(); +} + +void DetectionArea::setStopLine(const LineString3d & stopLine) +{ + parameters()[RoleName::RefLine] = {stopLine}; +} + +void DetectionArea::removeStopLine() { parameters()[RoleName::RefLine] = {}; } + +#if __cplusplus < 201703L +constexpr char DetectionArea::RuleName[]; // instantiate string in cpp file +#endif + +} // namespace autoware +} // namespace lanelet diff --git a/map/lanelet2_extension/lib/message_conversion.cpp b/map/lanelet2_extension/lib/message_conversion.cpp new file mode 100644 index 0000000000000..bc58e403d4f49 --- /dev/null +++ b/map/lanelet2_extension/lib/message_conversion.cpp @@ -0,0 +1,194 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#include "lanelet2_extension/utility/message_conversion.hpp" + +#include "lanelet2_extension/projection/mgrs_projector.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace lanelet +{ +namespace utils +{ +namespace conversion +{ +void toBinMsg(const lanelet::LaneletMapPtr & map, autoware_auto_mapping_msgs::msg::HADMapBin * msg) +{ + if (msg == nullptr) { + std::cerr << __FUNCTION__ << "msg is null pointer!"; + return; + } + + std::stringstream ss; + boost::archive::binary_oarchive oa(ss); + oa << *map; + auto id_counter = lanelet::utils::getId(); + oa << id_counter; + + std::string data_str(ss.str()); + + msg->data.clear(); + msg->data.assign(data_str.begin(), data_str.end()); +} + +void fromBinMsg(const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map) +{ + if (!map) { + std::cerr << __FUNCTION__ << ": map is null pointer!"; + return; + } + + std::string data_str; + data_str.assign(msg.data.begin(), msg.data.end()); + + std::stringstream ss; + ss << data_str; + boost::archive::binary_iarchive oa(ss); + oa >> *map; + lanelet::Id id_counter; + oa >> id_counter; + lanelet::utils::registerId(id_counter); + // *map = std::move(laneletMap); +} + +void fromBinMsg( + const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map, + lanelet::traffic_rules::TrafficRulesPtr * traffic_rules, + lanelet::routing::RoutingGraphPtr * routing_graph) +{ + fromBinMsg(msg, map); + *traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + *routing_graph = lanelet::routing::RoutingGraph::build(*map, **traffic_rules); +} + +void toGeomMsgPt(const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst) +{ + if (dst == nullptr) { + std::cerr << __FUNCTION__ << "pointer is null!"; + return; + } + dst->x = src.x; + dst->y = src.y; + dst->z = src.z; +} +void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst) +{ + if (dst == nullptr) { + std::cerr << __FUNCTION__ << "pointer is null!"; + return; + } + dst->x = src.x(); + dst->y = src.y(); + dst->z = src.z(); +} +void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst) +{ + if (dst == nullptr) { + std::cerr << __FUNCTION__ << "pointer is null!"; + return; + } + dst->x = src.x(); + dst->y = src.y(); + dst->z = src.z(); +} +void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst) +{ + if (dst == nullptr) { + std::cerr << __FUNCTION__ << "pointer is null!" << std::endl; + return; + } + dst->x = src.x(); + dst->y = src.y(); + dst->z = 0; +} + +void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst) +{ + if (dst == nullptr) { + std::cerr << __FUNCTION__ << "pointer is null!" << std::endl; + return; + } + dst->x = src.x(); + dst->y = src.y(); + dst->z = src.z(); +} + +geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src) +{ + geometry_msgs::msg::Point dst; + toGeomMsgPt(src, &dst); + return dst; +} +geometry_msgs::msg::Point toGeomMsgPt(const Eigen::Vector3d & src) +{ + geometry_msgs::msg::Point dst; + toGeomMsgPt(src, &dst); + return dst; +} +geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint3d & src) +{ + geometry_msgs::msg::Point dst; + toGeomMsgPt(src, &dst); + return dst; +} +geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint2d & src) +{ + geometry_msgs::msg::Point dst; + toGeomMsgPt(src, &dst); + return dst; +} + +lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src) +{ + lanelet::ConstPoint3d dst; + toLaneletPoint(src, &dst); + return dst; +} + +void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst) +{ + *dst = lanelet::Point3d(lanelet::InvalId, src.x, src.y, src.z); +} + +void toGeomMsgPoly(const lanelet::ConstPolygon3d & ll_poly, geometry_msgs::msg::Polygon * geom_poly) +{ + geom_poly->points.clear(); + geom_poly->points.reserve(ll_poly.size()); + for (const auto & ll_pt : ll_poly) { + geometry_msgs::msg::Point32 geom_pt32; + utils::conversion::toGeomMsgPt32(ll_pt.basicPoint(), &geom_pt32); + geom_poly->points.push_back(geom_pt32); + } +} + +} // namespace conversion +} // namespace utils +} // namespace lanelet diff --git a/map/lanelet2_extension/lib/mgrs_projector.cpp b/map/lanelet2_extension/lib/mgrs_projector.cpp new file mode 100644 index 0000000000000..3caa85afae4c1 --- /dev/null +++ b/map/lanelet2_extension/lib/mgrs_projector.cpp @@ -0,0 +1,130 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#include "lanelet2_extension/projection/mgrs_projector.hpp" + +#include +#include +#include +#include +#include + +namespace lanelet +{ +namespace projection +{ +MGRSProjector::MGRSProjector(Origin origin) : Projector(origin) {} + +BasicPoint3d MGRSProjector::forward(const GPSPoint & gps) const +{ + BasicPoint3d mgrs_point(forward(gps, 0)); + return mgrs_point; +} + +BasicPoint3d MGRSProjector::forward(const GPSPoint & gps, const int precision) const +{ + std::string prev_projected_grid = projected_grid_; + + BasicPoint3d mgrs_point{0., 0., gps.ele}; + BasicPoint3d utm_point{0., 0., gps.ele}; + int zone{}; + bool northp{}; + std::string mgrs_code; + + try { + GeographicLib::UTMUPS::Forward(gps.lat, gps.lon, zone, northp, utm_point.x(), utm_point.y()); + GeographicLib::MGRS::Forward( + zone, northp, utm_point.x(), utm_point.y(), gps.lat, precision, mgrs_code); + } catch (const GeographicLib::GeographicErr & err) { + std::cerr << err.what() << std::endl; + return mgrs_point; + } + + // get mgrs values from utm values + mgrs_point.x() = fmod(utm_point.x(), 1e5); + mgrs_point.y() = fmod(utm_point.y(), 1e5); + projected_grid_ = mgrs_code; + + if (!prev_projected_grid.empty() && prev_projected_grid != projected_grid_) { + std::cerr << "Projected MGRS Grid changed from last projection. Projected point" + "might be far away from previously projected point." + << std::endl + << "You may want to use different projector."; + } + + return mgrs_point; +} + +GPSPoint MGRSProjector::reverse(const BasicPoint3d & mgrs_point) const +{ + GPSPoint gps{0., 0., 0.}; + // reverse function cannot be used if mgrs_code_ is not set + if (isMGRSCodeSet()) { + gps = reverse(mgrs_point, mgrs_code_); + } else if (!projected_grid_.empty()) { + gps = reverse(mgrs_point, projected_grid_); + } else { + std::cerr << "cannot run reverse operation if mgrs code is not set in projector." << std::endl + << "use setMGRSCode function or explicitly give mgrs code as an argument."; + } + return gps; +} + +GPSPoint MGRSProjector::reverse( + const BasicPoint3d & mgrs_point, const std::string & mgrs_code) const +{ + GPSPoint gps{0., 0., mgrs_point.z()}; + BasicPoint3d utm_point{0., 0., gps.ele}; + + int zone{}; + int prec{}; + bool northp{}; + try { + GeographicLib::MGRS::Reverse( + mgrs_code, zone, northp, utm_point.x(), utm_point.y(), prec, false); + utm_point.x() += fmod(mgrs_point.x(), pow(10, 5 - prec)); + utm_point.y() += fmod(mgrs_point.y(), pow(10, 5 - prec)); + GeographicLib::UTMUPS::Reverse(zone, northp, utm_point.x(), utm_point.y(), gps.lat, gps.lon); + } catch (const GeographicLib::GeographicErr & err) { + std::cerr << "Failed to convert from MGRS to WGS"; + return gps; + } + + return gps; +} + +void MGRSProjector::setMGRSCode(const std::string & mgrs_code) { mgrs_code_ = mgrs_code; } + +void MGRSProjector::setMGRSCode(const GPSPoint & gps, const int precision) +{ + BasicPoint3d utm_point{0., 0., gps.ele}; + int zone{}; + bool northp{}; + std::string mgrs_code; + + try { + GeographicLib::UTMUPS::Forward(gps.lat, gps.lon, zone, northp, utm_point.x(), utm_point.y()); + GeographicLib::MGRS::Forward( + zone, northp, utm_point.x(), utm_point.y(), gps.lat, precision, mgrs_code); + } catch (const GeographicLib::GeographicErr & err) { + std::cerr << err.what() << std::endl; + } + + setMGRSCode(mgrs_code); +} + +} // namespace projection +} // namespace lanelet diff --git a/map/lanelet2_extension/lib/no_stopping_area.cpp b/map/lanelet2_extension/lib/no_stopping_area.cpp new file mode 100644 index 0000000000000..517da9fbf598f --- /dev/null +++ b/map/lanelet2_extension/lib/no_stopping_area.cpp @@ -0,0 +1,157 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace lanelet +{ +namespace autoware +{ +namespace +{ +template +bool findAndErase(const T & primitive, RuleParameters * member) +{ + if (member == nullptr) { + std::cerr << __FUNCTION__ << ": member is null pointer"; + return false; + } + auto it = std::find(member->begin(), member->end(), RuleParameter(primitive)); + if (it == member->end()) { + return false; + } + member->erase(it); + return true; +} + +template +Optional tryGetFront(const std::vector & vec) +{ + if (vec.empty()) { + return {}; + } + return vec.front(); +} + +template +RuleParameters toRuleParameters(const std::vector & primitives) +{ + auto cast_func = [](const auto & elem) { return static_cast(elem); }; + return utils::transform(primitives, cast_func); +} + +Polygons3d getPoly(const RuleParameterMap & paramsMap, RoleName role) +{ + auto params = paramsMap.find(role); + if (params == paramsMap.end()) { + return {}; + } + + Polygons3d result; + for (auto & param : params->second) { + auto p = boost::get(¶m); + if (p != nullptr) { + result.push_back(*p); + } + } + return result; +} + +ConstPolygons3d getConstPoly(const RuleParameterMap & params, RoleName role) +{ + auto cast_func = [](auto & poly) { return static_cast(poly); }; + return utils::transform(getPoly(params, role), cast_func); +} + +RegulatoryElementDataPtr constructNoStoppingAreaData( + Id id, const AttributeMap & attributes, const Polygons3d & noStoppingAreas, + const Optional & stopLine = {}) +{ + RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(noStoppingAreas)}}; + if (!!stopLine) { + rpm.insert({RoleNameString::RefLine, {*stopLine}}); + } + + auto data = std::make_shared(id, std::move(rpm), attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = "no_stopping_area"; + return data; +} +} // namespace + +NoStoppingArea::NoStoppingArea(const RegulatoryElementDataPtr & data) : RegulatoryElement(data) +{ + if (getConstPoly(data->parameters, RoleName::Refers).empty()) { + throw InvalidInputError("no stopping area defined!"); + } + if (getParameters(RoleName::RefLine).size() > 1) { + throw InvalidInputError("There can not exist more than one stop line!"); + } +} + +NoStoppingArea::NoStoppingArea( + Id id, const AttributeMap & attributes, const Polygons3d & no_stopping_areas, + const Optional & stopLine) +: NoStoppingArea(constructNoStoppingAreaData(id, attributes, no_stopping_areas, stopLine)) +{ +} + +ConstPolygons3d NoStoppingArea::noStoppingAreas() const +{ + return getConstPoly(parameters(), RoleName::Refers); +} +Polygons3d NoStoppingArea::noStoppingAreas() { return getPoly(parameters(), RoleName::Refers); } + +void NoStoppingArea::addNoStoppingArea(const Polygon3d & primitive) +{ + parameters()["no_stopping_area"].emplace_back(primitive); +} + +bool NoStoppingArea::removeNoStoppingArea(const Polygon3d & primitive) +{ + return findAndErase(primitive, ¶meters().find("no_stopping_area")->second); +} + +Optional NoStoppingArea::stopLine() const +{ + return tryGetFront(getParameters(RoleName::RefLine)); +} + +Optional NoStoppingArea::stopLine() +{ + return tryGetFront(getParameters(RoleName::RefLine)); +} + +void NoStoppingArea::setStopLine(const LineString3d & stopLine) +{ + parameters()[RoleName::RefLine] = {stopLine}; +} + +void NoStoppingArea::removeStopLine() { parameters()[RoleName::RefLine] = {}; } + +#if __cplusplus < 201703L +constexpr char NoStoppingArea::RuleName[]; // instantiate string in cpp file +#endif + +} // namespace autoware +} // namespace lanelet diff --git a/map/lanelet2_extension/lib/query.cpp b/map/lanelet2_extension/lib/query.cpp new file mode 100644 index 0000000000000..9cca2eb74b63a --- /dev/null +++ b/map/lanelet2_extension/lib/query.cpp @@ -0,0 +1,847 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#include "lanelet2_extension/utility/query.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/utilities.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using lanelet::utils::to2D; +namespace +{ +double getAngleDifference(const double angle1, const double angle2) +{ + Eigen::Vector2d vec1, vec2; + vec1 << std::cos(angle1), std::sin(angle1); + vec2 << std::cos(angle2), std::sin(angle2); + const double diff_angle = std::acos(vec1.dot(vec2)); + return std::fabs(diff_angle); +} + +} // namespace + +namespace lanelet +{ +namespace utils +{ +// returns all lanelets in laneletLayer - don't know how to convert +// PrimitiveLayer -> std::vector +lanelet::ConstLanelets query::laneletLayer(const lanelet::LaneletMapConstPtr & ll_map) +{ + lanelet::ConstLanelets lanelets; + if (!ll_map) { + std::cerr << "No map received!"; + return lanelets; + } + + for (auto li = ll_map->laneletLayer.begin(); li != ll_map->laneletLayer.end(); li++) { + lanelets.push_back(*li); + } + + return lanelets; +} + +lanelet::ConstLanelets query::subtypeLanelets( + const lanelet::ConstLanelets lls, const char subtype[]) +{ + lanelet::ConstLanelets subtype_lanelets; + + for (auto li = lls.begin(); li != lls.end(); li++) { + lanelet::ConstLanelet ll = *li; + + if (ll.hasAttribute(lanelet::AttributeName::Subtype)) { + lanelet::Attribute attr = ll.attribute(lanelet::AttributeName::Subtype); + if (attr.value() == subtype) { + subtype_lanelets.push_back(ll); + } + } + } + + return subtype_lanelets; +} + +lanelet::ConstLanelets query::crosswalkLanelets(const lanelet::ConstLanelets lls) +{ + return query::subtypeLanelets(lls, lanelet::AttributeValueString::Crosswalk); +} + +lanelet::ConstLanelets query::walkwayLanelets(const lanelet::ConstLanelets lls) +{ + return query::subtypeLanelets(lls, lanelet::AttributeValueString::Walkway); +} + +lanelet::ConstLanelets query::roadLanelets(const lanelet::ConstLanelets lls) +{ + return query::subtypeLanelets(lls, lanelet::AttributeValueString::Road); +} + +lanelet::ConstLanelets query::shoulderLanelets(const lanelet::ConstLanelets lls) +{ + return query::subtypeLanelets(lls, "road_shoulder"); +} + +std::vector query::trafficLights( + const lanelet::ConstLanelets lanelets) +{ + std::vector tl_reg_elems; + + for (auto i = lanelets.begin(); i != lanelets.end(); i++) { + lanelet::ConstLanelet ll = *i; + std::vector ll_tl_re = + ll.regulatoryElementsAs(); + + // insert unique tl into array + for (auto tli = ll_tl_re.begin(); tli != ll_tl_re.end(); tli++) { + lanelet::TrafficLightConstPtr tl_ptr = *tli; + lanelet::Id id = tl_ptr->id(); + bool unique_id = true; + for (auto ii = tl_reg_elems.begin(); ii != tl_reg_elems.end(); ii++) { + if (id == (*ii)->id()) { + unique_id = false; + break; + } + } + if (unique_id) { + tl_reg_elems.push_back(tl_ptr); + } + } + } + return tl_reg_elems; +} + +std::vector query::autowareTrafficLights( + const lanelet::ConstLanelets lanelets) +{ + std::vector tl_reg_elems; + + for (auto i = lanelets.begin(); i != lanelets.end(); i++) { + lanelet::ConstLanelet ll = *i; + std::vector ll_tl_re = + ll.regulatoryElementsAs(); + + // insert unique tl into array + for (auto tli = ll_tl_re.begin(); tli != ll_tl_re.end(); tli++) { + lanelet::AutowareTrafficLightConstPtr tl_ptr = *tli; + lanelet::Id id = tl_ptr->id(); + bool unique_id = true; + + for (auto ii = tl_reg_elems.begin(); ii != tl_reg_elems.end(); ii++) { + if (id == (*ii)->id()) { + unique_id = false; + break; + } + } + + if (unique_id) { + tl_reg_elems.push_back(tl_ptr); + } + } + } + return tl_reg_elems; +} + +std::vector query::detectionAreas( + const lanelet::ConstLanelets & lanelets) +{ + std::vector da_reg_elems; + + for (auto i = lanelets.begin(); i != lanelets.end(); i++) { + lanelet::ConstLanelet ll = *i; + std::vector ll_da_re = + ll.regulatoryElementsAs(); + + // insert unique tl into array + for (const auto & da_ptr : ll_da_re) { + lanelet::Id id = da_ptr->id(); + bool unique_id = true; + + for (auto ii = da_reg_elems.begin(); ii != da_reg_elems.end(); ii++) { + if (id == (*ii)->id()) { + unique_id = false; + break; + } + } + + if (unique_id) { + da_reg_elems.push_back(da_ptr); + } + } + } + return da_reg_elems; +} + +std::vector query::noStoppingAreas( + const lanelet::ConstLanelets & lanelets) +{ + std::vector no_reg_elems; + + for (auto i = lanelets.begin(); i != lanelets.end(); i++) { + lanelet::ConstLanelet ll = *i; + std::vector ll_no_re = + ll.regulatoryElementsAs(); + + // insert unique tl into array + for (const auto & no_ptr : ll_no_re) { + lanelet::Id id = no_ptr->id(); + bool unique_id = true; + + for (auto ii = no_reg_elems.begin(); ii != no_reg_elems.end(); ii++) { + if (id == (*ii)->id()) { + unique_id = false; + break; + } + } + + if (unique_id) { + no_reg_elems.push_back(no_ptr); + } + } + } + return no_reg_elems; +} + +lanelet::ConstPolygons3d query::getAllObstaclePolygons( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstPolygons3d obstacle_polygons; + for (const auto & poly : lanelet_map_ptr->polygonLayer) { + const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none"); + if (type.compare("obstacle") == 0) { + obstacle_polygons.push_back(poly); + } + } + return obstacle_polygons; +} + +lanelet::ConstPolygons3d query::getAllParkingLots( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstPolygons3d parking_lots; + for (const auto & poly : lanelet_map_ptr->polygonLayer) { + const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none"); + if (type.compare("parking_lot") == 0) { + parking_lots.push_back(poly); + } + } + return parking_lots; +} + +lanelet::ConstLineStrings3d query::getAllPedestrianMarkings( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstLineStrings3d pedestrian_markings; + for (const auto & ls : lanelet_map_ptr->lineStringLayer) { + const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none"); + if (type.compare("pedestrian_marking") == 0) { + pedestrian_markings.push_back(ls); + } + } + return pedestrian_markings; +} + +lanelet::ConstLineStrings3d query::getAllParkingSpaces( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + lanelet::ConstLineStrings3d parking_spaces; + for (const auto & ls : lanelet_map_ptr->lineStringLayer) { + const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none"); + if (type.compare("parking_space") == 0) { + parking_spaces.push_back(ls); + } + } + return parking_spaces; +} + +bool query::getLinkedLanelet( + const lanelet::ConstLineString3d & parking_space, + const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet) +{ + const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr); + const auto & all_road_lanelets = query::roadLanelets(all_lanelets); + const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); + return query::getLinkedLanelet( + parking_space, all_road_lanelets, all_parking_lots, linked_lanelet); +} + +bool query::getLinkedLanelet( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstLanelets & all_road_lanelets, + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet) +{ + const auto & linked_lanelets = + getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots); + if (linked_lanelets.empty()) { + return false; + } + + double min_distance = std::numeric_limits::max(); + for (const auto & lanelet : linked_lanelets) { + const double distance = boost::geometry::distance( + to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon()); + if (distance < min_distance) { + *linked_lanelet = lanelet; + min_distance = distance; + } + } + return true; +} + +lanelet::ConstLanelets query::getLinkedLanelets( + const lanelet::ConstLineString3d & parking_space, + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr); + const auto & all_road_lanelets = query::roadLanelets(all_lanelets); + const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); + + return query::getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots); +} + +lanelet::ConstLanelets query::getLinkedLanelets( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstLanelets & all_road_lanelets, + const lanelet::ConstPolygons3d & all_parking_lots) +{ + lanelet::ConstLanelets linked_lanelets; + + // get lanelets within same parking lot + lanelet::ConstPolygon3d linked_parking_lot; + if (!getLinkedParkingLot(parking_space, all_parking_lots, &linked_parking_lot)) { + return linked_lanelets; + } + const auto & candidate_lanelets = getLinkedLanelets(linked_parking_lot, all_road_lanelets); + + // get lanelets that are close to parking space and facing to parking space + for (const auto & lanelet : candidate_lanelets) { + // check if parking space is close to lanelet + const double distance = boost::geometry::distance( + to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon()); + constexpr double distance_thresh = 5.0; + if (distance > distance_thresh) { + continue; + } + + // check if parking space is facing lanelet + const Eigen::Vector3d direction = + parking_space.back().basicPoint() - parking_space.front().basicPoint(); + const Eigen::Vector3d new_pt = parking_space.front().basicPoint() - direction * distance_thresh; + + const lanelet::Point3d check_line_p1(lanelet::InvalId, new_pt.x(), new_pt.y(), new_pt.z()); + const lanelet::Point3d check_line_p2(lanelet::InvalId, parking_space.back().basicPoint()); + const lanelet::LineString3d check_line(lanelet::InvalId, {check_line_p1, check_line_p2}); + + const double new_distance = boost::geometry::distance( + to2D(check_line).basicLineString(), lanelet.polygon2d().basicPolygon()); + if (new_distance < std::numeric_limits::epsilon()) { + linked_lanelets.push_back(lanelet); + } + } + + return linked_lanelets; +} + +// get overlapping lanelets +lanelet::ConstLanelets query::getLinkedLanelets( + const lanelet::ConstPolygon3d & parking_lot, const lanelet::ConstLanelets & all_road_lanelets) +{ + lanelet::ConstLanelets linked_lanelets; + for (const auto & lanelet : all_road_lanelets) { + const double distance = boost::geometry::distance( + lanelet.polygon2d().basicPolygon(), to2D(parking_lot).basicPolygon()); + if (distance < std::numeric_limits::epsilon()) { + linked_lanelets.push_back(lanelet); + } + } + return linked_lanelets; +} + +lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +{ + const auto & all_parking_spaces = query::getAllParkingSpaces(lanelet_map_ptr); + const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); + return getLinkedParkingSpaces(lanelet, all_parking_spaces, all_parking_lots); +} + +lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, + const lanelet::ConstPolygons3d & all_parking_lots) +{ + lanelet::ConstLineStrings3d linked_parking_spaces; + + // get parking spaces that are in same parking lot. + lanelet::ConstPolygon3d linked_parking_lot; + if (!getLinkedParkingLot(lanelet, all_parking_lots, &linked_parking_lot)) { + return linked_parking_spaces; + } + const auto & possible_parking_spaces = + getLinkedParkingSpaces(linked_parking_lot, all_parking_spaces); + + // check for parking spaces that are within 5m and facing towards lanelet + for (const auto & parking_space : possible_parking_spaces) { + // check if parking space is close to lanelet + const double distance = boost::geometry::distance( + to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon()); + constexpr double distance_thresh = 5.0; + if (distance > distance_thresh) { + continue; + } + + // check if parking space is facing lanelet + const Eigen::Vector3d direction = + parking_space.back().basicPoint() - parking_space.front().basicPoint(); + const Eigen::Vector3d new_pt = parking_space.front().basicPoint() - direction * distance_thresh; + + const lanelet::Point3d check_line_p1(lanelet::InvalId, new_pt.x(), new_pt.y(), new_pt.z()); + const lanelet::Point3d check_line_p2(lanelet::InvalId, parking_space.back().basicPoint()); + const lanelet::LineString3d check_line(lanelet::InvalId, {check_line_p1, check_line_p2}); + + const double new_distance = boost::geometry::distance( + to2D(check_line).basicLineString(), lanelet.polygon2d().basicPolygon()); + if (new_distance < std::numeric_limits::epsilon()) { + linked_parking_spaces.push_back(parking_space); + } + } + return linked_parking_spaces; +} + +// get overlapping parking lot +bool query::getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, + lanelet::ConstPolygon3d * linked_parking_lot) +{ + for (const auto & parking_lot : all_parking_lots) { + const double distance = boost::geometry::distance( + lanelet.polygon2d().basicPolygon(), to2D(parking_lot).basicPolygon()); + if (distance < std::numeric_limits::epsilon()) { + *linked_parking_lot = parking_lot; + return true; + } + } + return false; +} + +// get overlapping parking lot +bool query::getLinkedParkingLot( + const lanelet::ConstLineString3d & parking_space, + const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot) +{ + for (const auto & parking_lot : all_parking_lots) { + const double distance = boost::geometry::distance( + to2D(parking_space).basicLineString(), to2D(parking_lot).basicPolygon()); + if (distance < std::numeric_limits::epsilon()) { + *linked_parking_lot = parking_lot; + return true; + } + } + return false; +} + +lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( + const lanelet::ConstPolygon3d & parking_lot, + const lanelet::ConstLineStrings3d & all_parking_spaces) +{ + lanelet::ConstLineStrings3d linked_parking_spaces; + for (const auto & parking_space : all_parking_spaces) { + const double distance = boost::geometry::distance( + to2D(parking_space).basicLineString(), to2D(parking_lot).basicPolygon()); + if (distance < std::numeric_limits::epsilon()) { + linked_parking_spaces.push_back(parking_space); + } + } + return linked_parking_spaces; +} + +// return all stop lines and ref lines from a given set of lanelets +std::vector query::stopLinesLanelets( + const lanelet::ConstLanelets lanelets) +{ + std::vector stoplines; + + for (auto lli = lanelets.begin(); lli != lanelets.end(); lli++) { + std::vector ll_stoplines; + ll_stoplines = query::stopLinesLanelet(*lli); + stoplines.insert(stoplines.end(), ll_stoplines.begin(), ll_stoplines.end()); + } + + return stoplines; +} + +// return all stop and ref lines from a given lanelet +std::vector query::stopLinesLanelet(const lanelet::ConstLanelet ll) +{ + std::vector stoplines; + + // find stop lines referenced by right of way reg. elems. + std::vector> right_of_way_reg_elems = + ll.regulatoryElementsAs(); + + if (right_of_way_reg_elems.size() > 0) { + // lanelet has a right of way elem element + for (auto j = right_of_way_reg_elems.begin(); j < right_of_way_reg_elems.end(); j++) { + if ((*j)->getManeuver(ll) == lanelet::ManeuverType::Yield) { + // lanelet has a yield reg. elem. + lanelet::Optional row_stopline_opt = (*j)->stopLine(); + if (!!row_stopline_opt) { + stoplines.push_back(row_stopline_opt.get()); + } + } + } + } + + // find stop lines referenced by traffic lights + std::vector> traffic_light_reg_elems = + ll.regulatoryElementsAs(); + + if (traffic_light_reg_elems.size() > 0) { + // lanelet has a traffic light elem element + for (auto j = traffic_light_reg_elems.begin(); j < traffic_light_reg_elems.end(); j++) { + lanelet::Optional traffic_light_stopline_opt = (*j)->stopLine(); + if (!!traffic_light_stopline_opt) { + stoplines.push_back(traffic_light_stopline_opt.get()); + } + } + } + // find stop lines referenced by traffic signs + std::vector> traffic_sign_reg_elems = + ll.regulatoryElementsAs(); + + if (traffic_sign_reg_elems.size() > 0) { + // lanelet has a traffic sign reg elem - can have multiple ref lines (but + // stop sign shod have 1 + for (auto j = traffic_sign_reg_elems.begin(); j < traffic_sign_reg_elems.end(); j++) { + lanelet::ConstLineStrings3d traffic_sign_stoplines = (*j)->refLines(); + if (traffic_sign_stoplines.size() > 0) { + stoplines.push_back(traffic_sign_stoplines.front()); + } + } + } + return stoplines; +} + +std::vector query::stopSignStopLines( + const lanelet::ConstLanelets lanelets, const std::string & stop_sign_id) +{ + std::vector stoplines; + + std::set checklist; + + for (const auto & ll : lanelets) { + // find stop lines referenced by traffic signs + std::vector> traffic_sign_reg_elems = + ll.regulatoryElementsAs(); + + if (traffic_sign_reg_elems.size() > 0) { + // lanelet has a traffic sign reg elem - can have multiple ref lines (but + // stop sign shod have 1 + for (const auto & ts : traffic_sign_reg_elems) { + // skip if traffic sign is not stop sign + if (ts->type() != stop_sign_id) { + continue; + } + + lanelet::ConstLineStrings3d traffic_sign_stoplines = ts->refLines(); + + // only add new items + if (traffic_sign_stoplines.size() > 0) { + auto id = traffic_sign_stoplines.front().id(); + if (checklist.find(id) == checklist.end()) { + checklist.insert(id); + stoplines.push_back(traffic_sign_stoplines.front()); + } + } + } + } + } + return stoplines; +} + +ConstLanelets query::getLaneletsWithinRange( + const lanelet::ConstLanelets & lanelets, const lanelet::BasicPoint2d & search_point, + const double range) +{ + ConstLanelets near_lanelets; + for (const auto & ll : lanelets) { + lanelet::BasicPolygon2d poly = ll.polygon2d().basicPolygon(); + double distance = lanelet::geometry::distance(poly, search_point); + if (distance <= range) { + near_lanelets.push_back(ll); + } + } + return near_lanelets; +} + +ConstLanelets query::getLaneletsWithinRange( + const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point, + const double range) +{ + return getLaneletsWithinRange( + lanelets, lanelet::BasicPoint2d(search_point.x, search_point.y), range); +} + +ConstLanelets query::getLaneChangeableNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet) +{ + return graph->besides(lanelet); +} + +ConstLanelets query::getLaneChangeableNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, + const geometry_msgs::msg::Point & search_point) +{ + const auto lanelets = + getLaneletsWithinRange(road_lanelets, search_point, std::numeric_limits::epsilon()); + ConstLanelets road_slices; + for (const auto & llt : lanelets) { + const auto tmp_road_slice = getLaneChangeableNeighbors(graph, llt); + road_slices.insert(road_slices.end(), tmp_road_slice.begin(), tmp_road_slice.end()); + } + return road_slices; +} + +ConstLanelets query::getAllNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet) +{ + ConstLanelets lanelets; + + ConstLanelets left_lanelets = getAllNeighborsLeft(graph, lanelet); + ConstLanelets right_lanelets = getAllNeighborsRight(graph, lanelet); + + std::reverse(left_lanelets.begin(), left_lanelets.end()); + lanelets.insert(lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + lanelets.push_back(lanelet); + lanelets.insert(lanelets.end(), right_lanelets.begin(), right_lanelets.end()); + + return lanelets; +} + +ConstLanelets query::getAllNeighborsRight( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet) +{ + ConstLanelets lanelets; + auto right_lane = + (!!graph->right(lanelet)) ? graph->right(lanelet) : graph->adjacentRight(lanelet); + while (!!right_lane) { + lanelets.push_back(right_lane.get()); + right_lane = (!!graph->right(right_lane.get())) ? graph->right(right_lane.get()) + : graph->adjacentRight(right_lane.get()); + } + return lanelets; +} + +ConstLanelets query::getAllNeighborsLeft( + const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet) +{ + ConstLanelets lanelets; + auto left_lane = (!!graph->left(lanelet)) ? graph->left(lanelet) : graph->adjacentLeft(lanelet); + while (!!left_lane) { + lanelets.push_back(left_lane.get()); + left_lane = (!!graph->left(left_lane.get())) ? graph->left(left_lane.get()) + : graph->adjacentLeft(left_lane.get()); + } + return lanelets; +} + +ConstLanelets query::getAllNeighbors( + const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, + const geometry_msgs::msg::Point & search_point) +{ + const auto lanelets = + getLaneletsWithinRange(road_lanelets, search_point, std::numeric_limits::epsilon()); + ConstLanelets road_slices; + for (const auto & llt : lanelets) { + const auto tmp_road_slice = getAllNeighbors(graph, llt); + road_slices.insert(road_slices.end(), tmp_road_slice.begin(), tmp_road_slice.end()); + } + return road_slices; +} + +bool query::getClosestLanelet( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, + ConstLanelet * closest_lanelet_ptr) +{ + if (closest_lanelet_ptr == nullptr) { + std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" + << std::endl; + return false; + } + + if (lanelets.empty()) { + return false; + } + + bool found = false; + + lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); + + // find by distance + lanelet::ConstLanelets candidate_lanelets; + { + double min_distance = std::numeric_limits::max(); + for (const auto & llt : lanelets) { + double distance = + boost::geometry::comparable_distance(llt.polygon2d().basicPolygon(), search_point); + + if (std::abs(distance - min_distance) <= std::numeric_limits::epsilon()) { + candidate_lanelets.push_back(llt); + } else if (distance < min_distance) { + found = true; + candidate_lanelets.clear(); + candidate_lanelets.push_back(llt); + min_distance = distance; + } + } + } + + // find by angle + { + double min_angle = std::numeric_limits::max(); + double pose_yaw = tf2::getYaw(search_pose.orientation); + for (const auto & llt : candidate_lanelets) { + lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline()); + double segment_angle = std::atan2( + segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); + double angle_diff = getAngleDifference(segment_angle, pose_yaw); + if (angle_diff < min_angle) { + min_angle = angle_diff; + *closest_lanelet_ptr = llt; + } else if ((segment_angle - pose_yaw) < 1e-04) { + min_angle = std::abs(segment_angle - pose_yaw); + *closest_lanelet_ptr = llt; + } + } + } + + return found; +} + +std::vector> getSucceedingLaneletSequencesRecursive( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length) +{ + std::vector> succeeding_lanelet_sequences; + + const auto next_lanelets = graph->following(lanelet); + const double lanelet_length = utils::getLaneletLength3d(lanelet); + + // end condition of the recursive function + if (next_lanelets.empty() || lanelet_length >= length) { + succeeding_lanelet_sequences.push_back({lanelet}); + return succeeding_lanelet_sequences; + } + + for (const auto & next_lanelet : next_lanelets) { + // get lanelet sequence after next_lanelet + auto tmp_lanelet_sequences = + getSucceedingLaneletSequencesRecursive(graph, next_lanelet, length - lanelet_length); + for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) { + tmp_lanelet_sequence.push_front(lanelet); + succeeding_lanelet_sequences.push_back(tmp_lanelet_sequence); + } + } + return succeeding_lanelet_sequences; +} + +std::vector> getPrecedingLaneletSequencesRecursive( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length, const lanelet::ConstLanelets & exclude_lanelets) +{ + std::vector> preceding_lanelet_sequences; + + const auto prev_lanelets = graph->previous(lanelet); + const double lanelet_length = utils::getLaneletLength3d(lanelet); + + // end condition of the recursive function + if (prev_lanelets.empty() || lanelet_length >= length) { + preceding_lanelet_sequences.push_back({lanelet}); + return preceding_lanelet_sequences; + } + + for (const auto & prev_lanelet : prev_lanelets) { + if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) { + // if prev_lanelet is included in exclude_lanelets, + // remove prev_lanelet from preceding_lanelet_sequences + continue; + } + + // get lanelet sequence after prev_lanelet + auto tmp_lanelet_sequences = getPrecedingLaneletSequencesRecursive( + graph, prev_lanelet, length - lanelet_length, exclude_lanelets); + for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) { + tmp_lanelet_sequence.push_back(lanelet); + preceding_lanelet_sequences.push_back(tmp_lanelet_sequence); + } + } + + if (preceding_lanelet_sequences.empty()) { + preceding_lanelet_sequences.push_back({lanelet}); + } + return preceding_lanelet_sequences; +} + +std::vector query::getSucceedingLaneletSequences( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length) +{ + std::vector lanelet_sequences_vec; + const auto next_lanelets = graph->following(lanelet); + for (const auto & next_lanelet : next_lanelets) { + const auto lanelet_sequences_deq = + getSucceedingLaneletSequencesRecursive(graph, next_lanelet, length); + for (const auto & lanelet_sequence : lanelet_sequences_deq) { + lanelet_sequences_vec.emplace_back(lanelet_sequence.begin(), lanelet_sequence.end()); + } + } + return lanelet_sequences_vec; +} + +std::vector query::getPrecedingLaneletSequences( + const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, + const double length, const lanelet::ConstLanelets & exclude_lanelets) +{ + std::vector lanelet_sequences_vec; + const auto prev_lanelets = graph->previous(lanelet); + for (const auto & prev_lanelet : prev_lanelets) { + if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) { + // if prev_lanelet is included in exclude_lanelets, + // remove prev_lanelet from preceding_lanelet_sequences + continue; + } + // convert deque into vector + const auto lanelet_sequences_deq = + getPrecedingLaneletSequencesRecursive(graph, prev_lanelet, length, exclude_lanelets); + for (const auto & lanelet_sequence : lanelet_sequences_deq) { + lanelet_sequences_vec.emplace_back(lanelet_sequence.begin(), lanelet_sequence.end()); + } + } + return lanelet_sequences_vec; +} + +} // namespace utils +} // namespace lanelet diff --git a/map/lanelet2_extension/lib/road_marking.cpp b/map/lanelet2_extension/lib/road_marking.cpp new file mode 100644 index 0000000000000..a63d74d213560 --- /dev/null +++ b/map/lanelet2_extension/lib/road_marking.cpp @@ -0,0 +1,80 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "lanelet2_extension/regulatory_elements/road_marking.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace lanelet +{ +namespace autoware +{ +namespace +{ +RegulatoryElementDataPtr constructRoadMarkingData( + Id id, const AttributeMap & attributes, const LineString3d & road_marking) +{ + RuleParameterMap rpm; + RuleParameters rule_parameters = {road_marking}; + rpm.insert(std::make_pair(RoleNameString::Refers, rule_parameters)); + + auto data = std::make_shared(id, rpm, attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = "road_marking"; + return data; +} +} // namespace + +RoadMarking::RoadMarking(const RegulatoryElementDataPtr & data) : RegulatoryElement(data) +{ + if (getParameters(RoleName::Refers).size() != 1) { + throw InvalidInputError("There must be exactly one road marking defined!"); + } +} + +RoadMarking::RoadMarking(Id id, const AttributeMap & attributes, const LineString3d & road_marking) +: RoadMarking(constructRoadMarkingData(id, attributes, road_marking)) +{ +} + +ConstLineString3d RoadMarking::roadMarking() const +{ + return getParameters(RoleName::Refers).front(); +} + +LineString3d RoadMarking::roadMarking() +{ + return getParameters(RoleName::Refers).front(); +} + +void RoadMarking::setRoadMarking(const LineString3d & road_marking) +{ + parameters()[RoleName::Refers] = {road_marking}; +} + +void RoadMarking::removeRoadMarking() { parameters()[RoleName::Refers] = {}; } + +#if __cplusplus < 201703L +constexpr char RoadMarking::RuleName[]; // instantiate string in cpp file +#endif + +} // namespace autoware +} // namespace lanelet diff --git a/map/lanelet2_extension/lib/utilities.cpp b/map/lanelet2_extension/lib/utilities.cpp new file mode 100644 index 0000000000000..ebd64ca544c2c --- /dev/null +++ b/map/lanelet2_extension/lib/utilities.cpp @@ -0,0 +1,599 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Kenji Miyake, Ryohsuke Mitsudome + +#include "lanelet2_extension/utility/utilities.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/query.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace lanelet +{ +namespace utils +{ +namespace +{ +[[maybe_unused]] bool exists(const std::vector & array, const int element) +{ + return std::find(array.begin(), array.end(), element) != array.end(); +} + +/** + * [getContactingLanelets retrieves id of lanelets which has distance 0m to + * search_point] + * @param lanelet_map [pointer to lanelet] + * @param trafficRules [traffic rules to ignore lanelets that are not + * traversable] + * @param search_point [2D point used for searching] + * @param contacting_lanelet_ids [array of lanelet ids that is contacting with + * search_point] + */ +[[maybe_unused]] void getContactingLanelets( + const lanelet::LaneletMapPtr lanelet_map, + const lanelet::traffic_rules::TrafficRulesPtr traffic_rules, + const lanelet::BasicPoint2d search_point, std::vector * contacting_lanelet_ids) +{ + if (!lanelet_map) { + std::cerr << "No lanelet map is set!" << std::endl; + return; + } + + if (contacting_lanelet_ids == nullptr) { + std::cerr << __FUNCTION__ << " contacting_lanelet_ids is null pointer!" << std::endl; + return; + } + + for (const auto & ll : lanelet_map->laneletLayer) { + if (!traffic_rules->canPass(ll)) { + continue; + } + lanelet::BasicPolygon2d poly = ll.polygon2d().basicPolygon(); + double distance = lanelet::geometry::distance(poly, search_point); + if (distance < std::numeric_limits::epsilon()) { + contacting_lanelet_ids->push_back(ll.id()); + } + } +} + +std::vector calculateSegmentDistances(const lanelet::ConstLineString3d & line_string) +{ + std::vector segment_distances; + segment_distances.reserve(line_string.size() - 1); + + for (size_t i = 1; i < line_string.size(); ++i) { + const auto distance = lanelet::geometry::distance(line_string[i], line_string[i - 1]); + segment_distances.push_back(distance); + } + + return segment_distances; +} + +std::vector calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) +{ + const auto segment_distances = calculateSegmentDistances(line_string); + + std::vector accumulated_lengths{0}; + accumulated_lengths.reserve(segment_distances.size() + 1); + std::partial_sum( + std::begin(segment_distances), std::end(segment_distances), + std::back_inserter(accumulated_lengths)); + + return accumulated_lengths; +} + +std::pair findNearestIndexPair( + const std::vector & accumulated_lengths, const double target_length) +{ + // List size + const auto N = accumulated_lengths.size(); + + // Front + if (target_length < accumulated_lengths.at(1)) { + return std::make_pair(0, 1); + } + + // Back + if (target_length > accumulated_lengths.at(N - 2)) { + return std::make_pair(N - 2, N - 1); + } + + // Middle + for (std::size_t i = 1; i < N; ++i) { + if ( + accumulated_lengths.at(i - 1) <= target_length && + target_length <= accumulated_lengths.at(i)) { + return std::make_pair(i - 1, i); + } + } + + // Throw an exception because this never happens + throw std::runtime_error("No nearest point found."); +} + +std::vector resamplePoints( + const lanelet::ConstLineString3d & line_string, const int num_segments) +{ + // Calculate length + const auto line_length = lanelet::geometry::length(line_string); + + // Calculate accumulated lengths + const auto accumulated_lengths = calculateAccumulatedLengths(line_string); + + // Create each segment + std::vector resampled_points; + for (auto i = 0; i <= num_segments; ++i) { + // Find two nearest points + const auto target_length = (static_cast(i) / num_segments) * line_length; + const auto index_pair = findNearestIndexPair(accumulated_lengths, target_length); + + // Apply linear interpolation + const lanelet::BasicPoint3d back_point = line_string[index_pair.first]; + const lanelet::BasicPoint3d front_point = line_string[index_pair.second]; + const auto direction_vector = (front_point - back_point); + + const auto back_length = accumulated_lengths.at(index_pair.first); + const auto front_length = accumulated_lengths.at(index_pair.second); + const auto segment_length = front_length - back_length; + const auto target_point = + back_point + (direction_vector * (target_length - back_length) / segment_length); + + // Add to list + resampled_points.push_back(target_point); + } + + return resampled_points; +} +lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) +{ + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto p1 = linestring[i]; + const auto p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; + } + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto p1 = linestring[start_index]; + const auto p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto p1 = linestring[i]; + const auto p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; + } + accumulated_length += length; + } + + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto p1 = linestring[end_index]; + const auto p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d(lanelet::InvalId, points); +} + +lanelet::ConstLanelet combineLanelets(const lanelet::ConstLanelets lanelets) +{ + lanelet::Points3d lefts, rights, centers; + for (const auto & llt : lanelets) { + for (const auto & pt : llt.leftBound()) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : llt.rightBound()) { + rights.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : llt.centerline()) { + centers.push_back(lanelet::Point3d(pt)); + } + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); + const auto center_line = lanelet::LineString3d(lanelet::InvalId, centers); + auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + combined_lanelet.setCenterline(center_line); + return std::move(combined_lanelet); +} + +} // namespace + +lanelet::LineString3d generateFineCenterline( + const lanelet::ConstLanelet & lanelet_obj, const double resolution) +{ + // Get length of longer border + const double left_length = lanelet::geometry::length(lanelet_obj.leftBound()); + const double right_length = lanelet::geometry::length(lanelet_obj.rightBound()); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d centerline(lanelet::utils::getId()); + for (int i = 0; i < num_segments + 1; i++) { + // Add ID for the average point of left and right + const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2; + const lanelet::Point3d center_point( + lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), + center_basic_point.z()); + centerline.push_back(center_point); + } + return centerline; +} + +lanelet::ConstLineString3d getCenterlineWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution) +{ + // Get length of longer border + const double left_length = lanelet::geometry::length(lanelet_obj.leftBound()); + const double right_length = lanelet::geometry::length(lanelet_obj.rightBound()); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = lanelet::utils::resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d centerline(lanelet::utils::getId()); + for (int i = 0; i < num_segments + 1; i++) { + // Add ID for the average point of left and right + const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2; + + const auto vec_right_2_left = (left_points.at(i) - right_points.at(i)).normalized(); + + const auto offset_center_basic_point = center_basic_point + vec_right_2_left * offset; + + const lanelet::Point3d center_point( + lanelet::utils::getId(), offset_center_basic_point.x(), offset_center_basic_point.y(), + offset_center_basic_point.z()); + centerline.push_back(center_point); + } + return static_cast(centerline); +} + +lanelet::ConstLanelet getExpandedLanelet( + const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset) +{ + using lanelet::geometry::offsetNoThrow; + using lanelet::geometry::internal::checkForInversion; + + const auto & orig_left_bound_2d = lanelet_obj.leftBound2d().basicLineString(); + const auto & orig_right_bound_2d = lanelet_obj.rightBound2d().basicLineString(); + + // Note: The lanelet::geometry::offset throws exception when the undesired inversion is found. + // Use offsetNoThrow until the logic is updated to handle the inversion. + // TODO(Horibe) update + const auto & expanded_left_bound_2d = offsetNoThrow(orig_left_bound_2d, left_offset); + const auto & expanded_right_bound_2d = offsetNoThrow(orig_right_bound_2d, right_offset); + rclcpp::Clock clock{RCL_ROS_TIME}; + try { + checkForInversion(orig_left_bound_2d, expanded_left_bound_2d, left_offset); + checkForInversion(orig_right_bound_2d, expanded_right_bound_2d, right_offset); + } catch (const lanelet::GeometryError & e) { + RCLCPP_ERROR_THROTTLE( + rclcpp::get_logger("lanelet2_extension"), clock, 1000, + "Fail to expand lanelet. output may be undesired. Lanelet points interval in map data could " + "be too narrow."); + } + + const auto toPoints3d = [](const lanelet::BasicLineString2d & ls2d, const double z) { + lanelet::Points3d output; + for (const auto & pt : ls2d) { + output.push_back(lanelet::Point3d(lanelet::InvalId, pt.x(), pt.y(), z)); + } + return output; + }; + + // Original z value cannot be used directly since the offset function can vary the points size. + const lanelet::Points3d ex_lefts = + toPoints3d(expanded_left_bound_2d, lanelet_obj.leftBound3d().basicLineString().at(0).z()); + const lanelet::Points3d ex_rights = + toPoints3d(expanded_right_bound_2d, lanelet_obj.rightBound3d().basicLineString().at(0).z()); + + const auto & extended_left_bound_3d = lanelet::LineString3d(lanelet::InvalId, ex_lefts); + const auto & expanded_right_bound_3d = lanelet::LineString3d(lanelet::InvalId, ex_rights); + const auto & lanelet = lanelet::Lanelet( + lanelet_obj.id(), extended_left_bound_3d, expanded_right_bound_3d, lanelet_obj.attributes()); + + return lanelet; +} + +lanelet::ConstLanelets getExpandedLanelets( + const lanelet::ConstLanelets & lanelet_obj, const double left_offset, const double right_offset) +{ + lanelet::ConstLanelets lanelets; + for (const auto & llt : lanelet_obj) { + lanelets.push_back(getExpandedLanelet(llt, left_offset, right_offset)); + } + return lanelets; +} + +void overwriteLaneletsCenterline( + lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool force_overwrite) +{ + for (auto & lanelet_obj : lanelet_map->laneletLayer) { + if (force_overwrite || !lanelet_obj.hasCustomCenterline()) { + const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution); + lanelet_obj.setCenterline(fine_center_line); + } + } +} + +lanelet::ConstLanelets getConflictingLanelets( + const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet) +{ + const auto & llt_or_areas = graph->conflicting(lanelet); + lanelet::ConstLanelets lanelets; + lanelets.reserve(llt_or_areas.size()); + for (const auto & l_or_a : llt_or_areas) { + auto llt_opt = l_or_a.lanelet(); + if (!!llt_opt) { + lanelets.push_back(llt_opt.get()); + } + } + return lanelets; +} + +bool lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon) +{ + if (polygon == nullptr) { + std::cerr << __func__ << ": polygon is null pointer! Failed to convert to polygon." + << std::endl; + return false; + } + if (linestring.size() != 2) { + std::cerr << __func__ << ": linestring" << linestring.id() << " must have 2 points! (" + << linestring.size() << " != 2)" << std::endl + << "Failed to convert to polygon."; + return false; + } + if (!linestring.hasAttribute("width")) { + std::cerr << __func__ << ": linestring" << linestring.id() + << " does not have width tag. Failed to convert to polygon."; + return false; + } + + const Eigen::Vector3d direction = + linestring.back().basicPoint() - linestring.front().basicPoint(); + const double width = linestring.attributeOr("width", 0.0); + const Eigen::Vector3d direction_left = + (Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ()) * direction).normalized(); + + const Eigen::Vector3d eigen_p1 = linestring.front().basicPoint() + direction_left * width / 2; + const Eigen::Vector3d eigen_p2 = linestring.back().basicPoint() + direction_left * width / 2; + const Eigen::Vector3d eigen_p3 = linestring.back().basicPoint() - direction_left * width / 2; + const Eigen::Vector3d eigen_p4 = linestring.front().basicPoint() - direction_left * width / 2; + + const lanelet::Point3d p1(lanelet::InvalId, eigen_p1.x(), eigen_p1.y(), eigen_p1.z()); + const lanelet::Point3d p2(lanelet::InvalId, eigen_p2.x(), eigen_p2.y(), eigen_p2.z()); + const lanelet::Point3d p3(lanelet::InvalId, eigen_p3.x(), eigen_p3.y(), eigen_p3.z()); + const lanelet::Point3d p4(lanelet::InvalId, eigen_p4.x(), eigen_p4.y(), eigen_p4.z()); + + *polygon = lanelet::Polygon3d(lanelet::InvalId, {p1, p2, p3, p4}); + + return true; +} + +bool lineStringToPolygon( + const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon) +{ + if (polygon == nullptr) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __func__ << ": polygon is null pointer! Failed to convert to polygon."); + return false; + } + if (linestring.size() < 4) { + if (linestring.size() < 3 || linestring.front().id() == linestring.back().id()) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __func__ << ": linestring" << linestring.id() + << " must have more than different 3 points! (size is " << linestring.size() << ")" + << std::endl + << "Failed to convert to polygon."); + return false; + } + } + + lanelet::Polygon3d llt_poly; + + for (const auto & lp : linestring) { + llt_poly.push_back(lanelet::Point3d( + lanelet::InvalId, lp.basicPoint().x(), lp.basicPoint().y(), lp.basicPoint().z())); + } + + if (linestring.front().id() == linestring.back().id()) { + llt_poly.pop_back(); + } + + *polygon = llt_poly; + + return true; +} + +double getLaneletLength2d(const lanelet::ConstLanelet & lanelet) +{ + return boost::geometry::length(lanelet::utils::to2D(lanelet.centerline()).basicLineString()); +} + +double getLaneletLength3d(const lanelet::ConstLanelet & lanelet) +{ + return boost::geometry::length(lanelet.centerline().basicLineString()); +} + +double getLaneletLength2d(const lanelet::ConstLanelets & lanelet_sequence) +{ + double length = 0; + for (const auto & llt : lanelet_sequence) { + length += getLaneletLength2d(llt); + } + return length; +} + +double getLaneletLength3d(const lanelet::ConstLanelets & lanelet_sequence) +{ + double length = 0; + for (const auto & llt : lanelet_sequence) { + length += getLaneletLength3d(llt); + } + return length; +} + +lanelet::ArcCoordinates getArcCoordinates( + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose) +{ + lanelet::ConstLanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet); + + double length = 0; + lanelet::ArcCoordinates arc_coordinates; + for (const auto & llt : lanelet_sequence) { + const auto & centerline_2d = lanelet::utils::to2D(llt.centerline()); + if (llt == closest_lanelet) { + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); + arc_coordinates = lanelet::geometry::toArcCoordinates( + centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + arc_coordinates.length += length; + break; + } + length += boost::geometry::length(centerline_2d); + } + return arc_coordinates; +} + +lanelet::ConstLineString3d getClosestSegment( + const lanelet::BasicPoint2d & search_pt, const lanelet::ConstLineString3d & linestring) +{ + if (linestring.size() < 2) { + return lanelet::LineString3d(); + } + + lanelet::ConstLineString3d closest_segment; + double min_distance = std::numeric_limits::max(); + + for (size_t i = 1; i < linestring.size(); i++) { + lanelet::BasicPoint3d prev_basic_pt = linestring[i - 1].basicPoint(); + lanelet::BasicPoint3d current_basic_pt = linestring[i].basicPoint(); + + lanelet::Point3d prev_pt( + lanelet::InvalId, prev_basic_pt.x(), prev_basic_pt.y(), prev_basic_pt.z()); + lanelet::Point3d current_pt( + lanelet::InvalId, current_basic_pt.x(), current_basic_pt.y(), current_basic_pt.z()); + + lanelet::LineString3d current_segment(lanelet::InvalId, {prev_pt, current_pt}); + double distance = lanelet::geometry::distance2d( + lanelet::utils::to2D(current_segment).basicLineString(), search_pt); + if (distance < min_distance) { + closest_segment = current_segment; + min_distance = distance; + } + } + return closest_segment; +} + +lanelet::CompoundPolygon3d getPolygonFromArcLength( + const lanelet::ConstLanelets & lanelets, const double s1, const double s2) +{ + const auto combined_lanelet = combineLanelets(lanelets); + const auto total_length = getLaneletLength2d(combined_lanelet); + + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = + ratio_s1 * boost::geometry::length(combined_lanelet.leftBound().basicLineString()); + const auto s2_left = + ratio_s2 * boost::geometry::length(combined_lanelet.leftBound().basicLineString()); + const auto s1_right = + ratio_s1 * boost::geometry::length(combined_lanelet.rightBound().basicLineString()); + const auto s2_right = + ratio_s2 * boost::geometry::length(combined_lanelet.rightBound().basicLineString()); + + const auto left_bound = + getLineStringFromArcLength(combined_lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = + getLineStringFromArcLength(combined_lanelet.rightBound(), s1_right, s2_right); + + const auto & lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + return lanelet.polygon3d(); +} + +double getLaneletAngle( + const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point) +{ + lanelet::BasicPoint2d llt_search_point(search_point.x, search_point.y); + lanelet::ConstLineString3d segment = getClosestSegment(llt_search_point, lanelet.centerline()); + return std::atan2( + segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); +} + +bool isInLanelet( + const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet, + const double radius) +{ + const lanelet::BasicPoint2d p(current_pose.position.x, current_pose.position.y); + if (boost::geometry::distance(p, lanelet.polygon2d().basicPolygon()) < radius) { + return true; + } + return false; +} + +} // namespace utils +} // namespace lanelet diff --git a/map/lanelet2_extension/lib/virtual_traffic_light.cpp b/map/lanelet2_extension/lib/virtual_traffic_light.cpp new file mode 100644 index 0000000000000..9d7fdc6fe01a8 --- /dev/null +++ b/map/lanelet2_extension/lib/virtual_traffic_light.cpp @@ -0,0 +1,68 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace lanelet +{ +namespace autoware +{ +namespace +{ +RegulatoryElementDataPtr constructVirtualTrafficLightData( + const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light) +{ + RuleParameterMap rpm; + RuleParameters rule_parameters = {virtual_traffic_light}; + rpm.insert(std::make_pair(RoleNameString::Refers, rule_parameters)); + + auto data = std::make_shared(id, rpm, attributes); + data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement; + data->attributes[AttributeName::Subtype] = "virtual_traffic_light"; + return data; +} +} // namespace + +VirtualTrafficLight::VirtualTrafficLight(const RegulatoryElementDataPtr & data) +: RegulatoryElement(data) +{ + if (getParameters("start_line").size() != 1) { + throw InvalidInputError("There must be exactly one start_line defined!"); + } + if (getParameters("end_line").empty()) { + throw InvalidInputError("No end_line defined!"); + } +} + +VirtualTrafficLight::VirtualTrafficLight( + const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light) +: VirtualTrafficLight(constructVirtualTrafficLightData(id, attributes, virtual_traffic_light)) +{ +} + +#if __cplusplus < 201703L +constexpr char VirtualTrafficLight::RuleName[]; // instantiate string in cpp file +#endif + +} // namespace autoware +} // namespace lanelet diff --git a/map/lanelet2_extension/lib/visualization.cpp b/map/lanelet2_extension/lib/visualization.cpp new file mode 100644 index 0000000000000..fc9f068669d96 --- /dev/null +++ b/map/lanelet2_extension/lib/visualization.cpp @@ -0,0 +1,1171 @@ +// Copyright 2015-2019 Autoware Foundation. All rights reserved. +// +// 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. +// +// Authors: Simon Thompson, Ryohsuke Mitsudome + +#include "lanelet2_extension/visualization/visualization.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/query.hpp" +#include "lanelet2_extension/utility/utilities.hpp" + +#include + +#include +#include + +#include +#include +#include +#include + +namespace +{ +template +bool exists(const std::unordered_set & set, const T & element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +void adjacentPoints( + const int i, const int N, const geometry_msgs::msg::Polygon poly, + geometry_msgs::msg::Point32 * p0, geometry_msgs::msg::Point32 * p1, + geometry_msgs::msg::Point32 * p2) +{ + if (p0 == nullptr || p1 == nullptr || p2 == nullptr) { + std::cerr << __FUNCTION__ << ": either p0, p1, or p2 is null pointer!" << std::endl; + return; + } + + *p1 = poly.points[i]; + if (i == 0) { + *p0 = poly.points[N - 1]; + } else { + *p0 = poly.points[i - 1]; + } + + if (i < N - 1) { + *p2 = poly.points[i + 1]; + } else { + *p2 = poly.points[0]; + } +} + +[[maybe_unused]] double hypot( + const geometry_msgs::msg::Point32 & p0, const geometry_msgs::msg::Point32 & p1) +{ + return sqrt(pow((p1.x - p0.x), 2.0) + pow((p1.y - p0.y), 2.0)); +} + +bool isAttributeValue( + const lanelet::ConstPoint3d p, const std::string attr_str, const std::string value_str) +{ + lanelet::Attribute attr = p.attribute(attr_str); + if (attr.value().compare(value_str) == 0) { + return true; + } + return false; +} + +bool isLaneletAttributeValue( + const lanelet::ConstLanelet ll, const std::string attr_str, const std::string value_str) +{ + lanelet::Attribute attr = ll.attribute(attr_str); + if (attr.value().compare(value_str) == 0) { + return true; + } + return false; +} + +void initLightMarker(visualization_msgs::msg::Marker * marker, const std::string ns) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + float s = 0.3; + + marker->header.frame_id = "map"; + marker->header.stamp = rclcpp::Time(); + marker->frame_locked = true; + marker->ns = ns; + marker->id = 0; + marker->lifetime = rclcpp::Duration(0, 0); + marker->type = visualization_msgs::msg::Marker::SPHERE; + marker->scale.x = s; + marker->scale.y = s; + marker->scale.z = s; +} + +bool inputLightMarker(visualization_msgs::msg::Marker * marker, lanelet::ConstPoint3d p) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return false; + } + + marker->id = p.id(); + + geometry_msgs::msg::Point point; + marker->pose.position.x = p.x(); + marker->pose.position.y = p.y(); + marker->pose.position.z = p.z(); + + std_msgs::msg::ColorRGBA color; + marker->color.r = 0.0f; + marker->color.g = 0.0f; + marker->color.b = 0.0f; + marker->color.a = 0.3f; + + if (isAttributeValue(p, "color", "red")) { + marker->color.r = 0.3f; + marker->color.g = 0.0f; + marker->color.b = 0.0f; + } else if (isAttributeValue(p, "color", "green")) { + marker->color.r = 0.0f; + marker->color.g = 0.3f; + marker->color.b = 0.0f; + } else if (isAttributeValue(p, "color", "yellow")) { + marker->color.r = 0.3f; + marker->color.g = 0.3f; + marker->color.b = 0.0f; + } else { + marker->color.r = 0.3f; + marker->color.g = 0.3f; + marker->color.b = 0.3f; + } + return true; +} + +void initLaneletDirectionMarker(visualization_msgs::msg::Marker * marker, const std::string ns) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + float s = 1.0; + + marker->header.frame_id = "map"; + marker->header.stamp = rclcpp::Time(); + marker->frame_locked = true; + marker->ns = ns; + marker->id = 0; + marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker->lifetime = rclcpp::Duration(0, 0); + + marker->pose.position.x = 0.0; // p.x(); + marker->pose.position.y = 0.0; // p.y(); + marker->pose.position.z = 0.0; // p.z(); + marker->pose.orientation.x = 0.0; + marker->pose.orientation.y = 0.0; + marker->pose.orientation.z = 0.0; + marker->pose.orientation.w = 1.0; + marker->scale.x = s; + marker->scale.y = s; + marker->scale.z = s; + marker->color.r = 1.0f; + marker->color.g = 1.0f; + marker->color.b = 1.0f; + marker->color.a = 0.999; +} + +void pushLaneletDirectionMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLanelet ll) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + lanelet::BasicPoint3d pt[3]; + pt[0].x() = 0.0; + pt[0].y() = -0.3; + pt[0].z() = 0.0; + pt[1].x() = 0.0; + pt[1].y() = 0.3; + pt[1].z() = 0; + pt[2].x() = 1.0; + pt[2].y() = 0.0; + pt[2].z() = 0; + + lanelet::BasicPoint3d pc, pc2; + lanelet::ConstLineString3d center_ls = ll.centerline(); + lanelet::Attribute attr = ll.attribute("turn_direction"); + + std_msgs::msg::ColorRGBA c; + c.r = 0.5; + c.g = 0.5; + c.b = 0.5; + c.a = 0.5; + + if (isLaneletAttributeValue(ll, "turn_direction", "right")) { + c.r = 0.5; + c.g = 0.5; + c.b = 0.6; + } else if (isLaneletAttributeValue(ll, "turn_direction", "left")) { + c.r = 0.5; + c.g = 0.6; + c.b = 0.6; + } + + for (std::size_t ci = 0; ci < center_ls.size() - 1;) { + pc = center_ls[ci]; + if (center_ls.size() > 1) { + pc2 = center_ls[ci + 1]; + } else { + return; + } + + double heading = atan2(pc2.y() - pc.y(), pc2.x() - pc.x()); + + lanelet::BasicPoint3d pt_tf[3]; + + Eigen::Vector3d axis(0, 0, 1); + Eigen::Transform t(Eigen::AngleAxis(heading, axis)); + + for (int i = 0; i < 3; i++) { + pt_tf[i] = t * pt[i] + pc; + } + + geometry_msgs::msg::Point gp[3]; + + for (int i = 0; i < 3; i++) { + std_msgs::msg::ColorRGBA cn = c; + + gp[i].x = pt_tf[i].x(); + gp[i].y = pt_tf[i].y(); + gp[i].z = pt_tf[i].z(); + marker->points.push_back(gp[i]); + marker->colors.push_back(cn); + } + ci = ci + 1; + } +} + +bool isClockWise(const geometry_msgs::msg::Polygon & polygon) +{ + const int N = polygon.points.size(); + const double x_offset = polygon.points[0].x; + const double y_offset = polygon.points[0].y; + double sum = 0.0; + for (std::size_t i = 0; i < polygon.points.size(); ++i) { + sum += (polygon.points[i].x - x_offset) * (polygon.points[(i + 1) % N].y - y_offset) - + (polygon.points[i].y - y_offset) * (polygon.points[(i + 1) % N].x - x_offset); + } + + return sum < 0.0; +} + +// Is angle AOB less than 180? +// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e +bool isAcuteAngle( + const geometry_msgs::msg::Point32 & a, const geometry_msgs::msg::Point32 & o, + const geometry_msgs::msg::Point32 & b) +{ + return (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x) >= 0; +} + +// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e +bool isWithinTriangle( + const geometry_msgs::msg::Point32 & a, const geometry_msgs::msg::Point32 & b, + const geometry_msgs::msg::Point32 & c, const geometry_msgs::msg::Point32 & p) +{ + double c1 = (b.x - a.x) * (p.y - b.y) - (b.y - a.y) * (p.x - b.x); + double c2 = (c.x - b.x) * (p.y - c.y) - (c.y - b.y) * (p.x - c.x); + double c3 = (a.x - c.x) * (p.y - a.y) - (a.y - c.y) * (p.x - a.x); + + return (c1 > 0.0 && c2 > 0.0 && c3 > 0.0) || (c1 < 0.0 && c2 < 0.0 && c3 < 0.0); +} + +visualization_msgs::msg::Marker createPolygonMarker( + const std::string & name_space, const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.frame_locked = true; + marker.id = 0; + marker.ns = name_space; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.lifetime = rclcpp::Duration(0, 0); + marker.pose.position.x = 0.0; + marker.pose.position.y = 0.0; + marker.pose.position.z = 0.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + marker.color = color; + return marker; +} + +void pushPolygonMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstPolygon3d & polygon, + const std_msgs::msg::ColorRGBA & color) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + if (polygon.size() < 3) { + return; + } + + geometry_msgs::msg::Polygon geom_poly; + lanelet::utils::conversion::toGeomMsgPoly(polygon, &geom_poly); + + std::vector triangles; + lanelet::visualization::polygon2Triangle(geom_poly, &triangles); + + for (const auto & tri : triangles) { + geometry_msgs::msg::Point geom_pts[3]; + for (int i = 0; i < 3; i++) { + lanelet::utils::conversion::toGeomMsgPt(tri.points[i], &geom_pts[i]); + marker->points.push_back(geom_pts[i]); + marker->colors.push_back(color); + } + } +} + +} // anonymous namespace + +namespace lanelet +{ +void visualization::lanelet2Triangle( + const lanelet::ConstLanelet & ll, std::vector * triangles) +{ + if (triangles == nullptr) { + std::cerr << __FUNCTION__ << ": triangles is null pointer!" << std::endl; + return; + } + + triangles->clear(); + geometry_msgs::msg::Polygon ll_poly; + lanelet2Polygon(ll, &ll_poly); + polygon2Triangle(ll_poly, triangles); +} + +void visualization::polygon2Triangle( + const geometry_msgs::msg::Polygon & polygon, std::vector * triangles) +{ + geometry_msgs::msg::Polygon poly = polygon; + if (!isClockWise(poly)) { + std::reverse(poly.points.begin(), poly.points.end()); + } + + // ear clipping: find smallest internal angle in polygon + int N = poly.points.size(); + + // array of angles for each vertex + std::vector is_acute_angle; + is_acute_angle.assign(N, false); + for (int i = 0; i < N; i++) { + geometry_msgs::msg::Point32 p0, p1, p2; + + adjacentPoints(i, N, poly, &p0, &p1, &p2); + is_acute_angle.at(i) = isAcuteAngle(p0, p1, p2); + } + + // start ear clipping + while (N >= 3) { + int clipped_vertex = -1; + + for (int i = 0; i < N; i++) { + bool theta = is_acute_angle.at(i); + if (theta == true) { + geometry_msgs::msg::Point32 p0, p1, p2; + adjacentPoints(i, N, poly, &p0, &p1, &p2); + + int j_begin = (i + 2) % N; + int j_end = (i - 1 + N) % N; + bool is_ear = true; + for (int j = j_begin; j != j_end; j = (j + 1) % N) { + if (isWithinTriangle(p0, p1, p2, poly.points.at(j))) { + is_ear = false; + break; + } + } + + if (is_ear) { + clipped_vertex = i; + break; + } + } + } + if (clipped_vertex < 0 || clipped_vertex >= N) { + // print in yellow to indicate warning + std::cerr << "\033[1;33mCould not find valid vertex for ear clipping triangulation. " + "Triangulation result might be invalid\033[0m" + << std::endl; + clipped_vertex = 0; + } + + // create triangle + geometry_msgs::msg::Point32 p0, p1, p2; + adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2); + geometry_msgs::msg::Polygon triangle; + triangle.points.push_back(p0); + triangle.points.push_back(p1); + triangle.points.push_back(p2); + triangles->push_back(triangle); + + // remove vertex of center of angle + auto it = poly.points.begin(); + std::advance(it, clipped_vertex); + poly.points.erase(it); + + // remove from angle list + auto it_angle = is_acute_angle.begin(); + std::advance(it_angle, clipped_vertex); + is_acute_angle.erase(it_angle); + + // update angle list + N = poly.points.size(); + if (clipped_vertex == N) { + clipped_vertex = 0; + } + adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2); + is_acute_angle.at(clipped_vertex) = isAcuteAngle(p0, p1, p2); + + int i_prev = (clipped_vertex == 0) ? N - 1 : clipped_vertex - 1; + adjacentPoints(i_prev, N, poly, &p0, &p1, &p2); + is_acute_angle.at(i_prev) = isAcuteAngle(p0, p1, p2); + } +} + +void visualization::lanelet2Polygon( + const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon) +{ + if (polygon == nullptr) { + std::cerr << __FUNCTION__ << ": polygon is null pointer!" << std::endl; + return; + } + + lanelet::CompoundPolygon3d ll_poly = ll.polygon3d(); + + polygon->points.clear(); + polygon->points.reserve(ll_poly.size()); + + for (const auto & pt : ll_poly) { + geometry_msgs::msg::Point32 pt32; + utils::conversion::toGeomMsgPt32(pt.basicPoint(), &pt32); + polygon->points.push_back(pt32); + } +} + +visualization_msgs::msg::MarkerArray visualization::laneletDirectionAsMarkerArray( + const lanelet::ConstLanelets lanelets, const std::string & additional_namespace) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + initLaneletDirectionMarker(&marker, additional_namespace + "lanelet direction"); + + for (auto lli = lanelets.begin(); lli != lanelets.end(); lli++) { + lanelet::ConstLanelet ll = *lli; + if (ll.hasAttribute(std::string("turn_direction"))) { + pushLaneletDirectionMarker(&marker, ll); + } + } + if (marker.points.empty()) { + return marker_array; + } + marker_array.markers.push_back(marker); + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::autowareTrafficLightsAsMarkerArray( + const std::vector tl_reg_elems, + const std_msgs::msg::ColorRGBA c, const rclcpp::Duration duration, const double scale) +{ + visualization_msgs::msg::MarkerArray tl_marker_array; + if (tl_reg_elems.empty()) { + return tl_marker_array; + } + visualization_msgs::msg::Marker marker_tri; + visualization_msgs::msg::Marker marker_sph; + initLightMarker(&marker_sph, "traffic_light"); + visualization::initTrafficLightTriangleMarker(&marker_tri, "traffic_light_triangle", duration); + + for (auto tli = tl_reg_elems.begin(); tli != tl_reg_elems.end(); tli++) { + lanelet::ConstLineStrings3d light_bulbs; + lanelet::AutowareTrafficLightConstPtr tl = *tli; + + const auto lights = tl->trafficLights(); + for (const auto & lsp : lights) { + if (lsp.isLineString()) { // traffic lights can either polygons or linestrings + lanelet::ConstLineString3d ls = static_cast(lsp); + visualization::pushTrafficLightTriangleMarker(&marker_tri, ls, c, scale); + } + } + + tl_marker_array.markers.push_back(marker_tri); + + light_bulbs = tl->lightBulbs(); + for (auto ls : light_bulbs) { + lanelet::ConstLineString3d l = static_cast(ls); + for (auto pt : l) { + if (pt.hasAttribute("color")) { + if (inputLightMarker(&marker_sph, pt)) { + tl_marker_array.markers.push_back(marker_sph); + } + } + } + } + } + + return tl_marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( + const std::vector tl_reg_elems, + const std_msgs::msg::ColorRGBA c, const rclcpp::Duration duration, const double scale) +{ + visualization_msgs::msg::MarkerArray tl_id_marker_array; + + for (auto tli = tl_reg_elems.begin(); tli != tl_reg_elems.end(); tli++) { + lanelet::ConstLineStrings3d light_bulbs; + lanelet::AutowareTrafficLightConstPtr tl = *tli; + + const auto lights = tl->trafficLights(); + for (const auto & lsp : lights) { + if (lsp.isLineString()) { // traffic lights can either polygons or + // linestrings + lanelet::ConstLineString3d ls = static_cast(lsp); + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.ns = "traffic_light_id"; + marker.id = ls.id(); + marker.type = marker.TEXT_VIEW_FACING; + marker.lifetime = duration; + marker.action = marker.ADD; + marker.pose.position.x = (ls.front().x() + ls.back().x()) / 2; + marker.pose.position.y = (ls.front().y() + ls.back().y()) / 2; + marker.pose.position.z = ls.front().z() + 1.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.color = c; + marker.scale.z = scale; + marker.frame_locked = true; + marker.text = std::to_string(ls.id()); + tl_id_marker_array.markers.push_back(marker); + } + } + } + + return tl_id_marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::detectionAreasAsMarkerArray( + const std::vector & da_reg_elems, + const std_msgs::msg::ColorRGBA c, const rclcpp::Duration duration) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + visualization_msgs::msg::Marker line_marker; + + if (da_reg_elems.empty()) { + return marker_array; + } + + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.frame_locked = true; + marker.ns = "detection_area"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.lifetime = duration; + marker.pose.position.x = 0.0; // p.x(); + marker.pose.position.y = 0.0; // p.y(); + marker.pose.position.z = 0.0; // p.z(); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + marker.color.r = 1.0f; + marker.color.g = 1.0f; + marker.color.b = 1.0f; + marker.color.a = 0.999; + + std_msgs::msg::ColorRGBA line_c; + line_c.r = 0.5; + line_c.g = 0.5; + line_c.b = 0.5; + line_c.a = 0.999; + visualization::initLineStringMarker(&line_marker, "map", "detection_area_stopline", line_c); + + for (const auto & da_reg_elem : da_reg_elems) { + marker.points.clear(); + marker.colors.clear(); + marker.id = da_reg_elem->id(); + + // area visualization + const auto detection_areas = da_reg_elem->detectionAreas(); + for (const auto & detection_area : detection_areas) { + geometry_msgs::msg::Polygon geom_poly; + utils::conversion::toGeomMsgPoly(detection_area, &geom_poly); + + std::vector triangles; + polygon2Triangle(geom_poly, &triangles); + + for (auto tri : triangles) { + geometry_msgs::msg::Point tri0[3]; + + for (int i = 0; i < 3; i++) { + utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]); + marker.points.push_back(tri0[i]); + marker.colors.push_back(c); + } + } // for triangles0 + } // for detection areas + marker_array.markers.push_back(marker); + + // stop line visualization + visualization::pushLineStringMarker(&line_marker, da_reg_elem->stopLine(), line_c, 0.5); + } // for regulatory elements + + marker_array.markers.push_back(line_marker); + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::noStoppingAreasAsMarkerArray( + const std::vector & no_reg_elems, + const std_msgs::msg::ColorRGBA c, const rclcpp::Duration duration) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + visualization_msgs::msg::Marker line_marker; + + if (no_reg_elems.empty()) { + return marker_array; + } + + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.frame_locked = true; + marker.ns = "no_stopping_area"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.lifetime = duration; + marker.pose.position.x = 0.0; // p.x(); + marker.pose.position.y = 0.0; // p.y(); + marker.pose.position.z = 0.0; // p.z(); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + marker.color.r = 1.0f; + marker.color.g = 1.0f; + marker.color.b = 1.0f; + marker.color.a = 0.999; + + std_msgs::msg::ColorRGBA line_c; + line_c.r = 0.5; + line_c.g = 0.5; + line_c.b = 0.5; + line_c.a = 0.999; + visualization::initLineStringMarker(&line_marker, "map", "no_stopping_area_stopline", line_c); + + for (const auto & no_reg_elem : no_reg_elems) { + marker.points.clear(); + marker.colors.clear(); + marker.id = no_reg_elem->id(); + + // area visualization + const auto no_stopping_areas = no_reg_elem->noStoppingAreas(); + for (const auto & no_stopping_area : no_stopping_areas) { + geometry_msgs::msg::Polygon geom_poly; + utils::conversion::toGeomMsgPoly(no_stopping_area, &geom_poly); + + std::vector triangles; + polygon2Triangle(geom_poly, &triangles); + + for (auto tri : triangles) { + geometry_msgs::msg::Point tri0[3]; + + for (int i = 0; i < 3; i++) { + utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]); + marker.points.push_back(tri0[i]); + marker.colors.push_back(c); + } + } // for triangles0 + } // for no_stopping areas + marker_array.markers.push_back(marker); + const auto & stop_line = no_reg_elem->stopLine(); + // stop line visualization + if (stop_line) { + visualization::pushLineStringMarker(&line_marker, stop_line.value(), line_c, 0.5); + } + } // for regulatory elements + if (!line_marker.points.empty()) { + marker_array.markers.push_back(line_marker); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::pedestrianMarkingsAsMarkerArray( + const lanelet::ConstLineStrings3d & pedestrian_markings, const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (pedestrian_markings.empty()) { + return marker_array; + } + + visualization_msgs::msg::Marker marker = createPolygonMarker("pedestrian_marking", c); + for (const auto & linestring : pedestrian_markings) { + lanelet::ConstPolygon3d polygon; + if (utils::lineStringToPolygon(linestring, &polygon)) { + pushPolygonMarker(&marker, polygon, c); + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + "pedestrian marking " << linestring.id() << " failed conversion."); + } + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::parkingLotsAsMarkerArray( + const lanelet::ConstPolygons3d & parking_lots, const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (parking_lots.empty()) { + return marker_array; + } + + visualization_msgs::msg::Marker marker = createPolygonMarker("parking_lots", c); + for (const auto & polygon : parking_lots) { + pushPolygonMarker(&marker, polygon, c); + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} +visualization_msgs::msg::MarkerArray visualization::parkingSpacesAsMarkerArray( + const lanelet::ConstLineStrings3d & parking_spaces, const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (parking_spaces.empty()) { + return marker_array; + } + + visualization_msgs::msg::Marker marker = createPolygonMarker("parking_space", c); + for (const auto & linestring : parking_spaces) { + lanelet::ConstPolygon3d polygon; + if (utils::lineStringWithWidthToPolygon(linestring, &polygon)) { + pushPolygonMarker(&marker, polygon, c); + } else { + std::cerr << "parking space " << linestring.id() << " failed conversion." << std::endl; + } + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::generateLaneletIdMarker( + const lanelet::ConstLanelets road_lanelets, const std_msgs::msg::ColorRGBA c, const double scale) +{ + visualization_msgs::msg::MarkerArray markers; + for (const auto & ll : road_lanelets) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = "lanelet_id"; + marker.id = ll.id(); + marker.type = marker.TEXT_VIEW_FACING; + marker.action = marker.ADD; + const auto centerline = ll.centerline(); + const size_t target_position_index = centerline.size() / 2; + const auto target_position = centerline[target_position_index]; + marker.pose.position.x = target_position.x(); + marker.pose.position.y = target_position.y(); + marker.pose.position.z = target_position.z(); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.color = c; + marker.scale.z = scale; + marker.frame_locked = true; + marker.text = std::to_string(ll.id()); + markers.markers.push_back(marker); + } + return markers; +} + +visualization_msgs::msg::MarkerArray visualization::obstaclePolygonsAsMarkerArray( + const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (obstacle_polygons.empty()) { + return marker_array; + } + + visualization_msgs::msg::Marker marker = createPolygonMarker("obstacles", c); + for (const auto & polygon : obstacle_polygons) { + pushPolygonMarker(&marker, polygon, c); + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::lineStringsAsMarkerArray( + const std::vector line_strings, const std::string name_space, + const std_msgs::msg::ColorRGBA c, const double lss) +{ + visualization_msgs::msg::MarkerArray ls_marker_array; + if (line_strings.empty()) { + return ls_marker_array; + } + std::unordered_set added; + visualization_msgs::msg::Marker ls_marker; + visualization::initLineStringMarker(&ls_marker, "map", name_space, c); + + for (auto i = line_strings.begin(); i != line_strings.end(); i++) { + const lanelet::ConstLineString3d & ls = *i; + if (!exists(added, ls.id())) { + visualization::pushLineStringMarker(&ls_marker, ls, c, lss); + added.insert(ls.id()); + } + } + ls_marker_array.markers.push_back(ls_marker); + return ls_marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArray( + const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA c, + const bool viz_centerline, const std::string & additional_namespace) +{ + double lss = 0.1; // line string size + double lss_center = std::max(lss * 0.1, 0.02); + + std::unordered_set added; + visualization_msgs::msg::Marker left_line_strip, right_line_strip, center_line_strip; + visualization::initLineStringMarker( + &left_line_strip, "map", additional_namespace + "left_lane_bound", c); + visualization::initLineStringMarker( + &right_line_strip, "map", additional_namespace + "right_lane_bound", c); + visualization::initLineStringMarker( + ¢er_line_strip, "map", additional_namespace + "center_lane_line", c); + + for (auto li = lanelets.begin(); li != lanelets.end(); li++) { + lanelet::ConstLanelet lll = *li; + + lanelet::ConstLineString3d left_ls = lll.leftBound(); + lanelet::ConstLineString3d right_ls = lll.rightBound(); + lanelet::ConstLineString3d center_ls = lll.centerline(); + + if (!exists(added, left_ls.id())) { + visualization::pushLineStringMarker(&left_line_strip, left_ls, c, lss); + added.insert(left_ls.id()); + } + if (!exists(added, right_ls.id())) { + visualization::pushLineStringMarker(&right_line_strip, right_ls, c, lss); + added.insert(right_ls.id()); + } + if (viz_centerline && !exists(added, center_ls.id())) { + visualization::pushLineStringMarker(¢er_line_strip, center_ls, c, lss_center); + added.insert(center_ls.id()); + } + } + + visualization_msgs::msg::MarkerArray marker_array; + if (!left_line_strip.points.empty()) { + marker_array.markers.push_back(left_line_strip); + } + if (!right_line_strip.points.empty()) { + marker_array.markers.push_back(right_line_strip); + } + if (!center_line_strip.points.empty()) { + marker_array.markers.push_back(center_line_strip); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::trafficLightsAsTriangleMarkerArray( + const std::vector tl_reg_elems, const std_msgs::msg::ColorRGBA c, + const rclcpp::Duration duration, const double scale) +{ + // convert to to an array of linestrings and publish as marker array using + // existing function + + std::vector line_strings; + visualization_msgs::msg::Marker marker; + visualization::initTrafficLightTriangleMarker(&marker, "traffic_light_triangle", duration); + + for (auto tli = tl_reg_elems.begin(); tli != tl_reg_elems.end(); tli++) { + lanelet::TrafficLightConstPtr tl = *tli; + lanelet::LineString3d ls; + + auto lights = tl->trafficLights(); + for (auto lsp : lights) { + if (lsp.isLineString()) { // traffic lights can either polygons or linestrings + lanelet::ConstLineString3d ls = static_cast(lsp); + visualization::pushTrafficLightTriangleMarker(&marker, ls, c, scale); + } + } + } + + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.push_back(marker); + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::laneletsAsTriangleMarkerArray( + const std::string ns, const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA c) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + + if (lanelets.empty()) { + return marker_array; + } + + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.frame_locked = true; + marker.ns = ns; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.lifetime = rclcpp::Duration(0, 0); + marker.pose.position.x = 0.0; // p.x(); + marker.pose.position.y = 0.0; // p.y(); + marker.pose.position.z = 0.0; // p.z(); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + marker.color.r = 1.0f; + marker.color.g = 1.0f; + marker.color.b = 1.0f; + marker.color.a = 0.999; + + for (auto ll : lanelets) { + std::vector triangles; + lanelet2Triangle(ll, &triangles); + + for (auto tri : triangles) { + geometry_msgs::msg::Point tri0[3]; + + for (int i = 0; i < 3; i++) { + utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]); + + marker.points.push_back(tri0[i]); + marker.colors.push_back(c); + } + } + } + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +void visualization::initTrafficLightTriangleMarker( + visualization_msgs::msg::Marker * marker, const std::string ns, const rclcpp::Duration duration) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + marker->header.frame_id = "map"; + marker->header.stamp = rclcpp::Time(); + marker->frame_locked = true; + marker->ns = ns; + marker->id = 0; + marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker->lifetime = duration; + + marker->pose.position.x = 0.0; // p.x(); + marker->pose.position.y = 0.0; // p.y(); + marker->pose.position.z = 0.0; // p.z(); + marker->pose.orientation.x = 0.0; + marker->pose.orientation.y = 0.0; + marker->pose.orientation.z = 0.0; + marker->pose.orientation.w = 1.0; + marker->scale.x = 1.0; + marker->scale.y = 1.0; + marker->scale.z = 1.0; + marker->color.r = 1.0f; + marker->color.g = 1.0f; + marker->color.b = 1.0f; + marker->color.a = 0.999; +} + +void visualization::pushTrafficLightTriangleMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d ls, + const std_msgs::msg::ColorRGBA cl, const double scale) +{ + if (marker == nullptr) { + std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl; + return; + } + + double h = 0.7; + if (ls.hasAttribute("height")) { + lanelet::Attribute attr = ls.attribute("height"); + h = std::stod(attr.value()); + } + + // construct triangles and add to marker + + // define polygon of traffic light border + Eigen::Vector3d v[4]; + v[0] << ls.front().x(), ls.front().y(), ls.front().z(); + v[1] << ls.back().x(), ls.back().y(), ls.back().z(); + v[2] << ls.back().x(), ls.back().y(), ls.back().z() + h; + v[3] << ls.front().x(), ls.front().y(), ls.front().z() + h; + + Eigen::Vector3d c = (v[0] + v[1] + v[2] + v[3]) / 4; + + if (scale > 0.0 && scale != 1.0) { + for (int i = 0; i < 4; i++) { + v[i] = (v[i] - c) * scale + c; + } + } + geometry_msgs::msg::Point tri0[3]; + utils::conversion::toGeomMsgPt(v[0], &tri0[0]); + utils::conversion::toGeomMsgPt(v[1], &tri0[1]); + utils::conversion::toGeomMsgPt(v[2], &tri0[2]); + geometry_msgs::msg::Point tri1[3]; + utils::conversion::toGeomMsgPt(v[0], &tri1[0]); + utils::conversion::toGeomMsgPt(v[2], &tri1[1]); + utils::conversion::toGeomMsgPt(v[3], &tri1[2]); + + for (int i = 0; i < 3; i++) { + marker->points.push_back(tri0[i]); + marker->colors.push_back(cl); + } + for (int i = 0; i < 3; i++) { + marker->points.push_back(tri1[i]); + marker->colors.push_back(cl); + } +} + +void visualization::initLineStringMarker( + visualization_msgs::msg::Marker * marker, const std::string frame_id, const std::string ns, + const std_msgs::msg::ColorRGBA c) +{ + if (marker == nullptr) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __FUNCTION__ << ": marker is null pointer!"); + return; + } + + marker->header.frame_id = frame_id; + marker->header.stamp = rclcpp::Time(); + marker->frame_locked = true; + marker->ns = ns; + marker->action = visualization_msgs::msg::Marker::ADD; + marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + + marker->id = 0; + marker->pose.orientation.x = 0.0; + marker->pose.orientation.y = 0.0; + marker->pose.orientation.z = 0.0; + marker->pose.orientation.w = 1.0; + marker->scale.x = 1.0; + marker->scale.y = 1.0; + marker->scale.z = 1.0; + marker->color = c; +} + +void visualization::pushLineStringMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA c, const float lss) +{ + if (marker == nullptr) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __FUNCTION__ << ": marker is null pointer!"); + return; + } + + // fill out lane line + if (ls.size() < 2) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __FUNCTION__ << ": marker line size is 1 or 0!"); + return; + } + for (auto i = ls.begin(); i + 1 != ls.end(); i++) { + geometry_msgs::msg::Point p; + const float heading = std::atan2((*(i + 1)).y() - (*i).y(), (*(i + 1)).x() - (*i).x()); + + const float x_offset = lss * 0.5 * std::sin(heading); + const float y_offset = lss * 0.5 * std::cos(heading); + + p.x = (*i).x() + x_offset; + p.y = (*i).y() - y_offset; + p.z = (*i).z(); + marker->points.push_back(p); + p.x = (*i).x() - x_offset; + p.y = (*i).y() + y_offset; + p.z = (*i).z(); + marker->points.push_back(p); + p.x = (*(i + 1)).x() + x_offset; + p.y = (*(i + 1)).y() - y_offset; + p.z = (*(i + 1)).z(); + marker->points.push_back(p); + marker->colors.push_back(c); + p.x = (*(i + 1)).x() + x_offset; + p.y = (*(i + 1)).y() - y_offset; + p.z = (*(i + 1)).z(); + marker->points.push_back(p); + p.x = (*(i + 1)).x() - x_offset; + p.y = (*(i + 1)).y() + y_offset; + p.z = (*(i + 1)).z(); + marker->points.push_back(p); + p.x = (*i).x() - x_offset; + p.y = (*i).y() + y_offset; + p.z = (*i).z(); + marker->points.push_back(p); + marker->colors.push_back(c); + } +} + +} // namespace lanelet diff --git a/map/lanelet2_extension/package.xml b/map/lanelet2_extension/package.xml new file mode 100644 index 0000000000000..06867a3c44ef0 --- /dev/null +++ b/map/lanelet2_extension/package.xml @@ -0,0 +1,36 @@ + + + lanelet2_extension + 0.1.0 + The lanelet2_extension package contains libraries to handle Lanelet2 format data. + + mitsudome-r + + Apache License 2.0 + + ament_cmake + ament_cmake_auto + + autoware_auto_mapping_msgs + geographiclib + geometry_msgs + lanelet2_core + lanelet2_io + lanelet2_maps + lanelet2_projection + lanelet2_routing + lanelet2_traffic_rules + lanelet2_validation + pugixml-dev + rclcpp + tf2 + visualization_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/map/lanelet2_extension/src/sample_code.cpp b/map/lanelet2_extension/src/sample_code.cpp new file mode 100644 index 0000000000000..a8e7aa9a90a45 --- /dev/null +++ b/map/lanelet2_extension/src/sample_code.cpp @@ -0,0 +1,120 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "lanelet2_extension/projection/mgrs_projector.hpp" +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" + +#include + +#include +#include +#include + +#include +#include + +void loadingAutowareOSMFile(const std::string map_file_path) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet::GPSPoint gps_point; + gps_point.lat = 49.0; + gps_point.lon = 8.4; + lanelet::Origin origin(gps_point); + lanelet::projection::UtmProjector projector(lanelet::Origin(gps_point), true, false); + + // Autoware OSM file parser is registered into lanelet2 library. + // Therefore, you can used it by just specifying "autoware_osm_handler" parser + // in load function. + lanelet_map = lanelet::load(map_file_path, "autoware_osm_handler", projector, &errors); + + // If you want to use default parser, explicitly name the parser + lanelet_map = lanelet::load(map_file_path, "osm_handler", projector, &errors); +} + +void usingMGRSProjector() +{ + // MGRS Projector projects lat/lon to x,y,z point in MGRS 100km grid. + // The origin is automatically calculated so you don't have to select any + // origin. + lanelet::projection::MGRSProjector projector; + + // Let's convert lat/lng point into mgrs xyz point. + lanelet::GPSPoint gps_point; + gps_point.lat = 49.0; + gps_point.lon = 8.4; + gps_point.ele = 0.0; + + lanelet::BasicPoint3d mgrs_point = projector.forward(gps_point); + std::cout << mgrs_point << std::endl; + + // You can get MGRS Code of projected grid. + std::string mgrs_grid = projector.getProjectedMGRSGrid(); + std::cout << mgrs_grid << std::endl; + + // You can also reverse project from MGRS point to lat/lon. + // You have to set which MGRS grid it is from or it will reuse last projected + // grid + lanelet::GPSPoint projected_gps_point = projector.reverse(mgrs_point); + std::cout << projected_gps_point.lat << " " << projected_gps_point.lon << std::endl; + lanelet::GPSPoint projected_gps_point2 = projector.reverse(mgrs_point, mgrs_grid); + std::cout << projected_gps_point2.lat << " " << projected_gps_point2.lon << " " << std::endl; +} + +void usingAutowareTrafficLight(const std::string map_file_path) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet::projection::MGRSProjector projector; + lanelet_map = lanelet::load(map_file_path, "autoware_osm_handler", projector, &errors); + + for (auto lanelet : lanelet_map->laneletLayer) { + // You can access to traffic light element as AutowareTrafficLight class + auto autoware_traffic_lights = + lanelet.regulatoryElementsAs(); + for (auto light : autoware_traffic_lights) { + // You can access to the position of each lamps(light bulb) in traffic + // light + for (auto light_bulb_string : light->lightBulbs()) { + std::cout << light_bulb_string.id() << std::endl; + } + // Since AutowareTrafficLight class is inheriting lanelet::TrafficLight + // class, you can also access to outline of traffic light by the same + // method. + for (auto light_string : light->trafficLights()) { + std::cout << light_string.id() << std::endl; + } + } + + // You can also access to same traffic light element as default TrafficLight + // class + auto traffic_lights = lanelet.regulatoryElementsAs(); + for (auto light : traffic_lights) { + for (auto light_string : light->trafficLights()) { + std::cout << light_string.id() << std::endl; + } + } + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_code"); + const std::string map_file_path = node->declare_parameter("map_file", ""); + loadingAutowareOSMFile(map_file_path); + usingMGRSProjector(); + usingAutowareTrafficLight(map_file_path); + return 0; +} diff --git a/map/lanelet2_extension/src/validation.cpp b/map/lanelet2_extension/src/validation.cpp new file mode 100644 index 0000000000000..fdaadb663b458 --- /dev/null +++ b/map/lanelet2_extension/src/validation.cpp @@ -0,0 +1,174 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "lanelet2_extension/projection/mgrs_projector.hpp" +#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace +{ +namespace keyword +{ +constexpr const char * Id = "id"; +constexpr const char * Osm = "osm"; +constexpr const char * Tag = "tag"; +constexpr const char * Key = "k"; +constexpr const char * Node = "node"; +constexpr const char * Elevation = "ele"; +} // namespace keyword + +void printUsage() +{ + std::cout << "Usage:" << std::endl + << "ros2 run lanelet2_extension autoware_lanelet2_validation" + " --ros-args -p map_file:=" + << std::endl; +} +} // namespace + +void validateElevationTag(const std::string filename) +{ + pugi::xml_document doc; + auto result = doc.load_file(filename.c_str()); + if (!result) { + std::cerr << result.description() << std::endl; + exit(1); + } + + auto osmNode = doc.child(keyword::Osm); + for (auto node = osmNode.child(keyword::Node); node; // NOLINT + node = node.next_sibling(keyword::Node)) { + const auto id = node.attribute(keyword::Id).as_llong(lanelet::InvalId); + if (!node.find_child_by_attribute(keyword::Tag, keyword::Key, keyword::Elevation)) { + std::cerr << "failed to find elevation tag for node: " << id << std::endl; + } + } +} + +void validateTrafficLight(const lanelet::LaneletMapPtr lanelet_map) +{ + if (!lanelet_map) { + std::cerr << "Missing map. Are you sure you set correct path for map?" << std::endl; + exit(1); + } + + for (auto lanelet : lanelet_map->laneletLayer) { + auto autoware_traffic_lights = + lanelet.regulatoryElementsAs(); + if (autoware_traffic_lights.empty()) { + continue; + } + for (auto light : autoware_traffic_lights) { + if (light->lightBulbs().size() == 0) { + std::cerr << "regulatory element traffic light " << light->id() + << " is missing optional light_bulb member. You won't be able to use region_tlr " + "node with this map" + << std::endl; + } + for (auto light_string : light->lightBulbs()) { + if (!light_string.hasAttribute("traffic_light_id")) { + std::cerr << "light_bulb " << light_string.id() << " is missing traffic_light_id tag" + << std::endl; + } + } + for (auto base_string_or_poly : light->trafficLights()) { + if (!base_string_or_poly.isLineString()) { + std::cerr + << "traffic_light " << base_string_or_poly.id() + << " is polygon, and only linestring class is currently supported for traffic lights" + << std::endl; + } + auto base_string = static_cast(base_string_or_poly); + if (!base_string.hasAttribute("height")) { + std::cerr << "traffic_light " << base_string.id() << " is missing height tag" + << std::endl; + } + } + } + } +} + +void validateTurnDirection(const lanelet::LaneletMapPtr lanelet_map) +{ + if (!lanelet_map) { + std::cerr << "Missing map. Are you sure you set correct path for map?" << std::endl; + exit(1); + } + + lanelet::traffic_rules::TrafficRulesPtr traffic_rules = + lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + lanelet::routing::RoutingGraphPtr vehicle_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map, *traffic_rules); + + for (const auto & lanelet : lanelet_map->laneletLayer) { + if (!traffic_rules->canPass(lanelet)) { + continue; + } + + const auto conflicting_lanelets_or_areas = vehicle_graph->conflicting(lanelet); + if (conflicting_lanelets_or_areas.size() == 0) { + continue; + } + if (!lanelet.hasAttribute("turn_direction")) { + std::cerr + << "lanelet " << lanelet.id() + << " seems to be intersecting other lanelet, but does not have turn_direction tagging." + << std::endl; + } + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("autoware_lanelet_validation"); + + std::string map_path; + try { + map_path = node->declare_parameter("map_file"); + } catch (...) { + std::cerr << "failed find map_file parameter! No file to load" << std::endl; + printUsage(); + return 1; + } + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet::projection::MGRSProjector projector; + lanelet_map = lanelet::load(map_path, "autoware_osm_handler", projector, &errors); + + std::cout << "starting validation" << std::endl; + + validateElevationTag(map_path); + validateTrafficLight(lanelet_map); + validateTurnDirection(lanelet_map); + + std::cout << "finished validation" << std::endl; + + return 0; +} diff --git a/map/lanelet2_extension/test/src/test_message_conversion.cpp b/map/lanelet2_extension/test/src/test_message_conversion.cpp new file mode 100644 index 0000000000000..745f98c9f7fb0 --- /dev/null +++ b/map/lanelet2_extension/test/src/test_message_conversion.cpp @@ -0,0 +1,158 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "lanelet2_extension/utility/message_conversion.hpp" +#include "lanelet2_extension/utility/query.hpp" + +#include +#include + +using lanelet::Lanelet; +using lanelet::LineString3d; +using lanelet::Point3d; +using lanelet::utils::getId; +using lanelet::utils::conversion::toGeomMsgPt; + +class TestSuite : public ::testing::Test +{ +public: + TestSuite() : single_lanelet_map_ptr(new lanelet::LaneletMap()) + { + Point3d p1, p2, p3, p4, p5, p6, p7; + LineString3d traffic_light_base, traffic_light_bulbs, stop_line; + + p1 = Point3d(getId(), 0., 0., 0.); + p2 = Point3d(getId(), 0., 1., 0.); + + p3 = Point3d(getId(), 1., 0., 0.); + p4 = Point3d(getId(), 1., 1., 0.); + + LineString3d ls_left(getId(), {p1, p2}); // NOLINT + LineString3d ls_right(getId(), {p3, p4}); // NOLINT + + Lanelet lanelet(getId(), ls_left, ls_right); + + single_lanelet_map_ptr->add(lanelet); + } + ~TestSuite() {} + lanelet::LaneletMapPtr single_lanelet_map_ptr; + +private: +}; + +TEST_F(TestSuite, BinMsgConversion) +{ + autoware_auto_mapping_msgs::msg::HADMapBin bin_msg; + lanelet::LaneletMapPtr regenerated_map(new lanelet::LaneletMap); + + lanelet::utils::conversion::toBinMsg(single_lanelet_map_ptr, &bin_msg); + + ASSERT_NE(0U, bin_msg.data.size()) << "converted bin message does not have any data"; + + lanelet::utils::conversion::fromBinMsg(bin_msg, regenerated_map); + + auto original_lanelet = lanelet::utils::query::laneletLayer(single_lanelet_map_ptr); + auto regenerated_lanelet = lanelet::utils::query::laneletLayer(regenerated_map); + + ASSERT_EQ(original_lanelet.front().id(), regenerated_lanelet.front().id()) + << "regenerated map has different id"; +} + +TEST_F(TestSuite, ToGeomMsgPt) +{ + Point3d lanelet_pt(getId(), -0.1, 0.2, 3.0); + + geometry_msgs::msg::Point32 geom_pt32; + geom_pt32.x = -0.1; + geom_pt32.y = 0.2; + geom_pt32.z = 3.0; + + geometry_msgs::msg::Point geom_pt; + toGeomMsgPt(geom_pt32, &geom_pt); + ASSERT_FLOAT_EQ(geom_pt32.x, geom_pt.x) + << " converted value is different from original geometry_msgs::msg::Point"; + ASSERT_FLOAT_EQ(geom_pt32.y, geom_pt.y) + << " converted value is different from original geometry_msgs::msg::Point"; + ASSERT_FLOAT_EQ(geom_pt32.z, geom_pt.z) + << " converted value is different from original geometry_msgs::msg::Point"; + + geom_pt = toGeomMsgPt(geom_pt32); + ASSERT_FLOAT_EQ(geom_pt32.x, geom_pt.x) + << " converted value is different from original geometry_msgs::msg::Point"; + ASSERT_FLOAT_EQ(geom_pt32.y, geom_pt.y) + << " converted value is different from original geometry_msgs::msg::Point"; + ASSERT_FLOAT_EQ(geom_pt32.z, geom_pt.z) + << " converted value is different from original geometry_msgs::msg::Point"; + + toGeomMsgPt(lanelet_pt.basicPoint(), &geom_pt); + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().x(), geom_pt.x) + << " converted value is different from original " + "lanelet::basicPoint"; + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().y(), geom_pt.y) + << " converted value is different from original " + "lanelet::basicPoint"; + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().z(), geom_pt.z) + << " converted value is different from original " + "lanelet::basicPoint"; + + geom_pt = toGeomMsgPt(lanelet_pt.basicPoint()); + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().x(), geom_pt.x) + << " converted value is different from original " + "lanelet::basicPoint"; + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().y(), geom_pt.y) + << " converted value is different from original " + "lanelet::basicPoint"; + ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().z(), geom_pt.z) + << " converted value is different from original " + "lanelet::basicPoint"; + + toGeomMsgPt(lanelet_pt, &geom_pt); + ASSERT_DOUBLE_EQ(lanelet_pt.x(), geom_pt.x) + << " converted value is different from original lanelet::Point3d"; + ASSERT_DOUBLE_EQ(lanelet_pt.y(), geom_pt.y) + << " converted value is different from original lanelet::Point3d"; + ASSERT_DOUBLE_EQ(lanelet_pt.z(), geom_pt.z) + << " converted value is different from original lanelet::Point3d"; + + geom_pt = toGeomMsgPt(lanelet_pt); + ASSERT_DOUBLE_EQ(lanelet_pt.x(), geom_pt.x) + << " converted value is different from original lanelet::Point3d"; + ASSERT_DOUBLE_EQ(lanelet_pt.y(), geom_pt.y) + << " converted value is different from original lanelet::Point3d"; + ASSERT_DOUBLE_EQ(lanelet_pt.z(), geom_pt.z) + << " converted value is different from original lanelet::Point3d"; + + lanelet::ConstPoint2d point_2d = lanelet::utils::to2D(lanelet_pt); + + toGeomMsgPt(point_2d, &geom_pt); + ASSERT_DOUBLE_EQ(point_2d.x(), geom_pt.x) + << " converted value is different from original lanelet::Point2d"; + ASSERT_DOUBLE_EQ(point_2d.y(), geom_pt.y) + << " converted value is different from original lanelet::Point2d"; + ASSERT_DOUBLE_EQ(0.0, geom_pt.z) + << " converted value is different from original lanelet::Point2d"; + + geom_pt = toGeomMsgPt(point_2d); + ASSERT_DOUBLE_EQ(point_2d.x(), geom_pt.x) + << " converted value is different from original lanelet::Point2d"; + ASSERT_DOUBLE_EQ(point_2d.y(), geom_pt.y) + << " converted value is different from original lanelet::Point2d"; + ASSERT_DOUBLE_EQ(0.0, geom_pt.z) + << " converted value is different from original lanelet::Point2d"; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/lanelet2_extension/test/src/test_projector.cpp b/map/lanelet2_extension/test/src/test_projector.cpp new file mode 100644 index 0000000000000..65304df250711 --- /dev/null +++ b/map/lanelet2_extension/test/src/test_projector.cpp @@ -0,0 +1,82 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "lanelet2_extension/projection/mgrs_projector.hpp" + +#include +#include + +class TestSuite : public ::testing::Test +{ +public: + TestSuite() {} + ~TestSuite() {} +}; + +TEST(TestSuite, ForwardProjection) +{ + lanelet::projection::MGRSProjector projector; + // lat/lon in Tokyo + lanelet::GPSPoint gps_point; + gps_point.lat = 35.652832; + gps_point.lon = 139.839478; + gps_point.ele = 12.3456789; + lanelet::BasicPoint3d mgrs_point = projector.forward(gps_point); + + // projected z value should not change + ASSERT_DOUBLE_EQ(mgrs_point.z(), gps_point.ele) + << "Forward projected z value should be " << gps_point.ele; + + // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html + // round the projected value to mm since the above reference only gives value + // in mm precision + ASSERT_EQ(projector.getProjectedMGRSGrid(), "54SUE") << "Projected grid should be " + << "54SUE"; + double rounded_x_mm = round(mgrs_point.x() * 1000) / 1000.0; + ASSERT_DOUBLE_EQ(rounded_x_mm, 94946.081) << "Forward projected x value should be " << 94946.081; + double rounded_y_mm = round(mgrs_point.y() * 1000) / 1000.0; + ASSERT_DOUBLE_EQ(rounded_y_mm, 46063.748) << "Forward projected y value should be " << 46063.748; +} + +TEST(TestSuite, ReverseProjection) +{ + lanelet::projection::MGRSProjector projector; + lanelet::BasicPoint3d mgrs_point; + mgrs_point.x() = 94946.0; + mgrs_point.y() = 46063.0; + mgrs_point.z() = 12.3456789; + + projector.setMGRSCode("54SUE"); + lanelet::GPSPoint gps_point = projector.reverse(mgrs_point); + + // projected z value should not change + ASSERT_DOUBLE_EQ(gps_point.ele, mgrs_point.z()) + << "Reverse projected z value should be " << mgrs_point.z(); + + // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html + // round the projected value since the above reference only gives value up to + // precision of 1e-8 + double rounded_lat = round(gps_point.lat * 1e8) / 1e8; + ASSERT_DOUBLE_EQ(rounded_lat, 35.65282525) + << "Reverse projected latitude value should be " << 35.65282525; + double rounded_lon = round(gps_point.lon * 1e8) / 1e8; + ASSERT_DOUBLE_EQ(rounded_lon, 139.83947721) + << "Reverse projected longitude value should be " << 139.83947721; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/lanelet2_extension/test/src/test_query.cpp b/map/lanelet2_extension/test/src/test_query.cpp new file mode 100644 index 0000000000000..ef65772d9503e --- /dev/null +++ b/map/lanelet2_extension/test/src/test_query.cpp @@ -0,0 +1,131 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "lanelet2_extension/utility/query.hpp" + +#include +#include + +using lanelet::Lanelet; +using lanelet::LineString3d; +using lanelet::LineStringOrPolygon3d; +using lanelet::Point3d; +using lanelet::Points3d; +using lanelet::utils::getId; + +class TestSuite : public ::testing::Test +{ +public: + TestSuite() : sample_map_ptr(new lanelet::LaneletMap()) + { // NOLINT + // create sample lanelets + Point3d p1, p2, p3, p4; + + p1 = Point3d(getId(), 0., 0., 0.); + p2 = Point3d(getId(), 0., 1., 0.); + + p3 = Point3d(getId(), 1., 0., 0.); + p4 = Point3d(getId(), 1., 1., 0.); + + LineString3d ls_left(getId(), {p1, p2}); // NOLINT + LineString3d ls_right(getId(), {p3, p4}); // NOLINT + + Lanelet road_lanelet(getId(), ls_left, ls_right); + road_lanelet.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + Lanelet crosswalk_lanelet(getId(), ls_left, ls_right); + crosswalk_lanelet.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Crosswalk; + + // create sample traffic light + Point3d p5, p6, p7, p8, p9, p10, p11, p12; + LineString3d traffic_light_base, traffic_light_bulbs, stop_line; + + p6 = Point3d(getId(), 0., 1., 4.); + p7 = Point3d(getId(), 1., 1., 4.); + + p8 = Point3d(getId(), 0., 1., 4.5); + p9 = Point3d(getId(), 0.5, 1., 4.5); + p10 = Point3d(getId(), 1., 1., 4.5); + + p11 = Point3d(getId(), 0., 0., 0.); + p12 = Point3d(getId(), 1., 0., 0.); + + traffic_light_base = LineString3d(getId(), Points3d{p6, p7}); // NOLINT + traffic_light_bulbs = LineString3d(getId(), Points3d{p8, p9, p10}); // NOLINT + stop_line = LineString3d(getId(), Points3d{p11, p12}); // NOLINT + + auto tl = lanelet::autoware::AutowareTrafficLight::make( + getId(), lanelet::AttributeMap(), {traffic_light_base}, stop_line, + {traffic_light_bulbs}); // NOLINT + + road_lanelet.addRegulatoryElement(tl); + + // add items to map + sample_map_ptr->add(road_lanelet); + sample_map_ptr->add(crosswalk_lanelet); + } + ~TestSuite() {} + + lanelet::LaneletMapPtr sample_map_ptr; + +private: +}; + +TEST_F(TestSuite, QueryLanelets) +{ + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr); + ASSERT_EQ(2U, all_lanelets.size()) << "failed to retrieve all lanelets"; + + lanelet::ConstLanelets subtype_lanelets = + lanelet::utils::query::subtypeLanelets(all_lanelets, lanelet::AttributeValueString::Road); + ASSERT_EQ(1U, subtype_lanelets.size()) << "failed to retrieve road lanelet by subtypeLanelets"; + + lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + ASSERT_EQ(1U, road_lanelets.size()) << "failed to retrieve road lanelets"; + + lanelet::ConstLanelets crosswalk_lanelets = + lanelet::utils::query::crosswalkLanelets(all_lanelets); + ASSERT_EQ(1U, crosswalk_lanelets.size()) << "failed to retrieve crosswalk lanelets"; +} + +TEST_F(TestSuite, QueryTrafficLights) +{ + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr); + + auto traffic_lights = lanelet::utils::query::trafficLights(all_lanelets); + ASSERT_EQ(1U, traffic_lights.size()) << "failed to retrieve traffic lights"; + + auto autoware_traffic_lights = lanelet::utils::query::autowareTrafficLights(all_lanelets); + ASSERT_EQ(1U, autoware_traffic_lights.size()) << "failed to retrieve autoware traffic lights"; +} + +TEST_F(TestSuite, QueryStopLine) +{ + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr); + lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + + auto stop_lines = lanelet::utils::query::stopLinesLanelets(all_lanelets); + ASSERT_EQ(1U, stop_lines.size()) << "failed to retrieve stop lines from all lanelets"; + + auto stop_lines2 = lanelet::utils::query::stopLinesLanelet(road_lanelets.front()); + ASSERT_EQ(1U, stop_lines2.size()) << "failed to retrieve stop lines from a lanelet"; +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/lanelet2_extension/test/src/test_regulatory_elements.cpp b/map/lanelet2_extension/test/src/test_regulatory_elements.cpp new file mode 100644 index 0000000000000..5512d7e0814ee --- /dev/null +++ b/map/lanelet2_extension/test/src/test_regulatory_elements.cpp @@ -0,0 +1,124 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp" + +#include +#include +#include +#include + +#include + +using lanelet::LineString3d; +using lanelet::LineStringOrPolygon3d; +using lanelet::Point3d; +using lanelet::Points3d; +using lanelet::utils::getId; + +namespace +{ +template +std::vector convertToVector(T item) +{ + std::vector vector = {item}; + return vector; +} +} // namespace + +class TestSuite : public ::testing::Test +{ +public: + TestSuite() {} + ~TestSuite() {} +}; + +TEST(TestSuite, FactoryConstructsTrafficLight) +{ + Point3d p1, p2, p3, p4, p5, p6, p7; + LineStringOrPolygon3d traffic_light_base; + LineString3d traffic_light_bulbs, stop_line; + + p1 = Point3d(getId(), 0., 1., 4.); + p2 = Point3d(getId(), 1., 1., 4.); + + p3 = Point3d(getId(), 0., 1., 4.5); + p4 = Point3d(getId(), 0.5, 1., 4.5); + p5 = Point3d(getId(), 1., 1., 4.5); + + p6 = Point3d(getId(), 0., 0., 0.); + p7 = Point3d(getId(), 1., 0., 0.); + + Points3d base = {p1, p2}; + Points3d bulbs = {p3, p4, p5}; + Points3d stop = {p6, p7}; + + traffic_light_base = LineString3d(getId(), base); + traffic_light_bulbs = LineString3d(getId(), bulbs); + stop_line = LineString3d(getId(), stop); + + auto tl = lanelet::autoware::AutowareTrafficLight::make( + getId(), lanelet::AttributeMap(), convertToVector(traffic_light_base), stop_line, + convertToVector(traffic_light_bulbs)); + + auto factoryTl = lanelet::RegulatoryElementFactory::create( + tl->attribute(lanelet::AttributeName::Subtype).value(), + std::const_pointer_cast(tl->constData())); + EXPECT_TRUE(!!std::dynamic_pointer_cast(factoryTl)); +} + +TEST(TestSuite, TrafficLightWorksAsExpected) +{ // NOLINT + Point3d p1, p2, p3, p4, p5, p6, p7; + LineStringOrPolygon3d traffic_light_base, traffic_light_base2; + LineString3d traffic_light_bulbs, traffic_light_bulbs2, stop_line; + + p1 = Point3d(getId(), 0., 1., 4.); + p2 = Point3d(getId(), 1., 1., 4.); + + p3 = Point3d(getId(), 0., 1., 4.5); + p4 = Point3d(getId(), 0.5, 1., 4.5); + p5 = Point3d(getId(), 1., 1., 4.5); + + p6 = Point3d(getId(), 0., 0., 0.); + p7 = Point3d(getId(), 1., 0., 0.); + + Points3d base = {p1, p2}; + Points3d bulbs = {p3, p4, p5}; + Points3d stop = {p6, p7}; + + traffic_light_base = {LineString3d(getId(), base)}; + traffic_light_base2 = {LineString3d(getId(), base)}; + traffic_light_bulbs = {LineString3d(getId(), bulbs)}; + traffic_light_bulbs2 = {LineString3d(getId(), bulbs)}; + stop_line = LineString3d(getId(), stop); + + auto tl = lanelet::autoware::AutowareTrafficLight::make( + getId(), lanelet::AttributeMap(), convertToVector(traffic_light_base), stop_line, + convertToVector(traffic_light_bulbs)); + tl->setStopLine(stop_line); + EXPECT_EQ(stop_line, tl->stopLine()); + tl->addTrafficLight(traffic_light_base2); + EXPECT_EQ(2ul, tl->trafficLights().size()); + tl->addLightBulbs(traffic_light_bulbs2); + EXPECT_EQ(2ul, tl->lightBulbs().size()); + tl->removeLightBulbs(traffic_light_bulbs); + EXPECT_EQ(1ul, tl->lightBulbs().size()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/lanelet2_extension/test/src/test_utilities.cpp b/map/lanelet2_extension/test/src/test_utilities.cpp new file mode 100644 index 0000000000000..966b093c0a608 --- /dev/null +++ b/map/lanelet2_extension/test/src/test_utilities.cpp @@ -0,0 +1,109 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "lanelet2_extension/utility/utilities.hpp" + +#include +#include +#include + +#include + +using lanelet::Lanelet; +using lanelet::LineString3d; +using lanelet::Point3d; +using lanelet::utils::getId; + +class TestSuite : public ::testing::Test +{ +public: + TestSuite() : sample_map_ptr(new lanelet::LaneletMap()) + { // NOLINT + // create sample lanelets + Point3d p1, p2, p3, p4, p5, p6, p7, p8, p9, p10; + + p1 = Point3d(getId(), 0., 0., 0.); + p2 = Point3d(getId(), 0., 1., 0.); + p3 = Point3d(getId(), 1., 0., 0.); + p4 = Point3d(getId(), 1., 1., 0.); + + LineString3d ls_left(getId(), {p1, p2}); // NOLINT + LineString3d ls_right(getId(), {p3, p4}); // NOLINT + + p5 = Point3d(getId(), 0., 2., 0.); + p6 = Point3d(getId(), 1., 2., 0.); + + LineString3d ls_left2(getId(), {p2, p5}); // NOLINT + LineString3d ls_right2(getId(), {p4, p6}); // NOLINT + + p7 = Point3d(getId(), 0., 3., 0.); + p8 = Point3d(getId(), 1., 3., 0.); + + LineString3d ls_left3(getId(), {p5, p7}); // NOLINT + LineString3d ls_right3(getId(), {p6, p8}); // NOLINT + + p9 = Point3d(getId(), 0., 1., 0.); + p10 = Point3d(getId(), 1., 1., 0.); + + LineString3d ls_left4(getId(), {p9, p5}); // NOLINT + LineString3d ls_right4(getId(), {p10, p6}); // NOLINT + + road_lanelet = Lanelet(getId(), ls_left, ls_right); + road_lanelet.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + next_lanelet = Lanelet(getId(), ls_left2, ls_right2); + next_lanelet.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + next_lanelet2 = Lanelet(getId(), ls_left3, ls_right3); + next_lanelet2.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + merging_lanelet = Lanelet(getId(), ls_left4, ls_right4); + merging_lanelet.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + sample_map_ptr->add(road_lanelet); + sample_map_ptr->add(next_lanelet); + sample_map_ptr->add(next_lanelet2); + sample_map_ptr->add(merging_lanelet); + } + ~TestSuite() {} + + lanelet::LaneletMapPtr sample_map_ptr; + Lanelet road_lanelet; + Lanelet next_lanelet; + Lanelet next_lanelet2; + Lanelet merging_lanelet; + +private: +}; + +TEST_F(TestSuite, OverwriteLaneletsCenterline) +{ + double resolution = 5.0; + bool force_overwrite = false; + lanelet::utils::overwriteLaneletsCenterline(sample_map_ptr, resolution, force_overwrite); + + for (const auto & lanelet : sample_map_ptr->laneletLayer) { + ASSERT_TRUE(lanelet.hasCustomCenterline()) << "failed to calculate fine centerline"; + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt new file mode 100644 index 0000000000000..a64d6a9ee2606 --- /dev/null +++ b/map/map_loader/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.5) +project(map_loader) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(PCL REQUIRED COMPONENTS io) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(pointcloud_map_loader_node SHARED + src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +) +target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) + +rclcpp_components_register_node(pointcloud_map_loader_node + PLUGIN "PointCloudMapLoaderNode" + EXECUTABLE pointcloud_map_loader +) + +ament_auto_add_library(elevation_map_loader_node SHARED + src/elevation_map_loader/elevation_map_loader_node.cpp +) +target_link_libraries(elevation_map_loader_node ${PCL_LIBRARIES}) + +rclcpp_components_register_node(elevation_map_loader_node + PLUGIN "ElevationMapLoaderNode" + EXECUTABLE elevation_map_loader +) + +ament_auto_add_library(lanelet2_map_loader_node SHARED + src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +) + +rclcpp_components_register_node(lanelet2_map_loader_node + PLUGIN "Lanelet2MapLoaderNode" + EXECUTABLE lanelet2_map_loader +) + +ament_auto_add_library(lanelet2_map_visualization_node SHARED + src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +) + +rclcpp_components_register_node(lanelet2_map_visualization_node + PLUGIN "Lanelet2MapVisualizationNode" + EXECUTABLE lanelet2_map_visualization +) + +install(PROGRAMS + script/map_hash_generator + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config + data +) diff --git a/map/map_loader/README.md b/map/map_loader/README.md new file mode 100644 index 0000000000000..a07b7d35cbf2e --- /dev/null +++ b/map/map_loader/README.md @@ -0,0 +1,165 @@ +# map_loader package + +This package provides the features of loading various maps. + +## pointcloud_map_loader + +### Feature + +pointcloud_map_loader loads PointCloud file and publishes the map data as sensor_msgs/PointCloud2 message. + +### How to run + +`ros2 run map_loader pointcloud_map_loader --ros-args -p "pcd_paths_or_directory:=[path/to/pointcloud1.pcd, path/to/pointcloud2.pcd, ...]"` + +### Published Topics + +- pointcloud_map (sensor_msgs/PointCloud2) : PointCloud Map + +--- + +## lanelet2_map_loader + +### Feature + +lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_lanelet2_msgs/MapBin message. +The node projects lan/lon coordinates into MGRS coordinates. + +### How to run + +`ros2 run map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm` + +### Published Topics + +- ~output/lanelet2_map (autoware_lanelet2_msgs/MapBin) : Binary data of loaded Lanelet2 Map + +--- + +## lanelet2_map_visualization + +### Feature + +lanelet2_map_visualization visualizes autoware_lanelet2_msgs/MapBin messages into visualization_msgs/MarkerArray. + +### How to Run + +`ros2 run map_loader lanelet2_map_visualization` + +### Subscribed Topics + +- ~input/lanelet2_map (autoware_lanelet2_msgs/MapBin) : binary data of Lanelet2 Map + +### Published Topics + +- ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz + +--- + +## elevation_map_loader + +### Feature + +Generate elevation_map from subscribed pointcloud_map and vector_map and publish it. +Save the generated elevation_map locally and load it from next time. + +The elevation value of each cell is the average value of z of the points of the lowest cluster. +Cells with No elevation value can be inpainted using the values of neighboring cells. + +

+ +

+ +### How to run + +`ros2 run map_loader elevation_map_loader --ros-args -p param_file_path:=path/to/elevation_map_parameters.yaml -p elevation_map_directory:=path/to/elevation_map_directory -p pointcloud_map_path:=path/to/pointcloud.pcd` + +### Subscribed Topics + +- input/pointcloud_map (sensor_msgs:PointCloud2) : PointCloud Map +- input/vector_map (autoware_lanelet2_msgs/MapBin) : binary data of Lanelet2 Map + +### Published Topics + +- output/elevation_map (grid_map_msgs/GridMap) : Elevation Map +- output/elevation_map_cloud (sensor_msgs:PointCloud2) : Pointcloud generated from the value of Elevation Map + +### Parameter description + +#### ROS parameters + +| Name | Type | Description | Default value | +| :-------------------------------- | :---------- | :--------------------------------------------------------------------------------------------------------- | :------------ | +| map_layer_name | std::string | elevation_map layer name | elevation | +| param_file_path | std::string | GridMap parameters config | path_default | +| elevation_map_file_path | std::string | elevation_map file (bag2) | path_default | +| map_frame | std::string | map_frame when loading elevation_map file | map | +| use_inpaint | bool | Whether to inpaint empty cells | true | +| inpaint_radius | float | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m] | 0.3 | +| use_elevation_map_cloud_publisher | bool | Whether to publish `output/elevation_map_cloud` | false | +| use_lane_filter | bool | Whether to filter elevation_map with vector_map | false | +| lane_margin | float | Value of how much to expand the range of vector_map [m] | 0.5 | +| lane_height_diff_thresh | float | Only point clouds in the height range of this value from vector_map are used to generate elevation_map [m] | 1.0 | +| lane_filter_voxel_size_x | float | Voxel size x for calculating point clouds in vector_map [m] | 0.04 | +| lane_filter_voxel_size_y | float | Voxel size y for calculating point clouds in vector_map [m] | 0.04 | +| lane_filter_voxel_size_z | float | Voxel size z for calculating point clouds in vector_map [m] | 0.04 | + +#### GridMap parameters + +The parameters are described on `config/elevation_map_parameters.yaml`. + +##### General parameters + +| Name | Type | Description | Default value | +| :--------------------------------------------- | :--- | :----------------------------------------------------------------------------------------------------------- | :------------ | +| pcl_grid_map_extraction/num_processing_threads | int | Number of threads for processing grid map cells. Filtering of the raw input point cloud is not parallelized. | 12 | + +##### Grid map parameters + +See: + +Resulting grid map parameters. + +| Name | Type | Description | Default value | +| :------------------------------------------------------- | :---- | :----------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pcl_grid_map_extraction/grid_map/min_num_points_per_cell | int | Minimum number of points in the point cloud that have to fall within any of the grid map cells. Otherwise the cell elevation will be set to NaN. | 3 | +| pcl_grid_map_extraction/grid_map/resolution | float | Resolution of the grid map. Width and length are computed automatically. | 0.3 | + +#### Point Cloud Pre-processing Parameters + +##### Rigid body transform parameters + +Rigid body transform that is applied to the point cloud before computing elevation. + +| Name | Type | Description | Default value | +| :-------------------------------------------------- | :---- | :---------------------------------------------------------------------------------------------------------------------- | :------------ | +| pcl_grid_map_extraction/cloud_transform/translation | float | Translation (xyz) that is applied to the input point cloud before computing elevation. | 0.0 | +| pcl_grid_map_extraction/cloud_transform/rotation | float | Rotation (intrinsic rotation, convention X-Y'-Z'') that is applied to the input point cloud before computing elevation. | 0.0 | + +##### Cluster extraction parameters + +Cluster extraction is based on pcl algorithms. See for more details. + +| Name | Type | Description | Default value | +| :----------------------------------------------------------- | :---- | :------------------------------------------------------------------------------------- | :------------ | +| pcl_grid_map_extraction/cluster_extraction/cluster_tolerance | float | Distance between points below which they will still be considered part of one cluster. | 0.2 | +| pcl_grid_map_extraction/cluster_extraction/min_num_points | int | Min number of points that a cluster needs to have (otherwise it will be discarded). | 3 | +| pcl_grid_map_extraction/cluster_extraction/max_num_points | int | Max number of points that a cluster can have (otherwise it will be discarded). | 1000000 | + +##### Outlier removal parameters + +See for more explanation on outlier removal. + +| Name | Type | Description | Default value | +| :--------------------------------------------------------- | :---- | :----------------------------------------------------------------------------- | :------------ | +| pcl_grid_map_extraction/outlier_removal/is_remove_outliers | float | Whether to perform statistical outlier removal. | false | +| pcl_grid_map_extraction/outlier_removal/mean_K | float | Number of neighbours to analyze for estimating statistics of a point. | 10 | +| pcl_grid_map_extraction/outlier_removal/stddev_threshold | float | Number of standard deviations under which points are considered to be inliers. | 1.0 | + +##### Subsampling parameters + +See for more explanation on point cloud downsampling. + +| Name | Type | Description | Default value | +| :------------------------------------------------------- | :---- | :-------------------------------------- | :------------ | +| pcl_grid_map_extraction/downsampling/is_downsample_cloud | bool | Whether to perform downsampling or not. | false | +| pcl_grid_map_extraction/downsampling/voxel_size | float | Voxel sizes (xyz) in meters. | 0.02 | diff --git a/map/map_loader/config/elevation_map_parameters.yaml b/map/map_loader/config/elevation_map_parameters.yaml new file mode 100644 index 0000000000000..51edc3ffd9f16 --- /dev/null +++ b/map/map_loader/config/elevation_map_parameters.yaml @@ -0,0 +1,28 @@ +pcl_grid_map_extraction: + num_processing_threads: 12 + cloud_transform: + translation: + x: 0.0 + y: 0.0 + z: 0.0 + rotation: #intrinsic rotation X-Y-Z (r-p-y)sequence + r: 0.0 + p: 0.0 + y: 0.0 + cluster_extraction: + cluster_tolerance: 0.2 + min_num_points: 3 + max_num_points: 1000000 + outlier_removal: + is_remove_outliers: false + mean_K: 10 + stddev_threshold: 1.0 + downsampling: + is_downsample_cloud: false + voxel_size: + x: 0.02 + y: 0.02 + z: 0.02 + grid_map: + min_num_points_per_cell: 3 + resolution: 0.3 diff --git a/map/map_loader/data/elevation_maps/.empty b/map/map_loader/data/elevation_maps/.empty new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/map/map_loader/include/map_loader/elevation_map_loader_node.hpp b/map/map_loader/include/map_loader/elevation_map_loader_node.hpp new file mode 100644 index 0000000000000..bc1e34fe81136 --- /dev/null +++ b/map/map_loader/include/map_loader/elevation_map_loader_node.hpp @@ -0,0 +1,96 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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. + +#ifndef MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ +#define MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +class ElevationMapLoaderNode : public rclcpp::Node +{ +public: + explicit ElevationMapLoaderNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr sub_pointcloud_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Publisher::SharedPtr pub_elevation_map_; + rclcpp::Publisher::SharedPtr pub_elevation_map_cloud_; + void onPointcloudMap(const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map); + void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr vector_map); + void setVerbosityLevelToDebugIfFlagSet(); + void createElevationMapFromPointcloud(); + autoware_utils::LinearRing2d getConvexHull( + const pcl::PointCloud::Ptr & input_cloud); + lanelet::ConstLanelets getIntersectedLanelets( + const autoware_utils::LinearRing2d & convex_hull, + const lanelet::ConstLanelets & road_lanelets_); + pcl::PointCloud::Ptr getLaneFilteredPointCloud( + const lanelet::ConstLanelets & joint_lanelets, + const pcl::PointCloud::Ptr & cloud); + bool checkPointWithinLanelets( + const pcl::PointXYZ & point, const lanelet::ConstLanelets & joint_lanelets); + void publishElevationMap(); + void inpaintElevationMap(const float radius); + pcl::PointCloud::Ptr createPointcloudFromElevationMap(); + void saveElevationMap(); + float calculateDistancePointFromPlane( + const pcl::PointXYZ & point, const lanelet::ConstLanelet & lanelet); + + pcl::PointCloud::Ptr map_pcl_ptr_; + grid_map::GridMap elevation_map_; + std::string layer_name_; + std::filesystem::path elevation_map_path_; + nlohmann::json hash_json_; + std::string map_frame_; + bool use_inpaint_; + float inpaint_radius_; + bool already_sub_vector_map_; + bool already_sub_pointcloud_map_; + bool use_elevation_map_file_; + bool use_elevation_map_cloud_publisher_; + pcl::shared_ptr grid_map_pcl_loader_; + + struct LaneFilter + { + float voxel_size_x_; + float voxel_size_y_; + float voxel_size_z_; + float lane_margin_; + float lane_height_diff_thresh_; + lanelet::ConstLanelets road_lanelets_; + bool use_lane_filter_; + }; + LaneFilter lane_filter_; +}; + +#endif // MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp new file mode 100644 index 0000000000000..a7cdfbeebe1d4 --- /dev/null +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -0,0 +1,33 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +#ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ + +#include + +#include + +#include + +class Lanelet2MapLoaderNode : public rclcpp::Node +{ +public: + explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::Publisher::SharedPtr pub_map_bin_; +}; + +#endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp new file mode 100644 index 0000000000000..e5a5fe3e3a6fa --- /dev/null +++ b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -0,0 +1,40 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +#ifndef MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ +#define MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ + +#include + +#include +#include + +#include +#include + +class Lanelet2MapVisualizationNode : public rclcpp::Node +{ +public: + explicit Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr sub_map_bin_; + rclcpp::Publisher::SharedPtr pub_marker_; + + bool viz_lanelets_centerline_; + + void onMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); +}; + +#endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp new file mode 100644 index 0000000000000..6bc55ef642706 --- /dev/null +++ b/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp @@ -0,0 +1,52 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +#ifndef MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#define MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ + +#include + +#include + +#include +#include + +class PointCloudMapLoaderNode : public rclcpp::Node +{ +public: + explicit PointCloudMapLoaderNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::Publisher::SharedPtr pub_pointcloud_map_; + + sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector & pcd_paths); +}; + +#endif // MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/launch/elevation_map_loader.launch.xml b/map/map_loader/launch/elevation_map_loader.launch.xml new file mode 100644 index 0000000000000..09c73cfcc4ff2 --- /dev/null +++ b/map/map_loader/launch/elevation_map_loader.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml new file mode 100644 index 0000000000000..7abc664bdc3d5 --- /dev/null +++ b/map/map_loader/launch/lanelet2_map_loader.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml new file mode 100644 index 0000000000000..4f10dd21d87e8 --- /dev/null +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/map/map_loader/media/elevation_map.png b/map/map_loader/media/elevation_map.png new file mode 100644 index 0000000000000..4791ce8fbb55f Binary files /dev/null and b/map/map_loader/media/elevation_map.png differ diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml new file mode 100644 index 0000000000000..750243a27cca0 --- /dev/null +++ b/map/map_loader/package.xml @@ -0,0 +1,37 @@ + + + map_loader + 0.1.0 + The map_loader package + mitsudome-r + Kenji Miyake + Apache License 2.0 + + ament_cmake + ament_cmake_auto + + autoware_auto_mapping_msgs + autoware_utils + geometry_msgs + grid_map_cv + grid_map_pcl + grid_map_ros + hash_library_vendor + lanelet2_extension + libpcl-all-dev + nlohmann-json-dev + pcl_conversions + rclcpp + rclcpp_components + std_msgs + tf2_geometry_msgs + tf2_ros + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/map/map_loader/script/map_hash_generator b/map/map_loader/script/map_hash_generator new file mode 100755 index 0000000000000..be467d23847eb --- /dev/null +++ b/map/map_loader/script/map_hash_generator @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 + +# Copyright 2021 Tier IV, Inc. +# +# 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. + +import hashlib +import pathlib + +from autoware_external_api_msgs.msg import MapHash +from autoware_external_api_msgs.msg import ResponseStatus +from autoware_external_api_msgs.srv import GetTextFile +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile + + +class MapHashGenerator(Node): + def __init__(self): + super().__init__("map_hash_generator") + self.lanelet_path = self.declare_parameter("lanelet2_map_path", "").value + self.lanelet_text = self.load_lanelet_text(self.lanelet_path) + self.lanelet_hash = self.generate_file_hash(self.lanelet_text) + + qos_profile = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + msg = MapHash() + msg.lanelet = self.lanelet_hash + self.pub = self.create_publisher(MapHash, "/api/autoware/get/map/info/hash", qos_profile) + self.pub.publish(msg) + + self.srv = self.create_service( + GetTextFile, "/api/autoware/get/map/lanelet/xml", self.on_get_lanelet_xml + ) # noqa: E501 + + def on_get_lanelet_xml(self, request, response): + response.status.code = ResponseStatus.SUCCESS + response.file.text = self.lanelet_text + return response + + @staticmethod + def load_lanelet_text(path): + path = pathlib.Path(path) + return path.read_text() if path.is_file() else "" + + @staticmethod + def generate_file_hash(data): + return hashlib.sha256(data.encode()).hexdigest() if data else "" + + +def main(args=None): + rclpy.init(args=args) + node = MapHashGenerator() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + pass diff --git a/map/map_loader/src/elevation_map_loader/elevation_map_loader_node.cpp b/map/map_loader/src/elevation_map_loader/elevation_map_loader_node.cpp new file mode 100644 index 0000000000000..106d12c502c52 --- /dev/null +++ b/map/map_loader/src/elevation_map_loader/elevation_map_loader_node.cpp @@ -0,0 +1,453 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "map_loader/elevation_map_loader_node.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +namespace +{ +bool isPcdFile(const std::string & p) +{ + if (!std::filesystem::is_regular_file(std::filesystem::status(p))) { + return false; + } + + const auto ext = p.substr(p.find_last_of(".") + 1); + + if (ext != "pcd" && ext != "PCD") { + return false; + } + + return true; +} + +std::string getMd5Sum(const std::string & data) +{ + MD5 digest_md5; + digest_md5.add(data.c_str(), data.size()); + return digest_md5.getHash(); +} + +// reference https://create.stephan-brumme.com/hash-library/ +std::string getMd5Sum(const std::filesystem::path & file_path) +{ + MD5 digest_md5; + const size_t buffer_size = 1'000'000; + auto buffer = std::array(); + std::ifstream file(file_path, std::ios::in | std::ios::binary); + + while (file) { + file.read(buffer.begin(), buffer_size); + const auto num_bytes_read = static_cast(file.gcount()); + digest_md5.add(buffer.begin(), num_bytes_read); + } + + return digest_md5.getHash(); +} + +nlohmann::json getPcdMapHashJson(const std::string & file_path) +{ + // Get PCD file paths + std::vector pcd_file_paths; + if (std::filesystem::is_directory(file_path)) { + for (const auto & file : std::filesystem::directory_iterator(file_path)) { + if (!isPcdFile(file.path())) { + continue; + } + pcd_file_paths.push_back(file.path()); + } + } else if (isPcdFile(file_path)) { + pcd_file_paths.push_back(file_path); + } + + // Create JSON meta file + nlohmann::json j; + for (const auto & file_path : pcd_file_paths) { + j[file_path.string()] = getMd5Sum(file_path); + } + + return j; +} +} // namespace + +ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & options) +: Node("elevation_map_loader", options) +{ + layer_name_ = this->declare_parameter("map_layer_name", std::string("elevation")); + std::string param_file_path = this->declare_parameter("param_file_path", "path_default"); + map_frame_ = this->declare_parameter("map_frame", "map"); + use_inpaint_ = this->declare_parameter("use_inpaint", true); + inpaint_radius_ = this->declare_parameter("inpaint_radius", 0.3); + use_elevation_map_cloud_publisher_ = + this->declare_parameter("use_elevation_map_cloud_publisher", false); + + lane_filter_.use_lane_filter_ = this->declare_parameter("use_lane_filter", false); + lane_filter_.lane_margin_ = this->declare_parameter("lane_margin", 0.5); + lane_filter_.lane_height_diff_thresh_ = this->declare_parameter("lane_height_diff_thresh", 1.0); + lane_filter_.voxel_size_x_ = declare_parameter("lane_filter_voxel_size_x", 0.04); + lane_filter_.voxel_size_y_ = declare_parameter("lane_filter_voxel_size_y", 0.04); + lane_filter_.voxel_size_z_ = declare_parameter("lane_filter_voxel_size_z", 0.04); + + grid_map_pcl_loader_ = pcl::make_shared(this->get_logger()); + grid_map_pcl_loader_->loadParameters(param_file_path); + + rclcpp::QoS durable_qos{1}; + durable_qos.transient_local(); + pub_elevation_map_ = + this->create_publisher("output/elevation_map", durable_qos); + + if (use_elevation_map_cloud_publisher_) { + pub_elevation_map_cloud_ = this->create_publisher( + "output/elevation_map_cloud", durable_qos); + } + + std::filesystem::path pointcloud_map_path( + this->declare_parameter("pointcloud_map_path", "path_default")); + hash_json_ = getPcdMapHashJson(pointcloud_map_path.lexically_normal().string()); + + const auto elevation_map_hash = getMd5Sum(hash_json_.dump()); + const std::string elevation_map_directory = + this->declare_parameter("elevation_map_directory", "path_default"); + elevation_map_path_ = std::filesystem::path(elevation_map_directory) / elevation_map_hash; + + use_elevation_map_file_ = false; + struct stat info; + if (stat(elevation_map_path_.c_str(), &info) != 0) { + RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map "); + already_sub_vector_map_ = false; + already_sub_pointcloud_map_ = false; + using std::placeholders::_1; + sub_pointcloud_map_ = this->create_subscription( + "input/pointcloud_map", durable_qos, + std::bind(&ElevationMapLoaderNode::onPointcloudMap, this, _1)); + sub_vector_map_ = this->create_subscription( + "input/vector_map", durable_qos, std::bind(&ElevationMapLoaderNode::onVectorMap, this, _1)); + } else if (info.st_mode & S_IFDIR) { + RCLCPP_INFO(this->get_logger(), "Load elevation map from: %s", elevation_map_path_.c_str()); + use_elevation_map_file_ = grid_map::GridMapRosConverter::loadFromBag( + elevation_map_path_, "elevation_map", elevation_map_); + publishElevationMap(); + } +} + +void ElevationMapLoaderNode::onPointcloudMap( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map) +{ + RCLCPP_INFO(this->get_logger(), "subscribe pointcloud_map"); + pcl::PointCloud map_pcl; + pcl::fromROSMsg(*pointcloud_map, map_pcl); + map_pcl_ptr_ = pcl::make_shared>(map_pcl); + + already_sub_pointcloud_map_ = true; + if (already_sub_vector_map_) { + publishElevationMap(); + } +} + +void ElevationMapLoaderNode::onVectorMap( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr vector_map) +{ + RCLCPP_INFO(this->get_logger(), "subscribe vector_map"); + already_sub_vector_map_ = false; + lanelet::LaneletMapPtr lanelet_map_ptr; + lanelet_map_ptr = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*vector_map, lanelet_map_ptr); + const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); + lane_filter_.road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); + + already_sub_vector_map_ = true; + if (already_sub_pointcloud_map_) { + publishElevationMap(); + } +} + +void ElevationMapLoaderNode::publishElevationMap() +{ + if (!use_elevation_map_file_) { + if (lane_filter_.use_lane_filter_) { + // calculate convex hull + const auto convex_hull = getConvexHull(map_pcl_ptr_); + // get intersected lanelets + lanelet::ConstLanelets intersected_lanelets = + getIntersectedLanelets(convex_hull, lane_filter_.road_lanelets_); + // filter pointcloud by lanelet + pcl::PointCloud::Ptr lane_filtered_map_pcl_ptr = + getLaneFilteredPointCloud(intersected_lanelets, map_pcl_ptr_); + grid_map_pcl_loader_->setInputCloud(lane_filtered_map_pcl_ptr); + } else { + grid_map_pcl_loader_->setInputCloud(map_pcl_ptr_); + } + createElevationMapFromPointcloud(); + elevation_map_ = grid_map_pcl_loader_->getGridMap(); + if (use_inpaint_) { + inpaintElevationMap(inpaint_radius_); + } + saveElevationMap(); + } + + elevation_map_.setFrameId(map_frame_); + auto msg = grid_map::GridMapRosConverter::toMessage(elevation_map_); + pub_elevation_map_->publish(std::move(msg)); + already_sub_pointcloud_map_ = false; + already_sub_vector_map_ = false; + + if (use_elevation_map_cloud_publisher_) { + pcl::PointCloud::Ptr elevation_map_cloud_ptr = + createPointcloudFromElevationMap(); + sensor_msgs::msg::PointCloud2 elevation_map_cloud_msg; + pcl::toROSMsg(*elevation_map_cloud_ptr, elevation_map_cloud_msg); + pub_elevation_map_cloud_->publish(elevation_map_cloud_msg); + } +} + +void ElevationMapLoaderNode::createElevationMapFromPointcloud() +{ + const auto start = std::chrono::high_resolution_clock::now(); + grid_map_pcl_loader_->preProcessInputCloud(); + grid_map_pcl_loader_->initializeGridMapGeometryFromInputCloud(); + grid_map_pcl_loader_->addLayerFromInputCloud(layer_name_); + grid_map::grid_map_pcl::printTimeElapsedToRosInfoStream( + start, "Finish creating elevation map. Total time: ", this->get_logger()); +} + +void ElevationMapLoaderNode::inpaintElevationMap(const float radius) +{ + // Convert elevation layer to OpenCV image to fill in holes. + // Get the inpaint mask (nonzero pixels indicate where values need to be filled in). + elevation_map_.add("inpaint_mask", 0.0); + + elevation_map_.setBasicLayers(std::vector()); + for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { + if (!elevation_map_.isValid(*iterator, layer_name_)) { + elevation_map_.at("inpaint_mask", *iterator) = 1.0; + } + } + cv::Mat original_image; + cv::Mat mask; + cv::Mat filled_image; + const float min_value = elevation_map_.get(layer_name_).minCoeffOfFinites(); + const float max_value = elevation_map_.get(layer_name_).maxCoeffOfFinites(); + + grid_map::GridMapCvConverter::toImage( + elevation_map_, layer_name_, CV_8UC3, min_value, max_value, original_image); + grid_map::GridMapCvConverter::toImage( + elevation_map_, "inpaint_mask", CV_8UC1, mask); + + const float radius_in_pixels = radius / elevation_map_.getResolution(); + cv::inpaint(original_image, mask, filled_image, radius_in_pixels, cv::INPAINT_NS); + + grid_map::GridMapCvConverter::addLayerFromImage( + filled_image, layer_name_, elevation_map_, min_value, max_value); + elevation_map_.erase("inpaint_mask"); +} + +autoware_utils::LinearRing2d ElevationMapLoaderNode::getConvexHull( + const pcl::PointCloud::Ptr & input_cloud) +{ + // downsample pointcloud to reduce convex hull calculation cost + pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + downsampled_cloud->points.reserve(input_cloud->points.size()); + pcl::VoxelGrid filter; + filter.setInputCloud(input_cloud); + filter.setLeafSize(0.5, 0.5, 100.0); + filter.filter(*downsampled_cloud); + + autoware_utils::MultiPoint2d candidate_points; + for (const auto & p : downsampled_cloud->points) { + candidate_points.emplace_back(p.x, p.y); + } + + autoware_utils::LinearRing2d convex_hull; + boost::geometry::convex_hull(candidate_points, convex_hull); + + return convex_hull; +} + +lanelet::ConstLanelets ElevationMapLoaderNode::getIntersectedLanelets( + const autoware_utils::LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets) +{ + lanelet::ConstLanelets intersected_lanelets; + for (const auto & road_lanelet : road_lanelets) { + if (boost::geometry::intersects(convex_hull, road_lanelet.polygon2d().basicPolygon())) { + intersected_lanelets.push_back(road_lanelet); + } + } + return intersected_lanelets; +} + +bool ElevationMapLoaderNode::checkPointWithinLanelets( + const pcl::PointXYZ & point, const lanelet::ConstLanelets & intersected_lanelets) +{ + autoware_utils::Point2d point2d(point.x, point.y); + for (const auto & lanelet : intersected_lanelets) { + if (lane_filter_.lane_margin_ > 0) { + if ( + boost::geometry::distance(point2d, lanelet.polygon2d().basicPolygon()) > + lane_filter_.lane_margin_) { + continue; + } + } else { + if (!boost::geometry::within(point2d, lanelet.polygon2d().basicPolygon())) { + continue; + } + } + + if (lane_filter_.lane_height_diff_thresh_ > 0) { + float distance = calculateDistancePointFromPlane(point, lanelet); + if (distance < lane_filter_.lane_height_diff_thresh_) { + return true; + } + } else { + return true; + } + } + return false; +} + +float ElevationMapLoaderNode::calculateDistancePointFromPlane( + const pcl::PointXYZ & point, const lanelet::ConstLanelet & lanelet) +{ + const Eigen::Vector3d point_3d(point.x, point.y, point.z); + const Eigen::Vector2d point_2d(point.x, point.y); + + const float distance_3d = boost::geometry::distance(point_3d, lanelet.centerline3d()); + const float distance_2d = boost::geometry::distance(point_2d, lanelet.centerline2d()); + const float distance = std::sqrt(distance_3d * distance_3d - distance_2d * distance_2d); + + return distance; +} + +pcl::PointCloud::Ptr ElevationMapLoaderNode::getLaneFilteredPointCloud( + const lanelet::ConstLanelets & intersected_lanelets, + const pcl::PointCloud::Ptr & cloud) +{ + pcl::PointCloud filtered_cloud; + filtered_cloud.header = cloud->header; + + pcl::PointCloud::Ptr centralized_cloud(new pcl::PointCloud); + centralized_cloud->reserve(cloud->size()); + + // The coordinates of the point cloud are too large, resulting in calculation errors, + // so offset them to the center. + // https://github.com/PointCloudLibrary/pcl/issues/4895 + Eigen::Vector4f centroid; + pcl::compute3DCentroid(*cloud, centroid); + for (const auto & p : cloud->points) { + centralized_cloud->points.push_back( + pcl::PointXYZ(p.x - centroid[0], p.y - centroid[1], p.z - centroid[2])); + } + + pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + pcl::VoxelGrid voxel_grid; + voxel_grid.setLeafSize(lane_filter_.voxel_size_x_, lane_filter_.voxel_size_y_, 100000.0); + voxel_grid.setInputCloud(centralized_cloud); + voxel_grid.setSaveLeafLayout(true); + voxel_grid.filter(*downsampled_cloud); + + std::unordered_map> downsampled2original_map; + for (const auto & p : centralized_cloud->points) { + if (std::isnan(p.x) || std::isnan(p.y) || std::isnan(p.z)) { + continue; + } + const size_t index = voxel_grid.getCentroidIndex(p); + downsampled2original_map[index].points.push_back(p); + } + + for (auto & point : downsampled_cloud->points) { + if (checkPointWithinLanelets( + pcl::PointXYZ(point.x + centroid[0], point.y + centroid[1], point.z + centroid[2]), + intersected_lanelets)) { + const size_t index = voxel_grid.getCentroidIndex(point); + for (auto & original_point : downsampled2original_map[index].points) { + original_point.x += centroid[0]; + original_point.y += centroid[1]; + original_point.z += centroid[2]; + filtered_cloud.points.push_back(original_point); + } + } + } + + pcl::PointCloud::Ptr filtered_cloud_ptr; + filtered_cloud_ptr = pcl::make_shared>(filtered_cloud); + return filtered_cloud_ptr; +} + +pcl::PointCloud::Ptr ElevationMapLoaderNode::createPointcloudFromElevationMap() +{ + pcl::PointCloud output_cloud; + output_cloud.header.frame_id = elevation_map_.getFrameId(); + + for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { + float z = elevation_map_.at(layer_name_, *iterator); + if (!std::isnan(z)) { + grid_map::Position position; + elevation_map_.getPosition(grid_map::Index(*iterator), position); + output_cloud.push_back(pcl::PointXYZ(position.x(), position.y(), z)); + } + } + output_cloud.width = output_cloud.points.size(); + output_cloud.height = 1; + output_cloud.points.resize(output_cloud.width * output_cloud.height); + output_cloud.is_dense = false; + + pcl::PointCloud::Ptr output_cloud_ptr; + output_cloud_ptr = pcl::make_shared>(output_cloud); + return output_cloud_ptr; +} + +void ElevationMapLoaderNode::saveElevationMap() +{ + const bool saving_successful = + grid_map::GridMapRosConverter::saveToBag(elevation_map_, elevation_map_path_, "elevation_map"); + + std::ofstream json_file(elevation_map_path_ / "input_pcd.json"); + json_file << hash_json_; + json_file.close(); + + RCLCPP_INFO_STREAM( + this->get_logger(), "Saving elevation map successful: " << std::boolalpha << saving_successful); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ElevationMapLoaderNode) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp new file mode 100644 index 0000000000000..e0e6ed449c362 --- /dev/null +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -0,0 +1,84 @@ +// Copyright 2021 TierIV +// +// 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. + +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * 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. + * + * Authors: Simon Thompson, Ryohsuke Mitsudome + * + */ + +#include "map_loader/lanelet2_map_loader_node.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options) +: Node("lanelet2_map_loader", options) +{ + const auto lanelet2_filename = declare_parameter("lanelet2_map_path", ""); + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(this->get_logger(), error); + } + if (!errors.empty()) { + return; + } + + const auto center_line_resolution = this->declare_parameter("center_line_resolution", 5.0); + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + + std::string format_version{}, map_version{}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + + pub_map_bin_ = this->create_publisher( + "output/lanelet2_map", rclcpp::QoS{1}.transient_local()); + + autoware_auto_mapping_msgs::msg::HADMapBin map_bin_msg; + map_bin_msg.header.stamp = this->now(); + map_bin_msg.header.frame_id = "map"; + map_bin_msg.format_version = format_version; + map_bin_msg.map_version = map_version; + lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); + + pub_map_bin_->publish(map_bin_msg); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Lanelet2MapLoaderNode) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp new file mode 100644 index 0000000000000..c74b59e057087 --- /dev/null +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -0,0 +1,199 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * 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. + * + * Authors: Simon Thompson, Ryohsuke Mitsudome + * + */ + +#include "map_loader/lanelet2_map_visualization_node.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +namespace +{ +void insertMarkerArray( + visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2) +{ + a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); +} + +void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) +{ + cl->r = r; + cl->g = g; + cl->b = b; + cl->a = a; +} +} // namespace + +Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options) +: Node("lanelet2_map_visualization", options) +{ + using std::placeholders::_1; + + viz_lanelets_centerline_ = this->declare_parameter("viz_lanelets_centerline", true); + + sub_map_bin_ = this->create_subscription( + "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), + std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1)); + + pub_marker_ = this->create_publisher( + "output/lanelet2_map_marker", rclcpp::QoS{1}.transient_local()); +} + +void Lanelet2MapVisualizationNode::onMapBin( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); + + lanelet::utils::conversion::fromBinMsg(*msg, viz_lanelet_map); + RCLCPP_INFO(this->get_logger(), "Map is loaded\n"); + + // get lanelets etc to visualize + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map); + lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + lanelet::ConstLanelets shoulder_lanelets = lanelet::utils::query::shoulderLanelets(all_lanelets); + lanelet::ConstLanelets crosswalk_lanelets = + lanelet::utils::query::crosswalkLanelets(all_lanelets); + lanelet::ConstLineStrings3d pedestrian_markings = + lanelet::utils::query::getAllPedestrianMarkings(viz_lanelet_map); + lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); + std::vector stop_lines = + lanelet::utils::query::stopLinesLanelets(road_lanelets); + std::vector tl_reg_elems = + lanelet::utils::query::trafficLights(all_lanelets); + std::vector aw_tl_reg_elems = + lanelet::utils::query::autowareTrafficLights(all_lanelets); + std::vector da_reg_elems = + lanelet::utils::query::detectionAreas(all_lanelets); + std::vector no_reg_elems = + lanelet::utils::query::noStoppingAreas(all_lanelets); + lanelet::ConstLineStrings3d parking_spaces = + lanelet::utils::query::getAllParkingSpaces(viz_lanelet_map); + lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map); + lanelet::ConstPolygons3d obstacle_polygons = + lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map); + + std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_pedestrian_markings, cl_ll_borders, + cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, cl_parking_lots, + cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas; + setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); + setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); + setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); + setColor(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999); + setColor(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999); + setColor(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999); + setColor(&cl_stoplines, 0.5, 0.5, 0.5, 0.999); + setColor(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); + setColor(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); + setColor(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); + setColor(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); + setColor(&cl_parking_lots, 0.5, 0.5, 0.0, 0.3); + setColor(&cl_parking_spaces, 1.0, 0.647, 0.0, 0.6); + setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); + + visualization_msgs::msg::MarkerArray map_marker_array; + + insertMarkerArray( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.5)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::laneletDirectionAsMarkerArray(shoulder_lanelets, "shoulder_")); + insertMarkerArray( + &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::pedestrianMarkingsAsMarkerArray( + pedestrian_markings, cl_pedestrian_markings)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "walkway_lanelets", walkway_lanelets, cl_cross)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::obstaclePolygonsAsMarkerArray(obstacle_polygons, cl_obstacle_polygons)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_areas)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::noStoppingAreasAsMarkerArray(no_reg_elems, cl_no_stopping_areas)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::laneletsBoundaryAsMarkerArray( + shoulder_lanelets, cl_shoulder_borders, viz_lanelets_centerline_, "shoulder_")); + insertMarkerArray( + &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( + road_lanelets, cl_ll_borders, viz_lanelets_centerline_)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::generateLaneletIdMarker(shoulder_lanelets, cl_lanelet_id)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); + + pub_marker_->publish(map_marker_array); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Lanelet2MapVisualizationNode) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp new file mode 100644 index 0000000000000..d5bd22eeda8ab --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -0,0 +1,128 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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 "map_loader/pointcloud_map_loader_node.hpp" + +#include +#include +#include +#include // To be replaced by std::filesystem in C++17 + +#include +#include + +namespace +{ +bool isPcdFile(const std::string & p) +{ + if (!rcutils_is_file(p.c_str())) { + return false; + } + + const auto ext = p.substr(p.find_last_of(".") + 1); + + if (ext != "pcd" && ext != "PCD") { + return false; + } + + return true; +} +} // namespace + +PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & options) +: Node("pointcloud_map_loader", options) +{ + rclcpp::QoS durable_qos{1}; + durable_qos.transient_local(); + pub_pointcloud_map_ = + this->create_publisher("output/pointcloud_map", durable_qos); + + const auto pcd_paths_or_directory = + declare_parameter("pcd_paths_or_directory", std::vector({})); + + std::vector pcd_paths{}; + + for (const auto & p : pcd_paths_or_directory) { + if (!rcutils_exists(p.c_str())) { + RCLCPP_ERROR_STREAM(get_logger(), "invalid path: " << p); + } + + if (isPcdFile(p)) { + pcd_paths.push_back(p); + } + + if (rcutils_is_directory(p.c_str())) { + glob_t glob_buf; + glob((p + "/*.pcd").c_str(), 0, NULL, &glob_buf); + for (size_t i = 0; i < glob_buf.gl_pathc; ++i) { + pcd_paths.push_back(glob_buf.gl_pathv[i]); + } + globfree(&glob_buf); + } + } + + const auto pcd = loadPCDFiles(pcd_paths); + + if (pcd.width == 0) { + RCLCPP_ERROR(get_logger(), "No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size()); + return; + } + + pub_pointcloud_map_->publish(pcd); +} + +sensor_msgs::msg::PointCloud2 PointCloudMapLoaderNode::loadPCDFiles( + const std::vector & pcd_paths) +{ + sensor_msgs::msg::PointCloud2 whole_pcd{}; + + sensor_msgs::msg::PointCloud2 partial_pcd; + for (const auto & path : pcd_paths) { + if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { + RCLCPP_ERROR_STREAM(get_logger(), "PCD load failed: " << path); + } + + if (whole_pcd.width == 0) { + whole_pcd = partial_pcd; + } else { + whole_pcd.width += partial_pcd.width; + whole_pcd.row_step += partial_pcd.row_step; + whole_pcd.data.reserve(whole_pcd.data.size() + partial_pcd.data.size()); + whole_pcd.data.insert(whole_pcd.data.end(), partial_pcd.data.begin(), partial_pcd.data.end()); + } + } + + whole_pcd.header.frame_id = "map"; + + return whole_pcd; +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode) diff --git a/map/map_tf_generator/CMakeLists.txt b/map/map_tf_generator/CMakeLists.txt new file mode 100644 index 0000000000000..bdaa26b4a6984 --- /dev/null +++ b/map/map_tf_generator/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.5) +project(map_tf_generator) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +### ROS Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +### non-ROS Dependencies +find_package(PCL REQUIRED) + +# Generate exe file +ament_auto_add_library(map_tf_generator_node SHARED + src/map_tf_generator_node.cpp +) +target_link_libraries(map_tf_generator_node ${PCL_LIBRARIES}) + +rclcpp_components_register_node(map_tf_generator_node + PLUGIN "MapTFGeneratorNode" + EXECUTABLE map_tf_generator +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +## Install +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/map/map_tf_generator/Readme.md b/map/map_tf_generator/Readme.md new file mode 100644 index 0000000000000..6cc8100d89107 --- /dev/null +++ b/map/map_tf_generator/Readme.md @@ -0,0 +1,41 @@ +# map_tf_generator + +## Purpose + +This node broadcasts `viewer` frames for visualization of pointcloud map in Rviz. +The position of `viewer` frames is the geometric center of input pointclouds. + +Note that there is no module to need `viewer` frames and this is used only for visualization. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------------- | ------------------------------- | ----------------------------------------------------------------- | +| `/map/pointcloud_map` | `sensor_msgs::msg::PointCloud2` | Subscribe pointcloud map to calculate position of `viewer` frames | + +### Output + +| Name | Type | Description | +| ------------ | ------------------------ | ------------------------- | +| `/tf_static` | `tf2_msgs/msg/TFMessage` | Broadcast `viewer` frames | + +## Parameters + +### Node Parameters + +None + +### Core Parameters + +| Name | Type | Default Value | Explanation | +| -------------- | ------ | ------------- | ------------------------------------- | +| `viewer_frame` | string | viewer | Name of `viewer` frame | +| `map_frame` | string | map | The parent frame name of viewer frame | + +## Assumptions / Known limits + +TBD. diff --git a/map/map_tf_generator/launch/map_tf_generator.launch.xml b/map/map_tf_generator/launch/map_tf_generator.launch.xml new file mode 100644 index 0000000000000..99a17fcf3a715 --- /dev/null +++ b/map/map_tf_generator/launch/map_tf_generator.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml new file mode 100644 index 0000000000000..cd1ef98eb8014 --- /dev/null +++ b/map/map_tf_generator/package.xml @@ -0,0 +1,25 @@ + + + + map_tf_generator + 0.1.0 + map_tf_generator package as a ROS 2 node + azumi-suzuki + Apache License 2.0 + + ament_cmake_auto + + libpcl-all-dev + pcl_conversions + rclcpp + std_msgs + tf2 + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/map/map_tf_generator/src/map_tf_generator_node.cpp b/map/map_tf_generator/src/map_tf_generator_node.cpp new file mode 100644 index 0000000000000..378d739ffbef3 --- /dev/null +++ b/map/map_tf_generator/src/map_tf_generator_node.cpp @@ -0,0 +1,92 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 + +#include + +#include +#include +#include +#include +#include + +#include +#include + +class MapTFGeneratorNode : public rclcpp::Node +{ +public: + using PointCloud = pcl::PointCloud; + explicit MapTFGeneratorNode(const rclcpp::NodeOptions & options) + : Node("map_tf_generator", options) + { + map_frame_ = declare_parameter("map_frame", "map"); + viewer_frame_ = declare_parameter("viewer_frame", "viewer"); + + sub_ = create_subscription( + "pointcloud_map", rclcpp::QoS{1}.transient_local(), + std::bind(&MapTFGeneratorNode::onPointCloud, this, std::placeholders::_1)); + + static_broadcaster_ = std::make_shared(this); + } + +private: + std::string map_frame_; + std::string viewer_frame_; + rclcpp::Subscription::SharedPtr sub_; + + std::shared_ptr static_broadcaster_; + + void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr clouds_ros) + { + PointCloud clouds; + pcl::fromROSMsg(*clouds_ros, clouds); + + const unsigned int sum = clouds.points.size(); + double coordinate[3] = {0, 0, 0}; + for (unsigned int i = 0; i < sum; i++) { + coordinate[0] += clouds.points[i].x; + coordinate[1] += clouds.points[i].y; + coordinate[2] += clouds.points[i].z; + } + coordinate[0] = coordinate[0] / sum; + coordinate[1] = coordinate[1] / sum; + coordinate[2] = coordinate[2] / sum; + + geometry_msgs::msg::TransformStamped static_transformStamped; + static_transformStamped.header.stamp = this->now(); + static_transformStamped.header.frame_id = map_frame_; + static_transformStamped.child_frame_id = viewer_frame_; + static_transformStamped.transform.translation.x = coordinate[0]; + static_transformStamped.transform.translation.y = coordinate[1]; + static_transformStamped.transform.translation.z = coordinate[2]; + tf2::Quaternion quat; + quat.setRPY(0, 0, 0); + static_transformStamped.transform.rotation.x = quat.x(); + static_transformStamped.transform.rotation.y = quat.y(); + static_transformStamped.transform.rotation.z = quat.z(); + static_transformStamped.transform.rotation.w = quat.w(); + + static_broadcaster_->sendTransform(static_transformStamped); + + RCLCPP_INFO_STREAM( + get_logger(), "broadcast static tf. map_frame:" + << map_frame_ << ", viewer_frame:" << viewer_frame_ << ", x:" << coordinate[0] + << ", y:" << coordinate[1] << ", z:" << coordinate[2]); + } +}; + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapTFGeneratorNode) diff --git a/map/util/lanelet2_map_preprocessor/CMakeLists.txt b/map/util/lanelet2_map_preprocessor/CMakeLists.txt new file mode 100644 index 0000000000000..7c2225f5a58c1 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.0.2) +project(lanelet2_map_preprocessor) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + lanelet2_extension + pcl_ros + roscpp +) + +catkin_package( +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable(fix_z_value_by_pcd src/fix_z_value_by_pcd.cpp) +add_dependencies(fix_z_value_by_pcd ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(fix_z_value_by_pcd ${catkin_LIBRARIES}) + +add_executable(transform_maps src/transform_maps.cpp) +add_dependencies(transform_maps ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(transform_maps ${catkin_LIBRARIES}) + +add_executable(merge_close_lines src/merge_close_lines.cpp) +add_dependencies(merge_close_lines ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(merge_close_lines ${catkin_LIBRARIES}) + +add_executable(merge_close_points src/merge_close_points.cpp) +add_dependencies(merge_close_points ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(merge_close_points ${catkin_LIBRARIES}) + +add_executable(remove_unreferenced_geometry src/remove_unreferenced_geometry.cpp) +add_dependencies(remove_unreferenced_geometry ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(remove_unreferenced_geometry ${catkin_LIBRARIES}) + +add_executable(fix_lane_change_tags src/fix_lane_change_tags.cpp) +add_dependencies(fix_lane_change_tags ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(fix_lane_change_tags ${catkin_LIBRARIES}) + +## Install executables and/or libraries +install( + TARGETS + fix_z_value_by_pcd + transform_maps + merge_close_lines + merge_close_points + remove_unreferenced_geometry + fix_lane_change_tags + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/map/util/lanelet2_map_preprocessor/COLCON_IGNORE b/map/util/lanelet2_map_preprocessor/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml new file mode 100644 index 0000000000000..e71adaea8bc73 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/package.xml @@ -0,0 +1,68 @@ + + + lanelet2_map_preprocessor + 0.1.0 + The lanelet2_map_preprocessor package + + + + + mitsudome-r + + + + + + Apache License 2.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + lanelet2_extension + pcl_ros + roscpp + lanelet2_extension + pcl_ros + roscpp + lanelet2_extension + pcl_ros + roscpp + + + + + + + + diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp new file mode 100644 index 0000000000000..8a7c47fe3c044 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -0,0 +1,122 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +void printUsage() +{ + std::cerr << "Please set following private parameters:" << std::endl + << "llt_map_path" << std::endl + << "output_path" << std::endl; +} + +using lanelet::utils::getId; +using lanelet::utils::to2D; + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + ROS_ERROR_STREAM(error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool exists(std::unordered_set & set, lanelet::Id element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +lanelet::Lanelets convertToVector(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::Lanelets lanelets; + for (lanelet::Lanelet lanelet : lanelet_map_ptr->laneletLayer) { + lanelets.push_back(lanelet); + } + return lanelets; +} +void fixTags(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + auto lanelets = convertToVector(lanelet_map_ptr); + lanelet::traffic_rules::TrafficRulesPtr trafficRules = + lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + lanelet::routing::RoutingGraphUPtr routingGraph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *trafficRules); + + for (auto & llt : lanelets) { + if (!routingGraph->conflicting(llt).empty()) { + continue; + } + llt.attributes().erase("turn_direction"); + if (!!routingGraph->adjacentRight(llt)) { + llt.rightBound().attributes()["lane_change"] = "yes"; + } + if (!!routingGraph->adjacentLeft(llt)) { + llt.leftBound().attributes()["lane_change"] = "yes"; + } + } +} + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "merge_lines"); + ros::NodeHandle pnh("~"); + + if (!pnh.hasParam("llt_map_path")) { + printUsage(); + return EXIT_FAILURE; + } + if (!pnh.hasParam("output_path")) { + printUsage(); + return EXIT_FAILURE; + } + + std::string llt_map_path, output_path; + pnh.getParam("llt_map_path", llt_map_path); + pnh.getParam("output_path", output_path); + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + + fixTags(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + return 0; +} diff --git a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp new file mode 100644 index 0000000000000..e47a2d5deb598 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp @@ -0,0 +1,188 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +void printUsage() +{ + std::cerr << "Please set following private parameters:" << std::endl + << "llt_map_path" << std::endl + << "pcd_map_path" << std::endl + << "output_path" << std::endl; +} + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + ROS_ERROR_STREAM(error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +{ + if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file + PCL_ERROR("Couldn't read file test_pcd.pcd \n"); + return false; + } + std::cout << "Loaded " << pcd_map_ptr->width * pcd_map_ptr->height << " data points." + << std::endl; + return true; +} + +double getMinHeightAroundPoint( + const pcl::PointCloud::Ptr & pcd_map_ptr, + const pcl::KdTreeFLANN & kdtree, const pcl::PointXYZ & search_pt, + const double search_radius3d, const double search_radius2d) +{ + std::vector pointIdxRadiusSearch; + std::vector pointRadiusSquaredDistance; + if ( + kdtree.radiusSearch( + search_pt, search_radius3d, pointIdxRadiusSearch, pointRadiusSquaredDistance) <= 0) { + std::cout << "no points found within 3d radius " << search_radius3d << std::endl; + return search_pt.z; + } + + double min_height = std::numeric_limits::max(); + bool found = false; + + for (std::size_t i = 0; i < pointIdxRadiusSearch.size(); i++) { + std::size_t pt_idx = pointIdxRadiusSearch.at(i); + const auto pt = pcd_map_ptr->points.at(pt_idx); + if (pt.z > min_height) { + continue; + } + double distance2d = std::hypot(pt.x - search_pt.x, pt.y - search_pt.y); + if (distance2d < search_radius2d) { + found = true; + min_height = pt.z; + } + } + if (!found) { + min_height = search_pt.z; + } + return min_height; +} + +bool exists(std::unordered_set & set, lanelet::Id element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +void adjustHeight( + const pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + std::unordered_set done; + double search_radius2d = 0.5; + double search_radius3d = 10; + + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(pcd_map_ptr); + + for (lanelet::Lanelet & llt : lanelet_map_ptr->laneletLayer) { + for (lanelet::Point3d & pt : llt.leftBound()) { + if (exists(done, pt.id())) { + continue; + } + pcl::PointXYZ pcl_pt; + pcl_pt.x = pt.x(); + pcl_pt.y = pt.y(); + pcl_pt.z = pt.z(); + double min_height = + getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; + pt.z() = min_height; + done.insert(pt.id()); + } + for (lanelet::Point3d & pt : llt.rightBound()) { + if (exists(done, pt.id())) { + continue; + } + pcl::PointXYZ pcl_pt; + pcl_pt.x = pt.x(); + pcl_pt.y = pt.y(); + pcl_pt.z = pt.z(); + double min_height = + getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; + pt.z() = min_height; + done.insert(pt.id()); + } + } +} + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "lanelet_map_height_adjuster"); + ros::NodeHandle pnh("~"); + + if (!pnh.hasParam("llt_map_path")) { + printUsage(); + return EXIT_FAILURE; + } + if (!pnh.hasParam("pcd_map_path")) { + printUsage(); + return EXIT_FAILURE; + } + if (!pnh.hasParam("output_path")) { + printUsage(); + return EXIT_FAILURE; + } + + std::string llt_map_path, pcd_map_path, output_path; + pnh.getParam("llt_map_path", llt_map_path); + pnh.getParam("pcd_map_path", pcd_map_path); + pnh.getParam("output_path", output_path); + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + return EXIT_FAILURE; + } + + adjustHeight(pcd_map_ptr, llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + return 0; +} diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp new file mode 100644 index 0000000000000..595fb49ce522c --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp @@ -0,0 +1,216 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +void printUsage() +{ + std::cerr << "Please set following private parameters:" << std::endl + << "llt_map_path" << std::endl + << "output_path" << std::endl; +} + +using lanelet::utils::getId; +using lanelet::utils::to2D; + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + ROS_ERROR_STREAM(error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool exists(std::unordered_set & set, lanelet::Id element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::LineStrings3d lines; + for (const lanelet::LineString3d line : lanelet_map_ptr->lineStringLayer) { + lines.push_back(line); + } + return lines; +} + +lanelet::ConstPoint3d get3DPointFrom2DArcLength( + const lanelet::ConstLineString3d & line, const double s) +{ + double accumulated_distance2d = 0; + if (line.size() < 2) { + return lanelet::Point3d(); + } + auto prev_pt = line.front(); + for (int i = 1; i < line.size(); i++) { + const auto & pt = line[i]; + double distance2d = lanelet::geometry::distance2d(to2D(prev_pt), to2D(pt)); + if (accumulated_distance2d + distance2d >= s) { + double ratio = (s - accumulated_distance2d) / distance2d; + auto interpolated_pt = prev_pt.basicPoint() * (1 - ratio) + pt.basicPoint() * ratio; + std::cout << interpolated_pt << std::endl; + return lanelet::ConstPoint3d( + lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + } + accumulated_distance2d += distance2d; + prev_pt = pt; + } + ROS_ERROR("interpolation failed"); + return lanelet::ConstPoint3d(); +} + +double getLineLength(const lanelet::ConstLineString3d & line) +{ + return boost::geometry::length(line.basicLineString()); +} + +bool areLinesSame( + const lanelet::ConstLineString3d & line1, const lanelet::ConstLineString3d & line2) +{ + bool same_ends = false; + if (line1.front() == line2.front() && line1.back() == line2.back()) { + same_ends = true; + } + if (line1.front() == line2.back() && line1.back() == line2.front()) { + same_ends = true; + } + if (!same_ends) { + return false; + } + + double sum_distance = 0; + for (const auto & pt : line1) { + sum_distance += boost::geometry::distance(pt.basicPoint(), line2); + } + for (const auto & pt : line2) { + sum_distance += boost::geometry::distance(pt.basicPoint(), line1); + } + + double avg_distance = sum_distance / (line1.size() + line2.size()); + std::cout << line1 << " " << line2 << " " << avg_distance << std::endl; + if (avg_distance < 1.0) { + return true; + } else { + return false; + } +} + +lanelet::BasicPoint3d getClosestPointOnLine( + const lanelet::BasicPoint3d & search_point, const lanelet::ConstLineString3d & line) +{ + auto arc_coordinate = lanelet::geometry::toArcCoordinates(to2D(line), to2D(search_point)); + std::cout << arc_coordinate.length << " " << arc_coordinate.distance << std::endl; + return get3DPointFrom2DArcLength(line, arc_coordinate.length).basicPoint(); +} + +lanelet::LineString3d mergeTwoLines( + const lanelet::LineString3d & line1, const lanelet::ConstLineString3d & line2) +{ + lanelet::Points3d new_points; + for (const auto & p1 : line1) { + lanelet::BasicPoint3d p1_basic_point = p1.basicPoint(); + lanelet::BasicPoint3d p2_basic_point = getClosestPointOnLine(p1, line2); + lanelet::BasicPoint3d new_basic_point = (p1_basic_point + p2_basic_point) / 2; + lanelet::Point3d new_point(lanelet::utils::getId(), new_basic_point); + new_points.push_back(new_point); + } + return lanelet::LineString3d(lanelet::utils::getId(), new_points); +} + +void copyData(lanelet::LineString3d & dst, lanelet::LineString3d & src) +{ + lanelet::Points3d points; + dst.clear(); + for (lanelet::Point3d pt : src) { + dst.push_back(pt); + } +} + +void mergeLines(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + auto lines = convertLineLayerToLineStrings(lanelet_map_ptr); + + for (size_t i = 0; i < lines.size(); i++) { + auto line_i = lines.at(i); + for (size_t j = 0; j < i; j++) { + auto line_j = lines.at(j); + if (areLinesSame(line_i, line_j)) { + auto merged_line = mergeTwoLines(line_i, line_j); + copyData(line_i, merged_line); + copyData(line_j, merged_line); + line_i.setId(line_j.id()); + std::cout << line_j << " " << line_i << std::endl; + // lanelet_map_ptr->add(merged_line); + for (lanelet::Point3d pt : merged_line) { + lanelet_map_ptr->add(pt); + } + break; + } + } + } +} + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "merge_lines"); + ros::NodeHandle pnh("~"); + + if (!pnh.hasParam("llt_map_path")) { + printUsage(); + return EXIT_FAILURE; + } + if (!pnh.hasParam("output_path")) { + printUsage(); + return EXIT_FAILURE; + } + + std::string llt_map_path, output_path; + pnh.getParam("llt_map_path", llt_map_path); + pnh.getParam("output_path", output_path); + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + + mergeLines(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + return 0; +} diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp new file mode 100644 index 0000000000000..2ee6d50b1bc39 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp @@ -0,0 +1,136 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +void printUsage() +{ + std::cerr << "Please set following private parameters:" << std::endl + << "llt_map_path" << std::endl + << "output_path" << std::endl; +} + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + ROS_ERROR_STREAM(error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool exists(std::unordered_set & set, lanelet::Id element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::Points3d points; + for (const lanelet::Point3d pt : lanelet_map_ptr->pointLayer) { + points.push_back(pt); + } + return points; +} + +// lanelet::LineString3d mergeClosePoints(const lanelet::ConstLineString3d& line1, const +// lanelet::ConstLineString3d& line2) +// { +// lanelet::Points3d new_points; +// for (const auto& p1 : line1) +// { +// p1_basic_point = p1.basicPoint(); +// lanelet::BasicPoint3d p2 = getClosestPointOnLine(line2, p1); +// lanelet::BasicPoint3d new_basic_point = (p1_basic_point + p2_basic_point)/2; +// lanelet::Point3d new_point(lanelet::utils::getId(), new_basic_point); +// new_points.push_back(new_point); +// } +// return lanelet::LineString3d(lanelet::utils::getId(), new_points); +// } + +void mergePoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + const auto & points = convertPointsLayerToPoints(lanelet_map_ptr); + + for (size_t i = 0; i < points.size(); i++) { + auto point_i = points.at(i); + for (size_t j = 0; j < i; j++) { + auto point_j = points.at(j); + + double distance = boost::geometry::distance(point_i, point_j); + if (distance < 0.1) { + const auto new_point = (point_i.basicPoint() + point_j.basicPoint()) / 2; + // const auto new_pt3d = lanelet::Point3d(lanelet::utils::getId(), new_point); + point_i.x() = new_point.x(); + point_i.y() = new_point.y(); + point_i.z() = new_point.z(); + point_j.x() = new_point.x(); + point_j.y() = new_point.y(); + point_j.z() = new_point.z(); + point_i.setId(point_j.id()); + } + } + } +} + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "merge_lines"); + ros::NodeHandle pnh("~"); + + if (!pnh.hasParam("llt_map_path")) { + printUsage(); + return EXIT_FAILURE; + } + if (!pnh.hasParam("output_path")) { + printUsage(); + return EXIT_FAILURE; + } + + std::string llt_map_path, pcd_map_path, output_path; + pnh.getParam("llt_map_path", llt_map_path); + pnh.getParam("output_path", output_path); + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + + mergePoints(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + return 0; +} diff --git a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp new file mode 100644 index 0000000000000..2bbc084564d3c --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp @@ -0,0 +1,113 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +void printUsage() +{ + std::cerr << "Please set following private parameters:" << std::endl + << "llt_map_path" << std::endl + << "output_path" << std::endl; +} + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + ROS_ERROR_STREAM(error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool exists(std::unordered_set & set, lanelet::Id element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::Points3d points; + for (const lanelet::Point3d pt : lanelet_map_ptr->pointLayer) { + points.push_back(pt); + } + return points; +} + +lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::LineStrings3d lines; + for (const lanelet::LineString3d line : lanelet_map_ptr->lineStringLayer) { + lines.push_back(line); + } + return lines; +} + +void removeUnreferencedGeometry(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap); + for (auto llt : lanelet_map_ptr->laneletLayer) { + new_map->add(llt); + } + lanelet_map_ptr = new_map; +} + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "remove_unreferenced_geometry"); + ros::NodeHandle pnh("~"); + + if (!pnh.hasParam("llt_map_path")) { + printUsage(); + return EXIT_FAILURE; + } + if (!pnh.hasParam("output_path")) { + printUsage(); + return EXIT_FAILURE; + } + + std::string llt_map_path, pcd_map_path, output_path; + pnh.getParam("llt_map_path", llt_map_path); + pnh.getParam("output_path", output_path); + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + removeUnreferencedGeometry(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + return 0; +} diff --git a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp new file mode 100644 index 0000000000000..7f27551ade2f2 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp @@ -0,0 +1,179 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +void printUsage() +{ + std::cerr << "Please set following private parameters:" << std::endl + << "llt_map_path" << std::endl + << "pcd_map_path" << std::endl + << "llt_output_path" << std::endl + << "pcd_output_path" << std::endl; +} + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + ROS_ERROR_STREAM(error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +{ + if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file + PCL_ERROR("Couldn't read file test_pcd.pcd \n"); + return false; + } + std::cout << "Loaded " << pcd_map_ptr->width * pcd_map_ptr->height << " data points." + << std::endl; + return true; +} + +void transformMaps( + pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr, + const Eigen::Affine3d affine) +{ + { + for (lanelet::Point3d pt : lanelet_map_ptr->pointLayer) { + Eigen::Vector3d eigen_pt(pt.x(), pt.y(), pt.z()); + auto transformed_pt = affine * eigen_pt; + pt.x() = transformed_pt.x(); + pt.y() = transformed_pt.y(); + pt.z() = transformed_pt.z(); + } + } + + { + for (auto & pt : pcd_map_ptr->points) { + Eigen::Vector3d eigen_pt(pt.x, pt.y, pt.z); + auto transformed_pt = affine * eigen_pt; + pt.x = transformed_pt.x(); + pt.y = transformed_pt.y(); + pt.z = transformed_pt.z(); + } + } +} + +Eigen::Affine3d createAffineMatrixFromXYZRPY( + const double x, const double y, const double z, const double roll, const double pitch, + const double yaw) +{ + double roll_rad = roll * M_PI / 180.0; + double pitch_rad = pitch * M_PI / 180.0; + double yaw_rad = yaw * M_PI / 180.0; + + Eigen::Translation trans(x, y, z); + Eigen::Matrix3d rot; + rot = Eigen::AngleAxisd(yaw_rad, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(pitch_rad, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(roll_rad, Eigen::Vector3d::UnitX()); + return trans * rot; +} + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "lanelet_map_height_adjuster"); + ros::NodeHandle pnh("~"); + + if (!pnh.hasParam("llt_map_path")) { + printUsage(); + return EXIT_FAILURE; + } + if (!pnh.hasParam("pcd_map_path")) { + printUsage(); + return EXIT_FAILURE; + } + if (!pnh.hasParam("llt_output_path")) { + printUsage(); + return EXIT_FAILURE; + } + if (!pnh.hasParam("pcd_output_path")) { + printUsage(); + return EXIT_FAILURE; + } + std::string llt_map_path, pcd_map_path, llt_output_path, pcd_output_path; + double x, y, z, roll, pitch, yaw; + pnh.getParam("llt_map_path", llt_map_path); + pnh.getParam("pcd_map_path", pcd_map_path); + pnh.getParam("llt_output_path", llt_output_path); + pnh.getParam("pcd_output_path", pcd_output_path); + pnh.param("x", x, 0.0); + pnh.param("y", y, 0.0); + pnh.param("z", z, 0.0); + pnh.param("roll", roll, 0.0); + pnh.param("pitch", pitch, 0.0); + pnh.param("yaw", yaw, 0.0); + + std::cout << "transforming maps with following parameters" << std::endl + << "x " << x << std::endl + << "y " << y << std::endl + << "z " << z << std::endl + << "roll " << roll << std::endl + << "pitch " << pitch << std::endl + << "yaw " << yaw << std::endl; + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + return EXIT_FAILURE; + } + Eigen::Affine3d affine = createAffineMatrixFromXYZRPY(x, y, z, roll, pitch, yaw); + + std::string mgrs_grid; + if (pnh.hasParam("mgrs_grid")) { + pnh.param("mgrs_grid", mgrs_grid, mgrs_grid); + projector.setMGRSCode(mgrs_grid); + } else { + std::cout << "no mgrs code set. using last projected grid instead" << std::endl; + mgrs_grid = projector.getProjectedMGRSGrid(); + } + + std::cout << "using mgrs grid: " << mgrs_grid << std::endl; + + transformMaps(pcd_map_ptr, llt_map_ptr, affine); + lanelet::write(llt_output_path, *llt_map_ptr, projector); + pcl::io::savePCDFileBinary(pcd_output_path, *pcd_map_ptr); + return 0; +}