diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 3766c5f631d06..3bbc956cb8b8c 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -34,6 +34,7 @@ ament_auto_add_library(behavior_velocity_planner SHARED src/planner_manager.cpp src/utilization/path_utilization.cpp src/utilization/util.cpp + src/utilization/debug.cpp ${scene_modules_src} ) diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index f694e4bbb6ff0..6f490135ca3dd 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -52,8 +52,8 @@ class BlindSpotModule : public SceneModuleInterface geometry_msgs::msg::Pose virtual_wall_pose; geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; - lanelet::CompoundPolygon3d conflict_area_for_blind_spot; - lanelet::CompoundPolygon3d detection_area_for_blind_spot; + geometry_msgs::msg::Polygon conflict_area_for_blind_spot; + geometry_msgs::msg::Polygon detection_area_for_blind_spot; autoware_auto_planning_msgs::msg::PathWithLaneId spline_path; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; }; diff --git a/planning/behavior_velocity_planner/include/utilization/debug.hpp b/planning/behavior_velocity_planner/include/utilization/debug.hpp new file mode 100644 index 0000000000000..af398ae539a1e --- /dev/null +++ b/planning/behavior_velocity_planner/include/utilization/debug.hpp @@ -0,0 +1,47 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILIZATION__DEBUG_HPP_ +#define UTILIZATION__DEBUG_HPP_ + +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace debug +{ +visualization_msgs::msg::MarkerArray createPolygonMarkerArray( + const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, + const rclcpp::Time & now, const double x, const double y, const double z, const double r, + const double g, const double b); +visualization_msgs::msg::MarkerArray createPathMarkerArray( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b); +visualization_msgs::msg::MarkerArray createObjectsMarkerArray( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double r, const double g, + const double b); +visualization_msgs::msg::MarkerArray createPointsMarkerArray( + const std::vector & points, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b); +} // namespace debug +} // namespace behavior_velocity_planner +#endif // UTILIZATION__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp b/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp index fab3c654b205a..7856a42114619 100644 --- a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp @@ -35,7 +35,6 @@ autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( const autoware_auto_planning_msgs::msg::Path & path); autoware_auto_planning_msgs::msg::Path filterStopPathPoint( const autoware_auto_planning_msgs::msg::Path & path); -std::vector calcEuclidDist(const std::vector & x, const std::vector & y); } // namespace behavior_velocity_planner #endif // UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp index c7f50215ee399..7163a49aae6ea 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -28,91 +29,6 @@ using tier4_autoware_utils::createMarkerScale; namespace { -visualization_msgs::msg::MarkerArray createPolygonMarkerArray( - const lanelet::CompoundPolygon3d & polygon, const std::string & ns, const int64_t lane_id, - const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - int32_t uid = planning_utils::bitShift(lane_id); - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - - marker.ns = ns; - marker.id = uid; - marker.lifetime = rclcpp::Duration::from_seconds(0.3); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = createMarkerScale(0.3, 0.0, 0.0); - marker.color = createMarkerColor(r, g, b, 0.8); - for (const auto & p : polygon) { - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - point.z = p.z(); - marker.points.push_back(point); - } - if (!marker.points.empty()) { - marker.points.push_back(marker.points.front()); - msg.markers.push_back(marker); - } - - return msg; -} - -visualization_msgs::msg::MarkerArray createObjectsMarkerArray( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, - const int64_t lane_id, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.ns = ns; - - int32_t uid = planning_utils::bitShift(lane_id); - int32_t i = 0; - for (const auto & object : objects.objects) { - marker.id = uid + i++; - marker.lifetime = rclcpp::Duration::from_seconds(1.0); - marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = object.kinematics.initial_pose_with_covariance.pose; - marker.scale = createMarkerScale(1.0, 1.0, 1.0); - marker.color = createMarkerColor(r, g, b, 0.8); - msg.markers.push_back(marker); - } - - return msg; -} - -visualization_msgs::msg::MarkerArray createPathMarkerArray( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, - const int64_t lane_id, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.ns = ns; - marker.id = lane_id; - marker.lifetime = rclcpp::Duration::from_seconds(0.3); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = createMarkerScale(0.3, 0.0, 0.0); - marker.color = createMarkerColor(r, g, b, 0.999); - - for (const auto & p : path.points) { - marker.points.push_back(p.point.pose.position); - } - - msg.markers.push_back(marker); - - return msg; -} - visualization_msgs::msg::MarkerArray createPoseMarkerArray( const geometry_msgs::msg::Pose & pose, const StateMachine::State & state, const std::string & ns, const int64_t id, const double r, const double g, const double b) @@ -163,7 +79,7 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createVirtualWallMarkerArr if (!isActivated() && !is_over_pass_judge_line_) { appendMarkerArray( motion_utils::createStopVirtualWallMarker( - debug_data_.virtual_wall_pose, "blind_spot", now, lane_id_), + debug_data_.virtual_wall_pose, "blind_spot", now, module_id_), &wall_marker, now); } return wall_marker; @@ -174,42 +90,44 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() visualization_msgs::msg::MarkerArray debug_marker_array; const auto state = state_machine_.getState(); - const auto current_time = this->clock_->now(); + const auto now = this->clock_->now(); appendMarkerArray( - createPathMarkerArray(debug_data_.path_raw, "path_raw", lane_id_, 0.0, 1.0, 1.0), - &debug_marker_array, current_time); + debug::createPathMarkerArray( + debug_data_.path_raw, "path_raw", lane_id_, now, 0.6, 0.3, 0.3, 0.0, 1.0, 1.0), + &debug_marker_array, now); appendMarkerArray( createPoseMarkerArray( - debug_data_.stop_point_pose, state, "stop_point_pose", lane_id_, 1.0, 0.0, 0.0), - &debug_marker_array, current_time); + debug_data_.stop_point_pose, state, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), + &debug_marker_array, now); appendMarkerArray( createPoseMarkerArray( - debug_data_.judge_point_pose, state, "judge_point_pose", lane_id_, 1.0, 1.0, 0.5), - &debug_marker_array, current_time); + debug_data_.judge_point_pose, state, "judge_point_pose", module_id_, 1.0, 1.0, 0.5), + &debug_marker_array, now); appendMarkerArray( - createPolygonMarkerArray( - debug_data_.conflict_area_for_blind_spot, "conflict_area_for_blind_spot", lane_id_, 0.0, 0.5, - 0.5), - &debug_marker_array, current_time); + debug::createPolygonMarkerArray( + debug_data_.conflict_area_for_blind_spot, "conflict_area_for_blind_spot", module_id_, now, + 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), + &debug_marker_array, now); appendMarkerArray( - createPolygonMarkerArray( - debug_data_.detection_area_for_blind_spot, "detection_area_for_blind_spot", lane_id_, 0.0, - 0.5, 0.5), - &debug_marker_array, current_time); + debug::createPolygonMarkerArray( + debug_data_.detection_area_for_blind_spot, "detection_area_for_blind_spot", module_id_, now, + 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), + &debug_marker_array, now); appendMarkerArray( - createObjectsMarkerArray( - debug_data_.conflicting_targets, "conflicting_targets", lane_id_, 0.99, 0.4, 0.0), - &debug_marker_array, current_time); + debug::createObjectsMarkerArray( + debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), + &debug_marker_array, now); appendMarkerArray( - createPathMarkerArray(debug_data_.spline_path, "spline", lane_id_, 0.5, 0.5, 0.5), - &debug_marker_array, current_time); + debug::createPathMarkerArray( + debug_data_.spline_path, "spline", lane_id_, now, 0.3, 0.1, 0.1, 0.5, 0.5, 0.5), + &debug_marker_array, now); return debug_marker_array; } diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 17c9374fac1f4..0a9e2ffedfa65 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -36,6 +36,24 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +namespace +{ +geometry_msgs::msg::Polygon toGeomPoly(const lanelet::CompoundPolygon3d & poly) +{ + geometry_msgs::msg::Polygon geom_poly; + + for (const auto & p : poly) { + geometry_msgs::msg::Point32 geom_point; + geom_point.x = p.x(); + geom_point.y = p.y(); + geom_point.z = p.z(); + geom_poly.points.push_back(geom_point); + } + + return geom_poly; +} +} // namespace + BlindSpotModule::BlindSpotModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, @@ -356,8 +374,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot( const auto areas_opt = generateBlindSpotPolygons( lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose); if (!!areas_opt) { - debug_data_.detection_area_for_blind_spot = areas_opt.get().detection_area; - debug_data_.conflict_area_for_blind_spot = areas_opt.get().conflict_area; + debug_data_.detection_area_for_blind_spot = toGeomPoly(areas_opt.get().detection_area); + debug_data_.conflict_area_for_blind_spot = toGeomPoly(areas_opt.get().conflict_area); autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr; cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp index f527e6abcb137..59618941b0923 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -129,44 +130,21 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray( return msg; } - -visualization_msgs::msg::MarkerArray createObstacleMarkerArray( - const std::vector & obstacle_points, const rclcpp::Time & now) -{ - visualization_msgs::msg::MarkerArray msg; - - { - auto marker = createDefaultMarker( - "map", now, "obstacles", 0, visualization_msgs::msg::Marker::SPHERE, - 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) { - marker.id = i; - marker.pose.position = obstacle_points.at(i); - - msg.markers.push_back(marker); - } - } - - return msg; -} - } // namespace visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray wall_marker; - const rclcpp::Time current_time = clock_->now(); + const rclcpp::Time now = clock_->now(); if (!debug_data_.stop_poses.empty()) { appendMarkerArray( - createCorrespondenceMarkerArray(detection_area_reg_elem_, current_time), &wall_marker, - current_time); + createCorrespondenceMarkerArray(detection_area_reg_elem_, now), &wall_marker, now); appendMarkerArray( - createObstacleMarkerArray(debug_data_.obstacle_points, current_time), &wall_marker, - current_time); + debug::createPointsMarkerArray( + debug_data_.obstacle_points, "obstalces", module_id_, now, 0.6, 0.6, 0.6, 1.0, 0.0, 0.0), + &wall_marker, now); } return wall_marker; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 734d0d444e216..94364e928bbda 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -31,12 +32,12 @@ using tier4_autoware_utils::createMarkerScale; visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, - const int64_t lane_id) + const int64_t module_id) { visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = planning_utils::bitShift(lane_id); + int32_t uid = planning_utils::bitShift(module_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -65,93 +66,6 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( return msg; } -visualization_msgs::msg::MarkerArray createPolygonMarkerArray( - const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t lane_id, - const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - - marker.ns = ns; - marker.id = lane_id; - marker.lifetime = rclcpp::Duration::from_seconds(0.3); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker.scale = createMarkerScale(0.3, 0.0, 0.0); - marker.color = createMarkerColor(r, g, b, 0.8); - for (const auto & p : polygon.points) { - geometry_msgs::msg::Point point; - point.x = p.x; - point.y = p.y; - point.z = p.z; - marker.points.push_back(point); - } - if (!marker.points.empty()) { - marker.points.push_back(marker.points.front()); - } - msg.markers.push_back(marker); - - return msg; -} - -visualization_msgs::msg::MarkerArray createObjectsMarkerArray( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, - const int64_t lane_id, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.ns = ns; - - int32_t uid = planning_utils::bitShift(lane_id); - int32_t i = 0; - for (const auto & object : objects.objects) { - marker.id = uid + i++; - marker.lifetime = rclcpp::Duration::from_seconds(1.0); - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = object.kinematics.initial_pose_with_covariance.pose; - marker.scale = createMarkerScale(3.0, 1.0, 1.0); - marker.color = createMarkerColor(r, g, b, 0.8); - msg.markers.push_back(marker); - } - - return msg; -} - -visualization_msgs::msg::MarkerArray createPathMarkerArray( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, - const int64_t lane_id, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - int32_t uid = planning_utils::bitShift(lane_id); - int32_t i = 0; - for (const auto & p : path.points) { - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.ns = ns; - marker.id = uid + i++; - marker.lifetime = rclcpp::Duration::from_seconds(0.3); - marker.type = visualization_msgs::msg::Marker::ARROW; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = p.point.pose; - marker.scale = createMarkerScale(0.6, 0.3, 0.3); - if (std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end()) { - // if p.lane_ids has lane_id - marker.color = createMarkerColor(r, g, b, 0.999); - } else { - marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.999); - } - msg.markers.push_back(marker); - } - - return msg; -} - visualization_msgs::msg::MarkerArray createPoseMarkerArray( const geometry_msgs::msg::Pose & pose, const std::string & ns, const int64_t id, const double r, const double g, const double b) @@ -196,68 +110,73 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( visualization_msgs::msg::MarkerArray debug_marker_array; const auto state = state_machine_.getState(); - const auto current_time = this->clock_->now(); + const auto now = this->clock_->now(); appendMarkerArray( - createPathMarkerArray(debug_data_.path_raw, "path_raw", lane_id_, 0.0, 1.0, 1.0), - &debug_marker_array, current_time); + debug::createPathMarkerArray( + debug_data_.path_raw, "path_raw", lane_id_, now, 0.6, 0.3, 0.3, 0.0, 1.0, 1.0), + &debug_marker_array, now); appendMarkerArray( - createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", lane_id_), - &debug_marker_array, current_time); + createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", module_id_), + &debug_marker_array, now); appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.detection_area_with_margin, "detection_area_with_margin", lane_id_), - &debug_marker_array, current_time); + debug_data_.detection_area_with_margin, "detection_area_with_margin", module_id_), + &debug_marker_array, now); appendMarkerArray( - createPolygonMarkerArray(debug_data_.ego_lane_polygon, "ego_lane", lane_id_, 0.0, 0.3, 0.7), - &debug_marker_array, current_time); + debug::createPolygonMarkerArray( + debug_data_.ego_lane_polygon, "ego_lane", module_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7), + &debug_marker_array, now); appendMarkerArray( - createPolygonMarkerArray( - debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", lane_id_, 0.0, 0.5, 0.5), - &debug_marker_array, current_time); + debug::createPolygonMarkerArray( + debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", module_id_, now, 0.3, 0.0, + 0.0, 0.0, 0.5, 0.5), + &debug_marker_array, now); appendMarkerArray( - createPolygonMarkerArray( + debug::createPolygonMarkerArray( debug_data_.candidate_collision_ego_lane_polygon, "candidate_collision_ego_lane_polygon", - lane_id_, 0.5, 0.0, 0.0), - &debug_marker_array, current_time); + module_id_, now, 0.3, 0.0, 0.0, 0.5, 0.0, 0.0), + &debug_marker_array, now); size_t i{0}; for (const auto & p : debug_data_.candidate_collision_object_polygons) { appendMarkerArray( - createPolygonMarkerArray( - p, "candidate_collision_object_polygons", lane_id_ + i++, 0.0, 0.5, 0.5), - &debug_marker_array, current_time); + debug::createPolygonMarkerArray( + p, "candidate_collision_object_polygons", module_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, + 0.5), + &debug_marker_array, now); } appendMarkerArray( - createObjectsMarkerArray( - debug_data_.conflicting_targets, "conflicting_targets", lane_id_, 0.99, 0.4, 0.0), - &debug_marker_array, current_time); + debug::createObjectsMarkerArray( + debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), + &debug_marker_array, now); appendMarkerArray( - createObjectsMarkerArray(debug_data_.stuck_targets, "stuck_targets", lane_id_, 0.99, 0.99, 0.2), - &debug_marker_array, current_time); + debug::createObjectsMarkerArray( + debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), + &debug_marker_array, now); appendMarkerArray( createPoseMarkerArray( - debug_data_.predicted_obj_pose, "predicted_obj_pose", lane_id_, 0.7, 0.85, 0.9), - &debug_marker_array, current_time); + debug_data_.predicted_obj_pose, "predicted_obj_pose", module_id_, 0.7, 0.85, 0.9), + &debug_marker_array, now); if (state == StateMachine::State::STOP) { appendMarkerArray( createPoseMarkerArray( - debug_data_.stop_point_pose, "stop_point_pose", lane_id_, 1.0, 0.0, 0.0), - &debug_marker_array, current_time); + debug_data_.stop_point_pose, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), + &debug_marker_array, now); appendMarkerArray( createPoseMarkerArray( - debug_data_.judge_point_pose, "judge_point_pose", lane_id_, 1.0, 1.0, 0.5), - &debug_marker_array, current_time); + debug_data_.judge_point_pose, "judge_point_pose", module_id_, 1.0, 1.0, 0.5), + &debug_marker_array, now); } return debug_marker_array; @@ -273,12 +192,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker if (debug_data_.stop_required) { appendMarkerArray( motion_utils::createStopVirtualWallMarker( - debug_data_.stop_wall_pose, "intersection", now, lane_id_), + debug_data_.stop_wall_pose, "intersection", now, module_id_), &wall_marker, now); } else if (state == StateMachine::State::STOP) { appendMarkerArray( motion_utils::createStopVirtualWallMarker( - debug_data_.slow_wall_pose, "intersection", now, lane_id_), + debug_data_.slow_wall_pose, "intersection", now, module_id_), &wall_marker, now); } return wall_marker; @@ -294,7 +213,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark if (state == StateMachine::State::STOP) { appendMarkerArray( createPoseMarkerArray( - debug_data_.stop_point_pose, "stop_point_pose", lane_id_, 1.0, 0.0, 0.0), + debug_data_.stop_point_pose, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), &debug_marker_array, now); } @@ -311,7 +230,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createVirtualWa if (state == StateMachine::State::STOP) { appendMarkerArray( motion_utils::createStopVirtualWallMarker( - debug_data_.virtual_wall_pose, "merge_from_private_road", now, lane_id_), + debug_data_.virtual_wall_pose, "merge_from_private_road", now, module_id_), &wall_marker, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp index f50265a32e8f7..b50fa38e48d6d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "utilization/debug.hpp" + #include "scene_module/no_stopping_area/scene_no_stopping_area.hpp" #include "utilization/util.hpp" @@ -55,15 +57,6 @@ geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) return msg; } -geometry_msgs::msg::Point toPoint2d(const geometry_msgs::msg::Point32 & poly) -{ - geometry_msgs::msg::Point msg; - msg.x = poly.x; - msg.y = poly.y; - msg.z = 0; - return msg; -} - visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const rclcpp::Time & now) { @@ -136,73 +129,35 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( return msg; } - -visualization_msgs::msg::MarkerArray createStuckPointsMarkerArray( - const std::vector & stuck_points, const rclcpp::Time & now) -{ - visualization_msgs::msg::MarkerArray msg; - { - auto marker = createDefaultMarker( - "map", now, "stuck_points", 0, visualization_msgs::msg::Marker::SPHERE, - createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(1.0, 1.0, 0.0, 0.999)); - marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); - for (size_t i = 0; i < stuck_points.size(); ++i) { - marker.id = i; - marker.pose.position = stuck_points.at(i); - msg.markers.push_back(marker); - } - } - return msg; -} - -visualization_msgs::msg::MarkerArray createNoStoppingAreaMarkerArray( - const geometry_msgs::msg::Polygon & stuck_vehicle_detect_area, const std::string & ns, - const rclcpp::Time & now) -{ - visualization_msgs::msg::MarkerArray msg; - { - auto marker = createDefaultMarker( - "map", now, ns.c_str(), 0, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(1.0, 1.0, 0.0, 0.999)); - marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); - - for (size_t i = 0; i < stuck_vehicle_detect_area.points.size(); ++i) { - marker.id = i; - marker.points.emplace_back(toPoint2d(stuck_vehicle_detect_area.points[i])); - } - marker.points.emplace_back(toPoint2d(stuck_vehicle_detect_area.points.at(0))); - msg.markers.push_back(marker); - } - return msg; -} - } // namespace visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; - const rclcpp::Time current_time = clock_->now(); + const rclcpp::Time now = clock_->now(); appendMarkerArray( - createLaneletInfoMarkerArray(no_stopping_area_reg_elem_, current_time), &debug_marker_array, - current_time); + createLaneletInfoMarkerArray(no_stopping_area_reg_elem_, now), &debug_marker_array, now); if (!debug_data_.stuck_points.empty()) { appendMarkerArray( - createStuckPointsMarkerArray(debug_data_.stuck_points, current_time), &debug_marker_array, - current_time); + debug::createPointsMarkerArray( + debug_data_.stuck_points, "stuck_points", module_id_, now, 0.3, 0.3, 0.3, 1.0, 1.0, 0.0), + &debug_marker_array, now); } if (!debug_data_.stuck_vehicle_detect_area.points.empty()) { appendMarkerArray( - createNoStoppingAreaMarkerArray( - debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", current_time), - &debug_marker_array, current_time); + debug::createPolygonMarkerArray( + debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", module_id_, now, 0.1, + 0.1, 0.1, 1.0, 1.0, 0.0), + &debug_marker_array, now); } if (!debug_data_.stop_line_detect_area.points.empty()) { appendMarkerArray( - createNoStoppingAreaMarkerArray( - debug_data_.stop_line_detect_area, "stop_line_detect_area", current_time), - &debug_marker_array, current_time); + debug::createPolygonMarkerArray( + debug_data_.stop_line_detect_area, "stop_line_detect_area", module_id_, now, 0.1, 0.1, 0.1, + 1.0, 1.0, 0.0), + &debug_marker_array, now); } return debug_marker_array; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index ef67609a0ad87..a0d70279679b4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -183,86 +184,43 @@ MarkerArray makeSlicePolygonMarker( } return debug_markers; } - -MarkerArray createPathMarkerArray( - const PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const double r, - const double g, const double b) -{ - MarkerArray msg; - int32_t uid = planning_utils::bitShift(lane_id); - int32_t i = 0; - for (const auto & p : path.points) { - Marker marker{}; - marker.header.frame_id = "map"; - marker.ns = ns; - marker.id = uid + i++; - marker.lifetime = rclcpp::Duration::from_seconds(0.3); - marker.type = Marker::ARROW; - marker.action = Marker::ADD; - marker.pose = p.point.pose; - marker.scale = createMarkerScale(0.6, 0.3, 0.3); - if (std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end()) { - // if p.lane_ids has lane_id - marker.color = createMarkerColor(r, g, b, 0.999); - } else { - marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.999); - } - msg.markers.push_back(marker); - } - - return msg; -} - -MarkerArray createOcclusionMarkerArray( - const std::vector & occlusion_points, const int64_t module_id) -{ - MarkerArray msg; - { - const Time now = rclcpp::Time(0); - auto marker = createDefaultMarker( - "map", now, "occlusion", 0, Marker::SPHERE, createMarkerScale(0.5, 0.5, 0.5), - createMarkerColor(1.0, 0.0, 0.0, 0.999)); - marker.lifetime = rclcpp::Duration::from_seconds(0.1); - for (size_t i = 0; i < occlusion_points.size(); ++i) { - marker.id = i + planning_utils::bitShift(module_id); - marker.pose.position = occlusion_points.at(i); - msg.markers.push_back(marker); - } - } - return msg; -} } // namespace MarkerArray OcclusionSpotModule::createDebugMarkerArray() { - const auto current_time = this->clock_->now(); + const auto now = this->clock_->now(); MarkerArray debug_marker_array; if (!debug_data_.possible_collisions.empty()) { - appendMarkerArray(makeDebugInfoMarkers(debug_data_), &debug_marker_array, current_time); + appendMarkerArray(makeDebugInfoMarkers(debug_data_), &debug_marker_array, now); } if (!debug_data_.detection_area_polygons.empty()) { appendMarkerArray( makeSlicePolygonMarker( debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z), - &debug_marker_array, current_time); + &debug_marker_array, now); } if (!debug_data_.close_partition.empty() && param_.is_show_occlusion) { appendMarkerArray( makePolygonMarker(debug_data_.close_partition, "close_partition", module_id_, debug_data_.z), - &debug_marker_array, current_time); + &debug_marker_array, now); } if (!debug_data_.path_interpolated.points.empty()) { + const int64_t virtual_lane_id = 0; appendMarkerArray( - createPathMarkerArray(debug_data_.path_raw, "path_raw", 0, 0.0, 1.0, 1.0), - &debug_marker_array, current_time); + debug::createPathMarkerArray( + debug_data_.path_raw, "path_raw", virtual_lane_id, now, 0.6, 0.3, 0.3, 0.0, 1.0, 1.0), + &debug_marker_array, now); appendMarkerArray( - createPathMarkerArray(debug_data_.path_interpolated, "path_interpolated", 0, 0.0, 1.0, 1.0), - &debug_marker_array, current_time); + debug::createPathMarkerArray( + debug_data_.path_interpolated, "path_interpolated", virtual_lane_id, now, 0.6, 0.3, 0.3, + 0.0, 1.0, 1.0), + &debug_marker_array, now); } if (!debug_data_.occlusion_points.empty()) { appendMarkerArray( - createOcclusionMarkerArray(debug_data_.occlusion_points, module_id_), &debug_marker_array, - current_time); + debug::createPointsMarkerArray( + debug_data_.occlusion_points, "occlusion", module_id_, now, 0.5, 0.5, 0.5, 1.0, 0.0, 0.0), + &debug_marker_array, now); } return debug_marker_array; } diff --git a/planning/behavior_velocity_planner/src/utilization/debug.cpp b/planning/behavior_velocity_planner/src/utilization/debug.cpp new file mode 100644 index 0000000000000..ec3bfef1fa2de --- /dev/null +++ b/planning/behavior_velocity_planner/src/utilization/debug.cpp @@ -0,0 +1,127 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace behavior_velocity_planner +{ +namespace debug +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; + +visualization_msgs::msg::MarkerArray createPolygonMarkerArray( + const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, + const rclcpp::Time & now, const double x, const double y, const double z, const double r, + const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + { + auto marker = createDefaultMarker( + "map", now, ns.c_str(), module_id, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(x, y, z), createMarkerColor(r, g, b, 0.8)); + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + + for (const auto & p : polygon.points) { + geometry_msgs::msg::Point point; + point.x = p.x; + point.y = p.y; + point.z = p.z; + marker.points.push_back(point); + } + + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + msg.markers.push_back(marker); + } + return msg; +} + +visualization_msgs::msg::MarkerArray createPathMarkerArray( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + for (size_t i = 0; i < path.points.size(); ++i) { + const auto & p = path.points.at(i); + + auto marker = createDefaultMarker( + "map", now, ns.c_str(), planning_utils::bitShift(lane_id) + i, + visualization_msgs::msg::Marker::ARROW, createMarkerScale(x, y, z), + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.pose = p.point.pose; + + if (std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end()) { + // if p.lane_ids has lane_id + marker.color = createMarkerColor(r, g, b, 0.999); + } else { + marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.999); + } + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createObjectsMarkerArray( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + auto marker = createDefaultMarker( + "map", now, ns, 0, visualization_msgs::msg::Marker::CUBE, createMarkerScale(3.0, 1.0, 1.0), + createMarkerColor(r, g, b, 0.8)); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + + marker.id = planning_utils::bitShift(module_id) + i; + marker.pose = object.kinematics.initial_pose_with_covariance.pose; + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createPointsMarkerArray( + const std::vector & points, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + auto marker = createDefaultMarker( + "map", now, ns, 0, visualization_msgs::msg::Marker::SPHERE, createMarkerScale(x, y, z), + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + for (size_t i = 0; i < points.size(); ++i) { + marker.id = i + planning_utils::bitShift(module_id); + marker.pose.position = points.at(i); + msg.markers.push_back(marker); + } + + return msg; +} +} // namespace debug +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp index c763063bd393d..d570492fd93b3 100644 --- a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp @@ -178,21 +178,4 @@ autoware_auto_planning_msgs::msg::Path filterStopPathPoint( } return filtered_path; } - -std::vector calcEuclidDist(const std::vector & x, const std::vector & y) -{ - if (x.size() != y.size()) { - std::cerr << "x y vector size should be the same." << std::endl; - } - - std::vector dist_v; - dist_v.push_back(0.0); - for (unsigned int i = 0; i < x.size() - 1; ++i) { - const double dx = x.at(i + 1) - x.at(i); - const double dy = y.at(i + 1) - y.at(i); - dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy)); - } - - return dist_v; -} } // namespace behavior_velocity_planner