Skip to content

Commit

Permalink
feat(autoware_test_utils): add parser for geometry_msgs and others (#…
Browse files Browse the repository at this point in the history 
…9167)

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin authored Oct 29, 2024
1 parent a5abc74 commit 6dad803
Show file tree
Hide file tree
Showing 3 changed files with 345 additions and 42 deletions.
47 changes: 41 additions & 6 deletions common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,15 @@
#include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
#include <autoware_planning_msgs/msg/lanelet_segment.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance.hpp>
#include <geometry_msgs/msg/twist_with_covariance.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>

#include <yaml-cpp/yaml.h>

#include <array>
#include <string>
#include <vector>

Expand All @@ -31,8 +35,15 @@ namespace autoware::test_utils
using autoware_planning_msgs::msg::LaneletPrimitive;
using autoware_planning_msgs::msg::LaneletRoute;
using autoware_planning_msgs::msg::LaneletSegment;
using geometry_msgs::msg::Accel;
using geometry_msgs::msg::AccelWithCovariance;
using geometry_msgs::msg::AccelWithCovarianceStamped;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseWithCovariance;
using geometry_msgs::msg::Twist;
using geometry_msgs::msg::TwistWithCovariance;
using nav_msgs::msg::Odometry;
using std_msgs::msg::Header;
using tier4_planning_msgs::msg::PathPointWithLaneId;
using tier4_planning_msgs::msg::PathWithLaneId;
Expand All @@ -50,23 +61,47 @@ using tier4_planning_msgs::msg::PathWithLaneId;
template <typename T>
T parse(const YAML::Node & node);

template <>
Header parse(const YAML::Node & node);

template <>
std::vector<Point> parse(const YAML::Node & node);

template <>
std::array<double, 36> parse(const YAML::Node & node);

template <>
Pose parse(const YAML::Node & node);

template <>
LaneletPrimitive parse(const YAML::Node & node);
PoseWithCovariance parse(const YAML::Node & node);

template <>
std::vector<LaneletPrimitive> parse(const YAML::Node & node);
Twist parse(const YAML::Node & node);

template <>
std::vector<LaneletSegment> parse(const YAML::Node & node);
TwistWithCovariance parse(const YAML::Node & node);

template <>
std::vector<Point> parse(const YAML::Node & node);
Odometry parse(const YAML::Node & node);

template <>
Header parse(const YAML::Node & node);
Accel parse(const YAML::Node & node);

template <>
AccelWithCovariance parse(const YAML::Node & node);

template <>
AccelWithCovarianceStamped parse(const YAML::Node & node);

template <>
LaneletPrimitive parse(const YAML::Node & node);

template <>
std::vector<LaneletPrimitive> parse(const YAML::Node & node);

template <>
std::vector<LaneletSegment> parse(const YAML::Node & node);

template <>
std::vector<PathPointWithLaneId> parse(const YAML::Node & node);
Expand Down
153 changes: 117 additions & 36 deletions common/autoware_test_utils/src/mock_data_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,47 @@

