Skip to content

Commit 307e373

Browse files
maxime-clemkyoichi-sugahara
maxime-clem
authored and
kyoichi-sugahara
committed
refactor(behavior_velocity_planner): new framework for creating virtual wall markers (#3851)
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 434ae31 commit 307e373

File tree

35 files changed

+453
-381
lines changed

35 files changed

+453
-381
lines changed

‎common/motion_utils/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@ ament_auto_add_library(motion_utils SHARED
1010
src/motion_utils.cpp
1111
src/distance/distance.cpp
1212
src/marker/marker_helper.cpp
13+
src/marker/virtual_wall_marker_creator.cpp
1314
src/resample/resample.cpp
1415
src/trajectory/interpolation.cpp
1516
src/trajectory/path_with_lane_id.cpp

‎common/motion_utils/include/motion_utils/marker/marker_helper.hpp

-43
Original file line numberDiff line numberDiff line change
@@ -46,49 +46,6 @@ visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(
4646

4747
visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
4848
const rclcpp::Time & now, const int32_t id);
49-
50-
visualization_msgs::msg::MarkerArray createVirtualWallMarkerFromPreviousPoses(
51-
const std::vector<Pose> & stop_poses, const std::vector<Pose> & previous_poses,
52-
const rclcpp::Time & now, int32_t id);
53-
54-
class VirtualWallMarkerCreator
55-
{
56-
public:
57-
virtual ~VirtualWallMarkerCreator() = default;
58-
59-
using create_wall_function = std::function<visualization_msgs::msg::MarkerArray(
60-
const geometry_msgs::msg::Pose & pose, const std::string & module_name,
61-
const rclcpp::Time & now, const int32_t id, const double longitudinal_offset,
62-
const std::string & ns_prefix)>;
63-
64-
using delete_wall_function =
65-
std::function<visualization_msgs::msg::MarkerArray(const rclcpp::Time & now, const int32_t id)>;
66-
67-
visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
68-
const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
69-
const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
70-
71-
visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
72-
const std::vector<Pose> & slow_down_poses, const std::string & module_name,
73-
const rclcpp::Time & now, const double longitudinal_offset = 0.0,
74-
const std::string & ns_prefix = "");
75-
76-
visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
77-
const std::vector<Pose> & dead_line_poses, const std::string & module_name,
78-
const rclcpp::Time & now, const double longitudinal_offset = 0.0,
79-
const std::string & ns_prefix = "");
80-
81-
private:
82-
visualization_msgs::msg::MarkerArray handleVirtualWallMarker(
83-
const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
84-
create_wall_function function_create_wall_marker,
85-
delete_wall_function function_delete_wall_marker, size_t & previous_virtual_walls_nb,
86-
const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
87-
88-
size_t previous_stop_poses_nb_ = 0UL;
89-
size_t previous_slow_down_poses_nb_ = 0UL;
90-
size_t previous_dead_line_poses_nb_ = 0UL;
91-
};
9249
} // namespace motion_utils
9350

9451
#endif // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_

‎common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp

+80
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
// Copyright 2023 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
16+
#define MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
17+
18+
#include "motion_utils/marker/marker_helper.hpp"
19+
20+
#include <geometry_msgs/msg/pose.hpp>
21+
#include <visualization_msgs/msg/marker.hpp>
22+
#include <visualization_msgs/msg/marker_array.hpp>
23+
24+
#include <string>
25+
#include <unordered_map>
26+
#include <vector>
27+
28+
namespace motion_utils
29+
{
30+
31+
/// @brief type of virtual wall associated with different marker styles and namespace
32+
enum VirtualWallType { stop, slowdown, deadline };
33+
/// @brief virtual wall to be visualized in rviz
34+
struct VirtualWall
35+
{
36+
geometry_msgs::msg::Pose pose{};
37+
std::string text{};
38+
std::string ns{};
39+
VirtualWallType style = stop;
40+
double longitudinal_offset{};
41+
};
42+
typedef std::vector<VirtualWall> VirtualWalls;
43+
44+
/// @brief class to manage the creation of virtual wall markers
45+
/// @details creates both ADD and DELETE markers
46+
class VirtualWallMarkerCreator
47+
{
48+
struct MarkerCount
49+
{
50+
size_t previous = 0UL;
51+
size_t current = 0UL;
52+
};
53+
54+
using create_wall_function = std::function<visualization_msgs::msg::MarkerArray(
55+
const geometry_msgs::msg::Pose & pose, const std::string & module_name,
56+
const rclcpp::Time & now, const int32_t id, const double longitudinal_offset,
57+
const std::string & ns_prefix)>;
58+
59+
VirtualWalls virtual_walls;
60+
std::unordered_map<std::string, MarkerCount> marker_count_per_namespace;
61+
62+
/// @brief internal cleanup: clear the stored markers and remove unused namespace from the map
63+
void cleanup();
64+
65+
public:
66+
/// @brief add a virtual wall
67+
/// @param virtual_wall virtual wall to add
68+
void add_virtual_wall(const VirtualWall & virtual_wall);
69+
/// @brief add virtual walls
70+
/// @param virtual_walls virtual walls to add
71+
void add_virtual_walls(const VirtualWalls & walls);
72+
73+
/// @brief create markers for the stored virtual walls
74+
/// @details also create DELETE markers for the namespace+ids that are no longer used
75+
/// @param now current time to be used for displaying the markers
76+
visualization_msgs::msg::MarkerArray create_markers(const rclcpp::Time & now = rclcpp::Time());
77+
};
78+
} // namespace motion_utils
79+
80+
#endif // MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_

‎common/motion_utils/src/marker/marker_helper.cpp

