-
Notifications
You must be signed in to change notification settings - Fork 639
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(behavior_path_planner): lane change revised visualization
Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
- Loading branch information
1 parent
bfb86cb
commit f1ef745
Showing
12 changed files
with
387 additions
and
12 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
88 changes: 88 additions & 0 deletions
88
...ng/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/debug.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,88 @@ | ||
// Copyright 2022 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__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_ | ||
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_ | ||
|
||
#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" | ||
|
||
#include <tier4_autoware_utils/geometry/geometry.hpp> | ||
#include <tier4_autoware_utils/ros/marker_helper.hpp> | ||
|
||
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp> | ||
#include <autoware_auto_perception_msgs/msg/predicted_path.hpp> | ||
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp> | ||
#include <visualization_msgs/msg/marker_array.hpp> | ||
|
||
#include <string> | ||
#include <unordered_map> | ||
#include <vector> | ||
|
||
namespace marker_utils | ||
{ | ||
using autoware_auto_perception_msgs::msg::PredictedObject; | ||
using autoware_auto_perception_msgs::msg::PredictedObjects; | ||
using autoware_auto_perception_msgs::msg::PredictedPath; | ||
using behavior_path_planner::LaneChangePath; | ||
using geometry_msgs::msg::Pose; | ||
using geometry_msgs::msg::Twist; | ||
using tier4_autoware_utils::Polygon2d; | ||
using visualization_msgs::msg::Marker; | ||
using visualization_msgs::msg::MarkerArray; | ||
namespace lane_change_markers | ||
{ | ||
|
||
struct LaneChangeObjectDebug | ||
{ | ||
std::string failed_reason; | ||
std::size_t lane_id; | ||
Pose current_pose; | ||
Twist current_twist; | ||
Pose relative_to_ego; | ||
bool is_front; | ||
bool allow_lane_change; | ||
std::vector<Pose> lerped_path; | ||
std::vector<PredictedPath> ego_predicted_path; | ||
Polygon2d ego_polygon; | ||
}; | ||
|
||
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}; | ||
} | ||
|
||
Marker initializeWithFrameStampAndNamespace(std::string && frame_id, std::string && ns); | ||
MarkerArray showObjectInfo( | ||
const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns); | ||
MarkerArray showAllLaneChangeLanes(const std::vector<LaneChangePath> & lanes, std::string && ns); | ||
MarkerArray showLerpedPose( | ||
const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns); | ||
MarkerArray showEgoPredictedPaths( | ||
const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns); | ||
MarkerArray showEgoPolygon( | ||
const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns); | ||
} // namespace lane_change_markers | ||
} // namespace marker_utils | ||
#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
188 changes: 188 additions & 0 deletions
188
planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,188 @@ | ||
// Copyright 2022 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. | ||
|
||
#include "behavior_path_planner/path_shifter/path_shifter.hpp" | ||
|
||
#include <behavior_path_planner/scene_module/lane_change/debug.hpp> | ||
#include <tier4_autoware_utils/ros/marker_helper.hpp> | ||
|
||
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp> | ||
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp> | ||
|
||
#include <cstdlib> | ||
#include <iomanip> | ||
#include <string> | ||
#include <vector> | ||
|
||
namespace marker_utils | ||
{ | ||
namespace lane_change_markers | ||
{ | ||
using geometry_msgs::msg::Point; | ||
|
||
Marker initializeWithFrameStampAndNamespace(std::string && frame_id, std::string && ns) | ||
{ | ||
Marker marker{}; | ||
marker.header.frame_id = frame_id; | ||
marker.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); | ||
marker.lifetime = rclcpp::Duration::from_seconds(0.2); | ||
marker.ns = ns.c_str(); | ||
return marker; | ||
} | ||
|
||
MarkerArray showObjectInfo( | ||
const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns) | ||
{ | ||
MarkerArray marker_array; | ||
|
||
constexpr float r{1.0}; | ||
constexpr float g{0.3}; | ||
constexpr float b{1.0}; | ||
constexpr float alpha{0.99}; | ||
int32_t id{0}; | ||
for (const auto & obj : obj_debug_vec) { | ||
Marker marker = initializeWithFrameStampAndNamespace("map", std::move(ns)); | ||
marker.id = ++id; | ||
marker.type = Marker::TEXT_VIEW_FACING; | ||
marker.action = Marker::ADD; | ||
marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0); | ||
marker.pose = obj.second.current_pose; | ||
marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, alpha); | ||
std::ostringstream ss; | ||
ss << obj.second.failed_reason << "\nLon: " << std::setprecision(4) | ||
<< obj.second.relative_to_ego.position.x | ||
<< "\nLat: " << obj.second.relative_to_ego.position.y; | ||
marker.text = ss.str(); | ||
marker_array.markers.push_back(marker); | ||
} | ||
return marker_array; | ||
} | ||
|
||
MarkerArray showAllLaneChangeLanes(const std::vector<LaneChangePath> & lanes, std::string && ns) | ||
{ | ||
MarkerArray marker_array; | ||
int32_t id{0}; | ||
|
||
constexpr auto colors = colorsList(); | ||
const auto loop_size = std::min(lanes.size(), colors.size()); | ||
|
||
float scale_val = 0.1; | ||
// float minus_res = 0.1; | ||
|
||
for (std::size_t idx = 0; idx < loop_size; ++idx) { | ||
Marker marker = initializeWithFrameStampAndNamespace("map", std::move(ns)); | ||
marker.id = ++id; | ||
marker.type = Marker::LINE_STRIP; | ||
marker.action = Marker::ADD; | ||
marker.scale = tier4_autoware_utils::createMarkerScale(scale_val, scale_val, 0.0); | ||
marker.color = | ||
tier4_autoware_utils::createMarkerColor(colors[idx][0], colors[idx][1], colors[idx][2], 0.9); | ||
for (const auto & point : lanes.at(idx).path.points) { | ||
marker.points.push_back(point.point.pose.position); | ||
} | ||
// scale_val = scale_val - minus_res; | ||
marker_array.markers.push_back(marker); | ||
} | ||
return marker_array; | ||
} | ||
|
||
MarkerArray showLerpedPose( | ||
const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns) | ||
{ | ||
MarkerArray marker_array; | ||
|
||
constexpr float r{1.0}; | ||
constexpr float g{0.3}; | ||
constexpr float b{1.0}; | ||
constexpr float alpha{0.99}; | ||
constexpr float scale = 0.5; | ||
int32_t id{0}; | ||
for (const auto & obj : obj_debug_vec) { | ||
Marker marker = initializeWithFrameStampAndNamespace("map", std::move(ns)); | ||
marker.id = ++id; | ||
marker.type = Marker::POINTS; | ||
marker.action = Marker::ADD; | ||
marker.scale = tier4_autoware_utils::createMarkerScale(scale, scale, scale); | ||
marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, alpha); | ||
for (const auto & point : obj.second.lerped_path) { | ||
marker.points.push_back(point.position); | ||
} | ||
marker_array.markers.push_back(marker); | ||
} | ||
return marker_array; | ||
} | ||
|
||
MarkerArray showEgoPredictedPaths( | ||
const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns) | ||
{ | ||
MarkerArray marker_array; | ||
constexpr auto colors = colorsList(); | ||
|
||
constexpr float scale_val = 0.2; | ||
// float minus_res = 0.1; | ||
|
||
int32_t id{0}; | ||
for (const auto & obj : obj_debug_vec) { | ||
const auto loop_size = std::min(obj.second.ego_predicted_path.size(), colors.size()); | ||
for (std::size_t idx = 0; idx < loop_size; ++idx) { | ||
const auto lane = obj.second.ego_predicted_path.at(idx); | ||
Marker marker = initializeWithFrameStampAndNamespace("map", std::move(ns)); | ||
marker.id = ++id; | ||
marker.type = Marker::POINTS; | ||
marker.action = Marker::ADD; | ||
marker.scale = tier4_autoware_utils::createMarkerScale(scale_val, scale_val, 0.0); | ||
marker.color = tier4_autoware_utils::createMarkerColor( | ||
colors[idx][0], colors[idx][1], colors[idx][2], 0.9); | ||
for (const auto & point : lane.path) { | ||
marker.points.push_back(point.position); | ||
} | ||
// scale_val = scale_val - minus_res; | ||
marker_array.markers.push_back(marker); | ||
} | ||
} | ||
return marker_array; | ||
} | ||
|
||
MarkerArray showEgoPolygon( | ||
const std::unordered_map<std::string, LaneChangeObjectDebug> & obj_debug_vec, std::string && ns) | ||
{ | ||
if (obj_debug_vec.empty()) { | ||
return MarkerArray{}; | ||
} | ||
Marker marker = initializeWithFrameStampAndNamespace("map", std::move(ns)); | ||
marker.type = Marker::LINE_STRIP; | ||
marker.action = Marker::ADD; | ||
marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.0, 0.0); | ||
marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.8); | ||
int32_t id{0}; | ||
|
||
MarkerArray marker_array; | ||
marker_array.markers.reserve(obj_debug_vec.size()); | ||
for (const auto & obj_datum : obj_debug_vec) { | ||
const auto & ego_polygon = obj_datum.second.ego_polygon.outer(); | ||
if (ego_polygon.empty()) { | ||
continue; | ||
} | ||
marker.id = ++id; | ||
marker.points.reserve(ego_polygon.size()); | ||
for (const auto & p : ego_polygon) { | ||
marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0)); | ||
} | ||
marker_array.markers.push_back(marker); | ||
} | ||
|
||
return marker_array; | ||
} | ||
} // namespace lane_change_markers | ||
} // namespace marker_utils |
Oops, something went wrong.