Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(behavior_velocity_planner): fixed virtual wall marker id duplication #3654

Merged
merged 4 commits into from
May 16, 2023
Merged

fix(behavior_velocity_planner): fixed virtual wall marker id duplication #3654

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
cfabf49
Add an optional namespace prefix in create virtual wall marker functions
maxime-clem May 10, 2023
822e71b
Add ns prefixes to the crosswalk and intersection virtual wall markers
maxime-clem May 10, 2023
3ae0c03
Add module_id to namespace prefix of intersection module
maxime-clem May 12, 2023
cc9e1cc
Use recommended prefix format for intersection virtual walls
maxime-clem May 15, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 11 additions & 8 deletions common/motion_utils/include/motion_utils/marker/marker_helper.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,15 +28,15 @@ using geometry_msgs::msg::Pose;

visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
const double longitudinal_offset = 0.0);
const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");

visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
const double longitudinal_offset = 0.0);
const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");

visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
const double longitudinal_offset = 0.0);
const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");

visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
const rclcpp::Time & now, const int32_t id);
Expand All @@ -58,29 +58,32 @@ class VirtualWallMarkerCreator

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 rclcpp::Time & now, const int32_t id, const double longitudinal_offset,
const std::string & ns_prefix)>;

using delete_wall_function =
std::function<visualization_msgs::msg::MarkerArray(const rclcpp::Time & now, const int32_t id)>;

visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
const double longitudinal_offset = 0.0);
const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");

visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
const std::vector<Pose> & slow_down_poses, const std::string & module_name,
const rclcpp::Time & now, const double longitudinal_offset = 0.0);
const rclcpp::Time & now, const double longitudinal_offset = 0.0,
const std::string & ns_prefix = "");

visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
const std::vector<Pose> & dead_line_poses, const std::string & module_name,
const rclcpp::Time & now, const double longitudinal_offset = 0.0);
const rclcpp::Time & now, const double longitudinal_offset = 0.0,
const std::string & ns_prefix = "");

private:
visualization_msgs::msg::MarkerArray handleVirtualWallMarker(
const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
create_wall_function function_create_wall_marker,
delete_wall_function function_delete_wall_marker, size_t & previous_virtual_walls_nb,
const double longitudinal_offset = 0.0);
const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");

size_t previous_stop_poses_nb_ = 0UL;
size_t previous_slow_down_poses_nb_ = 0UL;
Expand Down
33 changes: 19 additions & 14 deletions common/motion_utils/src/marker/marker_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,32 +86,35 @@ namespace motion_utils
{
visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
const int32_t id, const double longitudinal_offset)
const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
{
const auto pose_with_offset =
tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
return createVirtualWallMarkerArray(
pose_with_offset, module_name, "stop_", now, id, createMarkerColor(1.0, 0.0, 0.0, 0.5));
pose_with_offset, module_name, ns_prefix + "stop_", now, id,
createMarkerColor(1.0, 0.0, 0.0, 0.5));
}

visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
const int32_t id, const double longitudinal_offset)
const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
{
const auto pose_with_offset =
tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
return createVirtualWallMarkerArray(
pose_with_offset, module_name, "slow_down_", now, id, createMarkerColor(1.0, 1.0, 0.0, 0.5));
pose_with_offset, module_name, ns_prefix + "slow_down_", now, id,
createMarkerColor(1.0, 1.0, 0.0, 0.5));
}

visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
const int32_t id, const double longitudinal_offset)
const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
{
const auto pose_with_offset =
tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
return createVirtualWallMarkerArray(
pose_with_offset, module_name, "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5));
pose_with_offset, module_name, ns_prefix + "dead_line_", now, id,
createMarkerColor(0.0, 1.0, 0.0, 0.5));
}

visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
Expand All @@ -136,7 +139,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall
const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
create_wall_function function_create_wall_marker,
delete_wall_function function_delete_wall_marker, size_t & previous_virtual_walls_nb,
const double longitudinal_offset)
const double longitudinal_offset, const std::string & ns_prefix)
{
visualization_msgs::msg::MarkerArray wall_marker;

Expand All @@ -145,7 +148,8 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall

for (const auto & p : poses) {
appendMarkerArray(
function_create_wall_marker(p, module_name, now, id++, longitudinal_offset), &wall_marker);
function_create_wall_marker(p, module_name, now, id++, longitudinal_offset, ns_prefix),
&wall_marker);
}

