[Sprint 11/PI - 154] Akushon Unit Test

CMakeLists.txt

@@ -79,6 +79,20 @@ install(TARGETS
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(${PROJECT_NAME}_tests
+    "test/action/action_test.cpp" 
+    "test/action/interpolator_test.cpp" 
+    "test/node/akushon_node_test.cpp"
+    )
+
+  target_include_directories(${PROJECT_NAME}_tests PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+
+  target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME})
+
   ament_lint_auto_find_test_dependencies()
 endif()
 

data/test/action/right_kick.json

@@ -0,0 +1,89 @@
+{
+  "name": "Right Kick",
+  "next": "",
+  "poses": [
+    {
+      "joints": {
+        "left_ankle_pitch": 0,
+        "left_ankle_roll": 0,
+        "left_elbow": 0,
+        "left_hip_pitch": 0,
+        "left_hip_roll": 0,
+        "left_hip_yaw": 0,
+        "left_knee": 0,
+        "left_shoulder_pitch": 0,
+        "left_shoulder_roll": 1,
+        "neck_pitch": 2,
+        "neck_yaw": 1,
+        "right_ankle_pitch": 1,
+        "right_ankle_roll": 0,
+        "right_elbow": 0,
+        "right_hip_pitch": 0,
+        "right_hip_roll": 0,
+        "right_hip_yaw": 0,
+        "right_knee": 0,
+        "right_shoulder_pitch": 0,
+        "right_shoulder_roll": 1
+      },
+      "name": "r_walkready",
+      "pause": 0,
+      "speed": 1
+    },
+    {
+      "joints": {
+        "left_ankle_pitch": 50,
+        "left_ankle_roll": 50,
+        "left_elbow": 50,
+        "left_hip_pitch": 50,
+        "left_hip_roll": 50,
+        "left_hip_yaw": 50,
+        "left_knee": 50,
+        "left_shoulder_pitch": 23,
+        "left_shoulder_roll": 51,
+        "neck_pitch": 52,
+        "neck_yaw": 51,
+        "right_ankle_pitch": 51,
+        "right_ankle_roll": 50,
+        "right_elbow": 50,
+        "right_hip_pitch": 50,
+        "right_hip_roll": 50,
+        "right_hip_yaw": 50,
+        "right_knee": 50,
+        "right_shoulder_pitch": 53,
+        "right_shoulder_roll": 51
+      },
+      "name": "r_feet",
+      "pause": 1,
+      "speed": 0.1
+    },
+    {
+      "joints": {
+        "left_ankle_pitch": 0,
+        "left_ankle_roll": 0,
+        "left_elbow": 0,
+        "left_hip_pitch": 0,
+        "left_hip_roll": 0,
+        "left_hip_yaw": 0,
+        "left_knee": 0,
+        "left_shoulder_pitch": 0,
+        "left_shoulder_roll": 1,
+        "neck_pitch": 2,
+        "neck_yaw": 1,
+        "right_ankle_pitch": 1,
+        "right_ankle_roll": 0,
+        "right_elbow": 0,
+        "right_hip_pitch": 0,
+        "right_hip_roll": 0,
+        "right_hip_yaw": 0,
+        "right_knee": 0,
+        "right_shoulder_pitch": 0,
+        "right_shoulder_roll": 1
+      },
+      "name": "r_kick",
+      "pause": 0.5,
+      "speed": 0.3
+    }
+  ],
+  "start_delay": 0,
+  "stop_delay": 0
+}

src/akushon/action/process/joint_process.cpp

@@ -20,6 +20,7 @@
 
 #include <string>
 #include <vector>
+#include <cmath>
 
 #include "akushon/action/process/joint_process.hpp"
 

test/action/action_test.cpp

@@ -0,0 +1,42 @@
+// Copyright (c) 2021 Ichiro ITS
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#include <memory>
+
+#include "gtest/gtest.h"
+
+#include "akushon/action/model/action.hpp"
+#include "akushon/action/model/pose.hpp"
+
+TEST(ActionTest, ClassInitialization) {
+  akushon::Action action("test action");
+  ASSERT_EQ(action.get_name(), "test action");
+  ASSERT_TRUE(action.get_poses().empty());
+}
+
+TEST(ActionTest, SetterTest) {
+  akushon::Action action("test action");
+  action.set_name("new test action");
+  action.set_start_delay(1.0);
+  action.set_stop_delay(3.0);
+  ASSERT_EQ(action.get_name(), "new test action");
+  ASSERT_EQ(action.get_start_delay(), 1.0);
+  ASSERT_EQ(action.get_stop_delay(), 3.0);
+}

test/action/interpolator_test.cpp

@@ -0,0 +1,97 @@
+// Copyright (c) 2021 Ichiro ITS
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#include <fstream>
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "gtest/gtest.h"
+
+#include "akushon/action/model/action_name.hpp"
+#include "akushon/action/model/action.hpp"
+#include "akushon/action/node/action_manager.hpp"
+#include "akushon/action/process/interpolator.hpp"
+#include "nlohmann/json.hpp"
+#include "tachimawari/joint/joint.hpp"
+
+TEST(InterpolatorTest, JointTest) {
+  akushon::ActionManager action_manager;
+  float max_error = 0.5;
+
+  action_manager.load_data("../../src/akushon/data/test");
+
+  std::vector<akushon::Pose> poses = action_manager.get_action(akushon::Action::RIGHT_KICK).get_poses();
+
+  action_manager.start(akushon::Action::RIGHT_KICK, poses[0]);
+
+  float expected_positions[3][20] =
+    {
+      {
+        0, 0, 1, 1, 0,
+        0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0,
+        0, 1, 0, 1, 2
+      },
+      {
+        53, 23, 51, 51, 50,
+        50, 50, 50, 50, 50,
+        50, 50, 50, 50, 50,
+        50, 51, 50, 51, 52
+      },
+      {
+        0, 0, 1, 1, 0,
+        0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0,
+        0, 1, 0, 1, 2
+      }
+    };
+
+  std::vector<tachimawari::joint::Joint> action_manager_joints = action_manager.get_joints();
+
+  for (int i = 0; i < 20; i++){
+    EXPECT_NEAR(expected_positions[0][i], action_manager_joints[i].get_position(), max_error);
+  }
+
+  for (int i = 0; i < 1100; i++)
+  {
+    action_manager.process(i);
+  }
+
+  action_manager_joints = action_manager.get_joints();
+
+  for (int i = 0; i < 20; i++)
+  {
+    EXPECT_NEAR(expected_positions[1][i], action_manager_joints[i].get_position(), max_error);
+  }
+
+  for (int i = 1100; i < 2000; i++){
+    action_manager.process(i);
+  }
+
+  action_manager_joints = action_manager.get_joints();
+
+  for (int i = 0; i < 20; i++)
+  {
+    EXPECT_NEAR(expected_positions[2][i], action_manager_joints[i].get_position(), max_error);
+  }
+}

test/node/akushon_node_test.cpp

@@ -0,0 +1,41 @@
+// Copyright (c) 2021 Ichiro ITS
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#include <memory>
+
+#include "gtest/gtest.h"
+
+#include "akushon/action/node/action_manager.hpp"
+#include "akushon/node/akushon_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+TEST(AkushonNodeTest, CompileProcess) {
+  try {
+    rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("akushon_node");
+
+    akushon::AkushonNode akushon_node(node);
+
+    auto action_manager = std::make_shared<akushon::ActionManager>();
+    action_manager->load_data("");
+
+    akushon_node.set_action_manager(action_manager);
+  } catch (...) {
+  }
+}