From b9691ab2ca11a48fc3f1622a7674059419fc750d Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 14 Nov 2023 17:48:02 +0900 Subject: [PATCH] feat(interest_objects_marker_interface): change marker type Signed-off-by: Fumiya Watanabe --- .../src/marker_utils/utils.cpp | 2 + .../scene_module/lane_change/interface.cpp | 3 +- .../path_safety_checker/safety_check.cpp | 2 + .../interest_objects_marker_interface.hpp | 8 +- .../src/interest_objects_marker_interface.cpp | 132 +++++++++++++++--- 5 files changed, 127 insertions(+), 20 deletions(-) diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 031926f2d9d1d..f1f681fed06a0 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include #include #include @@ -45,6 +46,7 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; debug.current_obj_pose = obj.initial_pose.pose; + debug.extended_obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); debug.current_twist = obj.initial_twist.twist; return {tier4_autoware_utils::toHexString(obj.uuid), debug}; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 4969d05e4ff05..022d98d4c1649 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -315,7 +315,8 @@ void LaneChangeInterface::setObjectDebugVisualizationTmp() { const auto debug_data = module_type_->getDebugData(); for (const auto & [uuid, data] : debug_data) { - interest_objects_marker_interface_.insertObjectStatus(data.current_obj_pose, 1.5, data.is_safe); + interest_objects_marker_interface_.insertObjectStatus( + data.current_obj_pose, data.extended_obj_polygon, 1.5, data.is_safe); } return; } diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 40aa184105edb..c034b795196a3 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -318,6 +318,8 @@ std::vector getCollidedPolygons( const auto interpolated_data = getInterpolatedPoseWithVelocityAndPolygonStamped( predicted_ego_path, current_time, ego_vehicle_info); if (!interpolated_data) { + debug.expected_obj_pose = obj_pose; + debug.extended_obj_polygon = obj_polygon; continue; } const auto & ego_pose = interpolated_data->pose; diff --git a/planning/interest_objects_marker_interface/include/interest_objects_marker_interface/interest_objects_marker_interface.hpp b/planning/interest_objects_marker_interface/include/interest_objects_marker_interface/interest_objects_marker_interface.hpp index 9494adba90594..7531666ff04af 100644 --- a/planning/interest_objects_marker_interface/include/interest_objects_marker_interface/interest_objects_marker_interface.hpp +++ b/planning/interest_objects_marker_interface/include/interest_objects_marker_interface/interest_objects_marker_interface.hpp @@ -16,6 +16,7 @@ #define INTEREST_OBJECTS_MARKER_INTERFACE__INTEREST_OBJECTS_MARKER_INTERFACE_HPP_ #include "rclcpp/rclcpp.hpp" +#include #include #include @@ -28,12 +29,14 @@ namespace interest_objects_marker_interface { using geometry_msgs::msg::Pose; +using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; struct ObjectStatus { Pose pose{}; + Polygon2d polygon{}; double height{0.0}; bool safe{false}; }; @@ -42,7 +45,8 @@ class InterestObjectsMarkerInterface { public: InterestObjectsMarkerInterface(rclcpp::Node * node, const std::string & name); - void insertObjectStatus(const Pose & pose, const double obj_height, const bool safe); + void insertObjectStatus( + const Pose & pose, const Polygon2d & polygon, const double obj_height, const bool safe); void publishMarkerArray(); private: @@ -52,8 +56,6 @@ class InterestObjectsMarkerInterface std::string name_; std::string topic_namespace_ = "/planning/interest_objects_marker"; - - mutable std::mutex mutex_; }; } // namespace interest_objects_marker_interface diff --git a/planning/interest_objects_marker_interface/src/interest_objects_marker_interface.cpp b/planning/interest_objects_marker_interface/src/interest_objects_marker_interface.cpp index 924dd203effea..1d99fe0b1d2f7 100644 --- a/planning/interest_objects_marker_interface/src/interest_objects_marker_interface.cpp +++ b/planning/interest_objects_marker_interface/src/interest_objects_marker_interface.cpp @@ -13,38 +13,134 @@ // limitations under the License. #include "interest_objects_marker_interface/interest_objects_marker_interface.hpp" + +#include +#include +#include + +#include + namespace { using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Transform; +using geometry_msgs::msg::Vector3; using interest_objects_marker_interface::ObjectStatus; +using std_msgs::msg::ColorRGBA; +using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +using tier4_autoware_utils::calcAzimuthAngle; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createQuaternion; +using tier4_autoware_utils::createTranslation; -Marker createArrowMarker(const size_t & id, const ObjectStatus & status, const std::string & name) +using tier4_autoware_utils::cos; +using tier4_autoware_utils::getRPY; +using tier4_autoware_utils::pi; +using tier4_autoware_utils::sin; + +ColorRGBA getColor(const bool safe) { - const float r = status.safe ? 0.0f : 0.75f; - const float g = status.safe ? 0.75f : 0.0f; - const float b = 0.0f; + const float r = safe ? 0.0f : 214.0 / 255.0; + const float g = safe ? 211.0 / 255.0 : 0.0f; + const float b = safe ? 141.0 / 255.0 : 77.0 / 255.0; + return createMarkerColor(r, g, b, 0.95); +} + +double calcAzimuthAngleFromPolygon(const Polygon2d & polygon) +{ + const size_t polygon_size = polygon.outer().size(); + + const auto polygon_from = polygon.outer().at(polygon_size - 2); + Point p_from; + p_from.x = polygon_from.x(); + p_from.y = polygon_from.y(); + p_from.z = 0.0; + + const auto polygon_to = polygon.outer().back(); + Point p_to; + p_to.x = polygon_to.x(); + p_to.y = polygon_to.y(); + p_to.z = 0.0; + + return calcAzimuthAngle(p_from, p_to); +} + +Marker createCircleMarker( + const size_t & id, const ObjectStatus & status, const std::string & name, const double radius) +{ + if (status.polygon.outer().size() < 2) { + return Marker{}; + } + + const auto azimuth = calcAzimuthAngleFromPolygon(status.polygon); Marker marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, Marker::ARROW, - createMarkerScale(0.25, 0.5, 0.5), createMarkerColor(r, g, b, 0.999)); + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_circle", id, Marker::LINE_STRIP, + createMarkerScale(0.25, 0.0, 0.0), getColor(status.safe)); - Point src, dst; - src = status.pose.position; - src.z += status.height + 1.0; - dst = status.pose.position; - dst.z += status.height; + constexpr size_t num_points = 20; + std::vector points; + points.resize(num_points); - marker.points.push_back(src); - marker.points.push_back(dst); + for (size_t i = 0; i < num_points; ++i) { + const double ratio = static_cast(i) / static_cast(num_points); + const double theta = 2 * pi * ratio + azimuth; + points.at(i).x = status.pose.position.x + radius * cos(theta); + points.at(i).y = status.pose.position.y + radius * sin(theta); + points.at(i).z = status.pose.position.z; + marker.points.push_back(points.at(i)); + } + marker.points.push_back(points.front()); return marker; } + +MarkerArray createCrosshairMarker( + const size_t & id, const ObjectStatus & status, const std::string & name, const double radius) +{ + MarkerArray marker_array; + + if (status.polygon.outer().size() < 2) { + return marker_array; + } + + const auto azimuth = calcAzimuthAngleFromPolygon(status.polygon); + + marker_array.markers.push_back(createCircleMarker(id, status, name, radius)); + + for (size_t i = 0; i < 8; ++i) { + const double scale = (i % 2 == 0) ? 0.25 : 0.1; + const double radius_in = (i % 2 == 0) ? radius * 0.5 : radius * 0.75; + const double theta = 2 * pi * static_cast(i) / 8.0 + azimuth; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_cross_" + std::to_string(i), id, + Marker::LINE_STRIP, createMarkerScale(scale, 0.0, 0.0), getColor(status.safe)); + + Point src; + src.x = status.pose.position.x + radius * cos(theta); + src.y = status.pose.position.y + radius * sin(theta); + src.z = status.pose.position.z; + marker.points.push_back(src); + + Point dst; + dst.x = status.pose.position.x + radius_in * cos(theta); + dst.y = status.pose.position.y + radius_in * sin(theta); + dst.z = status.pose.position.z; + marker.points.push_back(dst); + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + } // namespace namespace interest_objects_marker_interface @@ -59,10 +155,11 @@ InterestObjectsMarkerInterface::InterestObjectsMarkerInterface( } void InterestObjectsMarkerInterface::insertObjectStatus( - const Pose & pose, const double obj_height, const bool safe) + const Pose & pose, const Polygon2d & polygon, double obj_height, const bool safe) { ObjectStatus status; status.pose = pose; + status.polygon = polygon; status.height = obj_height; status.safe = safe; @@ -74,8 +171,11 @@ void InterestObjectsMarkerInterface::publishMarkerArray() MarkerArray marker_array; for (size_t i = 0; i < obj_status_array_.size(); ++i) { const auto obj = obj_status_array_.at(i); - const Marker marker = createArrowMarker(i, obj, name_); - marker_array.markers.push_back(marker); + const double radius = 2.5; + const MarkerArray crosshair_markers = createCrosshairMarker(i, obj, name_, radius); + marker_array.markers.insert( + marker_array.markers.end(), crosshair_markers.markers.begin(), + crosshair_markers.markers.end()); } pub_marker_->publish(marker_array);