feat(autoware_test_utils): add visualization and yaml dumper for Path…

…WithLaneId

Signed-off-by: Mamoru Sobue <[email protected]>

common/autoware_test_utils/config/sample_topic_snapshot.yaml

@@ -24,6 +24,9 @@ fields:
   - name: dynamic_object
     type: PredictedObjects # autoware_perception_msgs::msg::PredictedObjects
     topic: /perception/object_recognition/objects
-#  - name: tracked_object
-#    type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects
-#    topic: /perception/object_recognition/tracking/objects
+  #  - name: tracked_object
+  #    type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects
+  #    topic: /perception/object_recognition/tracking/objects
+  #  - name: path_with_lane_id
+  #    type: PathWithLaneId # tier4_planning_msgs::msg::PathWithLaneId
+  #    topic: /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id

common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp

@@ -59,6 +59,7 @@ using autoware_perception_msgs::msg::TrafficLightGroupArray;
 using autoware_planning_msgs::msg::LaneletPrimitive;
 using autoware_planning_msgs::msg::LaneletRoute;
 using autoware_planning_msgs::msg::LaneletSegment;
+using autoware_planning_msgs::msg::PathPoint;
 using builtin_interfaces::msg::Duration;
 using builtin_interfaces::msg::Time;
 using geometry_msgs::msg::Accel;
@@ -97,6 +98,9 @@ Duration parse(const YAML::Node & node);
 template <>
 Time parse(const YAML::Node & node);
 
+template <>
+Point parse(const YAML::Node & node);
+
 template <>
 std::vector<Point> parse(const YAML::Node & node);
 
@@ -145,6 +149,15 @@ std::vector<PathPointWithLaneId> parse(const YAML::Node & node);
 template <>
 UUID parse(const YAML::Node & node);
 
+template <>
+PathPoint parse(const YAML::Node & node);
+
+template <>
+PathPointWithLaneId parse(const YAML::Node & node);
+
+template <>
+PathWithLaneId parse(const YAML::Node & node);
+
 template <>
 PredictedPath parse(const YAML::Node & node);
 

common/autoware_test_utils/include/autoware_test_utils/visualization.hpp

@@ -19,6 +19,10 @@
 
 #include <autoware/pyplot/patches.hpp>
 #include <autoware/pyplot/pyplot.hpp>
+#include <autoware/universe_utils/geometry/geometry.hpp>
+
+#include <tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp>
+#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_core/primitives/LineString.h>
@@ -150,7 +154,8 @@ inline void plot_lanelet2_object(
     const auto center = (left.front().basicPoint2d() + left.back().basicPoint2d() +
                          right.front().basicPoint2d() + right.back().basicPoint2d()) /
                         4.0;
-    axes.text(Args(center.x(), center.y(), std::to_string(lanelet.id())));
+    axes.text(
+      Args(center.x(), center.y(), std::to_string(lanelet.id())), Kwargs("clip_on"_a = true));
   }
 
   if (config_opt && config_opt.value().label) {
@@ -224,6 +229,99 @@ void plot_lanelet2_point(
 const lanelet::ConstPoint3d & point, autoware::pyplot::Axes & axes,
 const std::optional<PointConfig> & config_opt = std::nullopt);
 */
+
+struct DrivableAreaConfig
+{
+  static DrivableAreaConfig defaults() { return {"turquoise", 1.5}; }
+  std::optional<std::string> color{};
+  std::optional<double> linewidth{};
+};
+
+struct PathWithLaneIdConfig
+{
+  static PathWithLaneIdConfig defaults() { return {std::nullopt, "k", 1.0, std::nullopt, false}; }
+  std::optional<std::string> label{};
+  std::optional<std::string> color{};
+  std::optional<double> linewidth{};
+  std::optional<DrivableAreaConfig> da{};
+  bool lane_id{};
+};
+
+inline void plot_autoware_object(
+  const tier4_planning_msgs::msg::PathWithLaneId & path, autoware::pyplot::Axes & axes,
+  const std::optional<PathWithLaneIdConfig> & config_opt = std::nullopt)
+{
+  py::dict kwargs{};
+  if (config_opt) {
+    const auto & config = config_opt.value();
+    if (config.label) {
+      kwargs["label"] = config.label.value();
+    }
+    if (config.color) {
+      kwargs["color"] = config.color.value();
+    }
+    if (config.linewidth) {
+      kwargs["linewidth"] = config.linewidth.value();
+    }
+  }
+  std::vector<double> xs;
+  std::vector<double> ys;
+  std::vector<double> yaw_cos;
+  std::vector<double> yaw_sin;
+  std::vector<std::vector<lanelet::Id>> ids;
+  const bool plot_lane_id = config_opt ? config_opt.value().lane_id : false;
+  for (const auto & point : path.points) {
+    xs.push_back(point.point.pose.position.x);
+    ys.push_back(point.point.pose.position.y);
+    const auto th = autoware::universe_utils::getRPY(point.point.pose.orientation).z;
+    yaw_cos.push_back(std::cos(th));
+    yaw_sin.push_back(std::sin(th));
+    if (plot_lane_id) {
+      ids.emplace_back();
+      for (const auto & id : point.lane_ids) {
+        ids.back().push_back(id);
+      }
+    }
+  }
+  // plot centerline
+  axes.plot(Args(xs, ys), kwargs);
+  axes.quiver(
+    Args(xs, ys, yaw_cos, yaw_sin),
+    Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 1.0));
+  if (plot_lane_id) {
+    for (size_t i = 0; i < xs.size(); ++i) {
+      std::stringstream ss;
+      const char * delimiter = "";
+      for (const auto id : ids[i]) {
+        ss << std::exchange(delimiter, ",") << id;
+      }
+      axes.text(Args(xs[i], ys[i], ss.str()), Kwargs("clip_on"_a = true));
+    }
+  }
+  // plot drivable area
+  if (config_opt && config_opt.value().da) {
+    auto plot_boundary = [&](const decltype(path.left_bound) & points) {
+      std::vector<double> xs;
+      std::vector<double> ys;
+      for (const auto & point : points) {
+        xs.push_back(point.x);
+        ys.push_back(point.y);
+      }
+      const auto & cfg = config_opt.value().da.value();
+      py::dict kwargs{};
+      if (cfg.color) {
+        kwargs["color"] = cfg.color.value();
+      }
+      if (cfg.linewidth) {
+        kwargs["linewidth"] = cfg.linewidth.value();
+      }
+      axes.plot(Args(xs, ys), kwargs);
+    };
+    plot_boundary(path.left_bound);
+    plot_boundary(path.right_bound);
+  }
+}
+
 }  // namespace autoware::test_utils
 
 #endif  // AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_

