From a557f298011e0479df2f8b979c91f5f64e37ea1d Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 23 Aug 2022 14:27:24 +0900 Subject: [PATCH] feat(had_map_utils, tier4_integration_launch)!: remove unused packages (#1659) Signed-off-by: Kenji Miyake Signed-off-by: Kenji Miyake --- common/had_map_utils/CMakeLists.txt | 30 - .../had_map_utils/had_map_computation.hpp | 39 -- .../had_map_utils/had_map_conversion.hpp | 45 -- .../include/had_map_utils/had_map_query.hpp | 60 -- .../include/had_map_utils/had_map_utils.hpp | 46 -- .../had_map_utils/had_map_visualization.hpp | 258 --------- .../had_map_utils/visibility_control.hpp | 38 -- common/had_map_utils/package.xml | 33 -- .../had_map_utils/src/had_map_computation.cpp | 127 ----- .../had_map_utils/src/had_map_conversion.cpp | 72 --- common/had_map_utils/src/had_map_query.cpp | 128 ----- common/had_map_utils/src/had_map_utils.cpp | 174 ------ .../src/had_map_visualization.cpp | 516 ------------------ .../tier4_integration_launch/CMakeLists.txt | 11 - launch/tier4_integration_launch/README.md | 13 - .../integration_launch.drawio.svg | 228 -------- .../launch/ci_planning_simulator.launch.xml | 24 - .../launch/release.launch.xml | 21 - launch/tier4_integration_launch/package.xml | 25 - 19 files changed, 1888 deletions(-) delete mode 100644 common/had_map_utils/CMakeLists.txt delete mode 100644 common/had_map_utils/include/had_map_utils/had_map_computation.hpp delete mode 100644 common/had_map_utils/include/had_map_utils/had_map_conversion.hpp delete mode 100644 common/had_map_utils/include/had_map_utils/had_map_query.hpp delete mode 100644 common/had_map_utils/include/had_map_utils/had_map_utils.hpp delete mode 100644 common/had_map_utils/include/had_map_utils/had_map_visualization.hpp delete mode 100644 common/had_map_utils/include/had_map_utils/visibility_control.hpp delete mode 100644 common/had_map_utils/package.xml delete mode 100644 common/had_map_utils/src/had_map_computation.cpp delete mode 100644 common/had_map_utils/src/had_map_conversion.cpp delete mode 100644 common/had_map_utils/src/had_map_query.cpp delete mode 100644 common/had_map_utils/src/had_map_utils.cpp delete mode 100644 common/had_map_utils/src/had_map_visualization.cpp delete mode 100644 launch/tier4_integration_launch/CMakeLists.txt delete mode 100644 launch/tier4_integration_launch/README.md delete mode 100644 launch/tier4_integration_launch/integration_launch.drawio.svg delete mode 100644 launch/tier4_integration_launch/launch/ci_planning_simulator.launch.xml delete mode 100644 launch/tier4_integration_launch/launch/release.launch.xml delete mode 100644 launch/tier4_integration_launch/package.xml diff --git a/common/had_map_utils/CMakeLists.txt b/common/had_map_utils/CMakeLists.txt deleted file mode 100644 index 7daa4a439922..000000000000 --- a/common/had_map_utils/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(had_map_utils) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(CGAL REQUIRED COMPONENTS Core) -find_package(Eigen3 REQUIRED) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/had_map_utils.cpp - src/had_map_computation.cpp - src/had_map_conversion.cpp - src/had_map_query.cpp - src/had_map_visualization.cpp) - -# Disable warnings due to external dependencies -get_target_property(lanelet2_core_INCLUDE_DIR - lanelet2_core::lanelet2_core INTERFACE_INCLUDE_DIRECTORIES -) -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - ${EIGEN3_INCLUDE_DIR} - ${lanelet2_core_INCLUDE_DIR} - ${rclcpp_INCLUDE_DIRS} -) - -set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE) -target_link_libraries(${PROJECT_NAME} CGAL CGAL::CGAL CGAL::CGAL_Core) - -ament_auto_package() diff --git a/common/had_map_utils/include/had_map_utils/had_map_computation.hpp b/common/had_map_utils/include/had_map_utils/had_map_computation.hpp deleted file mode 100644 index 9c5f683984e1..000000000000 --- a/common/had_map_utils/include/had_map_utils/had_map_computation.hpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright 2021 Tier IV, Inc -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_ -#define HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_ - -#include "visibility_control.hpp" - -#include - -#include - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -lanelet::Polygon3d HAD_MAP_UTILS_PUBLIC coalesce_drivable_areas( - const autoware_auto_planning_msgs::msg::HADMapRoute & had_map_route, - const lanelet::LaneletMapPtr & lanelet_map_ptr); - -} // namespace had_map_utils -} // namespace common -} // namespace autoware - -#endif // HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp b/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp deleted file mode 100644 index 080773ace6f8..000000000000 --- a/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2020 Tier IV, Inc -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_ -#define HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_ - -#include "had_map_utils/visibility_control.hpp" - -#include - -#include - -#include - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -void HAD_MAP_UTILS_PUBLIC toBinaryMsg( - const std::shared_ptr & map, - autoware_auto_mapping_msgs::msg::HADMapBin & msg); - -void HAD_MAP_UTILS_PUBLIC fromBinaryMsg( - const autoware_auto_mapping_msgs::msg::HADMapBin & msg, - std::shared_ptr & map); - -} // namespace had_map_utils -} // namespace common -} // namespace autoware - -#endif // HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_query.hpp b/common/had_map_utils/include/had_map_utils/had_map_query.hpp deleted file mode 100644 index 9a6654605d5e..000000000000 --- a/common/had_map_utils/include/had_map_utils/had_map_query.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2020 Tier IV, Inc -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_ -#define HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_ - -#include "had_map_utils/visibility_control.hpp" - -#include -#include -#include -#include -#include - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -lanelet::Areas HAD_MAP_UTILS_PUBLIC getAreaLayer(const lanelet::LaneletMapPtr ll_map); - -lanelet::Areas HAD_MAP_UTILS_PUBLIC subtypeAreas(const lanelet::Areas areas, const char subtype[]); - -lanelet::Polygons3d HAD_MAP_UTILS_PUBLIC getPolygonLayer(const lanelet::LaneletMapPtr ll_map); - -lanelet::Polygons3d HAD_MAP_UTILS_PUBLIC -subtypePolygons(const lanelet::Polygons3d polygons, const char subtype[]); - -lanelet::LineStrings3d HAD_MAP_UTILS_PUBLIC getLineStringLayer(const lanelet::LaneletMapPtr ll_map); - -lanelet::LineStrings3d HAD_MAP_UTILS_PUBLIC -subtypeLineStrings(const lanelet::LineStrings3d linestrings, const char subtype[]); - -lanelet::ConstLanelets HAD_MAP_UTILS_PUBLIC -getConstLaneletLayer(const std::shared_ptr & ll_map); - -lanelet::Lanelets HAD_MAP_UTILS_PUBLIC -getLaneletLayer(const std::shared_ptr & ll_map); - -} // namespace had_map_utils -} // namespace common -} // namespace autoware - -#endif // HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_utils.hpp b/common/had_map_utils/include/had_map_utils/had_map_utils.hpp deleted file mode 100644 index 3d5549b72ad3..000000000000 --- a/common/had_map_utils/include/had_map_utils/had_map_utils.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2020 Tier IV, Inc -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_ -#define HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_ - -#include "had_map_utils/visibility_control.hpp" - -#include - -#include -#include -#include - -#include - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -using autoware::common::types::float64_t; - -void HAD_MAP_UTILS_PUBLIC overwriteLaneletsCenterline( - lanelet::LaneletMapPtr lanelet_map, const autoware::common::types::bool8_t force_overwrite); -lanelet::LineString3d HAD_MAP_UTILS_PUBLIC -generateFineCenterline(const lanelet::ConstLanelet & lanelet_obj, const float64_t resolution); - -} // namespace had_map_utils -} // namespace common -} // namespace autoware - -#endif // HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp b/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp deleted file mode 100644 index f1113778de64..000000000000 --- a/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp +++ /dev/null @@ -1,258 +0,0 @@ -// Copyright 2020 Tier IV, Inc -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_ -#define HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_ - -#include "had_map_utils/visibility_control.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ -/** - * \brief Set set rgba information to a Color Object - * \param[out] cl color object to be set - * \param r red value - * \param g green value - * \param b blue value - * \param a alpha value - */ -void HAD_MAP_UTILS_PUBLIC setColor( - std_msgs::msg::ColorRGBA * cl, const float32_t & r, const float32_t & g, const float32_t & b, - const float32_t & a); - -/** - * \brief Set the header information to a marker object - * \param m input marker - * \param id id of the marker - * \param t timestamp of the marker - * \param frame_id frame of the marker - * \param ns namespace of the marker - * \param c color of the marker - * \param action action used to visualize the marker - * \param type type of the marker - * \param scale scale of the marker - * \return visualization_msgs::msg::Marker - */ -void HAD_MAP_UTILS_PUBLIC setMarkerHeader( - visualization_msgs::msg::Marker * m, const int32_t & id, const rclcpp::Time & t, - const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, - const int32_t & action, const int32_t & type, const float32_t & scale); - -/** - * \brief creates marker with type LINE_STRIP from a lanelet::LineString3d object - * \param t timestamp set to the marker - * \param ls input linestring - * \param frame_id frame id set to the marker - * \param ns namespace set to the marker - * \param c color of the marker - * \param lss linestrip scale (i.e. width) - * \return created visualization_msgs::msg::Marker - */ -visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker( - const rclcpp::Time & t, const lanelet::LineString3d & ls, const std::string & frame_id, - const std::string & ns, const std_msgs::msg::ColorRGBA & c, const float32_t & lss); - -/** - * \brief creates marker with type LINE_STRIP from a lanelet::ConstLineString3d object - * \param t timestamp set to the marker - * \param ls input linestring - * \param frame_id frame id set to the marker - * \param ns namespace set to the marker - * \param c color of the marker - * \param lss linestrip scale (i.e. width) - * \return created visualization_msgs::msg::Marker - */ -visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker( - const rclcpp::Time & t, const lanelet::ConstLineString3d & ls, const std::string & frame_id, - const std::string & ns, const std_msgs::msg::ColorRGBA & c, const float32_t & lss); - -/** - * \brief converts lanelet::LineString into markers with type LINE_STRIP - * \param t time set to returned marker message - * \param ns namespace set to the marker - * \param linestrings input linestring objects - * \param c color of the marker - * \return created visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC lineStringsAsMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::LineStrings3d & linestrings, - const std_msgs::msg::ColorRGBA & c); - -/** - * \brief converts outer bound of lanelet::Lanelet into markers with type LINE_STRIP - * \param t time set to returned marker message - * \param lanelets input lanelet objects - * \param c color of the marker - * \param viz_centerline option to add centerline to the marker array - * \return created visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC laneletsBoundaryAsMarkerArray( - const rclcpp::Time & t, const lanelet::ConstLanelets & lanelets, - const std_msgs::msg::ColorRGBA & c, const bool8_t & viz_centerline); - -/** - * \brief creates marker with type LINE_STRIP from a lanelet::BasicPolygon object - * \param t timestamp set to the marker - * \param line_id id set to the marker - * \param pg input polygon - * \param frame_id frame id set to the marker - * \param ns namespace set to the marker - * \param c color of the marker - * \param lss linestrip scale (i.e. width) - * \return created visualization_msgs::msg::Marker - */ -visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC basicPolygon2Marker( - const rclcpp::Time & t, const int32_t & line_id, const lanelet::BasicPolygon3d & pg, - const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, - const float32_t & lss); - -/** - * \brief converts outer bound of lanelet::Area into markers with type LINE_STRIP - * \param t time set to returned marker message - * \param ns namespace set to the marker - * \param areas input area objects - * \param c color of the marker - * \return created visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC areasBoundaryAsMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::Areas & areas, - const std_msgs::msg::ColorRGBA & c); - -/** - * \brief converts outer bound of lanelet::Polygon into markers with type LINE_STRIP - * \param t Time set to returned marker message - * \param ns namespace set to the marker - * \param polygons input polygons - * \param c color of the marker - * \return created visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC polygonsBoundaryAsMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::Polygons3d & polygons, - const std_msgs::msg::ColorRGBA & c); - -/** - * \brief creates marker with type LINE_STRIP from a bounding box - * \param t Time set to returned marker message - * \param line_id id set to marker - * \param lower lower bound of the bounding box with length 3(x,y,z) - * \param upper upper bound of the bounding box with length 3(x,y,z) - * \param frame_id frame id set to the marker - * \param ns namespace set to the marker - * \param c color of the marker - * \param lss linestrip scale (i.e. width) - * \return created visualization_msgs::msg::Marker - */ -visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC bbox2Marker( - const rclcpp::Time & t, const int32_t & line_id, const float64_t lower[], const float64_t upper[], - const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, - const float32_t & lss); - -/** - * \brief creates marker array from bounding box - * \param t Time set to returned marker message - * \param ns Namespace set to returned marker message - * \param upper upper bound of the bounding box with length 3(x,y,z) - * \param lower lower bound of the bounding box with length 3(x,y,z) - * \param c Color of the marker array - * \return created visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC boundingBoxAsMarkerArray( - const rclcpp::Time & t, const std::string & ns, const float64_t upper[], const float64_t lower[], - const std_msgs::msg::ColorRGBA & c); - -/** - * \brief converts area enclosed by lanelet::Lanelet into list of triangles. - * \param ll input lanelet - * \return result of triangulation - */ -std::vector HAD_MAP_UTILS_PUBLIC -lanelet2Triangle(const lanelet::ConstLanelet & ll); - -/** - * \brief converts area enclosed by geometry_msg::msg::Polygon into list of triangles. - * \param polygon input polygon - * \return result of triangulation - */ -std::vector HAD_MAP_UTILS_PUBLIC -polygon2Triangle(const geometry_msgs::msg::Polygon & polygon); - -/** - * \brief converts lanelet::Area into geometry_msgs::msg::Polygon type - * \param area input area - * \return converted geometry_msgs::msg::Polygon - */ -geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC area2Polygon(const lanelet::ConstArea & area); - -/** - * \brief converts lanelet::Lanelet into geometry_msgs::msg::Polygon type - * \param ll input lanelet - * \return converted geometry_msgs::msg::Polygon - */ -geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC lanelet2Polygon(const lanelet::ConstLanelet & ll); - -/** - * \brief converts bounded area by lanelet::Lanelet into triangle markers - * \param t Time set to returned marker message - * \param ns Namespace set to returned marker message - * \param lanelets input lanelet::Lanelet - * \param c Color of the marker array - * \return Converted triangle markers enclosed by the Lanelet - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC laneletsAsTriangleMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::ConstLanelets & lanelets, - const std_msgs::msg::ColorRGBA & c); - -/** - * \brief converts bounded area by lanelet::Area into triangle markers - * \param t Time set to returned marker message - * \param ns Namespace set to returned marker message - * \param areas input lanelet::Area objects - * \param c Color of the marker array - * \return Converted triangle markers enclosed by the area - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC areasAsTriangleMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::Areas & areas, - const std_msgs::msg::ColorRGBA & c); - -} // namespace had_map_utils -} // namespace common -} // namespace autoware - -#endif // HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/visibility_control.hpp b/common/had_map_utils/include/had_map_utils/visibility_control.hpp deleted file mode 100644 index 978163fdc85a..000000000000 --- a/common/had_map_utils/include/had_map_utils/visibility_control.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_ -#define HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_ - -#if defined(_MSC_VER) && defined(_WIN64) -#if defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS) -#define HAD_MAP_UTILS_PUBLIC __declspec(dllexport) -#define HAD_MAP_UTILS_LOCAL -#else // defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS) -#define HAD_MAP_UTILS_PUBLIC __declspec(dllimport) -#define HAD_MAP_UTILS_LOCAL -#endif // defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS) -#elif defined(__GNUC__) && defined(__linux__) -#define HAD_MAP_UTILS_PUBLIC __attribute__((visibility("default"))) -#define HAD_MAP_UTILS_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__GNUC__) && defined(__APPLE__) -#define HAD_MAP_UTILS_PUBLIC __attribute__((visibility("default"))) -#define HAD_MAP_UTILS_LOCAL __attribute__((visibility("hidden"))) -#else // !(defined(__GNUC__) && defined(__APPLE__)) -#error "Unsupported Build Configuration" -#endif // _MSC_VER - -#endif // HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_ diff --git a/common/had_map_utils/package.xml b/common/had_map_utils/package.xml deleted file mode 100644 index aa7f7a9e7884..000000000000 --- a/common/had_map_utils/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - had_map_utils - 1.0.0 - Common utility functions and classes for HAD maps - TierIV, Inc. - Apache 2.0 - - ament_cmake_auto - - autoware_cmake - rclcpp - sensor_msgs - - autoware_auto_common - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - cgal - geometry_msgs - lanelet2 - lanelet2_core - lanelet2_io - visualization_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/had_map_utils/src/had_map_computation.cpp b/common/had_map_utils/src/had_map_computation.cpp deleted file mode 100644 index 722faf9f6135..000000000000 --- a/common/had_map_utils/src/had_map_computation.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright 2021 TierIV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "had_map_utils/had_map_computation.hpp" - -#include "had_map_utils/had_map_visualization.hpp" - -#include - -#include - -#include - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -using Kernel = CGAL::Exact_predicates_exact_constructions_kernel; -using CGAL_Point = Kernel::Point_2; -using CGAL_Polygon = CGAL::Polygon_2; -using CGAL_Polygon_with_holes = CGAL::Polygon_with_holes_2; - -// TODO(s.me) this is getting a bit long, break up -lanelet::Polygon3d coalesce_drivable_areas( - const autoware_auto_planning_msgs::msg::HADMapRoute & had_map_route, - const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - CGAL_Polygon_with_holes drivable_area; - - for (const auto & map_segment : had_map_route.segments) { - // Attempt to obtain a polygon from the primitive ID - geometry_msgs::msg::Polygon current_area_polygon{}; - const auto & lanelet_layer = lanelet_map_ptr->laneletLayer; - const auto & current_lanelet_candidate = lanelet_layer.find(map_segment.preferred_primitive_id); - if (current_lanelet_candidate != lanelet_layer.end()) { - current_area_polygon = lanelet2Polygon(*current_lanelet_candidate); - } else { - const auto & area_layer = lanelet_map_ptr->areaLayer; - const auto & current_area_candidate = area_layer.find(map_segment.preferred_primitive_id); - if (current_area_candidate != area_layer.end()) { - current_area_polygon = area2Polygon(*current_area_candidate); - } else { - // This might happen if a primitive is on the route, but outside of the bounding box that we - // query the map for. Not sure how to deal with this at this point though. - std::cerr << "Error: primitive ID " << map_segment.preferred_primitive_id - << " not found, skipping" << std::endl; - continue; - } - } - - if (drivable_area.outer_boundary().size() > 0) { - // Convert current_area_polygon to a CGAL_Polygon and make sure the orientation is correct - CGAL_Polygon to_join{}; - CGAL_Polygon_with_holes temporary_union; - const auto first_point = current_area_polygon.points.begin(); - for (auto area_point_it = current_area_polygon.points.begin(); - // Stop if we run out of points, or if we encounter the first point again - area_point_it < current_area_polygon.points.end() && - !(first_point != area_point_it && first_point->x == area_point_it->x && - first_point->y == area_point_it->y); - area_point_it++) { - to_join.push_back(CGAL_Point(area_point_it->x, area_point_it->y)); - } - - if (to_join.is_clockwise_oriented()) { - to_join.reverse_orientation(); - } - - // Merge this CGAL polygon with the growing drivable_area. We need an intermediate merge - // result because as far as I can tell from the CGAL docs, I can't "join to" a polygon - // in-place with the join() interface. - const auto polygons_overlap = CGAL::join(drivable_area, to_join, temporary_union); - if (!polygons_overlap && !drivable_area.outer_boundary().is_empty()) { - // TODO(s.me) cancel here? Right now we just ignore that polygon, if it doesn't - // overlap with the rest, there is no way to get to it anyway - std::cerr << "Error: polygons in union do not overlap!" << std::endl; - } else { - drivable_area = temporary_union; - } - } else { - // Otherwise, just set the current drivable area equal to the area to add to it, because - // CGAL seems to do "union(empty, non-empty) = empty" for some reason. - const auto first_point = current_area_polygon.points.begin(); - for (auto area_point_it = current_area_polygon.points.begin(); - area_point_it < current_area_polygon.points.end() && - // Stop if we run out of points, or if we encounter the first point again - !(first_point != area_point_it && first_point->x == area_point_it->x && - first_point->y == area_point_it->y); - area_point_it++) { - drivable_area.outer_boundary().push_back(CGAL_Point(area_point_it->x, area_point_it->y)); - } - if (drivable_area.outer_boundary().is_clockwise_oriented()) { - drivable_area.outer_boundary().reverse_orientation(); - } - } - } - - // At this point, all the polygons from the route should be merged into drivable_area, - // and we now need to turn this back into a lanelet polygon. - std::vector lanelet_drivable_area_points{}; - lanelet_drivable_area_points.reserve(drivable_area.outer_boundary().size()); - for (auto p = drivable_area.outer_boundary().vertices_begin(); - p != drivable_area.outer_boundary().vertices_end(); p++) { - lanelet_drivable_area_points.emplace_back(lanelet::Point3d( - lanelet::utils::getId(), CGAL::to_double(p->x()), CGAL::to_double(p->y()), 0.0)); - } - lanelet::Polygon3d lanelet_drivable_area(lanelet::utils::getId(), lanelet_drivable_area_points); - return lanelet_drivable_area; -} - -} // namespace had_map_utils -} // namespace common -} // namespace autoware diff --git a/common/had_map_utils/src/had_map_conversion.cpp b/common/had_map_utils/src/had_map_conversion.cpp deleted file mode 100644 index 5a4e0944d5a5..000000000000 --- a/common/had_map_utils/src/had_map_conversion.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright 2020 TierIV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -// lint -e537 pclint vs cpplint NOLINT - -#include "had_map_utils/had_map_conversion.hpp" - -#include -#include - -#include - -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -void toBinaryMsg( - const std::shared_ptr & map, - autoware_auto_mapping_msgs::msg::HADMapBin & msg) -{ - std::stringstream ss; - boost::archive::binary_oarchive oa(ss); - oa << *map; - auto id_counter = lanelet::utils::getId(); - oa << id_counter; - - std::string tmp_str = ss.str(); - msg.data.clear(); - msg.data.resize(tmp_str.size()); - msg.data.assign(tmp_str.begin(), tmp_str.end()); -} - -void fromBinaryMsg( - const autoware_auto_mapping_msgs::msg::HADMapBin & msg, - std::shared_ptr & map) -{ - 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); -} - -} // namespace had_map_utils -} // namespace common -} // namespace autoware diff --git a/common/had_map_utils/src/had_map_query.cpp b/common/had_map_utils/src/had_map_query.cpp deleted file mode 100644 index 2bb73e7bd723..000000000000 --- a/common/had_map_utils/src/had_map_query.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// Copyright 2020 TierIV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -// lint -e537 pclint vs cpplint NOLINT - -#include "had_map_utils/had_map_query.hpp" - -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -lanelet::Areas getAreaLayer(const lanelet::LaneletMapPtr ll_map) -{ - lanelet::Areas areas; - for (auto ai = ll_map->areaLayer.begin(); ai != ll_map->areaLayer.end(); ai++) { - areas.push_back(*ai); - } - return areas; -} - -lanelet::Areas subtypeAreas(const lanelet::Areas areas, const char subtype[]) -{ - lanelet::Areas subtype_areas; - for (auto ai = areas.begin(); ai != areas.end(); ai++) { - lanelet::Area a = *ai; - if (a.hasAttribute(lanelet::AttributeName::Subtype)) { - lanelet::Attribute attr = a.attribute(lanelet::AttributeName::Subtype); - if (attr.value() == subtype) { - subtype_areas.push_back(a); - } - } - } - return subtype_areas; -} - -lanelet::Polygons3d getPolygonLayer(const lanelet::LaneletMapPtr ll_map) -{ - lanelet::Polygons3d polygons; - for (auto ai = ll_map->polygonLayer.begin(); ai != ll_map->polygonLayer.end(); ai++) { - polygons.push_back(*ai); - } - return polygons; -} - -lanelet::Polygons3d subtypePolygons(const lanelet::Polygons3d polygons, const char subtype[]) -{ - lanelet::Polygons3d subtype_polygons; - for (auto pi = polygons.begin(); pi != polygons.end(); pi++) { - lanelet::Polygon3d p = *pi; - if (p.hasAttribute(lanelet::AttributeName::Subtype)) { - lanelet::Attribute attr = p.attribute(lanelet::AttributeName::Subtype); - if (attr.value() == subtype) { - subtype_polygons.push_back(p); - } - } - } - return subtype_polygons; -} - -lanelet::LineStrings3d getLineStringLayer(const lanelet::LaneletMapPtr ll_map) -{ - lanelet::LineStrings3d linestrings; - for (auto lsi = ll_map->lineStringLayer.begin(); lsi != ll_map->lineStringLayer.end(); lsi++) { - linestrings.push_back(*lsi); - } - return linestrings; -} - -lanelet::LineStrings3d subtypeLineStrings( - const lanelet::LineStrings3d linestrings, const char subtype[]) -{ - lanelet::LineStrings3d subtype_linestrings; - for (auto lsi = linestrings.begin(); lsi != linestrings.end(); lsi++) { - lanelet::LineString3d ls = *lsi; - if (ls.hasAttribute(lanelet::AttributeName::Subtype)) { - lanelet::Attribute attr = ls.attribute(lanelet::AttributeName::Subtype); - if (attr.value() == subtype) { - subtype_linestrings.push_back(ls); - } - } - } - return subtype_linestrings; -} - -lanelet::ConstLanelets getConstLaneletLayer(const std::shared_ptr & ll_map) -{ - lanelet::ConstLanelets lanelets; - for (auto li = ll_map->laneletLayer.begin(); li != ll_map->laneletLayer.end(); li++) { - lanelets.push_back(*li); - } - - return lanelets; -} -lanelet::Lanelets getLaneletLayer(const std::shared_ptr & ll_map) -{ - lanelet::Lanelets lanelets; - for (auto li = ll_map->laneletLayer.begin(); li != ll_map->laneletLayer.end(); li++) { - lanelets.push_back(*li); - } - - return lanelets; -} - -} // namespace had_map_utils -} // namespace common -} // namespace autoware diff --git a/common/had_map_utils/src/had_map_utils.cpp b/common/had_map_utils/src/had_map_utils.cpp deleted file mode 100644 index 81f9517f6a8d..000000000000 --- a/common/had_map_utils/src/had_map_utils.cpp +++ /dev/null @@ -1,174 +0,0 @@ -// Copyright 2020 TierIV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -// lint -e537 pclint vs cpplint NOLINT - -#include "had_map_utils/had_map_utils.hpp" - -#include - -#include - -#include -#include -#include -#include -#include -#include - -using autoware::common::types::float64_t; - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -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 float64_t 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 (size_t i = 1; i < N; ++i) { - if ( - accumulated_lengths.at(i - 1) <= target_length && - target_length <= accumulated_lengths.at(i)) { - return std::make_pair(i - 1, i); - } - } - - // Throw an exception because this never happens - throw std::runtime_error("findNearestIndexPair(): No nearest point found."); -} - -std::vector resamplePoints( - const lanelet::ConstLineString3d & line_string, const int32_t num_segments) -{ - // Calculate length - const auto line_length = lanelet::geometry::length(line_string); - - // Calculate accumulated lengths - const auto accumulated_lengths = calculateAccumulatedLengths(line_string); - if (accumulated_lengths.size() < 2) return {}; - - // Create each segment - std::vector resampled_points; - for (auto i = 0; i <= num_segments; ++i) { - // Find two nearest points - const float64_t target_length = - (static_cast(i) / num_segments) * static_cast(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 generateFineCenterline( - const lanelet::ConstLanelet & lanelet_obj, const float64_t resolution) -{ - // Get length of longer border - const float64_t left_length = - static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); - const float64_t right_length = - static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); - const float64_t longer_distance = (left_length > right_length) ? left_length : right_length; - const int32_t 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 (size_t i = 0; i < static_cast(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.0; - 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 autoware::common::types::bool8_t force_overwrite) -{ - for (auto & lanelet_obj : lanelet_map->laneletLayer) { - if (force_overwrite || !lanelet_obj.hasCustomCenterline()) { - const auto fine_center_line = generateFineCenterline(lanelet_obj, 2.0); - lanelet_obj.setCenterline(fine_center_line); - } - } -} - -} // namespace had_map_utils -} // namespace common -} // namespace autoware diff --git a/common/had_map_utils/src/had_map_visualization.cpp b/common/had_map_utils/src/had_map_visualization.cpp deleted file mode 100644 index 509eed71d418..000000000000 --- a/common/had_map_utils/src/had_map_visualization.cpp +++ /dev/null @@ -1,516 +0,0 @@ -// Copyright 2020 TierIV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -// lint -e537 pclint vs cpplint NOLINT - -#include "had_map_utils/had_map_visualization.hpp" - -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -using autoware::common::types::float64_t; - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -template -bool8_t exists(const std::unordered_set & set, const T & element) -{ - return std::find(set.begin(), set.end(), element) != set.end(); -} - -void setColor( - std_msgs::msg::ColorRGBA * cl, const float32_t & r, const float32_t & g, const float32_t & b, - const float32_t & a) -{ - cl->r = r; - cl->g = g; - cl->b = b; - cl->a = a; -} - -void setMarkerHeader( - visualization_msgs::msg::Marker * m, const int32_t & id, const rclcpp::Time & t, - const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, - const int32_t & action, const int32_t & type, const float32_t & scale) -{ - m->header.frame_id = frame_id; - m->header.stamp = t; - m->ns = ns; - m->action = action; - m->type = type; - - m->id = id; - m->pose.position.x = 0.0; - m->pose.position.y = 0.0; - m->pose.position.z = 0.0; - m->pose.orientation.x = 0.0; - m->pose.orientation.y = 0.0; - m->pose.orientation.z = 0.0; - m->pose.orientation.w = 1.0; - m->scale.x = scale; - m->scale.y = scale; - m->scale.z = scale; - m->color = c; -} - -visualization_msgs::msg::Marker lineString2Marker( - const rclcpp::Time & t, const lanelet::LineString3d & ls, const std::string & frame_id, - const std::string & ns, const std_msgs::msg::ColorRGBA & c, const float32_t & lss) -{ - visualization_msgs::msg::Marker line_strip; - setMarkerHeader( - &line_strip, static_cast(ls.id()), t, frame_id, ns, c, - visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::LINE_STRIP, lss); - - for (auto i = ls.begin(); i != ls.end(); i++) { - geometry_msgs::msg::Point p; - p.x = (*i).x(); - p.y = (*i).y(); - p.z = (*i).z(); - line_strip.points.push_back(p); - } - return line_strip; -} - -visualization_msgs::msg::Marker lineString2Marker( - const rclcpp::Time & t, const lanelet::ConstLineString3d & ls, const std::string & frame_id, - const std::string & ns, const std_msgs::msg::ColorRGBA & c, const float32_t & lss) -{ - visualization_msgs::msg::Marker line_strip; - setMarkerHeader( - &line_strip, static_cast(ls.id()), t, frame_id, ns, c, - visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::LINE_STRIP, lss); - - for (auto i = ls.begin(); i != ls.end(); i++) { - geometry_msgs::msg::Point p; - p.x = (*i).x(); - p.y = (*i).y(); - p.z = (*i).z(); - line_strip.points.push_back(p); - } - return line_strip; -} - -visualization_msgs::msg::MarkerArray lineStringsAsMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::LineStrings3d & linestrings, - const std_msgs::msg::ColorRGBA & c) -{ - float32_t lss = 0.1f; - visualization_msgs::msg::MarkerArray marker_array; - - for (auto lsi = linestrings.begin(); lsi != linestrings.end(); lsi++) { - lanelet::LineString3d ls = *lsi; - visualization_msgs::msg::Marker line_strip = lineString2Marker(t, ls, "map", ns, c, lss); - marker_array.markers.push_back(line_strip); - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray( - const rclcpp::Time & t, const lanelet::ConstLanelets & lanelets, - const std_msgs::msg::ColorRGBA & c, const bool8_t & viz_centerline) -{ - float32_t lss = 0.1f; - std::unordered_set added; - visualization_msgs::msg::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::msg::Marker left_line_strip, right_line_strip, center_line_strip; - if (!exists(added, left_ls.id())) { - left_line_strip = lineString2Marker(t, left_ls, "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())) { - right_line_strip = lineString2Marker(t, right_ls, "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())) { - center_line_strip = - lineString2Marker(t, center_ls, "map", "center_lane_line", c, std::max(lss * 0.1f, 0.01f)); - marker_array.markers.push_back(center_line_strip); - added.insert(center_ls.id()); - } - } - return marker_array; -} - -visualization_msgs::msg::Marker basicPolygon2Marker( - const rclcpp::Time & t, const int32_t & line_id, const lanelet::BasicPolygon3d & pg, - const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, - const float32_t & lss) -{ - visualization_msgs::msg::Marker line_strip; - setMarkerHeader( - &line_strip, line_id, t, frame_id, ns, c, visualization_msgs::msg::Marker::ADD, - visualization_msgs::msg::Marker::LINE_STRIP, lss); - - for (auto i = pg.begin(); i != pg.end(); i++) { - geometry_msgs::msg::Point p; - p.x = (*i).x(); - p.y = (*i).y(); - p.z = (*i).z(); - line_strip.points.push_back(p); - } - geometry_msgs::msg::Point pb; - auto i = pg.begin(); - pb.x = (*i).x(); - pb.y = (*i).y(); - pb.z = (*i).z(); - line_strip.points.push_back(pb); - return line_strip; -} - -visualization_msgs::msg::MarkerArray areasBoundaryAsMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::Areas & areas, - const std_msgs::msg::ColorRGBA & c) -{ - int pg_count = 0; - float32_t lss = 0.1f; - visualization_msgs::msg::MarkerArray marker_array; - for (auto area : areas) { - lanelet::CompoundPolygon3d cpg = area.outerBoundPolygon(); - lanelet::BasicPolygon3d bpg = cpg.basicPolygon(); - - visualization_msgs::msg::Marker line_strip = - basicPolygon2Marker(t, pg_count, bpg, "map", ns, c, lss); - marker_array.markers.push_back(line_strip); - pg_count++; - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray polygonsBoundaryAsMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::Polygons3d & polygons, - const std_msgs::msg::ColorRGBA & c) -{ - int32_t pg_count = 0; - float32_t lss = 0.1f; - visualization_msgs::msg::MarkerArray marker_array; - for (auto poly : polygons) { - lanelet::BasicPolygon3d bpg = poly.basicPolygon(); - visualization_msgs::msg::Marker line_strip = - basicPolygon2Marker(t, pg_count, bpg, "map", ns, c, lss); - marker_array.markers.push_back(line_strip); - pg_count++; - } - return marker_array; -} - -visualization_msgs::msg::Marker bbox2Marker( - const rclcpp::Time & t, const int32_t & line_id, const float64_t lower[], const float64_t upper[], - const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, - const float32_t & lss) -{ - visualization_msgs::msg::Marker line_strip; - setMarkerHeader( - &line_strip, line_id, t, frame_id, ns, c, visualization_msgs::msg::Marker::ADD, - visualization_msgs::msg::Marker::LINE_STRIP, lss); - - geometry_msgs::msg::Point bl, br, tl, tr; - bl.x = lower[0]; - bl.y = lower[0]; - bl.z = 0.0; - br.x = upper[0]; - br.y = lower[0]; - br.z = 0.0; - tl.x = lower[0]; - tl.y = upper[0]; - tl.z = 0.0; - tr.x = upper[0]; - tr.y = upper[0]; - tr.z = 0.0; - - line_strip.points.push_back(bl); - line_strip.points.push_back(br); - line_strip.points.push_back(tr); - line_strip.points.push_back(tl); - line_strip.points.push_back(bl); - return line_strip; -} - -visualization_msgs::msg::MarkerArray boundingBoxAsMarkerArray( - const rclcpp::Time & t, const std::string & ns, const float64_t upper[], const float64_t lower[], - const std_msgs::msg::ColorRGBA & c) -{ - float32_t lss = 0.2f; - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker line_strip = bbox2Marker(t, 0, upper, lower, "map", ns, c, lss); - marker_array.markers.push_back(line_strip); - return marker_array; -} - -geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src) -{ - geometry_msgs::msg::Point dst; - dst.x = static_cast(src.x); - dst.y = static_cast(src.y); - dst.z = static_cast(src.z); - return dst; -} - -geometry_msgs::msg::Point32 toGeomMsgPt32(const lanelet::BasicPoint3d & src) -{ - geometry_msgs::msg::Point32 dst; - dst.x = static_cast(src.x()); - dst.y = static_cast(src.y()); - dst.z = static_cast(src.z()); - return dst; -} -void adjacentPoints( - const size_t i, const size_t N, const geometry_msgs::msg::Polygon poly, - geometry_msgs::msg::Point32 * p0, geometry_msgs::msg::Point32 * p1, - geometry_msgs::msg::Point32 * p2) -{ - *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]; - } -} - -std::vector lanelet2Triangle(const lanelet::ConstLanelet & ll) -{ - geometry_msgs::msg::Polygon ls_poly = lanelet2Polygon(ll); - return polygon2Triangle(ls_poly); -} - -std::vector area2Triangle(const lanelet::Area & area) -{ - geometry_msgs::msg::Polygon ls_poly = area2Polygon(area); - return polygon2Triangle(ls_poly); -} - -// Is angle AOB less than 180? -// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e -bool8_t isAcuteAngle( - const geometry_msgs::msg::Point32 & vertex_a, const geometry_msgs::msg::Point32 & vertex_o, - const geometry_msgs::msg::Point32 & vertex_b) -{ - return (vertex_a.x - vertex_o.x) * (vertex_b.y - vertex_o.y) - - (vertex_a.y - vertex_o.y) * (vertex_b.x - vertex_o.x) >= - 0; -} - -// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e -bool8_t isWithinTriangle( - const geometry_msgs::msg::Point32 & vertex_a, const geometry_msgs::msg::Point32 & vertex_b, - const geometry_msgs::msg::Point32 & vertex_c, const geometry_msgs::msg::Point32 & pt) -{ - float64_t c1 = (vertex_b.x - vertex_a.x) * (pt.y - vertex_b.y) - - (vertex_b.y - vertex_a.y) * (pt.x - vertex_b.x); - float64_t c2 = (vertex_c.x - vertex_b.x) * (pt.y - vertex_c.y) - - (vertex_c.y - vertex_b.y) * (pt.x - vertex_c.x); - float64_t c3 = (vertex_a.x - vertex_c.x) * (pt.y - vertex_a.y) - - (vertex_a.y - vertex_c.y) * (pt.x - vertex_a.x); - - return (c1 > 0.0 && c2 > 0.0 && c3 > 0.0) || (c1 < 0.0 && c2 < 0.0 && c3 < 0.0); -} - -std::vector polygon2Triangle( - const geometry_msgs::msg::Polygon & polygon) -{ - std::vector triangles; - geometry_msgs::msg::Polygon poly = polygon; - size_t num_vertices = poly.points.size(); - - std::vector is_acute_angle; - is_acute_angle.assign(num_vertices, false); - for (size_t i = 0; i < num_vertices; i++) { - geometry_msgs::msg::Point32 p0, p1, p2; - - adjacentPoints(i, num_vertices, poly, &p0, &p1, &p2); - is_acute_angle.at(i) = isAcuteAngle(p0, p1, p2); - } - while (num_vertices >= 3) { - size_t clipped_vertex = 0; - - for (size_t i = 0; i < num_vertices; i++) { - bool8_t theta = is_acute_angle.at(i); - if (theta == true) { - geometry_msgs::msg::Point32 p0, p1, p2; - adjacentPoints(i, num_vertices, poly, &p0, &p1, &p2); - - size_t j_begin = (i + 2) % num_vertices; - size_t j_end = (i - 1 + num_vertices) % num_vertices; - bool8_t is_ear = true; - for (size_t j = j_begin; j != j_end; j = (j + 1) % num_vertices) { - if (isWithinTriangle(p0, p1, p2, poly.points.at(j))) { - is_ear = false; - break; - } - } - - if (is_ear) { - clipped_vertex = i; - break; - } - } - } - geometry_msgs::msg::Point32 p0, p1, p2; - adjacentPoints(clipped_vertex, num_vertices, poly, &p0, &p1, &p2); - geometry_msgs::msg::Polygon triangle; - triangle.points.push_back(p0); - triangle.points.push_back(p1); - triangle.points.push_back(p2); - triangles.push_back(triangle); - auto it = poly.points.begin(); - std::advance(it, clipped_vertex); - poly.points.erase(it); - - auto it_angle = is_acute_angle.begin(); - std::advance(it_angle, clipped_vertex); - is_acute_angle.erase(it_angle); - - num_vertices = poly.points.size(); - if (clipped_vertex == num_vertices) { - clipped_vertex = 0; - } - adjacentPoints(clipped_vertex, num_vertices, poly, &p0, &p1, &p2); - is_acute_angle.at(clipped_vertex) = isAcuteAngle(p0, p1, p2); - - size_t i_prev = (clipped_vertex == 0) ? num_vertices - 1 : clipped_vertex - 1; - adjacentPoints(i_prev, num_vertices, poly, &p0, &p1, &p2); - is_acute_angle.at(i_prev) = isAcuteAngle(p0, p1, p2); - } - return triangles; -} - -geometry_msgs::msg::Polygon area2Polygon(const lanelet::ConstArea & area) -{ - geometry_msgs::msg::Polygon polygon; - polygon.points.clear(); - polygon.points.reserve(area.outerBoundPolygon().size()); - - std::transform( - area.outerBoundPolygon().begin(), area.outerBoundPolygon().end(), - std::back_inserter(polygon.points), - [](lanelet::ConstPoint3d pt) { return toGeomMsgPt32(pt.basicPoint()); }); - return polygon; -} - -geometry_msgs::msg::Polygon lanelet2Polygon(const lanelet::ConstLanelet & ll) -{ - geometry_msgs::msg::Polygon polygon; - - const lanelet::CompoundPolygon3d & ll_poly = ll.polygon3d(); - polygon.points.clear(); - polygon.points.reserve(ll_poly.size()); - - std::transform( - ll_poly.begin(), ll_poly.end(), std::back_inserter(polygon.points), - [](lanelet::ConstPoint3d pt) { return toGeomMsgPt32(pt.basicPoint()); }); - return polygon; -} - -visualization_msgs::msg::MarkerArray laneletsAsTriangleMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::ConstLanelets & lanelets, - const std_msgs::msg::ColorRGBA & c) -{ - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker marker; - - if (lanelets.empty()) { - return marker_array; - } - - setMarkerHeader( - &marker, 0, t, "map", ns, c, visualization_msgs::msg::Marker::ADD, - visualization_msgs::msg::Marker::TRIANGLE_LIST, 1.0); - - for (auto ll : lanelets) { - std::vector triangles = lanelet2Triangle(ll); - - for (auto tri : triangles) { - geometry_msgs::msg::Point tri0[3]; - - for (size_t i = 0; i < 3; i++) { - tri0[i] = toGeomMsgPt(tri.points[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; -} - -visualization_msgs::msg::MarkerArray areasAsTriangleMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::Areas & areas, - const std_msgs::msg::ColorRGBA & c) -{ - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker marker; - - if (areas.empty()) { - return marker_array; - } - - setMarkerHeader( - &marker, 0, t, "map", ns, c, visualization_msgs::msg::Marker::ADD, - visualization_msgs::msg::Marker::TRIANGLE_LIST, 1.0); - - for (auto area : areas) { - std::vector triangles = area2Triangle(area); - for (auto tri : triangles) { - for (size_t i = 0; i < 3; i++) { - marker.points.push_back(toGeomMsgPt(tri.points[i])); - marker.colors.push_back(c); - } - } - } - - if (!marker.points.empty()) { - marker_array.markers.push_back(marker); - } - return marker_array; -} - -} // namespace had_map_utils -} // namespace common -} // namespace autoware diff --git a/launch/tier4_integration_launch/CMakeLists.txt b/launch/tier4_integration_launch/CMakeLists.txt deleted file mode 100644 index bdf730803405..000000000000 --- a/launch/tier4_integration_launch/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ - -cmake_minimum_required(VERSION 3.14) -project(tier4_integration_launch) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_package( - INSTALL_TO_SHARE - launch -) diff --git a/launch/tier4_integration_launch/README.md b/launch/tier4_integration_launch/README.md deleted file mode 100644 index 634c73fed14b..000000000000 --- a/launch/tier4_integration_launch/README.md +++ /dev/null @@ -1,13 +0,0 @@ -# tier4_integration_launch - -## Structure - -![tier4_integration_launch](./integration_launch.drawio.svg) - -## Package Dependencies - -- autoware_launch - -## Notes - -This package is only used for continuous integration. diff --git a/launch/tier4_integration_launch/integration_launch.drawio.svg b/launch/tier4_integration_launch/integration_launch.drawio.svg deleted file mode 100644 index c47744ff98b8..000000000000 --- a/launch/tier4_integration_launch/integration_launch.drawio.svg +++ /dev/null @@ -1,228 +0,0 @@ - - - - - - - -
-
-
- ci_planning_simulator.launch.xml -
-
-
- - package: tier4_integration_launch - -
-
-
-
- - - ci_planning_simulator.launch.xml... - - - - - - - -
-
-
- launch name -
-
-
- - package: package name - -
-
-
-
- - - launch name... - - - - - - - -
-
-
- ex: -
-
-
- - - ex: - - - - - - - -
-
-
- node name -
-
-
- - package: package name - -
-
-
-
- - - node name... - - - - - - - -
-
-
- planning_simulator.launch.xml -
-
-
- - package: autoware_launch - -
-
-
-
- - - planning_simulator.launch.xml... - - - - - - - -
-
-
- other name -
-
-
- - package: package name - -
-
-
-
- - - other name... - - - - - - - - - -
-
-
- scenario_runner.launch.xml -
-
-
- - package: scenario_runner - -
-
-
-
- - - scenario_runner.launch.xml... - - - - - - - - - -
-
-
- release.launch.xml -
-
-
- - package: tier4_integration_launch - -
-
-
-
- - - release.launch.xml... - - - - - - - -
-
-
- autoware.launch.xml -
-
-
- - package: autoware_launch - -
-
-
-
- - - autoware.launch.xml... - - - - - - - - - - - Viewer does not support full SVG 1.1 - - - - \ No newline at end of file diff --git a/launch/tier4_integration_launch/launch/ci_planning_simulator.launch.xml b/launch/tier4_integration_launch/launch/ci_planning_simulator.launch.xml deleted file mode 100644 index 924d9d0ad6fe..000000000000 --- a/launch/tier4_integration_launch/launch/ci_planning_simulator.launch.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_integration_launch/launch/release.launch.xml b/launch/tier4_integration_launch/launch/release.launch.xml deleted file mode 100644 index 0d6d50cbeed2..000000000000 --- a/launch/tier4_integration_launch/launch/release.launch.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_integration_launch/package.xml b/launch/tier4_integration_launch/package.xml deleted file mode 100644 index bc7cd01b4b67..000000000000 --- a/launch/tier4_integration_launch/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - tier4_integration_launch - 0.1.0 - The tier4_integration_launch package - Hiroyuki Obinata - Apache License 2.0 - - ament_cmake_auto - - autoware_cmake - - autoware_launch - python3-bson - python3-tornado - rviz2 - - ament_lint_auto - autoware_lint_common - - - ament_cmake - -