Skip to content

Commit

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

This reverts commit 156174f.
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara committed Sep 12, 2023
1 parent 847381e commit d0fe7c0
Show file tree
Hide file tree
Showing 3 changed files with 33 additions and 108 deletions.
94 changes: 0 additions & 94 deletions planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp
View file

This file was deleted.

15 changes: 15 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,21 @@ 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
32 changes: 18 additions & 14 deletions planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"

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

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);

Expand All @@ -91,7 +91,8 @@ 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), 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) {
Expand All @@ -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) {
Expand All @@ -136,27 +137,29 @@ 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);

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

0 comments on commit d0fe7c0

Please sign in to comment.