forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request autowarefoundation#547 from tier4/sync-upstream
chore: sync upstream
- Loading branch information
Showing
55 changed files
with
1,113 additions
and
538 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
80 changes: 80 additions & 0 deletions
80
common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,80 @@ | ||
| // Copyright 2023 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 MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ | ||
| #define MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ | ||
|
|
||
| #include "motion_utils/marker/marker_helper.hpp" | ||
|
|
||
| #include <geometry_msgs/msg/pose.hpp> | ||
| #include <visualization_msgs/msg/marker.hpp> | ||
| #include <visualization_msgs/msg/marker_array.hpp> | ||
|
|
||
| #include <string> | ||
| #include <unordered_map> | ||
| #include <vector> | ||
|
|
||
| namespace motion_utils | ||
| { | ||
|
|
||
| /// @brief type of virtual wall associated with different marker styles and namespace | ||
| enum VirtualWallType { stop, slowdown, deadline }; | ||
| /// @brief virtual wall to be visualized in rviz | ||
| struct VirtualWall | ||
| { | ||
| geometry_msgs::msg::Pose pose{}; | ||
| std::string text{}; | ||
| std::string ns{}; | ||
| VirtualWallType style = stop; | ||
| double longitudinal_offset{}; | ||
| }; | ||
| typedef std::vector<VirtualWall> VirtualWalls; | ||
|
|
||
| /// @brief class to manage the creation of virtual wall markers | ||
| /// @details creates both ADD and DELETE markers | ||
| class VirtualWallMarkerCreator | ||
| { | ||
| struct MarkerCount | ||
| { | ||
| size_t previous = 0UL; | ||
| size_t current = 0UL; | ||
| }; | ||
|
|
||
| using create_wall_function = std::function<visualization_msgs::msg::MarkerArray( | ||
| const geometry_msgs::msg::Pose & pose, const std::string & module_name, | ||
| const rclcpp::Time & now, const int32_t id, const double longitudinal_offset, | ||
| const std::string & ns_prefix)>; | ||
|
|
||
| VirtualWalls virtual_walls; | ||
| std::unordered_map<std::string, MarkerCount> marker_count_per_namespace; | ||
|
|
||
| /// @brief internal cleanup: clear the stored markers and remove unused namespace from the map | ||
| void cleanup(); | ||
|
|
||
| public: | ||
| /// @brief add a virtual wall | ||
| /// @param virtual_wall virtual wall to add | ||
| void add_virtual_wall(const VirtualWall & virtual_wall); | ||
| /// @brief add virtual walls | ||
| /// @param virtual_walls virtual walls to add | ||
| void add_virtual_walls(const VirtualWalls & walls); | ||
|
|
||
| /// @brief create markers for the stored virtual walls | ||
| /// @details also create DELETE markers for the namespace+ids that are no longer used | ||
| /// @param now current time to be used for displaying the markers | ||
| visualization_msgs::msg::MarkerArray create_markers(const rclcpp::Time & now = rclcpp::Time()); | ||
| }; | ||
| } // namespace motion_utils | ||
|
|
||
| #endif // MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
85 changes: 85 additions & 0 deletions
85
common/motion_utils/src/marker/virtual_wall_marker_creator.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,85 @@ | ||
| // Copyright 2023 Tier IV, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #include "motion_utils/marker/virtual_wall_marker_creator.hpp" | ||
|
|
||
| namespace motion_utils | ||
| { | ||
|
|
||
| void VirtualWallMarkerCreator::cleanup() | ||
| { | ||
| for (auto it = marker_count_per_namespace.begin(); it != marker_count_per_namespace.end();) { | ||
| const auto & marker_count = it->second; | ||
| const auto is_unused_namespace = marker_count.previous == 0 && marker_count.current == 0; | ||
| if (is_unused_namespace) | ||
| it = marker_count_per_namespace.erase(it); | ||
| else | ||
| ++it; | ||
| } | ||
| virtual_walls.clear(); | ||
| } | ||
|
|
||
| void VirtualWallMarkerCreator::add_virtual_wall(const VirtualWall & virtual_wall) | ||
| { | ||
| virtual_walls.push_back(virtual_wall); | ||
| } | ||
| void VirtualWallMarkerCreator::add_virtual_walls(const VirtualWalls & walls) | ||
| { | ||
| virtual_walls.insert(virtual_walls.end(), walls.begin(), walls.end()); | ||
| } | ||
|
|
||
| visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( | ||
| const rclcpp::Time & now) | ||
| { | ||
| visualization_msgs::msg::MarkerArray marker_array; | ||
| // update marker counts | ||
| for (auto & [ns, count] : marker_count_per_namespace) { | ||
| count.previous = count.current; | ||
| count.current = 0UL; | ||
| } | ||
| // convert to markers | ||
| create_wall_function create_fn; | ||
| for (const auto & virtual_wall : virtual_walls) { | ||
| switch (virtual_wall.style) { | ||
| case stop: | ||
| create_fn = motion_utils::createStopVirtualWallMarker; | ||
| break; | ||
| case slowdown: | ||
| create_fn = motion_utils::createSlowDownVirtualWallMarker; | ||
| break; | ||
| case deadline: | ||
| create_fn = motion_utils::createDeadLineVirtualWallMarker; | ||
| break; | ||
| } | ||
| auto markers = create_fn( | ||
| virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, | ||
| virtual_wall.ns); | ||
| for (auto & marker : markers.markers) { | ||
| marker.id = marker_count_per_namespace[marker.ns].current++; | ||
| marker_array.markers.push_back(marker); | ||
| } | ||
| } | ||
| // create delete markers | ||
| visualization_msgs::msg::Marker marker; | ||
| marker.action = visualization_msgs::msg::Marker::DELETE; | ||
| for (const auto & [ns, count] : marker_count_per_namespace) { | ||
| for (marker.id = count.current; marker.id < static_cast<int>(count.previous); ++marker.id) { | ||
| marker.ns = ns; | ||
| marker_array.markers.push_back(marker); | ||
| } | ||
| } | ||
| cleanup(); | ||
| return marker_array; | ||
| } | ||
| } // namespace motion_utils |
Oops, something went wrong.