From f35cc6b4bd6ac8e714aaebb78d024450d435b3e5 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Mon, 29 Jan 2024 11:33:58 +0900
Subject: [PATCH] feat(intersection, blind_spot): add objects of interest
marker
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
planning/behavior_velocity_blind_spot_module/src/scene.cpp | 4 +++-
planning/behavior_velocity_blind_spot_module/src/scene.hpp | 2 +-
.../src/scene_intersection_collision.cpp | 5 ++++-
3 files changed, 8 insertions(+), 3 deletions(-)
diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp
index efbff7e2af51d..54314e50308ff 100644
--- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp
+++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp
@@ -348,7 +348,7 @@ bool BlindSpotModule::checkObstacleInBlindSpot(
lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
- const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const
+ const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose)
{
/* get detection area */
if (turn_direction_ == TurnDirection::INVALID) {
@@ -409,6 +409,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot(
exist_in_right_conflict_area || exist_in_opposite_conflict_area;
if (exist_in_detection_area && exist_in_conflict_area) {
debug_data_.conflicting_targets.objects.push_back(object);
+ setObjectsOfInterestData(
+ object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED);
return true;
}
}
diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp
index 6f8450568939c..2ca2fe7950989 100644
--- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp
+++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp
@@ -113,7 +113,7 @@ class BlindSpotModule : public SceneModuleInterface
lanelet::routing::RoutingGraphPtr routing_graph_ptr,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
- const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const;
+ const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose);
/**
* @brief Create half lanelet
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp
index 4f7e8b45d107f..4496df77134e2 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp
@@ -621,6 +621,9 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
continue;
}
const auto & unsafe_info = object_info->is_unsafe().value();
+ setObjectsOfInterestData(
+ object_info->predicted_object().kinematics.initial_pose_with_covariance.pose,
+ object_info->predicted_object().shape, ColorName::RED);
// ==========================================================================================
// if ego is over the pass judge lines, then the visualization as "too_late_objects" or
// "misjudge_objects" is more important than that for "unsafe"
@@ -910,7 +913,7 @@ bool IntersectionModule::checkCollision(
}
}
object_ttc_time_array.layout.dim.at(0).size++;
- const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position;
+ const auto & pos = object..position;
const auto & shape = object.shape;
object_ttc_time_array.data.insert(
object_ttc_time_array.data.end(),