namespace autoware::test_utils
{

template <>
Header parse(const YAML::Node & node)
{
Header header;

header.stamp.sec = node["stamp"]["sec"].as<int>();
header.stamp.nanosec = node["stamp"]["nanosec"].as<uint32_t>();
header.frame_id = node["frame_id"].as<std::string>();

return header;
}

template <>
std::array<double, 36> parse(const YAML::Node & node)
{
std::array<double, 36> msg{};
const auto cov = node.as<std::vector<double>>();
for (size_t i = 0; i < 36; ++i) {
msg[i] = cov.at(i);
}
return msg;
}

template <>
std::vector<Point> parse(const YAML::Node & node)
{
std::vector<Point> geom_points;

std::transform(
node.begin(), node.end(), std::back_inserter(geom_points), [&](const YAML::Node & input) {
Point point;
point.x = input["x"].as<double>();
point.y = input["y"].as<double>();
point.z = input["z"].as<double>();
return point;
});

return geom_points;
}

template <>
Pose parse(const YAML::Node & node)
{
Expand All @@ -43,6 +84,79 @@ Pose parse(const YAML::Node & node)
return pose;
}

template <>
PoseWithCovariance parse(const YAML::Node & node)
{
PoseWithCovariance msg;
msg.pose = parse<Pose>(node["pose"]);
msg.covariance = parse<std::array<double, 36>>(node["covariance"]);
return msg;
}

template <>
Twist parse(const YAML::Node & node)
{
Twist msg;
msg.linear.x = node["linear"]["x"].as<double>();
msg.linear.y = node["linear"]["y"].as<double>();
msg.linear.z = node["linear"]["z"].as<double>();
msg.angular.x = node["angular"]["x"].as<double>();
msg.angular.y = node["angular"]["y"].as<double>();
msg.angular.z = node["angular"]["z"].as<double>();
return msg;
}

template <>
TwistWithCovariance parse(const YAML::Node & node)
{
TwistWithCovariance msg;
msg.twist = parse<Twist>(node["twist"]);
msg.covariance = parse<std::array<double, 36>>(node["covariance"]);
return msg;
}

template <>
Odometry parse(const YAML::Node & node)
{
Odometry msg;
msg.header = parse<Header>(node["header"]);
msg.child_frame_id = node["child_frame_id"].as<std::string>();
msg.pose = parse<PoseWithCovariance>(node["pose"]);
msg.twist = parse<TwistWithCovariance>(node["twist"]);
return msg;
}

template <>
Accel parse(const YAML::Node & node)
{
Accel msg;
msg.linear.x = node["linear"]["x"].as<double>();
msg.linear.y = node["linear"]["y"].as<double>();
msg.linear.z = node["linear"]["z"].as<double>();
msg.angular.x = node["angular"]["x"].as<double>();
msg.angular.y = node["angular"]["y"].as<double>();
msg.angular.z = node["angular"]["z"].as<double>();
return msg;
}

template <>
AccelWithCovariance parse(const YAML::Node & node)
{
AccelWithCovariance msg;
msg.accel = parse<Accel>(node["accel"]);
msg.covariance = parse<std::array<double, 36>>(node["covariance"]);
return msg;
}

template <>
AccelWithCovarianceStamped parse(const YAML::Node & node)
{
AccelWithCovarianceStamped msg;
msg.header = parse<Header>(node["header"]);
msg.accel = parse<AccelWithCovariance>(node["accel"]);
return msg;
}

template <>
LaneletPrimitive parse(const YAML::Node & node)
{
Expand Down Expand Up @@ -79,39 +193,6 @@ std::vector<LaneletSegment> parse(const YAML::Node & node)
return segments;
}

template <>
std::vector<Point> parse(const YAML::Node & node)
{
std::vector<Point> geom_points;

std::transform(
node.begin(), node.end(), std::back_inserter(geom_points), [&](const YAML::Node & input) {
Point point;
point.x = input["x"].as<double>();
point.y = input["y"].as<double>();
point.z = input["z"].as<double>();
return point;
});

return geom_points;
}

template <>
Header parse(const YAML::Node & node)
{
Header header;

if (!node["header"]) {
return header;
}

header.stamp.sec = node["header"]["stamp"]["sec"].as<int>();
header.stamp.nanosec = node["header"]["stamp"]["nanosec"].as<uint32_t>();
header.frame_id = node["header"]["frame_id"].as<std::string>();

return header;
}

template <>
std::vector<PathPointWithLaneId> parse<std::vector<PathPointWithLaneId>>(const YAML::Node & node)
{
Expand Down Expand Up @@ -167,10 +248,10 @@ template <>
PathWithLaneId parse(const std::string & filename)
{
PathWithLaneId path;
try {
YAML::Node yaml_node = YAML::LoadFile(filename);
YAML::Node yaml_node = YAML::LoadFile(filename);

path.header = parse<Header>(yaml_node);
try {
path.header = parse<Header>(yaml_node["header"]);
path.points = parse<std::vector<PathPointWithLaneId>>(yaml_node);
path.left_bound = parse<std::vector<Point>>(yaml_node["left_bound"]);
path.right_bound = parse<std::vector<Point>>(yaml_node["right_bound"]);
Expand Down
Loading

0 comments on commit 6dad803

Please sign in to comment.