-62
Original file line numberDiff line numberDiff line change
@@ -134,66 +134,4 @@ visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
134134
{
135135
return createDeletedVirtualWallMarkerArray("dead_line_", now, id);
136136
}
137-
138-
visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWallMarker(
139-
const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
140-
create_wall_function function_create_wall_marker,
141-
delete_wall_function function_delete_wall_marker, size_t & previous_virtual_walls_nb,
142-
const double longitudinal_offset, const std::string & ns_prefix)
143-
{
144-
visualization_msgs::msg::MarkerArray wall_marker;
145-
146-
int32_t id = 0;
147-
const auto max_id = static_cast<int32_t>(previous_virtual_walls_nb);
148-
149-
for (const auto & p : poses) {
150-
appendMarkerArray(
151-
function_create_wall_marker(p, module_name, now, id++, longitudinal_offset, ns_prefix),
152-
&wall_marker);
153-
}
154-
155-
while (id < max_id) {
156-
appendMarkerArray(function_delete_wall_marker(now, id++), &wall_marker, now);
157-
}
158-
159-
previous_virtual_walls_nb = poses.size();
160-
return wall_marker;
161-
}
162-
163-
visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createStopVirtualWallMarker(
164-
const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
165-
const double longitudinal_offset, const std::string & ns_prefix)
166-
{
167-
create_wall_function creator = motion_utils::createStopVirtualWallMarker;
168-
delete_wall_function deleter = motion_utils::createDeletedStopVirtualWallMarker;
169-
170-
return handleVirtualWallMarker(
171-
stop_poses, module_name, now, creator, deleter, previous_stop_poses_nb_, longitudinal_offset,
172-
ns_prefix);
173-
}
174-
175-
visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createSlowDownVirtualWallMarker(
176-
const std::vector<Pose> & slow_down_poses, const std::string & module_name,
177-
const rclcpp::Time & now, const double longitudinal_offset, const std::string & ns_prefix)
178-
{
179-
create_wall_function creator = motion_utils::createSlowDownVirtualWallMarker;
180-
delete_wall_function deleter = motion_utils::createDeletedSlowDownVirtualWallMarker;
181-
182-
return handleVirtualWallMarker(
183-
slow_down_poses, module_name, now, creator, deleter, previous_slow_down_poses_nb_,
184-
longitudinal_offset, ns_prefix);
185-
}
186-
187-
visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createDeadLineVirtualWallMarker(
188-
const std::vector<Pose> & dead_line_poses, const std::string & module_name,
189-
const rclcpp::Time & now, const double longitudinal_offset, const std::string & ns_prefix)
190-
{
191-
create_wall_function creator = motion_utils::createDeadLineVirtualWallMarker;
192-
delete_wall_function deleter = motion_utils::createDeletedDeadLineVirtualWallMarker;
193-
194-
return handleVirtualWallMarker(
195-
dead_line_poses, module_name, now, creator, deleter, previous_dead_line_poses_nb_,
196-
longitudinal_offset, ns_prefix);
197-
}
198-
199137
} // namespace motion_utils

‎common/motion_utils/src/marker/virtual_wall_marker_creator.cpp

+85
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
// Copyright 2023 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "motion_utils/marker/virtual_wall_marker_creator.hpp"
16+
17+
namespace motion_utils
18+
{
19+
20+
void VirtualWallMarkerCreator::cleanup()
21+
{
22+
for (auto it = marker_count_per_namespace.begin(); it != marker_count_per_namespace.end();) {
23+
const auto & marker_count = it->second;
24+
const auto is_unused_namespace = marker_count.previous == 0 && marker_count.current == 0;
25+
if (is_unused_namespace)
26+
it = marker_count_per_namespace.erase(it);
27+
else
28+
++it;
29+
}
30+
virtual_walls.clear();
31+
}
32+
33+
void VirtualWallMarkerCreator::add_virtual_wall(const VirtualWall & virtual_wall)
34+
{
35+
virtual_walls.push_back(virtual_wall);
36+
}
37+
void VirtualWallMarkerCreator::add_virtual_walls(const VirtualWalls & walls)
38+
{
39+
virtual_walls.insert(virtual_walls.end(), walls.begin(), walls.end());
40+
}
41+
42+
visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers(
43+
const rclcpp::Time & now)
44+
{
45+
visualization_msgs::msg::MarkerArray marker_array;
46+
// update marker counts
47+
for (auto & [ns, count] : marker_count_per_namespace) {
48+
count.previous = count.current;
49+
count.current = 0UL;
50+
}
51+
// convert to markers
52+
create_wall_function create_fn;
53+
for (const auto & virtual_wall : virtual_walls) {
54+
switch (virtual_wall.style) {
55+
case stop:
56+
create_fn = motion_utils::createStopVirtualWallMarker;
57+
break;
58+
case slowdown:
59+
create_fn = motion_utils::createSlowDownVirtualWallMarker;
60+
break;
61+
case deadline:
62+
create_fn = motion_utils::createDeadLineVirtualWallMarker;
63+
break;
64+
}
65+
auto markers = create_fn(
66+
virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset,
67+
virtual_wall.ns);
68+
for (auto & marker : markers.markers) {
69+
marker.id = marker_count_per_namespace[marker.ns].current++;
70+
marker_array.markers.push_back(marker);
71+
}
72+
}
73+
// create delete markers
74+
visualization_msgs::msg::Marker marker;
75+
marker.action = visualization_msgs::msg::Marker::DELETE;
76+
for (const auto & [ns, count] : marker_count_per_namespace) {
77+
for (marker.id = count.current; marker.id < static_cast<int>(count.previous); ++marker.id) {
78+
marker.ns = ns;
79+
marker_array.markers.push_back(marker);
80+
}
81+
}
82+
cleanup();
83+
return marker_array;
84+
}
85+
} // namespace motion_utils

0 commit comments

Comments
 (0)