Implement a test for ContactList json parser

ami-iit · May 27, 2021 · 1e8c53a · 1e8c53a
1 parent d464e68
commit 1e8c53a
Showing 1 changed file with 24 additions and 0 deletions.
diff --git a/src/Contacts/tests/Contacts/ContactListTest.cpp b/src/Contacts/tests/Contacts/ContactListTest.cpp
@@ -12,6 +12,7 @@
 #include <manif/manif.h>
 
 #include <BipedalLocomotion/Contacts/ContactList.h>
+#include <BipedalLocomotion/Contacts/ContactListJsonParser.h>
 #include <BipedalLocomotion/Contacts/Contact.h>
 
 using namespace BipedalLocomotion::Contacts;
@@ -116,3 +117,26 @@ TEST_CASE("ContactList")
         REQUIRE(ok);
     }
 }
+
+TEST_CASE("ContactList JSON parser")
+{
+    ContactListMap contactListMap;
+    contactListMap["left"].setDefaultName("left_foot");
+    REQUIRE(contactListMap["left"].addContact(manif::SE3d::Random(), 0.0, 1.0));
+    REQUIRE(contactListMap["left"].addContact(manif::SE3d::Random(), 2.0, 5.0));
+
+    contactListMap["right"].setDefaultName("right_foot");
+    REQUIRE(contactListMap["right"].addContact(manif::SE3d::Random(), 0.0, 3.0));
+
+    REQUIRE(contactListMapToJson(contactListMap, "contact_list.json"));
+
+    ContactListMap newContactListMap = contactListMapFromJson("contact_list.json");
+
+    REQUIRE(newContactListMap.size() == contactListMap.size());
+
+    for (const auto& [key, contacts] : newContactListMap)
+    {
+        for (int i = 0; i < contacts.size(); i++)
+            REQUIRE(contactsAreEqual(contactListMap[key][i], contacts[i]));
+    }
+}