Skip to content

Commit

Permalink
feat(autoware_test_utils): add path with lane id parser (#9098)
Browse files Browse the repository at this point in the history 
* add path with lane id parser

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* refactor parse to use template

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 authored Oct 22, 2024
1 parent 99b8664 commit 41d6312
Show file tree
Hide file tree
Showing 7 changed files with 267 additions and 77 deletions.
58 changes: 53 additions & 5 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 @@ -19,6 +19,7 @@
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
#include <autoware_planning_msgs/msg/lanelet_segment.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>

#include <yaml-cpp/yaml.h>

Expand All @@ -30,17 +31,64 @@ 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::Point;
using geometry_msgs::msg::Pose;
using std_msgs::msg::Header;
using tier4_planning_msgs::msg::PathPointWithLaneId;
using tier4_planning_msgs::msg::PathWithLaneId;

Pose parse_pose(const YAML::Node & node);
/**
* @brief Parses a YAML node and converts it into an object of type T.
*
* This function extracts data from the given YAML node and converts it into an object of type T.
* If no specialization exists for T, it will result in a compile-time error.
*
* @tparam T The type of object to parse the node contents into.
* @param node The YAML node to be parsed.
* @return T An object of type T containing the parsed data.
*/
template <typename T>
T parse(const YAML::Node & node);

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

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

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

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

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

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

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

/**
* @brief Parses a YAML file and converts it into an object of type T.
*
* This function reads the specified YAML file and converts its contents into an object of the given
* type T. If no specialization exists for T, it will result in a compile-time error.
*
* @tparam T The type of object to parse the file contents into.
* @param filename The path to the YAML file to be parsed.
* @return T An object of type T containing the parsed data.
*/
template <typename T>
T parse(const std::string & filename);

template <>
LaneletRoute parse(const std::string & filename);

template <>
PathWithLaneId parse(const std::string & filename);
} // namespace autoware::test_utils

#endif // AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_
56 changes: 2 additions & 54 deletions common/autoware_test_utils/src/autoware_test_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <autoware_test_utils/autoware_test_utils.hpp>
#include <autoware_test_utils/mock_data_parser.hpp>
#include <rclcpp/clock.hpp>
#include <rclcpp/executors/single_threaded_executor.hpp>
#include <rclcpp/logging.hpp>
Expand Down Expand Up @@ -301,60 +302,7 @@ PathWithLaneId loadPathWithLaneIdInYaml()
{
const auto yaml_path =
get_absolute_path_to_config("autoware_test_utils", "path_with_lane_id_data.yaml");
YAML::Node yaml_node = YAML::LoadFile(yaml_path);
PathWithLaneId path_msg;

// Convert YAML data to PathWithLaneId message
// Fill the header
path_msg.header.stamp.sec = yaml_node["header"]["stamp"]["sec"].as<int>();
path_msg.header.stamp.nanosec = yaml_node["header"]["stamp"]["nanosec"].as<uint32_t>();
path_msg.header.frame_id = yaml_node["header"]["frame_id"].as<std::string>();

// Fill the points
for (const auto & point_node : yaml_node["points"]) {
PathPointWithLaneId point;
// Fill the PathPoint data
point.point.pose.position.x = point_node["point"]["pose"]["position"]["x"].as<double>();
point.point.pose.position.y = point_node["point"]["pose"]["position"]["y"].as<double>();
point.point.pose.position.z = point_node["point"]["pose"]["position"]["z"].as<double>();
point.point.pose.orientation.x = point_node["point"]["pose"]["orientation"]["x"].as<double>();
point.point.pose.orientation.y = point_node["point"]["pose"]["orientation"]["y"].as<double>();
point.point.pose.orientation.z = point_node["point"]["pose"]["orientation"]["z"].as<double>();
point.point.pose.orientation.w = point_node["point"]["pose"]["orientation"]["w"].as<double>();
point.point.longitudinal_velocity_mps =
point_node["point"]["longitudinal_velocity_mps"].as<float>();
point.point.lateral_velocity_mps = point_node["point"]["lateral_velocity_mps"].as<float>();
point.point.heading_rate_rps = point_node["point"]["heading_rate_rps"].as<float>();
point.point.is_final = point_node["point"]["is_final"].as<bool>();
// Fill the lane_ids
for (const auto & lane_id_node : point_node["lane_ids"]) {
point.lane_ids.push_back(lane_id_node.as<int64_t>());
}

path_msg.points.push_back(point);
}

// Fill the left_bound
for (const auto & point_node : yaml_node["left_bound"]) {
Point point;
// Fill the Point data (left_bound)
point.x = point_node["x"].as<double>();
point.y = point_node["y"].as<double>();
point.z = point_node["z"].as<double>();

path_msg.left_bound.push_back(point);
}

// Fill the right_bound
for (const auto & point_node : yaml_node["right_bound"]) {
Point point;
// Fill the Point data
point.x = point_node["x"].as<double>();
point.y = point_node["y"].as<double>();
point.z = point_node["z"].as<double>();

path_msg.right_bound.push_back(point);
}
return path_msg;
return parse<PathWithLaneId>(yaml_path);
}
} // namespace autoware::test_utils
114 changes: 102 additions & 12 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,7 +29,8 @@

