Skip to content

Commit

Permalink
refactor(behavior_path_planner): use function with color name (autowa…
Browse files Browse the repository at this point in the history
…refoundation#4797)

* refactor(behavior_path_planner): use function with color name

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* 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 <zulfaqar.azmi@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
  • Loading branch information
2 people authored and kminoda committed Aug 30, 2023
1 parent 4f2b446 commit 96824aa
Show file tree
Hide file tree
Showing 3 changed files with 108 additions and 33 deletions.
Original file line number Diff line number Diff line change
@@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand All @@ -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};
Expand Down Expand Up @@ -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);

Expand All @@ -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) {
Expand All @@ -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) {
Expand All @@ -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));
Expand All @@ -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));
Expand All @@ -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};
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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;
Expand Down

0 comments on commit 96824aa

Please sign in to comment.