start implementing field feature parsing

rcss3d_agent/src/sexp_parser.cpp

@@ -154,6 +154,9 @@ std::optional<rcss3d_agent_msgs::msg::Vision> 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);
     }

rcss3d_agent/src/sexp_vision.cpp

@@ -38,6 +38,51 @@ std::optional<rcss3d_agent_msgs::msg::Ball> getBall(sexpresso::Sexp & seeSexp)
   }
   return std::nullopt;
 }
+std::vector<rcss3d_agent_msgs::msg::FieldFeature> getFieldFeatures(sexpresso::Sexp & seeSexp)
+{
+  std::vector<rcss3d_agent_msgs::msg::FieldFeature> 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 <distance> <angle1> <angle2> <orientation_w> <orientation_x> <orientation_y> <orientation_z>))
+      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 <distance> <angle1> <angle2> <orientation_w> <orientation_x> <orientation_y> <orientation_z>))
+      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 <distance> <angle1> <angle2> <orientation_w> <orientation_x> <orientation_y> <orientation_z>))
+      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<rcss3d_agent_msgs::msg::FieldLine> getFieldLines(sexpresso::Sexp & seeSexp)
 {
   std::vector<rcss3d_agent_msgs::msg::FieldLine> fieldLines;

rcss3d_agent/src/sexp_vision.hpp

@@ -18,6 +18,7 @@
 #include <optional>
 #include <vector>
 #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<rcss3d_agent_msgs::msg::Ball> getBall(sexpresso::Sexp & seeSexp);
+std::vector<rcss3d_agent_msgs::msg::FieldFeature> getFieldFeatures(sexpresso::Sexp & seeSexp);
 std::vector<rcss3d_agent_msgs::msg::FieldLine> getFieldLines(sexpresso::Sexp & seeSexp);
 std::vector<rcss3d_agent_msgs::msg::Flag> getFlags(sexpresso::Sexp & seeSexp);
 std::vector<rcss3d_agent_msgs::msg::Goalpost> getGoalposts(sexpresso::Sexp & seeSexp);

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)

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);

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"

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

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
+Player[] players

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
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
+  $<INSTALL_INTERFACE:include>
+  ${EIGEN3_INCLUDE_DIR})
 
 ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)

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<rcss3d_agent_msgs::msg::Goalpost> & goalpost);
 
 soccer_vision_3d_msgs::msg::MarkingArray getMarkingArray(
-  const std::vector<rcss3d_agent_msgs::msg::FieldLine> & fieldLines);
+  const std::vector<rcss3d_agent_msgs::msg::FieldLine> & fieldLines,
+  const std::vector<rcss3d_agent_msgs::msg::FieldFeature> & fieldFeatures);
 
 soccer_vision_3d_msgs::msg::RobotArray getRobotArray(
   const std::vector<rcss3d_agent_msgs::msg::Player> & players, std::string nameTeamOwn = "");

rcss3d_agent_msgs_to_soccer_interfaces/package.xml

@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>eigen</depend>
   <depend>rcss3d_agent_msgs</depend>
   <depend>soccer_vision_3d_msgs</depend>
 

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 <vector>
 #include "rcss3d_agent_msgs_to_soccer_interfaces/conversion.hpp"
+
+#include <cassert>
+#include <vector>
+#include <iostream>
+
+#include <Eigen/Dense>
+
 #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<rcss3d_agent_msgs::msg::FieldLine> & fieldLines)
+  const std::vector<rcss3d_agent_msgs::msg::FieldLine> & fieldLines,
+  const std::vector<rcss3d_agent_msgs::msg::FieldFeature> & 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;
 }
 

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<rcss3d_agent_msgs::msg::FieldLine> 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);
 }