diff --git a/CMakeLists.txt b/CMakeLists.txt index 16267be..f7393fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,6 +81,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 + $ + $) + + target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) + ament_lint_auto_find_test_dependencies() endif() diff --git a/data/test/action/right_kick.json b/data/test/action/right_kick.json new file mode 100644 index 0000000..51401a6 --- /dev/null +++ b/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 +} diff --git a/test/action/action_test.cpp b/test/action/action_test.cpp new file mode 100644 index 0000000..c8949f5 --- /dev/null +++ b/test/action/action_test.cpp @@ -0,0 +1,48 @@ +// 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 + +#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()); + ASSERT_EQ(action.get_start_delay(), 0.0); + ASSERT_EQ(action.get_stop_delay(), 0.0); + ASSERT_EQ(action.get_next_action(), ""); + +} + +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); + action.set_next_action("next action"); + ASSERT_EQ(action.get_name(), "new test action"); + ASSERT_EQ(action.get_start_delay(), 1.0); + ASSERT_EQ(action.get_stop_delay(), 3.0); + ASSERT_EQ(action.get_next_action(), "next action"); +} diff --git a/test/action/interpolator_test.cpp b/test/action/interpolator_test.cpp new file mode 100644 index 0000000..d5be58f --- /dev/null +++ b/test/action/interpolator_test.cpp @@ -0,0 +1,95 @@ +// 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 +#include +#include +#include +#include +#include + +#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_config("../../src/akushon/data/test"); + + std::vector 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 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); + } +} diff --git a/test/action/pose_test.cpp b/test/action/pose_test.cpp new file mode 100644 index 0000000..b1dc172 --- /dev/null +++ b/test/action/pose_test.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2022 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 + +#include "gtest/gtest.h" + +#include "akushon/action/model/action.hpp" +#include "akushon/action/model/pose.hpp" + +TEST(PoseTest, ClassInitialization) { + akushon::Pose pose("test pose"); + ASSERT_EQ(pose.get_name(), "test pose"); + ASSERT_EQ(action.get_speed(), 0.0); + ASSERT_EQ(action.get_pause(), 0.0); + ASSERT_TRUE(action.get_joints().empty()); +} + +TEST(PoseTest, SetterTest) { + akushon::Pose pose("test pose"); + pose.set_name("new test pose"); + pose.set_speed(3.0); + pose.set_pause(1.0); + ASSERT_EQ(pose.get_name(), "new test pose"); + ASSERT_EQ(pose.get_speed(), 3.0); + ASSERT_EQ(pose.get_pause(), 1.0); +} diff --git a/test/node/akushon_node_test.cpp b/test/node/akushon_node_test.cpp new file mode 100644 index 0000000..e7caf88 --- /dev/null +++ b/test/node/akushon_node_test.cpp @@ -0,0 +1,43 @@ +// 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 + +#include "gtest/gtest.h" + +#include "akushon/action/action.hpp" +#include "akushon/config/config.hpp" +#include "akushon/node/akushon_node.hpp" +#include "rclcpp/rclcpp.hpp" + +TEST(AkushonNodeTest, CompileProcess) { + try { + rclcpp::Node::SharedPtr node = std::make_shared("akushon_node"); + + akushon::AkushonNode akushon_node(node); + + auto action_manager = std::make_shared(); + action_manager->load_config(""); + + akushon_node.run_action_manager(action_manager); + akushon_node.run_config_service(""); + } catch (...) { + } +}