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(),