diff --git a/.github/workflows/linting.yml b/.github/workflows/linting.yml index 52c58367..b46c2124 100644 --- a/.github/workflows/linting.yml +++ b/.github/workflows/linting.yml @@ -37,7 +37,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - run: python3 -m pip install clang-format + - run: python3 -m pip install clang-format==21.1.2 - run: find ros2_ws/src -iname '*.h' -o -iname '*.hpp' -o -iname '*.cpp' | xargs clang-format --dry-run --Werror --style=file doxygen: diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 2a2075c3..73d581ef 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -47,7 +47,7 @@ repos: language: system pass_filenames: false - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v20.1.5 + rev: v21.1.2 hooks: - id: clang-format types_or: [c++, c, cuda] diff --git a/ros2_ws/src/rcdt_moveit/src/moveit_manager.cpp b/ros2_ws/src/rcdt_moveit/src/moveit_manager.cpp index 82cfe7f4..0e8aca95 100644 --- a/ros2_ws/src/rcdt_moveit/src/moveit_manager.cpp +++ b/ros2_ws/src/rcdt_moveit/src/moveit_manager.cpp @@ -34,7 +34,7 @@ void MoveitManager::initialize_clients() { client_node = std::make_shared("moveit_manager_client"); express_pose_in_other_frame_client = client_node->create_client( - "/express_pose_in_other_frame"); + "/pose_manipulator/express_pose_in_other_frame"); }; void MoveitManager::initialize_services() { diff --git a/ros2_ws/src/rcdt_utilities/CMakeLists.txt b/ros2_ws/src/rcdt_utilities/CMakeLists.txt index ba92c623..b019a62a 100644 --- a/ros2_ws/src/rcdt_utilities/CMakeLists.txt +++ b/ros2_ws/src/rcdt_utilities/CMakeLists.txt @@ -9,15 +9,40 @@ project(rcdt_utilities) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) -# Python executables: -install( - DIRECTORY src_py/ - DESTINATION lib/${PROJECT_NAME} -) +# Other dependencies: +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +find_package(geometry_msgs REQUIRED) +find_package(rcdt_messages REQUIRED) # Python package: ament_python_install_package(${PROJECT_NAME}) +# C++ executables: +include_directories(include) + +add_executable(manipulate_pose + src/manipulate_pose.cpp + src/node.cpp +) +ament_target_dependencies(manipulate_pose + geometry_msgs + rcdt_messages + rclcpp + tf2 + tf2_geometry_msgs + tf2_ros +) + +install( + TARGETS manipulate_pose + DESTINATION lib/${PROJECT_NAME} +) + # Shared folders: install( DIRECTORY launch rviz urdf @@ -27,7 +52,26 @@ install( # Default test: if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(manipulate_pose_test + tests/test_package.cpp + src/manipulate_pose.cpp + ) + ament_target_dependencies(manipulate_pose_test + geometry_msgs + rcdt_messages + rclcpp + tf2 + tf2_geometry_msgs + tf2_ros + ) + install( + TARGETS manipulate_pose_test + DESTINATION lib/${PROJECT_NAME} + ) endif() ament_package() diff --git a/ros2_ws/src/rcdt_utilities/README.md b/ros2_ws/src/rcdt_utilities/README.md deleted file mode 100644 index 8a70dce2..00000000 --- a/ros2_ws/src/rcdt_utilities/README.md +++ /dev/null @@ -1,45 +0,0 @@ - - -# RCDT Utilities - -This repository contains ROS2-based utility functions and nodes that can be used for ROS2 software development for any robot. This avoids duplication of often repeated code between different robotic specific repositories. - -## Usage - -This repository contains multiple services: -- /manipulate_pose/express_pose_in_other_frame -- /manipulate_pose/transform_pose -- /manipulate_pose/transform_pose_relative -- /moveit_controller/move_hand_to_pose -- /moveit_controller/move_to_configuration -- /moveit_controller/add_object -- /moveit_controller/clear_objects -- /is_between - -You can run these services by running the following in a sourced terminal: - -```bash -ros2 run rcdt_detection _node.py -``` - -## Moveit Controller Node - -This node can be used to use code-wise Moveit control. For Franka: - -`ros2 launch rcdt_franka franka.launch.py moveit:=node` - -By selecting option *node* for the moveit flag, the moveit_controller node will be started. One can move the robot using an Action call, for example: - -`ros2 action send_goal --feedback /moveit_controller rcdt_utilities_msgs/action/Moveit "{goal_pose: {header: {frame_id: fr3_link0}, pose: {position: {x: 0.28, y: 0.2, z: 0.5}, orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0}}}}"` - -## License - -This project is licensed under the Apache License Version 2.0 - see [LICENSE](LICENSE) for details. - -## Contributing - -Please read CODE_OF_CONDUCT, CONTRIBUTING, and PROJECT GOVERNANCE located in the overarching [RCDT robotics](https://github.com/alliander-opensource/rcdt_robotics) project repository for details on the process for submitting pull requests to us. diff --git a/ros2_ws/src/rcdt_utilities/include/rcdt_utilities/manipulate_pose.hpp b/ros2_ws/src/rcdt_utilities/include/rcdt_utilities/manipulate_pose.hpp new file mode 100644 index 00000000..f897dba7 --- /dev/null +++ b/ros2_ws/src/rcdt_utilities/include/rcdt_utilities/manipulate_pose.hpp @@ -0,0 +1,71 @@ +// # SPDX-FileCopyrightText: Alliander N. V. +// +// # SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform.hpp" +#include "rcdt_messages/srv/express_pose_in_other_frame.hpp" +#include "rcdt_messages/srv/transform_pose.hpp" +#include "tf2_ros/buffer.hpp" + +/** + * Class to manipulate poses using TF2. + */ +class PoseManipulator : public rclcpp::Node { + public: + PoseManipulator(); + + private: + std::shared_ptr tf_listener_{ + nullptr}; /**< TF2 Transform Listener */ + std::unique_ptr tf_buffer_; /**< TF2 Transform Buffer */ + + // Services + rclcpp::Service::SharedPtr + express_pose_in_other_frame_service_; /**< Service for + ExpressPoseInOtherFrame */ + /** @brief Callback for the ExpressPoseInOtherFrame service + * @param req The request message + * @param resp The response message + */ + void expressPoseInOtherFrame( + const std::shared_ptr< + rcdt_messages::srv::ExpressPoseInOtherFrame::Request> + req, + std::shared_ptr + resp); + + rclcpp::Service::SharedPtr + transform_pose_service_; /**< Service for TransformPose */ + /** @brief Callback for the TransformPose service + * @param req The request message + * @param resp The response message + */ + void transformPose( + const std::shared_ptr req, + std::shared_ptr resp); + + rclcpp::Service::SharedPtr + transform_pose_relative_service_; /**< Service for TransformPoseRelative + */ + /** @brief Callback for the TransformPoseRelative service + * @param req The request message + * @param resp The response message + */ + void transformPoseRelative( + const std::shared_ptr req, + std::shared_ptr resp); + + /** @brief Transforms a pose using a given transform + * @param pose_in The input pose + * @param tf The transform to apply + * @return The transformed pose + */ + geometry_msgs::msg::PoseStamped doTransform( + const geometry_msgs::msg::PoseStamped pose_in, + const geometry_msgs::msg::Transform tf); +}; diff --git a/ros2_ws/src/rcdt_utilities/include/tests/test_package.hpp b/ros2_ws/src/rcdt_utilities/include/tests/test_package.hpp new file mode 100644 index 00000000..26592ed0 --- /dev/null +++ b/ros2_ws/src/rcdt_utilities/include/tests/test_package.hpp @@ -0,0 +1,91 @@ +// # SPDX-FileCopyrightText: Alliander N. V. +// +// # SPDX-License-Identifier: Apache-2.0 + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcdt_messages/srv/express_pose_in_other_frame.hpp" +#include "rcdt_messages/srv/transform_pose.hpp" + +/** + * Class to test the package. + */ +class PackageTester : public rclcpp::Node { + public: + PackageTester() : Node("manipulate_pose_tester") { + express_pose_in_other_frame_client_ = + this->create_client( + "pose_manipulator/express_pose_in_other_frame"); + transform_pose_client_ = + this->create_client( + "pose_manipulator/transform_pose"); + } + + /** + * @brief Waits for the services to be available. + * @param timeout The duration to wait for the services. + * @return True if all services are available, false otherwise. + */ + bool waitForServices(std::chrono::seconds timeout) { + bool express_pose_in_other_frame_available = + express_pose_in_other_frame_client_->wait_for_service(timeout); + if (!express_pose_in_other_frame_available) { + RCLCPP_ERROR(this->get_logger(), + "Service 'express_pose_in_other_frame' " + "not available within timeout."); + return false; + } + + bool transform_pose_available = + transform_pose_client_->wait_for_service(timeout); + if (!transform_pose_available) { + RCLCPP_ERROR(this->get_logger(), + "Service 'transform_pose' not available within timeout."); + return false; + } + + return true; + } + + /** + * @brief Sends a TransformPose service request. + * @param req The TransformPose request message. + * @return A future and request ID for the service call. + */ + rclcpp::Client::FutureAndRequestId + sendTransformPoseRequest( + std::shared_ptr req) { + return transform_pose_client_->async_send_request(req); + } + + /** + * @brief Sends an ExpressPoseInOtherFrame service request. + * @param req The ExpressPoseInOtherFrame request message. + * @return A future and request ID for the service call. + */ + rclcpp::Client< + rcdt_messages::srv::ExpressPoseInOtherFrame>::FutureAndRequestId + sendExpressPoseInOtherFrameRequest( + std::shared_ptr + req) { + return express_pose_in_other_frame_client_->async_send_request(req); + } + + private: + rclcpp::Client::SharedPtr + express_pose_in_other_frame_client_; /**< Client to express pose in + another coordinate frame */ + rclcpp::Client::SharedPtr + transform_pose_client_; /**< Client to transform a pose using a + specified transform */ +}; diff --git a/ros2_ws/src/rcdt_utilities/launch/utils.launch.py b/ros2_ws/src/rcdt_utilities/launch/utils.launch.py index 129f8eca..44d66ab1 100644 --- a/ros2_ws/src/rcdt_utilities/launch/utils.launch.py +++ b/ros2_ws/src/rcdt_utilities/launch/utils.launch.py @@ -17,7 +17,7 @@ def launch_setup(context: LaunchContext) -> list: Returns: list: A list of actions to be executed in the launch description. """ - manipulate_pose = Node(package="rcdt_utilities", executable="manipulate_pose.py") + manipulate_pose = Node(package="rcdt_utilities", executable="manipulate_pose") return [ Register.on_start(manipulate_pose, context), diff --git a/ros2_ws/src/rcdt_utilities/rcdt_utilities/test_utils.py b/ros2_ws/src/rcdt_utilities/rcdt_utilities/test_utils.py index ca579356..70fb93a6 100644 --- a/ros2_ws/src/rcdt_utilities/rcdt_utilities/test_utils.py +++ b/ros2_ws/src/rcdt_utilities/rcdt_utilities/test_utils.py @@ -230,7 +230,7 @@ def callback_function(msg: String) -> None: def call_express_pose_in_other_frame( node: Node, pose: PoseStamped, target_frame: str, timeout: int ) -> ExpressPoseInOtherFrame.Response: - """Calls the /express_pose_in_other_frame service. + """Calls the /pose_manipulator/express_pose_in_other_frame service. Args: node (Node): An active rclpy Node. @@ -247,7 +247,7 @@ def call_express_pose_in_other_frame( client = create_ready_service_client( node, ExpressPoseInOtherFrame, - "/express_pose_in_other_frame", + "/pose_manipulator/express_pose_in_other_frame", timeout_sec=timeout, ) diff --git a/ros2_ws/src/rcdt_utilities/src/manipulate_pose.cpp b/ros2_ws/src/rcdt_utilities/src/manipulate_pose.cpp new file mode 100644 index 00000000..a5cc8aa9 --- /dev/null +++ b/ros2_ws/src/rcdt_utilities/src/manipulate_pose.cpp @@ -0,0 +1,114 @@ +// # SPDX-FileCopyrightText: Alliander N. V. +// +// # SPDX-License-Identifier: Apache-2.0 + +#include "rcdt_utilities/manipulate_pose.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rcdt_messages/srv/express_pose_in_other_frame.hpp" +#include "rcdt_messages/srv/transform_pose.hpp" + +PoseManipulator::PoseManipulator() : rclcpp::Node("pose_manipulator") { + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + express_pose_in_other_frame_service_ = this->create_service< + rcdt_messages::srv::ExpressPoseInOtherFrame>( + "~/express_pose_in_other_frame", + [this]( + std::shared_ptr + req, + std::shared_ptr + resp) { this->expressPoseInOtherFrame(req, resp); }); + + transform_pose_service_ = + this->create_service( + "~/transform_pose", + [this]( + std::shared_ptr req, + std::shared_ptr + resp) { this->transformPose(req, resp); }); + + transform_pose_relative_service_ = + this->create_service( + "~/transform_pose_relative", + [this]( + std::shared_ptr req, + std::shared_ptr + resp) { this->transformPoseRelative(req, resp); }); +} + +void PoseManipulator::expressPoseInOtherFrame( + const std::shared_ptr + req, + std::shared_ptr + resp) { + std::string source_frame = req->pose.header.frame_id; + std::string target_frame = req->target_frame; + + geometry_msgs::msg::TransformStamped tf; + try { + tf = tf_buffer_->lookupTransform(target_frame, source_frame, + rclcpp::Time(0)); + } catch (const tf2::TransformException& e) { + RCLCPP_WARN(this->get_logger(), + "Could not transform from frame %s to frame %s: %s", + source_frame.c_str(), target_frame.c_str(), e.what()); + resp->success = false; + return; + } + + if (req->pose.header.frame_id != tf.child_frame_id) { + RCLCPP_ERROR(this->get_logger(), + "Pose frame ID (%s) and Transform child frame ID (%s) are not " + "the same.", + req->pose.header.frame_id.c_str(), tf.child_frame_id.c_str()); + } + + try { + tf2::doTransform(req->pose, resp->pose, tf); + } catch (const tf2::TransformException& e) { + RCLCPP_ERROR(this->get_logger(), + "Could not transform pose to frame %s: %s.", + target_frame.c_str(), e.what()); + } + resp->success = true; +} + +void PoseManipulator::transformPose( + const std::shared_ptr req, + std::shared_ptr resp) { + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped.transform = req->transform; + tf2::doTransform(req->pose, resp->pose, tf_stamped); + + resp->success = true; +} + +void PoseManipulator::transformPoseRelative( + const std::shared_ptr req, + std::shared_ptr resp) { + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped.transform = req->transform; + tf2::doTransform(req->pose, resp->pose, tf_stamped); + + resp->pose.pose.position.x += req->pose.pose.position.x; + resp->pose.pose.position.y += req->pose.pose.position.y; + resp->pose.pose.position.z += req->pose.pose.position.z; + + resp->success = true; +} + +geometry_msgs::msg::PoseStamped PoseManipulator::doTransform( + const geometry_msgs::msg::PoseStamped pose_in, + const geometry_msgs::msg::Transform tf) { + geometry_msgs::msg::PoseStamped pose_out; + return pose_out; +} diff --git a/ros2_ws/src/rcdt_utilities/src/node.cpp b/ros2_ws/src/rcdt_utilities/src/node.cpp new file mode 100644 index 00000000..fce76c47 --- /dev/null +++ b/ros2_ws/src/rcdt_utilities/src/node.cpp @@ -0,0 +1,20 @@ +// # SPDX-FileCopyrightText: Alliander N. V. +// +// # SPDX-License-Identifier: Apache-2.0 + +#include + +#include "rcdt_utilities/manipulate_pose.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/ros2_ws/src/rcdt_utilities/src_py/manipulate_pose.py b/ros2_ws/src/rcdt_utilities/src_py/manipulate_pose.py deleted file mode 100755 index 3071605d..00000000 --- a/ros2_ws/src/rcdt_utilities/src_py/manipulate_pose.py +++ /dev/null @@ -1,162 +0,0 @@ -#!/usr/bin/env python3 - -# SPDX-FileCopyrightText: Alliander N. V. -# -# SPDX-License-Identifier: Apache-2.0 - -from copy import copy - -import rclpy -from geometry_msgs.msg import Point, PoseStamped, Transform, TransformStamped -from rcdt_messages.srv import ExpressPoseInOtherFrame, TransformPose -from rcdt_utilities.launch_utils import spin_node -from rclpy import logging, time -from rclpy.node import Node -from tf2_geometry_msgs import do_transform_pose_stamped -from tf2_ros import TransformException # ty: ignore[unresolved-import] -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener - -ros_logger = logging.get_logger(__name__) - - -class ManipulatePose(Node): - """Node to manipulate poses in different frames and apply transformations. - - Atributes: - tf_buffer (Buffer): Buffer to store transformations. - tf_listener (TransformListener): Listener to receive transformations. - - """ - - def __init__(self): - """Initialize the ManipulatePose node.""" - super().__init__("manipulate_pose") - - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) - self.create_service( - ExpressPoseInOtherFrame, - "/express_pose_in_other_frame", - self.express_pose_in_other_frame, - ) - self.create_service( - TransformPose, - "/transform_pose", - self.transform_pose_relative, - ) - - def express_pose_in_other_frame( - self, - request: ExpressPoseInOtherFrame.Request, - response: ExpressPoseInOtherFrame.Response, - ) -> ExpressPoseInOtherFrame.Response: - """Express a pose in another frame. - - Args: - request (ExpressPoseInOtherFrame.Request): The request containing the pose and target frame. - response (ExpressPoseInOtherFrame.Response): The response to be filled with the transformed pose. - - Returns: - ExpressPoseInOtherFrame.Response: The response containing the transformed pose and success status. - """ - source_frame = request.pose.header.frame_id - target_frame = request.target_frame - - try: - transform = self.tf_buffer.lookup_transform( - target_frame, source_frame, time.Time() - ) - except TransformException as ex: - self.get_logger().error( - f"Could not transform {target_frame} to {source_frame}: {ex}" - ) - response.success = False - return response - response.pose = do_transform_pose_stamped(request.pose, transform) - response.success = True - return response - - @staticmethod - def transform_pose( - request: TransformPose.Request, response: TransformPose.Response - ) -> TransformPose.Response: - """Transform a pose using a given transformation. - - Args: - request (TransformPose.Request): The request containing the pose and transformation. - response (TransformPose.Response): The response to be filled with the transformed pose. - - Returns: - TransformPose.Response: The response containing the transformed pose and success status. - """ - response.pose = apply_transform(request.pose, request.transform) - response.success = True - return response - - @staticmethod - def transform_pose_relative( - request: TransformPose.Request, response: TransformPose.Response - ) -> TransformPose.Response: - """Transform a pose relative to its current position using a given transformation. - - Args: - request (TransformPose.Request): The request containing the pose and transformation. - response (TransformPose.Response): The response to be filled with the transformed pose. - - Returns: - TransformPose.Response: The response containing the transformed pose and success status. - """ - response.pose = apply_transform_relative(request.pose, request.transform) - response.success = True - return response - - -def apply_transform(pose: PoseStamped, transform: Transform) -> PoseStamped: - """Apply a transformation to a PoseStamped object. - - Args: - pose (PoseStamped): The pose to be transformed. - transform (Transform): The transformation to apply. - - Returns: - PoseStamped: The transformed pose. - """ - transform_stamped = TransformStamped() - transform_stamped.transform = transform - transform_stamped.header = pose.header - return do_transform_pose_stamped(pose, transform_stamped) - - -def apply_transform_relative(pose: PoseStamped, transform: Transform) -> PoseStamped: - """Apply a transformation to a PoseStamped object relative to its current position. - - Args: - pose (PoseStamped): The pose to be transformed. - transform (Transform): The transformation to apply. - - Returns: - PoseStamped: The transformed pose with the original position added back. - """ - start_position = copy(pose.pose.position) - pose.pose.position = Point() - pose = apply_transform(pose, transform) - pose.pose.position.x += start_position.x - pose.pose.position.y += start_position.y - pose.pose.position.z += start_position.z - return pose - - -def main(args: list | None = None) -> None: - """Main function to initialize the ROS 2 node and start the executor. - - Args: - args (list | None): Command line arguments, defaults to None. - """ - rclpy.init(args=args) - node = ManipulatePose() - spin_node(node) - - -if __name__ == "__main__": - main() diff --git a/ros2_ws/src/rcdt_utilities/tests/test_package.cpp b/ros2_ws/src/rcdt_utilities/tests/test_package.cpp new file mode 100644 index 00000000..63f9e000 --- /dev/null +++ b/ros2_ws/src/rcdt_utilities/tests/test_package.cpp @@ -0,0 +1,151 @@ +// # SPDX-FileCopyrightText: Alliander N. V. +// +// # SPDX-License-Identifier: Apache-2.0 + +#include "tests/test_package.hpp" + +#include + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/transform.hpp" +#include "rcdt_messages/srv/transform_pose.hpp" +#include "rcdt_utilities/manipulate_pose.hpp" + +#define PI 3.14159265 + +/** + * @brief Test fixture class for the PackageTester Node. + * * This fixture ensures a clean setup and teardown for each test, + * making sure the PackageTester has access to all the right data. + */ +class PackageTesterFixture : public testing::Test { + public: + /** + * @brief Sets up the test suite. + */ + static void SetUpTestSuite() { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + executor_ = std::make_shared(); + } + + /** + * @brief Tears down the test suite. + */ + static void TearDownTestSuite() { + executor_.reset(); + rclcpp::shutdown(); + } + + protected: + /** + * @brief Sets up each test. + */ + void SetUp() override { + pose_manipulator_node_ = std::make_shared(); + tester_node_ = std::make_shared(); + + executor_->add_node(pose_manipulator_node_); + executor_->add_node(tester_node_); + } + + /** + * @brief Tears down each test. + */ + void TearDown() override { + executor_->remove_node(pose_manipulator_node_); + executor_->remove_node(tester_node_); + + pose_manipulator_node_.reset(); + tester_node_.reset(); + } + + std::shared_ptr + pose_manipulator_node_; /**< Node to manipulate poses */ + std::shared_ptr tester_node_; /**< Node to test the package */ + static std::shared_ptr + executor_; /**< Executor to spin nodes */ +}; + +std::shared_ptr + PackageTesterFixture::executor_ = nullptr; + +// TESTS +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(); + 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(); + 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); + + int _argc = 0; + const char** _argv = nullptr; + + rclcpp::init(_argc, _argv); + + return RUN_ALL_TESTS(); +}