From ae31c0164ae6e796b85a63907172eee8ddd469a3 Mon Sep 17 00:00:00 2001 From: AhmedEbrahim Date: Mon, 20 Mar 2023 11:23:23 +0200 Subject: [PATCH] feat: reverting changes of lanelet regulatory element Signed-off-by: AhmedEbrahim --- tmp/lanelet2_extension/CMakeLists.txt | 1 - .../regulatory_elements/invalid_lanelet.hpp | 73 ----------- .../lanelet2_extension/utility/query.hpp | 9 -- .../visualization/visualization.hpp | 10 -- .../lib/invalid_lanelet.cpp | 115 ------------------ tmp/lanelet2_extension/lib/query.cpp | 28 ----- tmp/lanelet2_extension/lib/visualization.cpp | 64 ---------- 7 files changed, 300 deletions(-) delete mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/invalid_lanelet.hpp delete mode 100644 tmp/lanelet2_extension/lib/invalid_lanelet.cpp diff --git a/tmp/lanelet2_extension/CMakeLists.txt b/tmp/lanelet2_extension/CMakeLists.txt index 629a5cc7..6b109cbe 100644 --- a/tmp/lanelet2_extension/CMakeLists.txt +++ b/tmp/lanelet2_extension/CMakeLists.txt @@ -44,7 +44,6 @@ ament_auto_add_library(lanelet2_extension_lib SHARED lib/virtual_traffic_light.cpp lib/visualization.cpp lib/route_checker.cpp - lib/invalid_lanelet.cpp ) target_link_libraries(lanelet2_extension_lib ${GeographicLib_LIBRARIES} diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/invalid_lanelet.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/invalid_lanelet.hpp deleted file mode 100644 index 0d153a46..00000000 --- a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/invalid_lanelet.hpp +++ /dev/null @@ -1,73 +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. - - -#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__INVALID_LANELET_HPP_ -#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__INVALID_LANELET_HPP_ - -// NOLINTBEGIN(readability-identifier-naming) - -#include -#include - -#include -#include - -namespace lanelet::autoware -{ -class InvalidLanelet : public lanelet::RegulatoryElement -{ -public: - using Ptr = std::shared_ptr; - static constexpr char RuleName[] = "invalid_lanelet"; - - static Ptr make(Id id, const AttributeMap & attributes, const Polygon3d & invalid_lanelet) - { - return Ptr{new InvalidLanelet(id, attributes, invalid_lanelet)}; - } - - /** - * @brief get the relevant invalid lanelet - * @return invalid lanelet - */ - [[nodiscard]] ConstPolygon3d invalidLanelet() const; - [[nodiscard]] Polygon3d invalidLanelet(); - - /** - * @brief add a new invalid lanelet - * @param primitive invalid lanelet to add - */ - void addInvalidLanelet(const Polygon3d & primitive); - - /** - * @brief remove an invalid lanelet - * @param primitive invalid lanelet to remove - * @return true if the invalid lanelet existed and was removed - */ - bool removeInvalidLanelet(const Polygon3d & 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; - InvalidLanelet(Id id, const AttributeMap & attributes, const Polygon3d & invalid_lanelet); - explicit InvalidLanelet(const lanelet::RegulatoryElementDataPtr & data); -}; -static lanelet::RegisterRegulatoryElement regInvalidLanelet; - -} // namespace lanelet::autoware - -// NOLINTEND(readability-identifier-naming) - -#endif // LANELET2_EXTENSION__REGULATORY_ELEMENTS__INVALID_LANELET_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp index b7fe1080..eb5853a5 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -23,7 +23,6 @@ #include "lanelet2_extension/regulatory_elements/detection_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/regulatory_elements/speed_bump.hpp" -#include "lanelet2_extension/regulatory_elements/invalid_lanelet.hpp" #include #include @@ -44,7 +43,6 @@ using AutowareTrafficLightConstPtr = std::shared_ptr; using NoStoppingAreaConstPtr = std::shared_ptr; using SpeedBumpConstPtr = std::shared_ptr; -using InvalidLaneletConstPtr = std::shared_ptr; } // namespace lanelet namespace lanelet::utils::query @@ -125,13 +123,6 @@ std::vector noStoppingAreas( */ std::vector speedBumps(const lanelet::ConstLanelets & lanelets); -/** - * [invalidLanelets extracts Invalid Lanelet regulatory elements from lanelets] - * @param lanelets [input lanelets] - * @return [invalid lanelets that are associated with input lanelets] - */ -std::vector invalidLanelets(const lanelet::ConstLanelets & lanelets); - // query all polygons that has given type in lanelet2 map lanelet::ConstPolygons3d getAllPolygonsByType( const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type); diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp index a1cb7f1d..24140e87 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp @@ -241,16 +241,6 @@ visualization_msgs::msg::MarkerArray speedBumpsAsMarkerArray( const std::vector & sb_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); -/** - * [InvalidLaneletsAsMarkerArray creates marker array to visualize invalid lanelets] - * @param il_reg_elems [invalid lanelet regulatory elements] - * @param c [color of the marker] - * @param duration [lifetime of the marker] - */ -visualization_msgs::msg::MarkerArray invalidLaneletsAsMarkerArray( - const std::vector & il_reg_elems, const std_msgs::msg::ColorRGBA & c, - const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); - /** * [pedestrianMarkingsAsMarkerArray creates marker array to visualize pedestrian markings] * @param pedestrian_markings [pedestrian marking polygon] diff --git a/tmp/lanelet2_extension/lib/invalid_lanelet.cpp b/tmp/lanelet2_extension/lib/invalid_lanelet.cpp deleted file mode 100644 index 0a728d89..00000000 --- a/tmp/lanelet2_extension/lib/invalid_lanelet.cpp +++ /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. - -// NOLINTBEGIN(readability-identifier-naming) - -#include "lanelet2_extension/regulatory_elements/invalid_lanelet.hpp" - -#include - -#include - -#include -#include -#include -#include - -namespace lanelet::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; -} - -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 constructInvalidLaneletData( - Id id, const AttributeMap & attributes, const Polygon3d & invalid_lanelet) -{ - RuleParameterMap rpm; - RuleParameters rule_parameters = {invalid_lanelet}; - 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] = "invalid_lanelet"; - return data; -} -} // namespace - -InvalidLanelet::InvalidLanelet(const RegulatoryElementDataPtr & data) : RegulatoryElement(data) -{ - if (getConstPoly(data->parameters, RoleName::Refers).size() != 1) { - throw InvalidInputError("There must be exactly one invalid lanelet defined!"); - } -} - -InvalidLanelet::InvalidLanelet(Id id, const AttributeMap & attributes, const Polygon3d & invalid_lanelet) -: InvalidLanelet(constructInvalidLaneletData(id, attributes, invalid_lanelet)) -{ -} - -ConstPolygon3d InvalidLanelet::invalidLanelet() const -{ - return getConstPoly(parameters(), RoleName::Refers).front(); -} - -Polygon3d InvalidLanelet::invalidLanelet() { return getPoly(parameters(), RoleName::Refers).front(); } - -void InvalidLanelet::addInvalidLanelet(const Polygon3d & primitive) -{ - parameters()["invalid_lanelet"].emplace_back(primitive); -} - -bool InvalidLanelet::removeInvalidLanelet(const Polygon3d & primitive) -{ - return findAndErase(primitive, ¶meters().find("invalid_lanelet")->second); -} - -} // namespace lanelet::autoware - -// NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp index 04102cd5..2a0b1d4a 100644 --- a/tmp/lanelet2_extension/lib/query.cpp +++ b/tmp/lanelet2_extension/lib/query.cpp @@ -240,34 +240,6 @@ std::vector query::speedBumps(const lanelet::ConstLa return sb_reg_elems; } -std::vector query::invalidLanelets(const lanelet::ConstLanelets & lanelets) -{ - std::vector il_reg_elems; - - for (const auto & ll : lanelets) { - std::vector ll_il_re = - ll.regulatoryElementsAs(); - - // insert unique id into array - for (const auto & il_ptr : ll_il_re) { - lanelet::Id id = il_ptr->id(); - bool unique_id = true; - - for (const auto & il_reg_elem : il_reg_elems) { - if (id == il_reg_elem->id()) { - unique_id = false; - break; - } - } - - if (unique_id) { - il_reg_elems.push_back(il_ptr); - } - } - } - return il_reg_elems; -} - lanelet::ConstPolygons3d query::getAllPolygonsByType( const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type) { diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index 1420a378..ed8f53df 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -792,70 +792,6 @@ visualization_msgs::msg::MarkerArray visualization::speedBumpsAsMarkerArray( return marker_array; } -visualization_msgs::msg::MarkerArray visualization::invalidLaneletsAsMarkerArray( - const std::vector & il_reg_elems, const std_msgs::msg::ColorRGBA & c, - const rclcpp::Duration & duration) -{ - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker marker; - visualization_msgs::msg::Marker line_marker; - - if (il_reg_elems.empty()) { - return marker_array; - } - - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Time(); - marker.frame_locked = true; - marker.ns = "invalid_lanelet"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; - marker.lifetime = duration; - marker.pose.position.x = 0.0; // p.x(); - marker.pose.position.y = 0.0; // p.y(); - marker.pose.position.z = 0.0; // p.z(); - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 1.0; - marker.scale.y = 1.0; - marker.scale.z = 1.0; - marker.color.r = 1.0f; - marker.color.g = 1.0f; - marker.color.b = 1.0f; - marker.color.a = 0.999; - - for (const auto & il_reg_elem : il_reg_elems) { - marker.points.clear(); - marker.colors.clear(); - marker.id = static_cast(il_reg_elem->id()); - - // area visualization - const auto invalid_lanelet = il_reg_elem->invalidLanelet(); - - geometry_msgs::msg::Polygon geom_poly; - utils::conversion::toGeomMsgPoly(invalid_lanelet, &geom_poly); - - std::vector triangles; - polygon2Triangle(geom_poly, &triangles); - - for (auto tri : triangles) { - geometry_msgs::msg::Point tri0[3]; - - for (int i = 0; i < 3; i++) { - utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]); - marker.points.push_back(tri0[i]); - marker.colors.push_back(c); - } - } // for triangles0 - - marker_array.markers.push_back(marker); - } // for regulatory elements - - return marker_array; -} - visualization_msgs::msg::MarkerArray visualization::pedestrianMarkingsAsMarkerArray( const lanelet::ConstLineStrings3d & pedestrian_markings, const std_msgs::msg::ColorRGBA & c) {