Skip to content

Commit

Permalink
refactor(behavior_velocity_planner): use marker_helper in tier4_autow…
Browse files Browse the repository at this point in the history 
…are_utils (#1417)

* feat(tier4_autoware_utils): add time argument in appendMarkerArray

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add default argument

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* refactor(behavior_velocity_planner): use marker_helper in tier4_autoware_utils

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* remove marker_helper.hpp

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 authored Jul 26, 2022
1 parent 72dc6b3 commit 5fafe9b
Show file tree
Hide file tree
Showing 8 changed files with 127 additions and 212 deletions.
104 changes: 0 additions & 104 deletions planning/behavior_velocity_planner/include/utilization/marker_helper.hpp
View file

This file was deleted.

26 changes: 15 additions & 11 deletions planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,13 +14,17 @@

#include <motion_utils/motion_utils.hpp>
#include <scene_module/blind_spot/scene.hpp>
#include <utilization/marker_helper.hpp>
#include <utilization/util.hpp>

#include <string>

namespace behavior_velocity_planner
{
using tier4_autoware_utils::appendMarkerArray;
using tier4_autoware_utils::createMarkerColor;
using tier4_autoware_utils::createMarkerOrientation;
using tier4_autoware_utils::createMarkerScale;

namespace
{
using State = BlindSpotModule::State;
Expand Down Expand Up @@ -161,7 +165,7 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createVirtualWallMarkerArr
appendMarkerArray(
motion_utils::createStopVirtualWallMarker(
debug_data_.virtual_wall_pose, "blind_spot", now, lane_id_),
now, &wall_marker);
&wall_marker, now);
}
return wall_marker;
}
Expand All @@ -174,39 +178,39 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray()
const auto current_time = this->clock_->now();

appendMarkerArray(
createPathMarkerArray(debug_data_.path_raw, "path_raw", lane_id_, 0.0, 1.0, 1.0), current_time,
&debug_marker_array);
createPathMarkerArray(debug_data_.path_raw, "path_raw", lane_id_, 0.0, 1.0, 1.0),
&debug_marker_array, current_time);

appendMarkerArray(
createPoseMarkerArray(
debug_data_.stop_point_pose, state, "stop_point_pose", lane_id_, 1.0, 0.0, 0.0),
current_time, &debug_marker_array);
&debug_marker_array, current_time);

appendMarkerArray(
createPoseMarkerArray(
debug_data_.judge_point_pose, state, "judge_point_pose", lane_id_, 1.0, 1.0, 0.5),
current_time, &debug_marker_array);
&debug_marker_array, current_time);

appendMarkerArray(
createPolygonMarkerArray(
debug_data_.conflict_area_for_blind_spot, "conflict_area_for_blind_spot", lane_id_, 0.0, 0.5,
0.5),
current_time, &debug_marker_array);
&debug_marker_array, current_time);

appendMarkerArray(
createPolygonMarkerArray(
debug_data_.detection_area_for_blind_spot, "detection_area_for_blind_spot", lane_id_, 0.0,
0.5, 0.5),
current_time, &debug_marker_array);
&debug_marker_array, current_time);

appendMarkerArray(
createObjectsMarkerArray(
debug_data_.conflicting_targets, "conflicting_targets", lane_id_, 0.99, 0.4, 0.0),
current_time, &debug_marker_array);
&debug_marker_array, current_time);

appendMarkerArray(
createPathMarkerArray(debug_data_.spline_path, "spline", lane_id_, 0.5, 0.5, 0.5), current_time,
&debug_marker_array);
createPathMarkerArray(debug_data_.spline_path, "spline", lane_id_, 0.5, 0.5, 0.5),
&debug_marker_array, current_time);

return debug_marker_array;
}
Expand Down
37 changes: 20 additions & 17 deletions planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@

#include <motion_utils/motion_utils.hpp>
#include <scene_module/detection_area/scene.hpp>
#include <utilization/marker_helper.hpp>
#include <utilization/util.hpp>

#ifdef ROS_DISTRO_GALACTIC
Expand All @@ -27,6 +26,11 @@

