feat(autoware_test_utils): add parser for PredictedObjects (autowaref…

…oundation#9176)

feat(autoware_test_utils): add parser for predicted objects

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp

@@ -15,7 +15,10 @@
 #ifndef AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_
 #define AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_
 
+#include <builtin_interfaces/msg/duration.hpp>
+
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <autoware_planning_msgs/msg/lanelet_segment.hpp>
@@ -33,9 +36,16 @@
 
 namespace autoware::test_utils
 {
+using autoware_perception_msgs::msg::ObjectClassification;
+using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::PredictedObjectKinematics;
+using autoware_perception_msgs::msg::PredictedObjects;
+using autoware_perception_msgs::msg::PredictedPath;
+using autoware_perception_msgs::msg::Shape;
 using autoware_planning_msgs::msg::LaneletPrimitive;
 using autoware_planning_msgs::msg::LaneletRoute;
 using autoware_planning_msgs::msg::LaneletSegment;
+using builtin_interfaces::msg::Duration;
 using geometry_msgs::msg::Accel;
 using geometry_msgs::msg::AccelWithCovariance;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
@@ -48,6 +58,7 @@ using nav_msgs::msg::Odometry;
 using std_msgs::msg::Header;
 using tier4_planning_msgs::msg::PathPointWithLaneId;
 using tier4_planning_msgs::msg::PathWithLaneId;
+using unique_identifier_msgs::msg::UUID;
 
 /**
  * @brief Parses a YAML node and converts it into an object of type T.
@@ -65,6 +76,9 @@ T parse(const YAML::Node & node);
 template <>
 Header parse(const YAML::Node & node);
 
+template <>
+Duration parse(const YAML::Node & node);
+
 template <>
 std::vector<Point> parse(const YAML::Node & node);
 
@@ -107,6 +121,27 @@ std::vector<LaneletSegment> parse(const YAML::Node & node);
 template <>
 std::vector<PathPointWithLaneId> parse(const YAML::Node & node);
 
+template <>
+UUID parse(const YAML::Node & node);
+
+template <>
+PredictedPath parse(const YAML::Node & node);
+
+template <>
+ObjectClassification parse(const YAML::Node & node);
+
+template <>
+Shape parse(const YAML::Node & node);
+
+template <>
+PredictedObjectKinematics parse(const YAML::Node & node);
+
+template <>
+PredictedObject parse(const YAML::Node & node);
+
+template <>
+PredictedObjects parse(const YAML::Node & node);
+
 /**
  * @brief Parses a YAML file and converts it into an object of type T.
  *

common/autoware_test_utils/src/mock_data_parser.cpp

@@ -35,6 +35,15 @@ Header parse(const YAML::Node & node)
   return header;
 }
 
+template <>
+Duration parse(const YAML::Node & node)
+{
+  Duration msg;
+  msg.sec = node["sec"].as<int>();
+  msg.nanosec = node["nanosec"].as<int>();
+  return msg;
+}
+
 template <>
 std::array<double, 36> parse(const YAML::Node & node)
 {
@@ -221,6 +230,97 @@ std::vector<PathPointWithLaneId> parse<std::vector<PathPointWithLaneId>>(const Y
   return path_points;
 }
 
+template <>
+UUID parse(const YAML::Node & node)
+{
+  UUID msg;
+  const auto uuid = node["uuid"].as<std::vector<uint8_t>>();
+  for (unsigned i = 0; i < 16; ++i) {
+    msg.uuid.at(i) = uuid.at(i);
+  }
+  return msg;
+}
+
+template <>
+ObjectClassification parse(const YAML::Node & node)
+{
+  ObjectClassification msg;
+  msg.label = node["label"].as<uint8_t>();
+  msg.probability = node["probability"].as<float>();
+  return msg;
+}
+
+template <>
+Shape parse(const YAML::Node & node)
+{
+  Shape msg;
+  msg.type = node["type"].as<uint8_t>();
+  for (const auto & footprint_point_node : node["footprint"]["points"]) {
+    geometry_msgs::msg::Point32 point;
+    point.x = footprint_point_node["x"].as<float>();
+    point.y = footprint_point_node["y"].as<float>();
+    point.z = footprint_point_node["z"].as<float>();
+    msg.footprint.points.push_back(point);
+  }
+  msg.dimensions.x = node["dimensions"]["x"].as<double>();
+  msg.dimensions.y = node["dimensions"]["y"].as<double>();
+  msg.dimensions.z = node["dimensions"]["z"].as<double>();
+  return msg;
+}
+
+template <>
+PredictedPath parse(const YAML::Node & node)
+{
+  PredictedPath path;
+  for (const auto & path_pose_node : node["path"]) {
+    path.path.push_back(parse<Pose>(path_pose_node));
+  }
+  path.time_step = parse<Duration>(node["time_step"]);
+  path.confidence = node["confidence"].as<float>();
+  return path;
+}
+
+template <>
+PredictedObjectKinematics parse(const YAML::Node & node)
+{
+  PredictedObjectKinematics msg;
+  msg.initial_pose_with_covariance =
+    parse<PoseWithCovariance>(node["initial_pose_with_covariance"]);
+  msg.initial_twist_with_covariance =
+    parse<TwistWithCovariance>(node["initial_twist_with_covariance"]);
+  msg.initial_acceleration_with_covariance =
+    parse<AccelWithCovariance>(node["initial_acceleration_with_covariance"]);
+  for (const auto & predicted_path_node : node["predicted_paths"]) {
+    msg.predicted_paths.push_back(parse<PredictedPath>(predicted_path_node));
+  }
+  return msg;
+}
+
+template <>
+PredictedObject parse(const YAML::Node & node)
+{
+  PredictedObject msg;
+  msg.object_id = parse<UUID>(node["object_id"]);
+  msg.existence_probability = node["existence_probability"].as<float>();
+  for (const auto & classification_node : node["classification"]) {
+    msg.classification.push_back(parse<ObjectClassification>(classification_node));
+  }
+  msg.kinematics = parse<PredictedObjectKinematics>(node["kinematics"]);
+  msg.shape = parse<Shape>(node["shape"]);
+  return msg;
+}
+
+template <>
+PredictedObjects parse(const YAML::Node & node)
+{
+  PredictedObjects msg;
+  msg.header = parse<Header>(node["header"]);
+  for (const auto & object_node : node["objects"]) {
+    msg.objects.push_back(parse<PredictedObject>(object_node));
+  }
+  return msg;
+}
+
 template <>
 LaneletRoute parse(const std::string & filename)
 {