while (id < max_id) {
Expand All @@ -158,37 +162,38 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall

visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createStopVirtualWallMarker(
const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
const double longitudinal_offset)
const double longitudinal_offset, const std::string & ns_prefix)
{
create_wall_function creator = motion_utils::createStopVirtualWallMarker;
delete_wall_function deleter = motion_utils::createDeletedStopVirtualWallMarker;

return handleVirtualWallMarker(
stop_poses, module_name, now, creator, deleter, previous_stop_poses_nb_, longitudinal_offset);
stop_poses, module_name, now, creator, deleter, previous_stop_poses_nb_, longitudinal_offset,
ns_prefix);
}

visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createSlowDownVirtualWallMarker(
const std::vector<Pose> & slow_down_poses, const std::string & module_name,
const rclcpp::Time & now, const double longitudinal_offset)
const rclcpp::Time & now, const double longitudinal_offset, const std::string & ns_prefix)
{
create_wall_function creator = motion_utils::createSlowDownVirtualWallMarker;
delete_wall_function deleter = motion_utils::createDeletedSlowDownVirtualWallMarker;

return handleVirtualWallMarker(
slow_down_poses, module_name, now, creator, deleter, previous_slow_down_poses_nb_,
longitudinal_offset);
longitudinal_offset, ns_prefix);
}

visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createDeadLineVirtualWallMarker(
const std::vector<Pose> & dead_line_poses, const std::string & module_name,
const rclcpp::Time & now, const double longitudinal_offset)
const rclcpp::Time & now, const double longitudinal_offset, const std::string & ns_prefix)
{
create_wall_function creator = motion_utils::createDeadLineVirtualWallMarker;
delete_wall_function deleter = motion_utils::createDeletedDeadLineVirtualWallMarker;

return handleVirtualWallMarker(
dead_line_poses, module_name, now, creator, deleter, previous_dead_line_poses_nb_,
longitudinal_offset);
longitudinal_offset, ns_prefix);
}

} // namespace motion_utils
4 changes: 2 additions & 2 deletions planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -232,15 +232,15 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createVirtualWallMarkerArr
}
appendMarkerArray(
virtual_wall_marker_creator_crosswalk_->createStopVirtualWallMarker(
stop_poses, "crosswalk", now),
stop_poses, "crosswalk", now, 0.0, std::to_string(module_id_) + "_"),
&wall_marker);
for (const auto & p : debug_data_.slow_poses) {
const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
slow_down_poses.push_back(p_front);
}
appendMarkerArray(
virtual_wall_marker_creator_crosswalk_->createSlowDownVirtualWallMarker(
slow_down_poses, "crosswalk", now),
slow_down_poses, "crosswalk", now, 0.0, std::to_string(module_id_) + "_"),
&wall_marker);

return wall_marker;
Expand Down
9 changes: 6 additions & 3 deletions planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -207,19 +207,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker
if (!activated_) {
appendMarkerArray(
virtual_wall_marker_creator_->createStopVirtualWallMarker(
{debug_data_.collision_stop_wall_pose}, "intersection", now),
{debug_data_.collision_stop_wall_pose}, "intersection", now, 0.0,
std::to_string(module_id_) + "_"),
&wall_marker, now);
}
if (!occlusion_first_stop_activated_) {
appendMarkerArray(
virtual_wall_marker_creator_->createStopVirtualWallMarker(
{debug_data_.occlusion_first_stop_wall_pose}, "intersection", now),
{debug_data_.occlusion_first_stop_wall_pose}, "intersection", now, 0.0,
std::to_string(module_id_) + "occlusion_first_stop_"),
&wall_marker, now);
}
if (!occlusion_activated_) {
appendMarkerArray(
virtual_wall_marker_creator_->createStopVirtualWallMarker(
{debug_data_.occlusion_stop_wall_pose}, "intersection_occlusion", now),
{debug_data_.occlusion_stop_wall_pose}, "intersection_occlusion", now, 0.0,
std::to_string(module_id_) + "occlusion_"),
&wall_marker, now);
}

Expand Down