diff --git a/rcss3d_agent/src/sexp_parser.cpp b/rcss3d_agent/src/sexp_parser.cpp index 192e6e5..1d5a237 100644 --- a/rcss3d_agent/src/sexp_parser.cpp +++ b/rcss3d_agent/src/sexp_parser.cpp @@ -154,6 +154,9 @@ std::optional SexpParser::getVision() if (auto ball = sexp_vision::getBall(*seeSexp); ball.has_value()) { vision.ball.push_back(ball.value()); } + for (auto fieldFeature : sexp_vision::getFieldFeatures(*seeSexp)) { + vision.field_features.push_back(fieldFeature); + } for (auto fieldLine : sexp_vision::getFieldLines(*seeSexp)) { vision.field_lines.push_back(fieldLine); } diff --git a/rcss3d_agent/src/sexp_vision.cpp b/rcss3d_agent/src/sexp_vision.cpp index 28cc449..2d951ed 100644 --- a/rcss3d_agent/src/sexp_vision.cpp +++ b/rcss3d_agent/src/sexp_vision.cpp @@ -38,6 +38,51 @@ std::optional getBall(sexpresso::Sexp & seeSexp) } return std::nullopt; } +std::vector getFieldFeatures(sexpresso::Sexp & seeSexp) +{ + std::vector fieldFeatures; + for (auto const & arg : seeSexp.arguments()) { + auto const & s = arg.value.sexp; + if (s.at(0).value.str == "V") { + // Corners have form: (V (ffs )) + rcss3d_agent_msgs::msg::FieldFeature fieldFeature; + fieldFeature.type = rcss3d_agent_msgs::msg::FieldFeature::TYPE_CORNER; + fieldFeature.center.r = std::stof(s.at(1).value.sexp.at(1).value.str); + fieldFeature.center.phi = std::stof(s.at(1).value.sexp.at(2).value.str); + fieldFeature.center.theta = std::stof(s.at(1).value.sexp.at(3).value.str); + fieldFeature.orientation_w = std::stof(s.at(1).value.sexp.at(4).value.str); + fieldFeature.orientation_x = std::stof(s.at(1).value.sexp.at(5).value.str); + fieldFeature.orientation_y = std::stof(s.at(1).value.sexp.at(6).value.str); + fieldFeature.orientation_z = std::stof(s.at(1).value.sexp.at(7).value.str); + fieldFeatures.push_back(fieldFeature); + } else if (s.at(0).value.str == "CC") { + // Centre circles have form: (CC (ffs )) + rcss3d_agent_msgs::msg::FieldFeature fieldFeature; + fieldFeature.type = rcss3d_agent_msgs::msg::FieldFeature::TYPE_CENTRE_CIRCLE; + fieldFeature.center.r = std::stof(s.at(1).value.sexp.at(1).value.str); + fieldFeature.center.phi = std::stof(s.at(1).value.sexp.at(2).value.str); + fieldFeature.center.theta = std::stof(s.at(1).value.sexp.at(3).value.str); + fieldFeature.orientation_w = std::stof(s.at(1).value.sexp.at(4).value.str); + fieldFeature.orientation_x = std::stof(s.at(1).value.sexp.at(5).value.str); + fieldFeature.orientation_y = std::stof(s.at(1).value.sexp.at(6).value.str); + fieldFeature.orientation_z = std::stof(s.at(1).value.sexp.at(7).value.str); + fieldFeatures.push_back(fieldFeature); + } else if (s.at(0).value.str == "T") { + // T-Junctions have form: (T (ffs )) + rcss3d_agent_msgs::msg::FieldFeature fieldFeature; + fieldFeature.type = rcss3d_agent_msgs::msg::FieldFeature::TYPE_T_JUNCTION; + fieldFeature.center.r = std::stof(s.at(1).value.sexp.at(1).value.str); + fieldFeature.center.phi = std::stof(s.at(1).value.sexp.at(2).value.str); + fieldFeature.center.theta = std::stof(s.at(1).value.sexp.at(3).value.str); + fieldFeature.orientation_w = std::stof(s.at(1).value.sexp.at(4).value.str); + fieldFeature.orientation_x = std::stof(s.at(1).value.sexp.at(5).value.str); + fieldFeature.orientation_y = std::stof(s.at(1).value.sexp.at(6).value.str); + fieldFeature.orientation_z = std::stof(s.at(1).value.sexp.at(7).value.str); + fieldFeatures.push_back(fieldFeature); + } + } + return fieldFeatures; +} std::vector getFieldLines(sexpresso::Sexp & seeSexp) { std::vector fieldLines; diff --git a/rcss3d_agent/src/sexp_vision.hpp b/rcss3d_agent/src/sexp_vision.hpp index a984f86..e0f4cb6 100644 --- a/rcss3d_agent/src/sexp_vision.hpp +++ b/rcss3d_agent/src/sexp_vision.hpp @@ -18,6 +18,7 @@ #include #include #include "rcss3d_agent_msgs/msg/ball.hpp" +#include "rcss3d_agent_msgs/msg/field_feature.hpp" #include "rcss3d_agent_msgs/msg/field_line.hpp" #include "rcss3d_agent_msgs/msg/flag.hpp" #include "rcss3d_agent_msgs/msg/goalpost.hpp" @@ -31,6 +32,7 @@ namespace sexp_vision { std::optional getBall(sexpresso::Sexp & seeSexp); +std::vector getFieldFeatures(sexpresso::Sexp & seeSexp); std::vector getFieldLines(sexpresso::Sexp & seeSexp); std::vector getFlags(sexpresso::Sexp & seeSexp); std::vector getGoalposts(sexpresso::Sexp & seeSexp); diff --git a/rcss3d_agent/test/test_sexp_parser.cpp b/rcss3d_agent/test/test_sexp_parser.cpp index 7a68346..3bf8959 100644 --- a/rcss3d_agent/test/test_sexp_parser.cpp +++ b/rcss3d_agent/test/test_sexp_parser.cpp @@ -222,11 +222,13 @@ TEST(TestVisions, TestHasVision) { rcss3d_agent::SexpParser parser( "(See " - "(G2R (pol 17.55 -3.33 4.31)) " - "(F1R (pol 18.52 18.94 1.54)) " - "(B (pol 8.51 -0.21 -0.17)) " - "(P (team teamRed) (id 1) (head (pol 16.98 -0.21 3.19))) " - "(L (pol 12.11 -40.77 -2.40) (pol 12.95 -37.76 -2.41)))"); + "(G2R (pol 17.55 -3.33 4.31)) " // Goal post + "(F1R (pol 18.52 18.94 1.54)) " // Flag + "(B (pol 8.51 -0.21 -0.17)) " // Ball + "(P (team teamRed) (id 1) (head (pol 16.98 -0.21 3.19))) " // Player + "(L (pol 12.11 -40.77 -2.40) (pol 12.95 -37.76 -2.41)) " // Line + "(V (ffs 17.47 13.02 -7.83 50.63)))" // Field Feature (Corner) + ); auto visionOptional = parser.getVision(); ASSERT_TRUE(visionOptional.has_value()); @@ -236,6 +238,7 @@ TEST(TestVisions, TestHasVision) EXPECT_EQ(vision.ball.size(), 1u); EXPECT_EQ(vision.players.size(), 1u); EXPECT_EQ(vision.field_lines.size(), 1u); + EXPECT_EQ(vision.field_features.size(), 1u); } TEST(TestGameState, TestGameState) diff --git a/rcss3d_agent/test/test_sexp_vision.cpp b/rcss3d_agent/test/test_sexp_vision.cpp index 3a271d5..076bccb 100644 --- a/rcss3d_agent/test/test_sexp_vision.cpp +++ b/rcss3d_agent/test/test_sexp_vision.cpp @@ -40,7 +40,11 @@ static const char * sexp = "(rlowerarm (pol 0.18 -33.55 -20.16)) " "(llowerarm (pol 0.18 34.29 -19.80))) " "(L (pol 12.11 -40.77 -2.40) (pol 12.95 -37.76 -2.41)) " - "(L (pol 12.97 -37.56 -2.24) (pol 13.32 -32.98 -2.20))"; + "(L (pol 12.97 -37.56 -2.24) (pol 13.32 -32.98 -2.20)) " + "(V (ffs 17.47 13.02 -7.83 50.63)) " + "(CC (ffs 16.06 -5.69 -7.81 3.60)) " + "(T (ffs 12.15 -0.36 -4.95 183.04))"; + static sexpresso::Sexp seeSexp = sexpresso::parse(sexp); TEST(TestBall, TestNoBall) @@ -60,6 +64,41 @@ TEST(TestBall, TestHasBall) EXPECT_NEAR(ball.center.theta, -0.17, 0.01); } +TEST(TestFieldFeatures, TestNoFieldFeatures) +{ + ASSERT_EQ(rcss3d_agent::sexp_vision::getFieldFeatures(seeSexpEmpty).size(), 0u); +} + +TEST(TestFieldFeatures, TestFieldFeatures) +{ + auto fieldFeatures = rcss3d_agent::sexp_vision::getFieldFeatures(seeSexp); + ASSERT_EQ(fieldFeatures.size(), 3u); + + // (V (ffs 17.47 13.02 -7.83 50.63)) + rcss3d_agent_msgs::msg::FieldFeature & ff1 = fieldFeatures.at(0); + EXPECT_EQ(ff1.type, rcss3d_agent_msgs::msg::FieldFeature::TYPE_CORNER); + EXPECT_NEAR(ff1.center.r, 17.47, 0.01); + EXPECT_NEAR(ff1.center.phi, 13.02, 0.01); + EXPECT_NEAR(ff1.center.theta, -7.83, 0.01); + // EXPECT_NEAR(ff1.orientation, 50.63, 0.01); + + // (CC (ffs 16.06 -5.69 -7.81 3.60))) + rcss3d_agent_msgs::msg::FieldFeature & ff2 = fieldFeatures.at(1); + EXPECT_EQ(ff2.type, rcss3d_agent_msgs::msg::FieldFeature::TYPE_CENTRE_CIRCLE); + EXPECT_NEAR(ff2.center.r, 16.06, 0.01); + EXPECT_NEAR(ff2.center.phi, -5.69, 0.01); + EXPECT_NEAR(ff2.center.theta, -7.81, 0.01); + // EXPECT_NEAR(ff2.orientation, 3.60, 0.01); + + // (T (ffs 12.15 -0.36 -4.95 183.04)) + rcss3d_agent_msgs::msg::FieldFeature & ff3 = fieldFeatures.at(2); + EXPECT_EQ(ff3.type, rcss3d_agent_msgs::msg::FieldFeature::TYPE_T_JUNCTION); + EXPECT_NEAR(ff3.center.r, 12.15, 0.01); + EXPECT_NEAR(ff3.center.phi, -0.36, 0.01); + EXPECT_NEAR(ff3.center.theta, -4.95, 0.01); + // EXPECT_NEAR(ff3.orientation, 183.04, 0.01); +} + TEST(TestFieldLines, TestNoFieldLines) { ASSERT_EQ(rcss3d_agent::sexp_vision::getFieldLines(seeSexpEmpty).size(), 0u); diff --git a/rcss3d_agent_msgs/CMakeLists.txt b/rcss3d_agent_msgs/CMakeLists.txt index 349c4de..c11ca25 100644 --- a/rcss3d_agent_msgs/CMakeLists.txt +++ b/rcss3d_agent_msgs/CMakeLists.txt @@ -27,6 +27,7 @@ set(msg_files "msg/Beam.msg" "msg/Effector.msg" "msg/FieldLine.msg" + "msg/FieldFeature.msg" "msg/Flag.msg" "msg/ForceResistance.msg" "msg/GameState.msg" diff --git a/rcss3d_agent_msgs/msg/FieldFeature.msg b/rcss3d_agent_msgs/msg/FieldFeature.msg new file mode 100644 index 0000000..d79b088 --- /dev/null +++ b/rcss3d_agent_msgs/msg/FieldFeature.msg @@ -0,0 +1,10 @@ +uint8 TYPE_CENTRE_CIRCLE=0 +uint8 TYPE_CORNER=1 +uint8 TYPE_T_JUNCTION=2 + +uint8 type +Spherical center # The point at the center of the field feature +float32 orientation_w # W-component of relative orientation quaternion of the field feature +float32 orientation_x # X-component of relative orientation quaternion of the field feature +float32 orientation_y # Y-component of relative orientation quaternion of the field feature +float32 orientation_z # Z-component of relative orientation quaternion of the field feature diff --git a/rcss3d_agent_msgs/msg/Vision.msg b/rcss3d_agent_msgs/msg/Vision.msg index 40db175..fb84b7a 100644 --- a/rcss3d_agent_msgs/msg/Vision.msg +++ b/rcss3d_agent_msgs/msg/Vision.msg @@ -2,7 +2,8 @@ # Details: https://gitlab.com/robocup-sim/SimSpark/-/wikis/Perceptors#vision-perceptors Ball[<=1] ball +FieldFeature[] field_features FieldLine[] field_lines Flag[] flags Goalpost[] goalposts -Player[] players \ No newline at end of file +Player[] players diff --git a/rcss3d_agent_msgs_to_soccer_interfaces/CMakeLists.txt b/rcss3d_agent_msgs_to_soccer_interfaces/CMakeLists.txt index 6922e6b..91ca18b 100644 --- a/rcss3d_agent_msgs_to_soccer_interfaces/CMakeLists.txt +++ b/rcss3d_agent_msgs_to_soccer_interfaces/CMakeLists.txt @@ -9,17 +9,20 @@ endif() find_package(ament_cmake REQUIRED) find_package(rcss3d_agent_msgs REQUIRED) find_package(soccer_vision_3d_msgs REQUIRED) +find_package(Eigen3 REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS rcss3d_agent_msgs - soccer_vision_3d_msgs) + soccer_vision_3d_msgs + Eigen3) # Build add_library(${PROJECT_NAME} SHARED src/conversion.cpp) target_include_directories(${PROJECT_NAME} PUBLIC $ - $) + $ + ${EIGEN3_INCLUDE_DIR}) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) diff --git a/rcss3d_agent_msgs_to_soccer_interfaces/include/rcss3d_agent_msgs_to_soccer_interfaces/conversion.hpp b/rcss3d_agent_msgs_to_soccer_interfaces/include/rcss3d_agent_msgs_to_soccer_interfaces/conversion.hpp index dd3e264..4cdf1aa 100644 --- a/rcss3d_agent_msgs_to_soccer_interfaces/include/rcss3d_agent_msgs_to_soccer_interfaces/conversion.hpp +++ b/rcss3d_agent_msgs_to_soccer_interfaces/include/rcss3d_agent_msgs_to_soccer_interfaces/conversion.hpp @@ -23,8 +23,9 @@ #include "soccer_vision_3d_msgs/msg/marking_array.hpp" #include "soccer_vision_3d_msgs/msg/robot_array.hpp" #include "rcss3d_agent_msgs/msg/ball.hpp" -#include "rcss3d_agent_msgs/msg/goalpost.hpp" +#include "rcss3d_agent_msgs/msg/field_feature.hpp" #include "rcss3d_agent_msgs/msg/field_line.hpp" +#include "rcss3d_agent_msgs/msg/goalpost.hpp" #include "rcss3d_agent_msgs/msg/player.hpp" namespace rcss3d_agent_msgs_to_soccer_interfaces @@ -37,7 +38,8 @@ soccer_vision_3d_msgs::msg::GoalpostArray getGoalpostArray( const std::vector & goalpost); soccer_vision_3d_msgs::msg::MarkingArray getMarkingArray( - const std::vector & fieldLines); + const std::vector & fieldLines, + const std::vector & fieldFeatures); soccer_vision_3d_msgs::msg::RobotArray getRobotArray( const std::vector & players, std::string nameTeamOwn = ""); diff --git a/rcss3d_agent_msgs_to_soccer_interfaces/package.xml b/rcss3d_agent_msgs_to_soccer_interfaces/package.xml index 09bb7e9..a8c8565 100644 --- a/rcss3d_agent_msgs_to_soccer_interfaces/package.xml +++ b/rcss3d_agent_msgs_to_soccer_interfaces/package.xml @@ -9,6 +9,7 @@ ament_cmake + eigen rcss3d_agent_msgs soccer_vision_3d_msgs diff --git a/rcss3d_agent_msgs_to_soccer_interfaces/src/conversion.cpp b/rcss3d_agent_msgs_to_soccer_interfaces/src/conversion.cpp index 0f7e36c..158ff44 100644 --- a/rcss3d_agent_msgs_to_soccer_interfaces/src/conversion.cpp +++ b/rcss3d_agent_msgs_to_soccer_interfaces/src/conversion.cpp @@ -12,8 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include "rcss3d_agent_msgs_to_soccer_interfaces/conversion.hpp" + +#include +#include +#include + +#include + #include "polar_to_point.hpp" #include "soccer_vision_3d_msgs/msg/ball.hpp" #include "deg2rad.hpp" @@ -68,7 +74,8 @@ soccer_vision_3d_msgs::msg::GoalpostArray getGoalpostArray( } soccer_vision_3d_msgs::msg::MarkingArray getMarkingArray( - const std::vector & fieldLines) + const std::vector & fieldLines, + const std::vector & fieldFeatures) { soccer_vision_3d_msgs::msg::MarkingArray markingArray; markingArray.header.frame_id = "CameraTop_frame"; @@ -81,6 +88,69 @@ soccer_vision_3d_msgs::msg::MarkingArray getMarkingArray( markingArray.segments.push_back(markingSegment); } + for (auto & fieldFeature : fieldFeatures) { + switch (fieldFeature.type) { + case rcss3d_agent_msgs::msg::FieldFeature::TYPE_CENTRE_CIRCLE: + { + // Not supported yet + break; + } + case rcss3d_agent_msgs::msg::FieldFeature::TYPE_CORNER: + { + soccer_vision_3d_msgs::msg::MarkingIntersection markingIntersection; + // Not evaluating heading_rays for now + markingIntersection.center = rcss3d_agent_msgs_to_soccer_interfaces::polar_to_point( + fieldFeature.center.r, deg2rad(fieldFeature.center.phi), deg2rad(fieldFeature.center.theta)); + markingIntersection.num_rays = 2; + + // Create quaternion from Euler angles + Eigen::Quaternionf q {fieldFeature.orientation_w, fieldFeature.orientation_x, + fieldFeature.orientation_y, fieldFeature.orientation_z }; + // Convert quaternion to 3x3 rotation matrix + Eigen::Matrix3f m = q.toRotationMatrix(); + Eigen::Vector3f ray1 = m * Eigen::Vector3f{0.2, 0.2, 0.0}; + Eigen::Vector3f ray2 = m * Eigen::Vector3f{0.2, -0.2, 0.0}; + geometry_msgs::msg::Vector3 ray1_msg, ray2_msg; + ray1_msg.set__x(ray1.x()).set__y(ray1.y()).set__z(ray1.z()); + ray2_msg.set__x(ray2.x()).set__y(ray2.y()).set__z(ray2.z()); + markingIntersection.rays.push_back(ray1_msg); + markingIntersection.rays.push_back(ray2_msg); + markingArray.intersections.push_back(markingIntersection); + break; + } + case rcss3d_agent_msgs::msg::FieldFeature::TYPE_T_JUNCTION: + { + soccer_vision_3d_msgs::msg::MarkingIntersection markingIntersection; + // Not evaluating heading_rays for now + markingIntersection.center = rcss3d_agent_msgs_to_soccer_interfaces::polar_to_point( + fieldFeature.center.r, deg2rad(fieldFeature.center.phi), deg2rad(fieldFeature.center.theta)); + markingIntersection.num_rays = 3; + + // Create quaternion from Euler angles + Eigen::Quaternionf q {fieldFeature.orientation_w, fieldFeature.orientation_x, + fieldFeature.orientation_y, fieldFeature.orientation_z }; + // Convert quaternion to 3x3 rotation matrix + Eigen::Matrix3f m = q.toRotationMatrix(); + Eigen::Vector3f ray1 = m * Eigen::Vector3f{0.0, 0.2, 0.0}; + Eigen::Vector3f ray2 = m * Eigen::Vector3f{0.0, -0.2, 0.0}; + Eigen::Vector3f ray3 = m * Eigen::Vector3f{-0.2, 0.0, 0.0}; + geometry_msgs::msg::Vector3 ray1_msg, ray2_msg, ray3_msg; + ray1_msg.set__x(ray1.x()).set__y(ray1.y()).set__z(ray1.z()); + ray2_msg.set__x(ray2.x()).set__y(ray2.y()).set__z(ray2.z()); + ray3_msg.set__x(ray3.x()).set__y(ray3.y()).set__z(ray3.z()); + markingIntersection.rays.push_back(ray1_msg); + markingIntersection.rays.push_back(ray2_msg); + markingIntersection.rays.push_back(ray3_msg); + markingArray.intersections.push_back(markingIntersection); + break; + } + default: + { + assert(false); // This should never happen (if it does, it's a bug in the agent + break; + } + } + } return markingArray; } diff --git a/rcss3d_agent_msgs_to_soccer_interfaces/test/test_conversion.cpp b/rcss3d_agent_msgs_to_soccer_interfaces/test/test_conversion.cpp index 6377a63..4f46dcd 100644 --- a/rcss3d_agent_msgs_to_soccer_interfaces/test/test_conversion.cpp +++ b/rcss3d_agent_msgs_to_soccer_interfaces/test/test_conversion.cpp @@ -84,13 +84,19 @@ TEST(SimToSoccerVision3D, TestGoalpostArrayMultipleGoalposts) EXPECT_EQ(goalpostArray.posts.size(), 2u); } -TEST(SimToSoccerVision3D, TestGoalpostArrayNoFieldLines) +TEST(SimToSoccerVision3D, TestMarkingArrayNoFieldFeatures) { - auto markingArray = rcss3d_agent_msgs_to_soccer_interfaces::getMarkingArray({}); + auto markingArray = rcss3d_agent_msgs_to_soccer_interfaces::getMarkingArray({}, {}); EXPECT_EQ(markingArray.segments.size(), 0u); } -TEST(SimToSoccerVision3D, TestGoalpostArrayOneFieldLine) +TEST(SimToSoccerVision3D, TestMarkingArrayNoFieldLines) +{ + auto markingArray = rcss3d_agent_msgs_to_soccer_interfaces::getMarkingArray({}, {}); + EXPECT_EQ(markingArray.segments.size(), 0u); +} + +TEST(SimToSoccerVision3D, TestMarkingArrayOneFieldLine) { rcss3d_agent_msgs::msg::FieldLine fieldLine; fieldLine.start.r = 1.0; @@ -100,7 +106,7 @@ TEST(SimToSoccerVision3D, TestGoalpostArrayOneFieldLine) fieldLine.end.phi = -45; fieldLine.end.theta = 45; - auto markingArray = rcss3d_agent_msgs_to_soccer_interfaces::getMarkingArray({fieldLine}); + auto markingArray = rcss3d_agent_msgs_to_soccer_interfaces::getMarkingArray({fieldLine}, {}); EXPECT_EQ(markingArray.header.frame_id, "CameraTop_frame"); EXPECT_EQ(markingArray.segments.size(), 1u); EXPECT_NEAR(markingArray.segments[0].start.x, 0.5, 0.01); @@ -111,11 +117,27 @@ TEST(SimToSoccerVision3D, TestGoalpostArrayOneFieldLine) EXPECT_NEAR(markingArray.segments[0].end.z, 0.7071, 0.01); } -TEST(SimToSoccerVision3D, TestGoalpostArrayMultipleFieldLines) +TEST(SimToSoccerVision3D, TestMarkingArrayOneFieldFeature) +{ + rcss3d_agent_msgs::msg::FieldFeature fieldFeature; + fieldFeature.type = rcss3d_agent_msgs::msg::FieldFeature::TYPE_T_JUNCTION; + fieldFeature.center.r = 1.0; + fieldFeature.center.phi = 45; + fieldFeature.center.theta = 45; + + auto markingArray = rcss3d_agent_msgs_to_soccer_interfaces::getMarkingArray({}, {fieldFeature}); + EXPECT_EQ(markingArray.intersections.size(), 1u); + EXPECT_NEAR(markingArray.intersections[0].center.x, 0.5, 0.01); + EXPECT_NEAR(markingArray.intersections[0].center.y, 0.5, 0.01); + EXPECT_NEAR(markingArray.intersections[0].center.z, 0.7071, 0.01); + EXPECT_EQ(markingArray.intersections[0].num_rays, 3); +} + +TEST(SimToSoccerVision3D, TestMarkingArrayMultipleFieldLines) { std::vector fieldLines(2); - auto markingArray = rcss3d_agent_msgs_to_soccer_interfaces::getMarkingArray(fieldLines); + auto markingArray = rcss3d_agent_msgs_to_soccer_interfaces::getMarkingArray({fieldLines}, {}); EXPECT_EQ(markingArray.segments.size(), 2u); }