namespace behavior_velocity_planner
{
using tier4_autoware_utils::appendMarkerArray;
using tier4_autoware_utils::createDefaultMarker;
using tier4_autoware_utils::createMarkerColor;
using tier4_autoware_utils::createMarkerScale;

namespace
{
using DebugData = DetectionAreaModule::DebugData;
Expand Down Expand Up @@ -62,8 +66,8 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray(
{
auto marker = createDefaultMarker(
"map", now, "detection_area_id", detection_area_reg_elem.id(),
visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerColor(1.0, 1.0, 1.0, 0.999));
marker.scale = createMarkerScale(0.0, 0.0, 1.0);
visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0),
createMarkerColor(1.0, 1.0, 1.0, 0.999));
marker.lifetime = rclcpp::Duration::from_seconds(0.5);

for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) {
Expand All @@ -81,8 +85,8 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray(
{
auto marker = createDefaultMarker(
"map", now, "detection_area_polygon", detection_area_reg_elem.id(),
visualization_msgs::msg::Marker::LINE_LIST, createMarkerColor(0.1, 0.1, 1.0, 0.500));
marker.scale = createMarkerScale(0.1, 0.0, 0.0);
visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0),
createMarkerColor(0.1, 0.1, 1.0, 0.500));
marker.lifetime = rclcpp::Duration::from_seconds(0.5);

for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) {
Expand All @@ -107,8 +111,8 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray(
{
auto marker = createDefaultMarker(
"map", now, "detection_area_correspondence", detection_area_reg_elem.id(),
visualization_msgs::msg::Marker::LINE_LIST, createMarkerColor(0.1, 0.1, 1.0, 0.500));
marker.scale = createMarkerScale(0.1, 0.0, 0.0);
visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0),
createMarkerColor(0.1, 0.1, 1.0, 0.500));
marker.lifetime = rclcpp::Duration::from_seconds(0.5);

for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) {
Expand All @@ -134,8 +138,7 @@ visualization_msgs::msg::MarkerArray createObstacleMarkerArray(
{
auto marker = createDefaultMarker(
"map", now, "obstacles", 0, visualization_msgs::msg::Marker::SPHERE,
createMarkerColor(1.0, 0.0, 0.0, 0.999));
marker.scale = createMarkerScale(0.6, 0.6, 0.6);
createMarkerScale(0.6, 0.6, 0.6), createMarkerColor(1.0, 0.0, 0.0, 0.999));
marker.lifetime = rclcpp::Duration::from_seconds(0.5);

for (size_t i = 0; i < obstacle_points.size(); ++i) {
Expand All @@ -158,12 +161,12 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray

if (!debug_data_.stop_poses.empty()) {
appendMarkerArray(
createCorrespondenceMarkerArray(detection_area_reg_elem_, current_time), current_time,
&wall_marker);
createCorrespondenceMarkerArray(detection_area_reg_elem_, current_time), &wall_marker,
current_time);

appendMarkerArray(
createObstacleMarkerArray(debug_data_.obstacle_points, current_time), current_time,
&wall_marker);
createObstacleMarkerArray(debug_data_.obstacle_points, current_time), &wall_marker,
current_time);
}

return wall_marker;
Expand All @@ -180,16 +183,16 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createVirtualWallMarke
const auto p_front =
tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
appendMarkerArray(
motion_utils::createStopVirtualWallMarker(p_front, "detection_area", now, id++), now,
&wall_marker);
motion_utils::createStopVirtualWallMarker(p_front, "detection_area", now, id++), &wall_marker,
now);
}

for (const auto & p : debug_data_.dead_line_poses) {
const auto p_front =
tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
appendMarkerArray(
motion_utils::createDeadLineVirtualWallMarker(p_front, "detection_area", now, id++), now,
&wall_marker);
motion_utils::createDeadLineVirtualWallMarker(p_front, "detection_area", now, id++),
&wall_marker, now);
}

return wall_marker;
Expand Down
Loading

0 comments on commit 5fafe9b

Please sign in to comment.