From 96824aa1deeb509799a4a7b91c5ade54bdc51e50 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 29 Aug 2023 23:07:20 +0900 Subject: [PATCH] refactor(behavior_path_planner): use function with color name (#4797) * refactor(behavior_path_planner): use function with color name Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../marker_utils/colors.hpp | 94 +++++++++++++++++++ .../marker_utils/utils.hpp | 15 --- .../src/marker_utils/lane_change/debug.cpp | 32 +++---- 3 files changed, 108 insertions(+), 33 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp 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 new file mode 100644 index 0000000000000..82c387d04e525 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp @@ -0,0 +1,94 @@ +// 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 86964117de34c..657437615f809 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 @@ -52,21 +52,6 @@ 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 4a76c7e7dfb11..aab2be6f3054e 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,6 +12,7 @@ // 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" @@ -33,14 +34,13 @@ 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), createMarkerColor(0.0, 1.0, 1.0, 0.999)); + createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); 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()}; - constexpr auto colors = colorsList(); + const auto colors = colors::colors_list(); const auto loop_size = std::min(lanes.size(), colors.size()); marker_array.markers.reserve(loop_size); @@ -91,8 +91,7 @@ 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), - createMarkerColor(color[0], color[1], color[2], 0.9)); + "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), color); marker.points.reserve(lanes.at(idx).path.points.size()); for (const auto & point : lanes.at(idx).path.points) { @@ -114,7 +113,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), - createMarkerColor(1.0, 0.3, 1.0, 0.9)); + colors::magenta()); marker.points.reserve(info.lerped_path.size()); for (const auto & point : info.lerped_path) { @@ -137,17 +136,15 @@ 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), - createMarkerColor(1.0, 1.0, 1.0, 0.9)); + colors::green()); 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), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(1.5, 1.5, 1.5), colors::white()); 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); @@ -155,11 +152,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 ? green_color : red_color; + const auto color = info.is_safe ? colors::green() : colors::red(); 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 = createMarkerColor(color[0], color[1], color[2], 0.8); + ego_marker.color = color; 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)); @@ -176,7 +173,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 = createMarkerColor(color[0], color[1], color[2], 0.8); + obj_marker.color = color; 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)); @@ -196,7 +193,7 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) { - constexpr auto colors = colorsList(); + const auto colors = colors::colors_list(); const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); MarkerArray marker_array; int32_t id{0}; @@ -207,8 +204,7 @@ 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), - createMarkerColor(color[0], color[1], color[2], 0.999)); + "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color); marker.points.reserve(2); marker.points.push_back(info.expected_ego_pose.position); marker.points.push_back(info.expected_obj_pose.position); @@ -232,7 +228,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), createMarkerColor(0.0, 1.0, 0.0, 0.5)); + createMarkerScale(0.1, 5.0, 2.0), colors::green()); wall_marker.pose = lane_changing_pose; wall_marker.pose.position.z += 1.0; marker_array.markers.push_back(wall_marker); @@ -241,7 +237,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), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(0.0, 0.0, 1.0), colors::white()); text_marker.pose = lane_changing_pose; text_marker.pose.position.z += 2.0; text_marker.text = module_name;