refactor(behavior_path_planner): use function with color name

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 <vector>
+
+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<ColorRGBA> 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_

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<std::array<float, 3>, 10> colorsList()
-{
-  constexpr std::array<float, 3> red = {1., 0., 0.};
-  constexpr std::array<float, 3> green = {0., 1., 0.};
-  constexpr std::array<float, 3> blue = {0., 0., 1.};
-  constexpr std::array<float, 3> yellow = {1., 1., 0.};
-  constexpr std::array<float, 3> aqua = {0., 1., 1.};
-  constexpr std::array<float, 3> magenta = {1., 0., 1.};
-  constexpr std::array<float, 3> medium_orchid = {0.729, 0.333, 0.827};
-  constexpr std::array<float, 3> light_pink = {1, 0.713, 0.756};
-  constexpr std::array<float, 3> light_yellow = {1, 1, 0.878};
-  constexpr std::array<float, 3> 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);

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<LaneChangePath> & 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<LaneChangePath> & 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,29 +136,27 @@ 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);
 
   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;