diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp deleted file mode 100644 index 82c387d04e525..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ - -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - -#include "std_msgs/msg/color_rgba.hpp" - -#include - -namespace marker_utils::colors -{ -using std_msgs::msg::ColorRGBA; - -inline ColorRGBA red(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(1., 0., 0., a); -} - -inline ColorRGBA green(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(0., 1., 0., a); -} - -inline ColorRGBA blue(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(0., 0., 1., a); -} - -inline ColorRGBA yellow(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(1., 1., 0., a); -} - -inline ColorRGBA aqua(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(0., 1., 1., a); -} - -inline ColorRGBA magenta(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(1., 0., 1., a); -} - -inline ColorRGBA medium_orchid(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(0.729, 0.333, 0.827, a); -} - -inline ColorRGBA light_pink(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(1., 0.713, 0.756, a); -} - -inline ColorRGBA light_yellow(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(1., 1., 0.878, a); -} - -inline ColorRGBA light_steel_blue(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(0.690, 0.768, 0.870, a); -} - -inline ColorRGBA white(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(1., 1., 1., a); -} - -inline ColorRGBA grey(float a = 0.99) -{ - return tier4_autoware_utils::createMarkerColor(.5, .5, .5, a); -} - -inline std::vector colors_list(float a = 0.99) -{ - return {red(a), green(a), blue(a), yellow(a), aqua(a), - magenta(a), medium_orchid(a), light_pink(a), light_yellow(a), light_steel_blue(a)}; -} -} // namespace marker_utils::colors - -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index b6166ea802f5f..cbedf045edc5d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -51,6 +51,21 @@ using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +constexpr std::array, 10> colorsList() +{ + constexpr std::array red = {1., 0., 0.}; + constexpr std::array green = {0., 1., 0.}; + constexpr std::array blue = {0., 0., 1.}; + constexpr std::array yellow = {1., 1., 0.}; + constexpr std::array aqua = {0., 1., 1.}; + constexpr std::array magenta = {1., 0., 1.}; + constexpr std::array medium_orchid = {0.729, 0.333, 0.827}; + constexpr std::array light_pink = {1, 0.713, 0.756}; + constexpr std::array light_yellow = {1, 1, 0.878}; + constexpr std::array light_steel_blue = {0.690, 0.768, 0.870}; + return {red, green, blue, yellow, aqua, + magenta, medium_orchid, light_pink, light_yellow, light_steel_blue}; +} inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index aab2be6f3054e..4a76c7e7dfb11 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" @@ -34,13 +33,14 @@ namespace marker_utils::lane_change_markers { using geometry_msgs::msg::Point; using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) { Marker obj_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(0.0, 1.0, 1.0, 0.999)); MarkerArray marker_array; int32_t id{0}; @@ -80,7 +80,7 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes int32_t id{0}; const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - const auto colors = colors::colors_list(); + constexpr auto colors = colorsList(); const auto loop_size = std::min(lanes.size(), colors.size()); marker_array.markers.reserve(loop_size); @@ -91,7 +91,8 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes const auto & color = colors.at(idx); Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), color); + "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), + createMarkerColor(color[0], color[1], color[2], 0.9)); marker.points.reserve(lanes.at(idx).path.points.size()); for (const auto & point : lanes.at(idx).path.points) { @@ -113,7 +114,7 @@ MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::st for (const auto & [uuid, info] : obj_debug_vec) { Marker marker = createDefaultMarker( "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3), - colors::magenta()); + createMarkerColor(1.0, 0.3, 1.0, 0.9)); marker.points.reserve(info.lerped_path.size()); for (const auto & point : info.lerped_path) { @@ -136,15 +137,17 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); Marker ego_marker = createDefaultMarker( "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val), - colors::green()); + createMarkerColor(1.0, 1.0, 1.0, 0.9)); Marker obj_marker = ego_marker; auto text_marker = createDefaultMarker( "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), colors::white()); + createMarkerScale(1.5, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 1.0)); MarkerArray marker_array; + constexpr auto red_color = colorsList().at(0); + constexpr auto green_color = colorsList().at(1); const auto reserve_size = obj_debug_vec.size(); marker_array.markers.reserve(reserve_size * 4); @@ -152,11 +155,11 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin int32_t idx = {0}; for (const auto & [uuid, info] : obj_debug_vec) { - const auto color = info.is_safe ? colors::green() : colors::red(); + const auto & color = info.is_safe ? green_color : red_color; const auto & ego_polygon = info.extended_ego_polygon.outer(); const auto poly_z = info.current_pose.position.z; // temporally ego_marker.id = ++id; - ego_marker.color = color; + ego_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); ego_marker.points.reserve(ego_polygon.size()); for (const auto & p : ego_polygon) { ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); @@ -173,7 +176,7 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin const auto & obj_polygon = info.extended_obj_polygon.outer(); obj_marker.id = ++id; - obj_marker.color = color; + obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); obj_marker.points.reserve(obj_polygon.size()); for (const auto & p : obj_polygon) { obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); @@ -193,7 +196,7 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) { - const auto colors = colors::colors_list(); + constexpr auto colors = colorsList(); const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); MarkerArray marker_array; int32_t id{0}; @@ -204,7 +207,8 @@ MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::s for (const auto & [uuid, info] : obj_debug_vec) { const auto & color = colors.at(idx); Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color); + "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), + createMarkerColor(color[0], color[1], color[2], 0.999)); marker.points.reserve(2); marker.points.push_back(info.expected_ego_pose.position); marker.points.push_back(info.expected_obj_pose.position); @@ -228,7 +232,7 @@ MarkerArray createLaneChangingVirtualWallMarker( { auto wall_marker = createDefaultMarker( "map", now, ns + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(0.1, 5.0, 2.0), colors::green()); + createMarkerScale(0.1, 5.0, 2.0), createMarkerColor(0.0, 1.0, 0.0, 0.5)); wall_marker.pose = lane_changing_pose; wall_marker.pose.position.z += 1.0; marker_array.markers.push_back(wall_marker); @@ -237,7 +241,7 @@ MarkerArray createLaneChangingVirtualWallMarker( { auto text_marker = createDefaultMarker( "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), colors::white()); + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); text_marker.pose = lane_changing_pose; text_marker.pose.position.z += 2.0; text_marker.text = module_name;