added translation and rotation tests for TransformPose

Author: geurto

ros2_ws/src/rcdt_utilities/include/tests/test_package.hpp

@@ -4,23 +4,28 @@
 
 #include "rcdt_messages/srv/express_pose_in_other_frame.hpp"
 #include "rcdt_messages/srv/transform_pose.hpp"
+#include <chrono>
 #include <gtest/gtest.h>
 #include <iostream>
+#include <memory>
 #include <rcdt_messages/srv/express_pose_in_other_frame.hpp>
 #include <rcdt_messages/srv/transform_pose.hpp>
 #include <rclcpp/executors.hpp>
 #include <rclcpp/executors/multi_threaded_executor.hpp>
+#include <rclcpp/executors/single_threaded_executor.hpp>
+#include <rclcpp/future_return_code.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <thread>
 
 class PackageTester : public rclcpp::Node {
 public:
   PackageTester() : Node("manipulate_pose_tester") {
     express_pose_in_other_frame_client_ =
         this->create_client<rcdt_messages::srv::ExpressPoseInOtherFrame>(
-            "~/express_pose_in_other_frame");
+            "pose_manipulator/express_pose_in_other_frame");
     transform_pose_client_ =
         this->create_client<rcdt_messages::srv::TransformPose>(
-            "~/transform_pose");
+            "pose_manipulator/transform_pose");
   }
 
   bool waitForServices(std::chrono::seconds timeout) {
@@ -43,6 +48,20 @@ class PackageTester : public rclcpp::Node {
     return true;
   }
 
+  rclcpp::Client<rcdt_messages::srv::TransformPose>::FutureAndRequestId
+  sendTransformPoseRequest(
+      std::shared_ptr<rcdt_messages::srv::TransformPose::Request> req) {
+    return transform_pose_client_->async_send_request(req);
+  }
+
+  rclcpp::Client<
+      rcdt_messages::srv::ExpressPoseInOtherFrame>::FutureAndRequestId
+  sendExpressPoseInOtherFrameRequest(
+      std::shared_ptr<rcdt_messages::srv::ExpressPoseInOtherFrame::Request>
+          req) {
+    return express_pose_in_other_frame_client_->async_send_request(req);
+  }
+
 private:
   rclcpp::Client<rcdt_messages::srv::ExpressPoseInOtherFrame>::SharedPtr
       express_pose_in_other_frame_client_;

ros2_ws/src/rcdt_utilities/tests/test_package.cpp

@@ -3,7 +3,17 @@
 // # SPDX-License-Identifier: Apache-2.0
 
 #include "tests/test_package.hpp"
+#include "geometry_msgs/msg/transform.hpp"
+#include "rcdt_messages/srv/transform_pose.hpp"
 #include "rcdt_utilities/manipulate_pose.h"
+#include <chrono>
+#include <future>
+#include <gtest/gtest.h>
+#include <memory>
+#include <rclcpp/duration.hpp>
+#include <rclcpp/future_return_code.hpp>
+
+#define PI 3.14159265
 
 /**
  * @brief Test fixture class for the PackageTester Node.
@@ -27,15 +37,22 @@ class PackageTesterFixture : public testing::Test {
 
 protected:
   void SetUp() override {
+    pose_manipulator_node_ = std::make_shared<PoseManipulator>();
     tester_node_ = std::make_shared<PackageTester>();
+
+    executor_->add_node(pose_manipulator_node_);
     executor_->add_node(tester_node_);
   }
 
   void TearDown() override {
+    executor_->remove_node(pose_manipulator_node_);
     executor_->remove_node(tester_node_);
+
+    pose_manipulator_node_.reset();
     tester_node_.reset();
   }
 
+  std::shared_ptr<PoseManipulator> pose_manipulator_node_;
   std::shared_ptr<PackageTester> tester_node_;
   static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
 };
@@ -44,10 +61,66 @@ std::shared_ptr<rclcpp::executors::MultiThreadedExecutor>
     PackageTesterFixture::executor_ = nullptr;
 
 // TESTS
-TEST_F(PackageTesterFixture, TestTrue) {
+TEST_F(PackageTesterFixture, TestNodeInitialization) {
   ASSERT_EQ(std::string(tester_node_->get_name()), "manipulate_pose_tester");
 }
 
+TEST_F(PackageTesterFixture, TestServiceInitialization) {
+  ASSERT_TRUE(tester_node_->waitForServices(std::chrono::seconds(1)));
+}
+
+TEST_F(PackageTesterFixture, TestTransformPoseTranslation) {
+  geometry_msgs::msg::PoseStamped pose_in;
+  pose_in.pose.position.y = 1;
+  pose_in.pose.position.z = 2;
+
+  geometry_msgs::msg::Transform tf;
+  tf.translation.x = 5;
+  tf.translation.y = 5;
+  tf.translation.z = 5;
+
+  auto req = std::make_shared<rcdt_messages::srv::TransformPose::Request>();
+  req->pose = pose_in;
+  req->transform = tf;
+
+  auto future_and_id = tester_node_->sendTransformPoseRequest(req);
+  auto result_code = executor_->spin_until_future_complete(
+      future_and_id.future, std::chrono::milliseconds(500));
+
+  ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS);
+  auto response = future_and_id.future.get();
+  ASSERT_NE(response, nullptr);
+
+  ASSERT_EQ(response->pose.pose.position.x, 5);
+  ASSERT_EQ(response->pose.pose.position.y, 6);
+  ASSERT_EQ(response->pose.pose.position.z, 7);
+}
+
+TEST_F(PackageTesterFixture, TestTransformPoseRotation) {
+  geometry_msgs::msg::PoseStamped pose_in;
+  pose_in.pose.position.x = 5;
+
+  // apply rotation of +PI/2 around Z-axis, which shifts pose to Y-axis
+  geometry_msgs::msg::Transform tf;
+  tf.rotation.w = 0.7071068;
+  tf.rotation.z = 0.7071068;
+
+  auto req = std::make_shared<rcdt_messages::srv::TransformPose::Request>();
+  req->pose = pose_in;
+  req->transform = tf;
+
+  auto future_and_id = tester_node_->sendTransformPoseRequest(req);
+  auto result_code = executor_->spin_until_future_complete(
+      future_and_id.future, std::chrono::milliseconds(500));
+
+  ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS);
+  auto response = future_and_id.future.get();
+  ASSERT_NE(response, nullptr);
+
+  ASSERT_EQ(response->pose.pose.position.x, 0);
+  ASSERT_NEAR(response->pose.pose.position.y, 5, 1e-4);
+}
+
 // MAIN
 int main(int argc, char **argv) {
   testing::InitGoogleTest(&argc, argv);