common/autoware_test_utils/src/mock_data_parser.cpp

@@ -65,6 +65,16 @@ std::array<double, 36> parse(const YAML::Node & node)
   return msg;
 }
 
+template <>
+Point parse(const YAML::Node & node)
+{
+  Point geom_point;
+  geom_point.x = node["x"].as<double>();
+  geom_point.y = node["y"].as<double>();
+  geom_point.z = node["z"].as<double>();
+  return geom_point;
+}
+
 template <>
 std::vector<Point> parse(const YAML::Node & node)
 {
@@ -285,6 +295,46 @@ Shape parse(const YAML::Node & node)
   return msg;
 }
 
+template <>
+PathPoint parse(const YAML::Node & node)
+{
+  PathPoint point;
+  point.pose = parse<Pose>(node["pose"]);
+  point.longitudinal_velocity_mps = node["longitudinal_velocity_mps"].as<float>();
+  point.lateral_velocity_mps = node["lateral_velocity_mps"].as<float>();
+  point.heading_rate_rps = node["heading_rate_rps"].as<float>();
+  point.is_final = node["is_final"].as<bool>();
+  return point;
+}
+
+template <>
+PathPointWithLaneId parse(const YAML::Node & node)
+{
+  PathPointWithLaneId point;
+  point.point = parse<PathPoint>(node["point"]);
+  for (const auto & lane_id_node : node["lane_ids"]) {
+    point.lane_ids.push_back(lane_id_node.as<int64_t>());
+  }
+  return point;
+}
+
+template <>
+PathWithLaneId parse(const YAML::Node & node)
+{
+  PathWithLaneId path;
+  path.header = parse<Header>(node["header"]);
+  for (const auto & point_node : node["points"]) {
+    path.points.push_back(parse<PathPointWithLaneId>(point_node));
+  }
+  for (const auto & left_bound_node : node["left_bound"]) {
+    path.left_bound.push_back(parse<Point>(left_bound_node));
+  }
+  for (const auto & right_bound_node : node["right_bound"]) {
+    path.right_bound.push_back(parse<Point>(right_bound_node));
+  }
+  return path;
+}
+
 template <>
 PredictedPath parse(const YAML::Node & node)
 {

common/autoware_test_utils/src/topic_snapshot_saver.cpp

@@ -25,6 +25,7 @@
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <std_srvs/srv/empty.hpp>
+#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <yaml-cpp/yaml.h>
 
@@ -46,7 +47,8 @@ using MessageType = std::variant<
   autoware_adapi_v1_msgs::msg::OperationModeState,        // 3
   autoware_planning_msgs::msg::LaneletRoute,              // 4
   autoware_perception_msgs::msg::TrafficLightGroupArray,  // 5
-  autoware_perception_msgs::msg::TrackedObjects           // 6
+  autoware_perception_msgs::msg::TrackedObjects,          // 6
+  tier4_planning_msgs::msg::PathWithLaneId                // 7
   >;
 
 std::optional<size_t> get_topic_index(const std::string & name)
@@ -72,6 +74,9 @@ std::optional<size_t> get_topic_index(const std::string & name)
   if (name == "TrackedObjects") {
     return 6;
   }
+  if (name == "PathWithLaneId") {
+    return 7;
+  }
   return std::nullopt;
 }
 
@@ -196,6 +201,7 @@ class TopicSnapShotSaver
       REGISTER_CALLBACK(4);
       REGISTER_CALLBACK(5);
       REGISTER_CALLBACK(6);
+      REGISTER_CALLBACK(7);
     }
   }
 
@@ -243,6 +249,7 @@ class TopicSnapShotSaver
       REGISTER_WRITE_TYPE(4);
       REGISTER_WRITE_TYPE(5);
       REGISTER_WRITE_TYPE(6);
+      REGISTER_WRITE_TYPE(7);
     }
 
     const std::string desc = std::string(R"(#
@@ -253,7 +260,7 @@ class TopicSnapShotSaver
 # map_path_uri: package://<package-name>/<resource-path>
 # fields(this is array)
 #   - name: <field-name-for-your-yaml-of-this-topic, str>
-#     type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD}
+#     type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD}
 #     topic: <topic-name, str>
 #
 )");