diff --git a/map/lanelet2_extension/CMakeLists.txt b/map/lanelet2_extension/CMakeLists.txt deleted file mode 100644 index b1f237283fe8e..0000000000000 --- a/map/lanelet2_extension/CMakeLists.txt +++ /dev/null @@ -1,116 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(lanelet2_extension) - -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 -) - -add_compile_options(-std=c++14) - -find_package(catkin REQUIRED COMPONENTS - roscpp - lanelet2_core - lanelet2_io - lanelet2_maps - lanelet2_projection - lanelet2_routing - lanelet2_traffic_rules - lanelet2_validation - autoware_lanelet2_msgs - geometry_msgs - visualization_msgs - roslint -) - -set(ROSLINT_CPP_OPTS "--filter=-build/c++14") -roslint_cpp() - -catkin_package( - INCLUDE_DIRS include - LIBRARIES lanelet2_extension_lib - CATKIN_DEPENDS roscpp lanelet2_core lanelet2_io lanelet2_maps lanelet2_projection lanelet2_routing lanelet2_traffic_rules lanelet2_validation autoware_lanelet2_msgs geometry_msgs visualization_msgs -) - -include_directories( - include - ${GeographicLib_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${PUGIXML_INCLUDE_DIRS} -) - -add_definitions (${GeographicLib_DEFINITIONS}) - -add_library( lanelet2_extension_lib - lib/autoware_osm_parser.cpp - lib/autoware_traffic_light.cpp - lib/detection_area.cpp - lib/message_conversion.cpp - lib/mgrs_projector.cpp - lib/query.cpp - lib/road_marking.cpp - lib/utilities.cpp - lib/visualization.cpp -) - -add_dependencies(lanelet2_extension_lib - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -target_link_libraries(lanelet2_extension_lib - ${catkin_LIBRARIES} - ${GeographicLib_LIBRARIES} -) - -add_executable(lanelet2_extension_sample src/sample_code.cpp) -add_dependencies(lanelet2_extension_sample ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(lanelet2_extension_sample - ${catkin_LIBRARIES} - lanelet2_extension_lib -) - -add_executable(autoware_lanelet2_validation src/validation.cpp) -add_dependencies(autoware_lanelet2_validation ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(autoware_lanelet2_validation - ${catkin_LIBRARIES} - ${PUGIXML_LIBRAREIS} - lanelet2_extension_lib -) - -install(TARGETS lanelet2_extension_lib lanelet2_extension_sample autoware_lanelet2_validation - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -if(CATKIN_ENABLE_TESTING) - roslint_add_test() - find_package(rostest REQUIRED) - add_rostest_gtest(message_conversion-test test/test_message_conversion.test test/src/test_message_conversion.cpp) - target_link_libraries(message_conversion-test ${catkin_LIBRARIES} lanelet2_extension_lib) - add_rostest_gtest(projector-test test/test_projector.test test/src/test_projector.cpp) - target_link_libraries(projector-test ${catkin_LIBRARIES} lanelet2_extension_lib) - add_rostest_gtest(query-test test/test_query.test test/src/test_query.cpp) - target_link_libraries(query-test ${catkin_LIBRARIES} lanelet2_extension_lib) - add_rostest_gtest(regulatory_elements-test test/test_regulatory_elements.test test/src/test_regulatory_elements.cpp) - target_link_libraries(regulatory_elements-test ${catkin_LIBRARIES} lanelet2_extension_lib) - add_rostest_gtest(utilities-test test/test_utilities.test test/src/test_utilities.cpp) - target_link_libraries(utilities-test ${catkin_LIBRARIES} lanelet2_extension_lib) -endif() diff --git a/map/lanelet2_extension/README.md b/map/lanelet2_extension/README.md deleted file mode 100644 index d38fa8d1cb149..0000000000000 --- a/map/lanelet2_extension/README.md +++ /dev/null @@ -1,65 +0,0 @@ -# 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 wich 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 lanelet_msgs::MapBinMsg -* 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 - -#### Utilties -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. -Currenly 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 meanto 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: -``` -rosrun 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 deleted file mode 100644 index 0ae10872cb01f..0000000000000 Binary files a/map/lanelet2_extension/docs/detection_area.png and /dev/null differ diff --git a/map/lanelet2_extension/docs/extra_regulatory_elements.md b/map/lanelet2_extension/docs/extra_regulatory_elements.md deleted file mode 100644 index 6a4f2e59aba02..0000000000000 --- a/map/lanelet2_extension/docs/extra_regulatory_elements.md +++ /dev/null @@ -1,61 +0,0 @@ -# Extra Regulatory Elements - -## Detection Area -This regualtory element specifies region of interest which vehicle must pay attention whenever it is driving along the associated lanelet. When there are any obstcle 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 - - - -Sample detection area in .osm format is shown below: -``` - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -``` - -## 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 the what road marking it represents (e.g. stopline). - - - diff --git a/map/lanelet2_extension/docs/lanelet2_format_extension.md b/map/lanelet2_extension/docs/lanelet2_format_extension.md deleted file mode 100644 index 8912c8cd0a105..0000000000000 --- a/map/lanelet2_extension/docs/lanelet2_format_extension.md +++ /dev/null @@ -1,165 +0,0 @@ -# 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. - -There are also [extra regulatory elements](./ExtraRegulatoryElements) added for Autoware: -* Detection Area -* Road Marking - -## 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. -``` - - - -``` - -### 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 verticle direction(not the position). - -The Following image illustrates how LineString is used to represent shape of Traffic Light in Autoware. - - - -Here is an example osm syntax for traffic light object. -``` - - - - - - - -``` - -### Turn Diretions -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. - - - -Here is an example of osm sytax for lanelets in intersections. -``` - - - - - - - - - - -``` - -## 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 hiararchy 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: -``` - - - - ... - ... - ... - -``` - -### 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: -``` - - - - - - -``` - -### 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. - - - -Here is an exmaple of osm syntax for light_bulb object: -``` - - - - - - - - - - - - - - - - - - - - - - - - -``` - -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. - - - - -``` - - - - - - - -``` diff --git a/map/lanelet2_extension/docs/light_bulbs.png b/map/lanelet2_extension/docs/light_bulbs.png deleted file mode 100644 index dc1e6e14d7b38..0000000000000 Binary files a/map/lanelet2_extension/docs/light_bulbs.png and /dev/null differ diff --git a/map/lanelet2_extension/docs/road_mark.png b/map/lanelet2_extension/docs/road_mark.png deleted file mode 100644 index 98b41025d5d2b..0000000000000 Binary files a/map/lanelet2_extension/docs/road_mark.png and /dev/null differ diff --git a/map/lanelet2_extension/docs/traffic_light.png b/map/lanelet2_extension/docs/traffic_light.png deleted file mode 100644 index aa97c7bc61c7b..0000000000000 Binary files a/map/lanelet2_extension/docs/traffic_light.png and /dev/null differ diff --git a/map/lanelet2_extension/docs/traffic_light_regulatory_element.png b/map/lanelet2_extension/docs/traffic_light_regulatory_element.png deleted file mode 100644 index 17b75677e81c6..0000000000000 Binary files a/map/lanelet2_extension/docs/traffic_light_regulatory_element.png and /dev/null differ diff --git a/map/lanelet2_extension/docs/turn_direction.png b/map/lanelet2_extension/docs/turn_direction.png deleted file mode 100644 index 3fd63585e10fe..0000000000000 Binary files a/map/lanelet2_extension/docs/turn_direction.png and /dev/null differ diff --git a/map/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.h b/map/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.h deleted file mode 100644 index a6d7ccb38a02a..0000000000000 --- a/map/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * 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_H -#define LANELET2_EXTENSION_IO_AUTOWARE_OSM_PARSER_H - -#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 catched 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_H diff --git a/map/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.h b/map/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.h deleted file mode 100644 index 8dba84f617000..0000000000000 --- a/map/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.h +++ /dev/null @@ -1,115 +0,0 @@ -/* - * 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_H -#define LANELET2_EXTENSION_PROJECTION_MGRS_PROJECTOR_H - -#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 foward 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_H diff --git a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.h b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.h deleted file mode 100644 index 2e5b6e515717f..0000000000000 --- a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.h +++ /dev/null @@ -1,96 +0,0 @@ -/* - * 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_H -#define LANELET2_EXTENSION_REGULATORY_ELEMENTS_AUTOWARE_TRAFFIC_LIGHT_H - -#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 -// defintion errors -/* -#if __cplusplus < 201703L -constexpr char AutowareTrafficLight::RuleName[]; // instanciate string in -cpp file #endif -*/ -} // namespace autoware -} // namespace lanelet - -#endif // LANELET2_EXTENSION_REGULATORY_ELEMENTS_AUTOWARE_TRAFFIC_LIGHT_H diff --git a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.h b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.h deleted file mode 100644 index 01ad87092dabb..0000000000000 --- a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * 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_H -#define LANELET2_EXTENSION_REGULATORY_ELEMENTS_DETECTION_AREA_H - -#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_H diff --git a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.h b/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.h deleted file mode 100644 index 5a8d7fc648a8f..0000000000000 --- a/map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * Copyright 2020 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 LANELET2_EXTENSION_REGULATORY_ELEMENTS_ROAD_MARKING_H -#define LANELET2_EXTENSION_REGULATORY_ELEMENTS_ROAD_MARKING_H - -#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_H diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.h b/map/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.h deleted file mode 100644 index 2252ab6158d05..0000000000000 --- a/map/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.h +++ /dev/null @@ -1,101 +0,0 @@ -/* - * 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_H -#define LANELET2_EXTENSION_UTILITY_MESSAGE_CONVERSION_H - -#include -#include -#include -#include -#include - -#include -#include - -namespace lanelet -{ -namespace utils -{ -namespace conversion -{ -/** - * [toBinMsg convervets 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_lanelet2_msgs::MapBin * 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_lanelet2_msgs::MapBin & msg, lanelet::LaneletMapPtr map); -void fromBinMsg( - const autoware_lanelet2_msgs::MapBin & 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::Point3, - * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ] - * @param dst [converted geometry_msgs point] - */ -void toGeomMsgPt(const geometry_msgs::Point32 & src, geometry_msgs::Point * dst); -void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::Point * dst); -void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::Point * dst); -void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::Point * dst); - -/** - * [toGeomMsgPt converts various point types to geometry_msgs point] - * @param src [input point(geometry_msgs::Point3, - * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ] - * @return [converted geometry_msgs point] - */ -geometry_msgs::Point toGeomMsgPt(const geometry_msgs::Point32 & src); -geometry_msgs::Point toGeomMsgPt(const Eigen::Vector3d & src); -geometry_msgs::Point toGeomMsgPt(const lanelet::ConstPoint3d & src); -geometry_msgs::Point toGeomMsgPt(const lanelet::ConstPoint2d & src); - -lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::Point & src); -void toLaneletPoint(const geometry_msgs::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::Polygon * geom_poly); - -/** - * [toGeomMsgPt32 converts Eigen::Vector3d(lanelet:BasicPoint3d to - * geometry_msgs::Point32)] - * @param src [input point] - * @param dst [conveted point] - */ -void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::Point32 * dst); - -} // namespace conversion -} // namespace utils -} // namespace lanelet - -#endif // LANELET2_EXTENSION_UTILITY_MESSAGE_CONVERSION_H diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/query.h b/map/lanelet2_extension/include/lanelet2_extension/utility/query.h deleted file mode 100644 index db193060be26f..0000000000000 --- a/map/lanelet2_extension/include/lanelet2_extension/utility/query.h +++ /dev/null @@ -1,229 +0,0 @@ -/* - * 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_H -#define LANELET2_EXTENSION_UTILITY_QUERY_H - -#include -#include -#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; -} // 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); - -/** - * [trafficLights extracts Traffic Light regulatory element from lanelets] - * @param lanelets [input lanelets] - * @return [traffic light that are associated with input lanenets] - */ -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 - * lanenets] - */ -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); - -// query all parking lots in lanelet2 map -lanelet::ConstPolygons3d getAllParkingLots(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); - -/** - * [stopSignes extracts stoplines that are associated with stopsignes] - * @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::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::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::Point & search_point); - -bool getClosestLanelet( - const ConstLanelets & lanelets, const geometry_msgs::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); - -/** - * [getPreceedingLaneletSequences 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 getPreceedingLaneletSequences( - const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, - const double length); - -} // namespace query -} // namespace utils -} // namespace lanelet - -#endif // LANELET2_EXTENSION_UTILITY_QUERY_H diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.h b/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.h deleted file mode 100644 index 986765325bb69..0000000000000 --- a/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.h +++ /dev/null @@ -1,69 +0,0 @@ -/* - * 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_H -#define LANELET2_EXTENSION_UTILITY_UTILITIES_H - -#include -#include - -#include -#include - -#include - -namespace lanelet -{ -namespace utils -{ -lanelet::LineString3d generateFineCenterline( - const lanelet::ConstLanelet & lanelet_obj, const double resolution = 5.0); - -/** - * @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); - -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::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::Point & search_point); - -} // namespace utils -} // namespace lanelet - -#endif // LANELET2_EXTENSION_UTILITY_UTILITIES_H diff --git a/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.h b/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.h deleted file mode 100644 index 46e2a5922d7d2..0000000000000 --- a/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.h +++ /dev/null @@ -1,175 +0,0 @@ -/* - * 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_H -#define LANELET2_EXTENSION_VISUALIZATION_VISUALIZATION_H - -#include -#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::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::Polygon * polygon); - -/** - * [lineString2Marker creates marker to visualize shape of linestring] - * @param ls [input linestring] - * @param line_strip [output marker message] - * @param frame_id [frame id of the marker] - * @param ns [namespace of the marker] - * @param c [color of the marker] - * @param lss [thickness of the marker] - */ -void lineString2Marker( - const lanelet::ConstLineString3d ls, visualization_msgs::Marker * line_strip, - const std::string frame_id, const std::string ns, const std_msgs::ColorRGBA c, - const float lss = 0.1); -/** - * [trafficLight2TriangleMarker creates marker to visualize shape of traffic - * lights] - * @param ls [linestring that represents traffic light shape] - * @param marker [created marker] - * @param ns [namespace of the marker] - * @param cl [color of the marker] - * @param duration [lifetime of the marker] - */ -void trafficLight2TriangleMarker( - const lanelet::ConstLineString3d ls, visualization_msgs::Marker * marker, const std::string ns, - const std_msgs::ColorRGBA cl, const ros::Duration duration = ros::Duration(), - 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 visuazlize centerline or not] - * @return [created marker array] - */ -visualization_msgs::MarkerArray laneletsBoundaryAsMarkerArray( - const lanelet::ConstLanelets & lanelets, const std_msgs::ColorRGBA c, const bool viz_centerline); -/** - * [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::MarkerArray laneletsAsTriangleMarkerArray( - const std::string ns, const lanelet::ConstLanelets & lanelets, const std_msgs::ColorRGBA c); - -/** - * [laneletDirectionAsMarkerArray create marker array to visualize direction of - * the lanelet] - * @param lanelets [input lanelets] - * @return [created marker array] - */ -visualization_msgs::MarkerArray laneletDirectionAsMarkerArray( - const lanelet::ConstLanelets lanelets); - -/** - * [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::MarkerArray lineStringsAsMarkerArray( - const std::vector line_strings, const std::string name_space, - const std_msgs::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::MarkerArray autowareTrafficLightsAsMarkerArray( - const std::vector tl_reg_elems, - const std_msgs::ColorRGBA c, const ros::Duration duration = ros::Duration(), - 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::MarkerArray trafficLightsAsTriangleMarkerArray( - const std::vector tl_reg_elems, const std_msgs::ColorRGBA c, - const ros::Duration duration = ros::Duration(), 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::MarkerArray detectionAreasAsMarkerArray( - const std::vector & da_reg_elems, const std_msgs::ColorRGBA c, - const ros::Duration duration = ros::Duration()); - -visualization_msgs::MarkerArray parkingLotsAsMarkerArray( - const lanelet::ConstPolygons3d & parking_lots, const std_msgs::ColorRGBA & c); -visualization_msgs::MarkerArray parkingSpacesAsMarkerArray( - const lanelet::ConstLineStrings3d & parking_spaces, const std_msgs::ColorRGBA & c); - -} // namespace visualization -} // namespace lanelet - -#endif // LANELET2_EXTENSION_VISUALIZATION_VISUALIZATION_H diff --git a/map/lanelet2_extension/lib/autoware_osm_parser.cpp b/map/lanelet2_extension/lib/autoware_osm_parser.cpp deleted file mode 100644 index cc4d2e98f75dc..0000000000000 --- a/map/lanelet2_extension/lib/autoware_osm_parser.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/* - * 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 - -#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 occured 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 deleted file mode 100644 index 14b4688e3ed9f..0000000000000 --- a/map/lanelet2_extension/lib/autoware_traffic_light.cpp +++ /dev/null @@ -1,149 +0,0 @@ -/* - * 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 - -#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; -} - -ConstLineStringsOrPolygons3d getConstLsOrPoly(const RuleParameterMap & params, RoleName role) -{ - auto cast_func = [](auto & lsOrPoly) { - return static_cast(lsOrPoly); - }; - return utils::transform(getLsOrPoly(params, role), cast_func); -} - -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[]; // instanciate 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 deleted file mode 100644 index 3c92e70134ff8..0000000000000 --- a/map/lanelet2_extension/lib/detection_area.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/* - * 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 - -#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[]; // instanciate 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 deleted file mode 100644 index 6abd42bcf85bd..0000000000000 --- a/map/lanelet2_extension/lib/message_conversion.cpp +++ /dev/null @@ -1,195 +0,0 @@ -/* - * 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 -#include -#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_lanelet2_msgs::MapBin * msg) -{ - if (msg == nullptr) { - ROS_ERROR_STREAM(__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_lanelet2_msgs::MapBin & msg, lanelet::LaneletMapPtr map) -{ - if (!map) { - ROS_ERROR_STREAM(__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_lanelet2_msgs::MapBin & 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::Point32 & src, geometry_msgs::Point * dst) -{ - if (dst == nullptr) { - ROS_ERROR_STREAM(__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::Point * dst) -{ - if (dst == nullptr) { - ROS_ERROR_STREAM(__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::Point * dst) -{ - if (dst == nullptr) { - ROS_ERROR_STREAM(__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::Point * dst) -{ - if (dst == nullptr) { - ROS_ERROR_STREAM(__FUNCTION__ << "pointer is null!"); - return; - } - dst->x = src.x(); - dst->y = src.y(); - dst->z = 0; -} - -void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::Point32 * dst) -{ - if (dst == nullptr) { - ROS_ERROR_STREAM(__FUNCTION__ << "pointer is null!"); - return; - } - dst->x = src.x(); - dst->y = src.y(); - dst->z = src.z(); -} - -geometry_msgs::Point toGeomMsgPt(const geometry_msgs::Point32 & src) -{ - geometry_msgs::Point dst; - toGeomMsgPt(src, &dst); - return dst; -} -geometry_msgs::Point toGeomMsgPt(const Eigen::Vector3d & src) -{ - geometry_msgs::Point dst; - toGeomMsgPt(src, &dst); - return dst; -} -geometry_msgs::Point toGeomMsgPt(const lanelet::ConstPoint3d & src) -{ - geometry_msgs::Point dst; - toGeomMsgPt(src, &dst); - return dst; -} -geometry_msgs::Point toGeomMsgPt(const lanelet::ConstPoint2d & src) -{ - geometry_msgs::Point dst; - toGeomMsgPt(src, &dst); - return dst; -} - -lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::Point & src) -{ - lanelet::ConstPoint3d dst; - toLaneletPoint(src, &dst); - return dst; -} - -void toLaneletPoint(const geometry_msgs::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::Polygon * geom_poly) -{ - geom_poly->points.clear(); - geom_poly->points.reserve(ll_poly.size()); - for (const auto & ll_pt : ll_poly) { - geometry_msgs::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 deleted file mode 100644 index d40ba3265c507..0000000000000 --- a/map/lanelet2_extension/lib/mgrs_projector.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/* - * 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 -#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 (GeographicLib::GeographicErr err) { - ROS_ERROR_STREAM(err.what()); - 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_) { - ROS_ERROR_STREAM( - "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 { - ROS_ERROR_STREAM( - "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, 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 (GeographicLib::GeographicErr err) { - ROS_ERROR_STREAM("Failed to convert from MGRS to WGS" << err.what()); - 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 (GeographicLib::GeographicErr err) { - ROS_ERROR_STREAM(err.what()); - } - - setMGRSCode(mgrs_code); -} - -} // namespace projection -} // namespace lanelet diff --git a/map/lanelet2_extension/lib/query.cpp b/map/lanelet2_extension/lib/query.cpp deleted file mode 100644 index e0955a0d771b4..0000000000000 --- a/map/lanelet2_extension/lib/query.cpp +++ /dev/null @@ -1,757 +0,0 @@ -/* - * 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 - -#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 -// PrimitveLayer -> std::vector -lanelet::ConstLanelets query::laneletLayer(const lanelet::LaneletMapConstPtr & ll_map) -{ - lanelet::ConstLanelets lanelets; - if (!ll_map) { - ROS_WARN("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)); -} - -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; -} - -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::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 lanel -std::vector query::stopLinesLanelet(const lanelet::ConstLanelet ll) -{ - std::vector stoplines; - - // find stop lines referened by right ofway 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 elemetn - 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 elemetn - 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::Point & search_point, - const double range) -{ - 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::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::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::Pose & search_pose, - ConstLanelet * closest_lanelet_ptr) -{ - if (closest_lanelet_ptr == nullptr) { - ROS_ERROR("argument closest_lanelet_ptr is null! Failed to find closest lanelet"); - 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; - } - } - } - - 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 sequnce 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> getPreceedingLaneletSequencesRecursive( - const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, - const double length) -{ - std::vector> preceeding_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) { - preceeding_lanelet_sequences.push_back({lanelet}); - return preceeding_lanelet_sequences; - } - - for (const auto & prev_lanelet : prev_lanelets) { - // get lanelet sequnce after prev_lanelet - auto tmp_lanelet_sequences = - getPreceedingLaneletSequencesRecursive(graph, prev_lanelet, length - lanelet_length); - for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) { - tmp_lanelet_sequence.push_back(lanelet); - preceeding_lanelet_sequences.push_back(tmp_lanelet_sequence); - } - } - return preceeding_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::getPreceedingLaneletSequences( - const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet, - const double length) -{ - std::vector lanelet_sequences_vec; - const auto prev_lanelets = graph->previous(lanelet); - for (const auto & prev_lanelet : prev_lanelets) { - // convert deque into vector - const auto lanelet_sequences_deq = - getPreceedingLaneletSequencesRecursive(graph, prev_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; -} - -} // namespace utils -} // namespace lanelet diff --git a/map/lanelet2_extension/lib/road_marking.cpp b/map/lanelet2_extension/lib/road_marking.cpp deleted file mode 100644 index 09ed3f053c1a8..0000000000000 --- a/map/lanelet2_extension/lib/road_marking.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/* - * Copyright 2020 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 - -#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[]; // instanciate 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 deleted file mode 100644 index 36bea9d2c9652..0000000000000 --- a/map/lanelet2_extension/lib/utilities.cpp +++ /dev/null @@ -1,466 +0,0 @@ -/* - * 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 -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace lanelet -{ -namespace utils -{ -namespace -{ -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 - * traversible] - * @param search_point [2D point used for searching] - * @param contacting_lanelet_ids [array of lanelet ids that is contacting with - * search_point] - */ -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) { - ROS_ERROR_STREAM("No lanelet map is set!"); - return; - } - - if (contacting_lanelet_ids == nullptr) { - ROS_ERROR_STREAM(__FUNCTION__ << " contacting_lanelet_ids is null pointer!"); - 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 (auto 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 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; -} - -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) { - ROS_ERROR_STREAM(__func__ << ": polygon is null pointer! Failed to convert to polygon."); - return false; - } - if (linestring.size() != 2) { - ROS_ERROR_STREAM( - __func__ << ": linestring" << linestring.id() << " must have 2 points! (" << linestring.size() - << " != 2)" << std::endl - << "Failed to convert to polygon."); - return false; - } - if (!linestring.hasAttribute("width")) { - ROS_ERROR_STREAM( - __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; -} - -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::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::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()); -} - -} // namespace utils -} // namespace lanelet diff --git a/map/lanelet2_extension/lib/visualization.cpp b/map/lanelet2_extension/lib/visualization.cpp deleted file mode 100644 index f2ec751987e9c..0000000000000 --- a/map/lanelet2_extension/lib/visualization.cpp +++ /dev/null @@ -1,850 +0,0 @@ -/* - * 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 -#include -#include - -#include - -#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::Polygon poly, geometry_msgs::Point32 * p0, - geometry_msgs::Point32 * p1, geometry_msgs::Point32 * p2) -{ - if (p0 == nullptr || p1 == nullptr || p2 == nullptr) { - ROS_ERROR_STREAM(__FUNCTION__ << ": either p0, p1, or p2 is null pointer!"); - 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]; -} - -double hypot(const geometry_msgs::Point32 & p0, const geometry_msgs::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 lightAsMarker( - lanelet::ConstPoint3d p, visualization_msgs::Marker * marker, const std::string ns) -{ - if (marker == nullptr) { - ROS_ERROR_STREAM(__FUNCTION__ << ": marker is null pointer!"); - return; - } - - marker->header.frame_id = "map"; - marker->header.stamp = ros::Time(); - marker->frame_locked - true; - marker->ns = ns; - marker->id = p.id(); - marker->lifetime = ros::Duration(); - marker->type = visualization_msgs::Marker::SPHERE; - marker->pose.position.x = p.x(); - marker->pose.position.y = p.y(); - marker->pose.position.z = 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; - - float s = 0.3; - - marker->scale.x = s; - marker->scale.y = s; - marker->scale.z = s; - - marker->color.r = 0.0f; - marker->color.g = 0.0f; - marker->color.b = 0.0f; - marker->color.a = 0.999f; - - if (isAttributeValue(p, "color", "red")) - marker->color.r = 1.0f; - else if (isAttributeValue(p, "color", "green")) - marker->color.g = 1.0f; - else if (isAttributeValue(p, "color", "yellow")) { - marker->color.r = 1.0f; - marker->color.g = 1.0f; - } else { - marker->color.r = 1.0f; - marker->color.g = 1.0f; - marker->color.b = 1.0f; - } -} - -void laneletDirectionAsMarker( - const lanelet::ConstLanelet ll, visualization_msgs::Marker * marker, const int id, - const std::string ns) -{ - if (marker == nullptr) { - ROS_ERROR_STREAM(__FUNCTION__ << ": marker is null pointer!"); - return; - } - - marker->header.frame_id = "map"; - marker->header.stamp = ros::Time(); - marker->frame_locked = true; - marker->ns = ns; - marker->id = id; - marker->type = visualization_msgs::Marker::TRIANGLE_LIST; - marker->lifetime = ros::Duration(); - - 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(); - float s = 1.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; - - lanelet::Attribute attr = ll.attribute("turn_direction"); - double turn_dir = 0; - - std_msgs::ColorRGBA c; - c.r = 0.0; - c.g = 0.0; - c.b = 1.0; - c.a = 0.6; - - if (isLaneletAttributeValue(ll, "turn_direction", "right")) { - turn_dir = -M_PI / 2.0; - c.r = 1.0; - c.g = 0.0; - c.b = 1.0; - } else if (isLaneletAttributeValue(ll, "turn_direction", "left")) { - turn_dir = M_PI / 2.0; - c.r = 0.0; - c.g = 1.0; - c.b = 1.0; - } - - for (int 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::Point gp[3]; - - for (int i = 0; i < 3; i++) { - std_msgs::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; - } -} - -// Is angle AOB less than 180? -// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e -bool isAcuteAngle( - const geometry_msgs::Point32 & a, const geometry_msgs::Point32 & o, - const geometry_msgs::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::Point32 & a, const geometry_msgs::Point32 & b, - const geometry_msgs::Point32 & c, const geometry_msgs::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::Marker polygonAsMarker( - const lanelet::ConstPolygon3d & polygon, const std::string & name_space, - const std_msgs::ColorRGBA & color) -{ - visualization_msgs::Marker marker; - if (polygon.size() < 3) { - return marker; - } - - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.frame_locked = true; - marker.id = polygon.id(); - marker.ns = name_space; - marker.type = visualization_msgs::Marker::TRIANGLE_LIST; - marker.lifetime = ros::Duration(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; - - geometry_msgs::Polygon geom_poly; - lanelet::utils::conversion::toGeomMsgPoly(polygon, &geom_poly); - - std::vector triangles; - lanelet::visualization::polygon2Triangle(geom_poly, &triangles); - - marker.points.reserve(polygon.size() - 2); - marker.colors.reserve(polygon.size() - 2); - for (const auto & tri : triangles) { - geometry_msgs::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); - } - } - return marker; -} - -} // anonymous namespace - -namespace lanelet -{ -void visualization::lanelet2Triangle( - const lanelet::ConstLanelet & ll, std::vector * triangles) -{ - if (triangles == nullptr) { - ROS_ERROR_STREAM(__FUNCTION__ << ": triangles is null pointer!"); - return; - } - - triangles->clear(); - geometry_msgs::Polygon ll_poly; - lanelet2Polygon(ll, &ll_poly); - polygon2Triangle(ll_poly, triangles); -} - -void visualization::polygon2Triangle( - const geometry_msgs::Polygon & polygon, std::vector * triangles) -{ - geometry_msgs::Polygon poly = polygon; - // 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::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::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) { - ROS_WARN( - "Could not find valid vertex for ear clipping triangulation. Triangulation result might be " - "invalid"); - clipped_vertex = 0; - } - - // create triangle - geometry_msgs::Point32 p0, p1, p2; - adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2); - geometry_msgs::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::Polygon * polygon) -{ - if (polygon == nullptr) { - ROS_ERROR_STREAM(__FUNCTION__ << ": polygon is null pointer!"); - return; - } - - lanelet::CompoundPolygon3d ll_poly = ll.polygon3d(); - - polygon->points.clear(); - polygon->points.reserve(ll_poly.size()); - - for (const auto & pt : ll_poly) { - geometry_msgs::Point32 pt32; - utils::conversion::toGeomMsgPt32(pt.basicPoint(), &pt32); - polygon->points.push_back(pt32); - } -} - -visualization_msgs::MarkerArray visualization::laneletDirectionAsMarkerArray( - const lanelet::ConstLanelets lanelets) -{ - visualization_msgs::MarkerArray marker_array; - int ll_dir_count = 0; - for (auto lli = lanelets.begin(); lli != lanelets.end(); lli++) { - lanelet::ConstLanelet ll = *lli; - - // bool road_found = false; - if (ll.hasAttribute(std::string("turn_direction"))) { - lanelet::Attribute attr = ll.attribute("turn_direction"); - visualization_msgs::Marker marker; - - laneletDirectionAsMarker(ll, &marker, ll_dir_count, "lanelet direction"); - - marker_array.markers.push_back(marker); - ll_dir_count++; - } - } - - return (marker_array); -} - -visualization_msgs::MarkerArray visualization::autowareTrafficLightsAsMarkerArray( - const std::vector tl_reg_elems, - const std_msgs::ColorRGBA c, const ros::Duration duration, const double scale) -{ - visualization_msgs::MarkerArray tl_marker_array; - - int tl_count = 0; - 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 ligths can either polygons or - { // linestrings - lanelet::ConstLineString3d ls = static_cast(lsp); - - visualization_msgs::Marker marker; - visualization::trafficLight2TriangleMarker( - ls, &marker, "traffic_light_triangle", c, duration, scale); - tl_marker_array.markers.push_back(marker); - } - } - - light_bulbs = tl->lightBulbs(); - for (auto ls : light_bulbs) { - lanelet::ConstLineString3d l = static_cast(ls); - - for (auto pt : l) { - if (pt.hasAttribute("color")) { - visualization_msgs::Marker marker; - lightAsMarker(pt, &marker, "traffic_light"); - tl_marker_array.markers.push_back(marker); - - tl_count++; - } - } - } - } - - return (tl_marker_array); -} - -visualization_msgs::MarkerArray visualization::detectionAreasAsMarkerArray( - const std::vector & da_reg_elems, const std_msgs::ColorRGBA c, - const ros::Duration duration) -{ - visualization_msgs::MarkerArray marker_array; - visualization_msgs::Marker marker; - - if (da_reg_elems.empty()) { - return marker_array; - } - - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.frame_locked = true; - marker.ns = "detection_area"; - marker.id = 0; - marker.type = visualization_msgs::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; - - int da_count = 0; - 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::Polygon geom_poly; - utils::conversion::toGeomMsgPoly(detection_area, &geom_poly); - - std::vector triangles; - polygon2Triangle(geom_poly, &triangles); - - for (auto tri : triangles) { - geometry_msgs::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_msgs::Marker ls_marker; - std_msgs::ColorRGBA ls_c; - ls_c.r = 1; - ls_c.g = 1; - ls_c.b = 1; - ls_c.a = 0.999; - visualization::lineString2Marker( - da_reg_elem->stopLine(), &ls_marker, "map", "detection_area", ls_c, 0.1); - marker_array.markers.push_back(ls_marker); - } // for regulatory elements - - return (marker_array); -} - -visualization_msgs::MarkerArray visualization::parkingLotsAsMarkerArray( - const lanelet::ConstPolygons3d & parking_lots, const std_msgs::ColorRGBA & c) -{ - visualization_msgs::MarkerArray marker_array; - - if (parking_lots.empty()) { - return marker_array; - } - - for (const auto & polygon : parking_lots) { - const visualization_msgs::Marker marker = polygonAsMarker(polygon, "parking_lots", c); - if (!marker.points.empty()) { - marker_array.markers.push_back(marker); - } - } - return marker_array; -} -visualization_msgs::MarkerArray visualization::parkingSpacesAsMarkerArray( - const lanelet::ConstLineStrings3d & parking_spaces, const std_msgs::ColorRGBA & c) -{ - visualization_msgs::MarkerArray marker_array; - - if (parking_spaces.empty()) { - return marker_array; - } - - for (const auto & linestring : parking_spaces) { - lanelet::ConstPolygon3d polygon; - if (utils::lineStringWithWidthToPolygon(linestring, &polygon)) { - visualization_msgs::Marker marker = polygonAsMarker(polygon, "parking_space", c); - marker.id = linestring.id(); - if (!marker.points.empty()) { - marker_array.markers.push_back(marker); - } - } else { - ROS_ERROR_STREAM("parking space " << linestring.id() << " failed conversion."); - } - } - return marker_array; -} - -visualization_msgs::MarkerArray visualization::lineStringsAsMarkerArray( - const std::vector line_strings, const std::string name_space, - const std_msgs::ColorRGBA c, const double lss) -{ - std::unordered_set added; - visualization_msgs::MarkerArray ls_marker_array; - for (auto i = line_strings.begin(); i != line_strings.end(); i++) { - const lanelet::ConstLineString3d & ls = *i; - if (!exists(added, ls.id())) { - visualization_msgs::Marker ls_marker; - visualization::lineString2Marker(ls, &ls_marker, "map", name_space, c, 0.1); - ls_marker_array.markers.push_back(ls_marker); - added.insert(ls.id()); - } - } - - return (ls_marker_array); -} - -visualization_msgs::MarkerArray visualization::laneletsBoundaryAsMarkerArray( - const lanelet::ConstLanelets & lanelets, const std_msgs::ColorRGBA c, const bool viz_centerline) -{ - double lss = 0.05; // line string size - std::unordered_set added; - visualization_msgs::MarkerArray marker_array; - 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(); - - visualization_msgs::Marker left_line_strip, right_line_strip, center_line_strip; - if (!exists(added, left_ls.id())) { - visualization::lineString2Marker(left_ls, &left_line_strip, "map", "left_lane_bound", c, lss); - marker_array.markers.push_back(left_line_strip); - added.insert(left_ls.id()); - } - if (!exists(added, right_ls.id())) { - visualization::lineString2Marker( - right_ls, &right_line_strip, "map", "right_lane_bound", c, lss); - marker_array.markers.push_back(right_line_strip); - added.insert(right_ls.id()); - } - if (viz_centerline && !exists(added, center_ls.id())) { - visualization::lineString2Marker( - center_ls, ¢er_line_strip, "map", "center_lane_line", c, std::max(lss * 0.1, 0.01)); - marker_array.markers.push_back(center_line_strip); - added.insert(center_ls.id()); - } - } - return marker_array; -} - -visualization_msgs::MarkerArray visualization::trafficLightsAsTriangleMarkerArray( - const std::vector tl_reg_elems, const std_msgs::ColorRGBA c, - const ros::Duration duration, const double scale) -{ - // convert to to an array of linestrings and publish as marker array using - // exisitng function - - int tl_count = 0; - std::vector line_strings; - visualization_msgs::MarkerArray marker_array; - - 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 ligths can either polygons or - { // linestrings - lanelet::ConstLineString3d ls = static_cast(lsp); - - visualization_msgs::Marker marker; - visualization::trafficLight2TriangleMarker( - ls, &marker, "traffic_light_triangle", c, duration, scale); - marker_array.markers.push_back(marker); - tl_count++; - } - } - } - - return (marker_array); -} - -visualization_msgs::MarkerArray visualization::laneletsAsTriangleMarkerArray( - const std::string ns, const lanelet::ConstLanelets & lanelets, const std_msgs::ColorRGBA c) -{ - visualization_msgs::MarkerArray marker_array; - visualization_msgs::Marker marker; - - if (lanelets.empty()) { - return marker_array; - } - - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.frame_locked = true; - marker.ns = ns; - marker.id = 0; - marker.type = visualization_msgs::Marker::TRIANGLE_LIST; - marker.lifetime = ros::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; - - for (auto ll : lanelets) { - std::vector triangles; - lanelet2Triangle(ll, &triangles); - - for (auto tri : triangles) { - geometry_msgs::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::trafficLight2TriangleMarker( - const lanelet::ConstLineString3d ls, visualization_msgs::Marker * marker, const std::string ns, - const std_msgs::ColorRGBA cl, const ros::Duration duration, const double scale) -{ - if (marker == nullptr) { - ROS_ERROR_STREAM(__FUNCTION__ << ": marker is null pointer!"); - return; - } - marker->header.frame_id = "map"; - marker->header.stamp = ros::Time(); - marker->frame_locked = true; - marker->ns = ns; - marker->id = ls.id(); - marker->type = visualization_msgs::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; - - 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::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::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::lineString2Marker( - const lanelet::ConstLineString3d ls, visualization_msgs::Marker * line_strip, - const std::string frame_id, const std::string ns, const std_msgs::ColorRGBA c, const float lss) -{ - if (line_strip == nullptr) { - ROS_ERROR_STREAM(__FUNCTION__ << ": line_strip is null pointer!"); - return; - } - - line_strip->header.frame_id = frame_id; - line_strip->header.stamp = ros::Time(); - line_strip->frame_locked = true; - line_strip->ns = ns; - line_strip->action = visualization_msgs::Marker::ADD; - line_strip->type = visualization_msgs::Marker::LINE_STRIP; - - line_strip->id = ls.id(); - line_strip->pose.orientation.x = 0.0; - line_strip->pose.orientation.y = 0.0; - line_strip->pose.orientation.z = 0.0; - line_strip->pose.orientation.w = 1.0; - line_strip->scale.x = lss; - line_strip->color = c; - - // fill out lane line - for (auto i = ls.begin(); i != ls.end(); i++) { - geometry_msgs::Point p; - p.x = (*i).x(); - p.y = (*i).y(); - p.z = (*i).z(); - line_strip->points.push_back(p); - } -} - -} // namespace lanelet diff --git a/map/lanelet2_extension/package.xml b/map/lanelet2_extension/package.xml deleted file mode 100644 index d25725d187097..0000000000000 --- a/map/lanelet2_extension/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - lanelet2_extension - 0.1.0 - The lanelet2_extension pacakge contains libraries to handle Lanelet2 format data. - - mitsudome-r - - Apach 2 - - catkin - - roscpp - geographiclib - lanelet2_core - lanelet2_io - lanelet2_maps - lanelet2_projection - lanelet2_routing - lanelet2_traffic_rules - lanelet2_validation - autoware_lanelet2_msgs - geometry_msgs - visualization_msgs - pugixml-dev - roslint - diff --git a/map/lanelet2_extension/src/sample_code.cpp b/map/lanelet2_extension/src/sample_code.cpp deleted file mode 100644 index 3ea7b7aee125f..0000000000000 --- a/map/lanelet2_extension/src/sample_code.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/* - * 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 - -#include -#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 calcaulted 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 acess 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[]) -{ - ros::init(argc, argv, "lanelet2_extension_example"); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - - std::string map_file_path; - pnh.param("map_file", map_file_path, map_file_path); - - 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 deleted file mode 100644 index 116b89c0a4d26..0000000000000 --- a/map/lanelet2_extension/src/validation.cpp +++ /dev/null @@ -1,175 +0,0 @@ -/* - * 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 -#include -#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 - << "rosrun lanelet2_extension autoware_lanelet2_validation" - "_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) { - ROS_FATAL_STREAM(result.description()); - exit(1); - } - - auto osmNode = doc.child("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)) { - ROS_ERROR_STREAM("failed to find elevation tag for node: " << id); - } - } -} - -void validateTrafficLight(const lanelet::LaneletMapPtr lanelet_map) -{ - if (!lanelet_map) { - ROS_FATAL_STREAM("Missing map. Are you sure you set correct path for map?"); - 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) { - ROS_WARN_STREAM( - "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"); - } - for (auto light_string : light->lightBulbs()) { - if (!light_string.hasAttribute("traffic_light_id")) { - ROS_ERROR_STREAM( - "light_bulb " << light_string.id() << " is missing traffic_light_id tag"); - } - } - for (auto base_string_or_poly : light->trafficLights()) { - if (!base_string_or_poly.isLineString()) { - ROS_ERROR_STREAM( - "traffic_light " << base_string_or_poly.id() - << " is polygon, and only linestring class is currently supported for " - "traffic lights"); - } - auto base_string = static_cast(base_string_or_poly); - if (!base_string.hasAttribute("height")) { - ROS_ERROR_STREAM("traffic_light " << base_string.id() << " is missing height tag"); - } - } - } - } -} - -void validateTurnDirection(const lanelet::LaneletMapPtr lanelet_map) -{ - if (!lanelet_map) { - ROS_FATAL_STREAM("Missing map. Are you sure you set correct path for map?"); - 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")) { - ROS_ERROR_STREAM( - "lanelet " << lanelet.id() - << " seems to be intersecting other lanelet, but does " - "not have turn_direction tagging."); - } - } -} - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "autoware_lanelet_valdiation"); - ros::NodeHandle node; - ros::NodeHandle private_rosnode("~"); - - if (!private_rosnode.hasParam("map_file")) { - ROS_FATAL_STREAM("failed find map_file parameter! No file to load"); - printUsage(); - return 1; - } - - std::string map_path = ""; - private_rosnode.getParam("map_file", map_path); - - 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 deleted file mode 100644 index 261358c23039c..0000000000000 --- a/map/lanelet2_extension/test/src/test_message_conversion.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/* - * 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 -#include - -#include - -#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_lanelet2_msgs::MapBin bin_msg; - lanelet::LaneletMapPtr regenerated_map(new lanelet::LaneletMap); - - lanelet::utils::conversion::toBinMsg(single_lanelet_map_ptr, &bin_msg); - - ASSERT_NE(0, 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()) - << "regerated map has different id"; -} - -TEST_F(TestSuite, ToGeomMsgPt) -{ - Point3d lanelet_pt(getId(), -0.1, 0.2, 3.0); - - geometry_msgs::Point32 geom_pt32; - geom_pt32.x = -0.1; - geom_pt32.y = 0.2; - geom_pt32.z = 3.0; - - geometry_msgs::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::Point"; - ASSERT_FLOAT_EQ(geom_pt32.y, geom_pt.y) - << " converted value is different from original geometry_msgs::Point"; - ASSERT_FLOAT_EQ(geom_pt32.z, geom_pt.z) - << " converted value is different from original geometry_msgs::Point"; - - geom_pt = toGeomMsgPt(geom_pt32); - ASSERT_FLOAT_EQ(geom_pt32.x, geom_pt.x) - << " converted value is different from original geometry_msgs::Point"; - ASSERT_FLOAT_EQ(geom_pt32.y, geom_pt.y) - << " converted value is different from original geometry_msgs::Point"; - ASSERT_FLOAT_EQ(geom_pt32.z, geom_pt.z) - << " converted value is different from original geometry_msgs::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); - ros::init(argc, argv, "TestNode"); - return RUN_ALL_TESTS(); -} diff --git a/map/lanelet2_extension/test/src/test_projector.cpp b/map/lanelet2_extension/test/src/test_projector.cpp deleted file mode 100644 index 61d223f30e469..0000000000000 --- a/map/lanelet2_extension/test/src/test_projector.cpp +++ /dev/null @@ -1,87 +0,0 @@ -/* - * 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 - -#include -#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); - ros::init(argc, argv, "TestNode"); - return RUN_ALL_TESTS(); -} diff --git a/map/lanelet2_extension/test/src/test_query.cpp b/map/lanelet2_extension/test/src/test_query.cpp deleted file mode 100644 index 0115f3e63571d..0000000000000 --- a/map/lanelet2_extension/test/src/test_query.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* - * 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 -#include -#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(2, all_lanelets.size()) << "failed to retrieve all lanelets"; - - lanelet::ConstLanelets subtype_lanelets = - lanelet::utils::query::subtypeLanelets(all_lanelets, lanelet::AttributeValueString::Road); - ASSERT_EQ(1, subtype_lanelets.size()) << "failed to retrieve road lanelet by subtypeLanelets"; - - lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); - ASSERT_EQ(1, road_lanelets.size()) << "failed to retrieve road lanelets"; - - lanelet::ConstLanelets crosswalk_lanelets = - lanelet::utils::query::crosswalkLanelets(all_lanelets); - ASSERT_EQ(1, 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(1, traffic_lights.size()) << "failed to retrieve traffic lights"; - - auto autoware_traffic_lights = lanelet::utils::query::autowareTrafficLights(all_lanelets); - ASSERT_EQ(1, 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(1, stop_lines.size()) << "failed to retrieve stop lines from all lanelets"; - - auto stop_lines2 = lanelet::utils::query::stopLinesLanelet(road_lanelets.front()); - ASSERT_EQ(1, stop_lines2.size()) << "failed to retrieve stop lines from a lanelet"; -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "TestNode"); - 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 deleted file mode 100644 index ad9f718b87f38..0000000000000 --- a/map/lanelet2_extension/test/src/test_regulatory_elements.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/* - * 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 -#include - -#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); - ros::init(argc, argv, "TestNode"); - return RUN_ALL_TESTS(); -} diff --git a/map/lanelet2_extension/test/src/test_utilities.cpp b/map/lanelet2_extension/test/src/test_utilities.cpp deleted file mode 100644 index f842a20e95d71..0000000000000 --- a/map/lanelet2_extension/test/src/test_utilities.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* - * 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 -#include -#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); - ros::init(argc, argv, "TestNode"); - return RUN_ALL_TESTS(); -} diff --git a/map/lanelet2_extension/test/test_message_conversion.test b/map/lanelet2_extension/test/test_message_conversion.test deleted file mode 100644 index 0147c064737bc..0000000000000 --- a/map/lanelet2_extension/test/test_message_conversion.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/map/lanelet2_extension/test/test_projector.test b/map/lanelet2_extension/test/test_projector.test deleted file mode 100644 index 70cfff5cf1471..0000000000000 --- a/map/lanelet2_extension/test/test_projector.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/map/lanelet2_extension/test/test_query.test b/map/lanelet2_extension/test/test_query.test deleted file mode 100644 index 648bbbd9f90d6..0000000000000 --- a/map/lanelet2_extension/test/test_query.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/map/lanelet2_extension/test/test_regulatory_elements.test b/map/lanelet2_extension/test/test_regulatory_elements.test deleted file mode 100644 index 8e064ff43a77a..0000000000000 --- a/map/lanelet2_extension/test/test_regulatory_elements.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/map/lanelet2_extension/test/test_utilities.test b/map/lanelet2_extension/test/test_utilities.test deleted file mode 100644 index 8b204c685af4c..0000000000000 --- a/map/lanelet2_extension/test/test_utilities.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt deleted file mode 100644 index cf81a0f26a2a7..0000000000000 --- a/map/map_loader/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(map_loader) - -add_compile_options(-std=c++14) - -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - lanelet2_extension - pcl_ros - roscpp - std_msgs - tf2_geometry_msgs - tf2_ros - visualization_msgs -) - -find_package(Boost REQUIRED COMPONENTS filesystem) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - geometry_msgs - std_msgs - tf2_geometry_msgs - visualization_msgs - DEPENDS - Boost -) - -include_directories( - include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -add_executable(pointcloud_map_loader src/pointcloud_map_loader/main.cpp src/pointcloud_map_loader/pointcloud_map_loader_node.cpp) -target_link_libraries(pointcloud_map_loader ${Boost_LIBRARIES} ${catkin_LIBRARIES}) -add_dependencies(pointcloud_map_loader ${catkin_EXPORTED_TARGETS}) - -add_executable(lanelet2_map_loader src/lanelet2_map_loader/lanelet2_map_loader.cpp) -target_link_libraries(lanelet2_map_loader ${catkin_LIBRARIES}) -add_dependencies(lanelet2_map_loader ${catkin_EXPORTED_TARGETS}) - -add_executable(lanelet2_map_visualization src/lanelet2_map_loader/lanelet2_map_visualization.cpp) -target_link_libraries(lanelet2_map_visualization ${catkin_LIBRARIES}) -add_dependencies(lanelet2_map_visualization ${catkin_EXPORTED_TARGETS}) - -## Install executables and/or libraries -install( - TARGETS - pointcloud_map_loader - lanelet2_map_loader - lanelet2_map_visualization - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Install project namespaced headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -## Install files -install( - DIRECTORY - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/map/map_loader/README.md b/map/map_loader/README.md deleted file mode 100644 index 3c3d34e1376a6..0000000000000 --- a/map/map_loader/README.md +++ /dev/null @@ -1,54 +0,0 @@ -# map_loader package - -This package provides the features of loading various maps. - -## pointcloud_map_loader - -### Feature - -pointcloud_map_loader loads PointCloud file and publish the map data as sensor_msgs/PointCloud2 message. - -### How to run - -`rosrun map_loader pointcloud_map_loader 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 publish the map data as autoware_lanelet2_msgs/MapBin message. -The node projects lan/lon coordinates into MGRS coordinates. - -### How to run - -`rosrun map_loader lanelet2_map_loader 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 - -`rosrun 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 diff --git a/map/map_loader/include/map_loader/pointcloud_map_loader_node.h b/map/map_loader/include/map_loader/pointcloud_map_loader_node.h deleted file mode 100644 index f4dfee3d6e59a..0000000000000 --- a/map/map_loader/include/map_loader/pointcloud_map_loader_node.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright 2020 TierIV. 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. - */ - -/* - * 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 -#include - -#include - -#include - -class PointCloudMapLoaderNode -{ -public: - explicit PointCloudMapLoaderNode(const std::vector & pcd_paths); - -private: - ros::NodeHandle nh_{""}; - ros::NodeHandle private_nh_{"~"}; - - ros::Publisher pub_pointcloud_map_; - - sensor_msgs::PointCloud2 loadPCDFiles(const std::vector & pcd_paths); -}; diff --git a/map/map_loader/launch/lanelet2_map_loader.launch b/map/map_loader/launch/lanelet2_map_loader.launch deleted file mode 100644 index be11a2f016585..0000000000000 --- a/map/map_loader/launch/lanelet2_map_loader.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/map/map_loader/launch/pointcloud_map_loader.launch b/map/map_loader/launch/pointcloud_map_loader.launch deleted file mode 100644 index 251f9fada03c3..0000000000000 --- a/map/map_loader/launch/pointcloud_map_loader.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml deleted file mode 100644 index 4935505e55f86..0000000000000 --- a/map/map_loader/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - map_loader - 0.1.0 - The map_loader package - mitsudome-r - Kenji Miyake - Apache 2 - - catkin - - geometry_msgs - lanelet2_extension - libboost-filesystem-dev - libpcl-all-dev - pcl_ros - roscpp - std_msgs - tf2_geometry_msgs - tf2_ros - tf - visualization_msgs - - diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader.cpp deleted file mode 100644 index 8b5edf0c62c5e..0000000000000 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/* - * 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 - -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include - -void printUsage() -{ - ROS_ERROR_STREAM("Usage:"); - ROS_ERROR_STREAM("rosrun map_loader lanelet2_map_loader [.OSM]"); - ROS_ERROR_STREAM( - "rosrun map_loader lanelet2_map_loader download [X] [Y]: WARNING not implemented"); -} - -int main(int argc, char ** argv) -{ - ros::init(argc, argv, "lanelet_map_loader"); - ros::NodeHandle pnh("~"); - - if (argc < 2) { - printUsage(); - return EXIT_FAILURE; - } - - std::string mode(argv[1]); - if (mode == "download" && argc < 4) { - printUsage(); - return EXIT_FAILURE; - } - - std::string lanelet2_filename(argv[1]); - - lanelet::ErrorMessages errors; - - lanelet::projection::MGRSProjector projector; - lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - - for (const auto & error : errors) { - ROS_ERROR_STREAM(error); - } - if (!errors.empty()) { - return EXIT_FAILURE; - } - - double center_line_resolution; - pnh.param("center_line_resolution", 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); - - ros::Publisher map_bin_pub = - pnh.advertise("output/lanelet2_map", 1, true); - autoware_lanelet2_msgs::MapBin map_bin_msg; - map_bin_msg.header.stamp = ros::Time::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); - - map_bin_pub.publish(map_bin_msg); - - ros::spin(); - - return 0; -} diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization.cpp deleted file mode 100644 index 15d21baff1a4f..0000000000000 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/* - * 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 - -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include - -static bool g_viz_lanelets_centerline = true; -static ros::Publisher g_map_pub; - -void insertMarkerArray( - visualization_msgs::MarkerArray * a1, const visualization_msgs::MarkerArray & a2) -{ - a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); -} - -void setColor(std_msgs::ColorRGBA * cl, double r, double g, double b, double a) -{ - cl->r = r; - cl->g = g; - cl->b = b; - cl->a = a; -} - -void binMapCallback(autoware_lanelet2_msgs::MapBin msg) -{ - lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); - - lanelet::utils::conversion::fromBinMsg(msg, viz_lanelet_map); - ROS_INFO("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 crosswalk_lanelets = - lanelet::utils::query::crosswalkLanelets(all_lanelets); - 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); - lanelet::ConstLineStrings3d parking_spaces = - lanelet::utils::query::getAllParkingSpaces(viz_lanelet_map); - lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map); - - std_msgs::ColorRGBA cl_road, cl_cross, cl_ll_borders, cl_stoplines, cl_trafficlights, - cl_detection_areas, cl_parking_lots, cl_parking_spaces; - setColor(&cl_road, 0.2, 0.7, 0.7, 0.3); - setColor(&cl_cross, 0.2, 0.7, 0.2, 0.3); - setColor(&cl_ll_borders, 1.0, 1.0, 1.0, 0.999); - setColor(&cl_stoplines, 1.0, 0.0, 0.0, 0.5); - setColor(&cl_trafficlights, 0.7, 0.7, 0.7, 0.8); - setColor(&cl_detection_areas, 0.7, 0.7, 0.7, 0.3); - setColor(&cl_parking_lots, 0.7, 0.7, 0.0, 0.3); - setColor(&cl_parking_spaces, 1.0, 0.647, 0.0, 0.6); - - visualization_msgs::MarkerArray map_marker_array; - - insertMarkerArray( - &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( - road_lanelets, cl_ll_borders, g_viz_lanelets_centerline)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( - "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( - "walkway_lanelets", walkway_lanelets, cl_cross)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.5)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_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)); - - g_map_pub.publish(map_marker_array); -} - -int main(int argc, char ** argv) -{ - ros::init(argc, argv, "lanelet2_map_visualizer"); - ros::NodeHandle pnh("~"); - ros::Subscriber bin_map_sub; - - bin_map_sub = pnh.subscribe("input/lanelet2_map", 1, binMapCallback); - g_map_pub = pnh.advertise("output/lanelet2_map_marker", 1, true); - - ros::spin(); - - return 0; -} diff --git a/map/map_loader/src/pointcloud_map_loader/main.cpp b/map/map_loader/src/pointcloud_map_loader/main.cpp deleted file mode 100644 index f7046b737fff7..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/main.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Copyright 2020 TierIV. 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. - */ - -/* - * 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 - -#include - -#include - -namespace fs = boost::filesystem; - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "pointcloud_map_loader"); - - std::vector pcd_paths; - for (int i = 1; i < argc; ++i) { - const fs::path arg(argv[i]); - - if (!fs::exists(arg)) { - const std::string msg = "invalid path: " + arg.string(); - throw std::runtime_error(msg); - } - - if (fs::is_regular_file(arg)) { - pcd_paths.push_back(argv[i]); - } - - if (fs::is_directory(arg)) { - for (const auto & f : fs::directory_iterator(arg)) { - const auto & p = f.path(); - - if (!fs::is_regular_file(p)) { - continue; - } - - if (p.extension() != ".pcd" && p.extension() != ".PCD") { - continue; - } - - pcd_paths.push_back(p.string()); - } - } - } - - PointCloudMapLoaderNode pointcloud_map_loader_node(pcd_paths); - - ros::spin(); - - return 0; -} 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 deleted file mode 100644 index 87b28db056d6d..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright 2020 TierIV. 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. - */ - -/* - * 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 - -#include -#include - -PointCloudMapLoaderNode::PointCloudMapLoaderNode(const std::vector & pcd_paths) -{ - pub_pointcloud_map_ = - private_nh_.advertise("output/pointcloud_map", 1, true); - - const auto pcd = loadPCDFiles(pcd_paths); - - if (pcd.width == 0) { - ROS_ERROR("No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size()); - return; - } - - pub_pointcloud_map_.publish(pcd); -} - -sensor_msgs::PointCloud2 PointCloudMapLoaderNode::loadPCDFiles( - const std::vector & pcd_paths) -{ - sensor_msgs::PointCloud2 whole_pcd{}; - - sensor_msgs::PointCloud2 partial_pcd; - for (const auto & path : pcd_paths) { - if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { - ROS_ERROR_STREAM("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; -} diff --git a/map/map_tf_generator/CMakeLists.txt b/map/map_tf_generator/CMakeLists.txt deleted file mode 100644 index dec52fe33cc31..0000000000000 --- a/map/map_tf_generator/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(map_tf_generator) - -add_compile_options(-std=c++14) - -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - pcl_ros - tf2 - tf2_ros -) - -catkin_package() - -include_directories( - ${catkin_INCLUDE_DIRS} -) - -add_executable( - map_tf_generator nodes/map_tf_generator.cpp -) - -target_link_libraries( - map_tf_generator ${catkin_LIBRARIES} -) - -install(TARGETS map_tf_generator - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install( - DIRECTORY - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/map/map_tf_generator/launch/map_tf_generator.launch b/map/map_tf_generator/launch/map_tf_generator.launch deleted file mode 100644 index 3deb1092c2bc6..0000000000000 --- a/map/map_tf_generator/launch/map_tf_generator.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/map/map_tf_generator/nodes/map_tf_generator.cpp b/map/map_tf_generator/nodes/map_tf_generator.cpp deleted file mode 100644 index e4529e03359bc..0000000000000 --- a/map/map_tf_generator/nodes/map_tf_generator.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright 2020 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 -#include -#include -#include -#include -#include - -typedef pcl::PointCloud PointCloud; -std::string map_frame_ = "map"; -std::string viewer_frame_ = "viewer"; - -void Callback(const PointCloud::ConstPtr & clouds) -{ - const unsigned int sum = clouds->points.size(); - double coordinate[3] = {0, 0, 0}; - for (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::TransformStamped static_transformStamped; - static_transformStamped.header.stamp = ros::Time::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 tf2_ros::StaticTransformBroadcaster static_broadcaster; - static_broadcaster.sendTransform(static_transformStamped); - - ROS_INFO_STREAM( - "broadcast static tf. map_frame:" << map_frame_ << ", viewer_frame:" << viewer_frame_ - << ", x:" << coordinate[0] << ", y:" << coordinate[1] - << ", z:" << coordinate[2]); -} - -int main(int argc, char ** argv) -{ - ros::init(argc, argv, "map_tf_generator"); - ros::NodeHandle nh; - ros::NodeHandle nh_private("~"); - - nh_private.getParam("map_frame", map_frame_); - nh_private.getParam("viewer_frame", viewer_frame_); - - ros::Subscriber sub = nh.subscribe("pointcloud_map", 1, &Callback); - - ros::spin(); - - return 0; -}; diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml deleted file mode 100644 index cd0d55d9e90f3..0000000000000 --- a/map/map_tf_generator/package.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - map_tf_generator - 0.1.0 - The map_tf_generator package - azumi-suzuki - Apache 2 - - catkin - - roscpp - std_msgs - tf2 - tf2_ros - - pcl_ros - diff --git a/map/util/lanelet2_map_preprocessor/CMakeLists.txt b/map/util/lanelet2_map_preprocessor/CMakeLists.txt deleted file mode 100644 index 7c2225f5a58c1..0000000000000 --- a/map/util/lanelet2_map_preprocessor/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -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/package.xml b/map/util/lanelet2_map_preprocessor/package.xml deleted file mode 100644 index 16c6b1ba4acdc..0000000000000 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - lanelet2_map_preprocessor - 0.1.0 - The lanelet2_map_preprocessor package - - - - - mitsudome-r - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 deleted file mode 100644 index aef6aeed1d8ea..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/* - * Copyright 2020 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 - -#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 deleted file mode 100644 index 4cb14c6558309..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp +++ /dev/null @@ -1,192 +0,0 @@ -/* - * Copyright 2020 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 - -#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 deleted file mode 100644 index 186d6d405b719..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/* - * Copyright 2020 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 - -#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 deleted file mode 100644 index 73f748fcecc83..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/* - * Copyright 2020 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 - -#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 deleted file mode 100644 index f2be7632b1de2..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/* - * Copyright 2020 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 - -#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 deleted file mode 100644 index 08f4667dc76a2..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/* - * Copyright 2020 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 - -#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; -}