namespace autoware::test_utils
{
Pose parse_pose(const YAML::Node & node)
template <>
Pose parse(const YAML::Node & node)
{
Pose pose;
pose.position.x = node["position"]["x"].as<double>();
Expand All @@ -42,7 +43,8 @@ Pose parse_pose(const YAML::Node & node)
return pose;
}

LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node)
template <>
LaneletPrimitive parse(const YAML::Node & node)
{
LaneletPrimitive primitive;
primitive.id = node["id"].as<int64_t>();
Expand All @@ -51,43 +53,131 @@ LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node)
return primitive;
}

std::vector<LaneletPrimitive> parse_lanelet_primitives(const YAML::Node & node)
template <>
std::vector<LaneletPrimitive> parse(const YAML::Node & node)
{
std::vector<LaneletPrimitive> primitives;
primitives.reserve(node.size());
std::transform(node.begin(), node.end(), std::back_inserter(primitives), [&](const auto & p) {
return parse_lanelet_primitive(p);
return parse<LaneletPrimitive>(p);
});

return primitives;
}

std::vector<LaneletSegment> parse_segments(const YAML::Node & node)
template <>
std::vector<LaneletSegment> parse(const YAML::Node & node)
{
std::vector<LaneletSegment> segments;
std::transform(node.begin(), node.end(), std::back_inserter(segments), [&](const auto & input) {
LaneletSegment segment;
segment.preferred_primitive = parse_lanelet_primitive(input["preferred_primitive"]);
segment.primitives = parse_lanelet_primitives(input["primitives"]);
segment.preferred_primitive = parse<LaneletPrimitive>(input["preferred_primitive"]);
segment.primitives = parse<std::vector<LaneletPrimitive>>(input["primitives"]);
return segment;
});

return segments;
}

// cppcheck-suppress unusedFunction
LaneletRoute parse_lanelet_route_file(const std::string & filename)
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)
{
std::vector<PathPointWithLaneId> path_points;

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

const auto & points = node["points"];
path_points.reserve(points.size());
std::transform(
points.begin(), points.end(), std::back_inserter(path_points), [&](const YAML::Node & input) {
PathPointWithLaneId point;
if (!input["point"]) {
return point;
}
const auto & point_node = input["point"];
point.point.pose = parse<Pose>(point_node["pose"]);

point.point.longitudinal_velocity_mps = point_node["longitudinal_velocity_mps"].as<float>();
point.point.lateral_velocity_mps = point_node["lateral_velocity_mps"].as<float>();
point.point.heading_rate_rps = point_node["heading_rate_rps"].as<float>();
point.point.is_final = point_node["is_final"].as<bool>();
// Fill the lane_ids
for (const auto & lane_id_node : input["lane_ids"]) {
point.lane_ids.push_back(lane_id_node.as<int64_t>());
}

return point;
});

return path_points;
}

template <>
LaneletRoute parse(const std::string & filename)
{
LaneletRoute lanelet_route;
try {
YAML::Node config = YAML::LoadFile(filename);

lanelet_route.start_pose = (config["start_pose"]) ? parse_pose(config["start_pose"]) : Pose();
lanelet_route.goal_pose = (config["goal_pose"]) ? parse_pose(config["goal_pose"]) : Pose();
lanelet_route.segments = parse_segments(config["segments"]);
lanelet_route.start_pose = (config["start_pose"]) ? parse<Pose>(config["start_pose"]) : Pose();
lanelet_route.goal_pose = (config["goal_pose"]) ? parse<Pose>(config["goal_pose"]) : Pose();
lanelet_route.segments = parse<std::vector<LaneletSegment>>(config["segments"]);
} catch (const std::exception & e) {
RCLCPP_DEBUG(rclcpp::get_logger("autoware_test_utils"), "Exception caught: %s", e.what());
}
return lanelet_route;
}

template <>
PathWithLaneId parse(const std::string & filename)
{
PathWithLaneId path;
try {
YAML::Node yaml_node = YAML::LoadFile(filename);

path.header = parse<Header>(yaml_node);
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"]);
} catch (const std::exception & e) {
RCLCPP_DEBUG(rclcpp::get_logger("autoware_test_utils"), "Exception caught: %s", e.what());
}

return path;
}
} // namespace autoware::test_utils
Loading

0 comments on commit 41d6312

Please sign in to comment.