287 migrate manipulate pose

.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:

.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]

ros2_ws/src/rcdt_moveit/src/moveit_manager.cpp

@@ -34,7 +34,7 @@ void MoveitManager::initialize_clients() {
   client_node = std::make_shared<rclcpp::Node>("moveit_manager_client");
   express_pose_in_other_frame_client =
       client_node->create_client<ExpressPoseInOtherFrame>(
-          "/express_pose_in_other_frame");
+          "/pose_manipulator/express_pose_in_other_frame");
 };
 
 void MoveitManager::initialize_services() {

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()

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 <memory>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_ros/transform_listener.hpp>
+
+#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<tf2_ros::TransformListener> tf_listener_{
+      nullptr};                                /**< TF2 Transform Listener */
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_; /**< TF2 Transform Buffer */
+
+  // Services
+  rclcpp::Service<rcdt_messages::srv::ExpressPoseInOtherFrame>::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<rcdt_messages::srv::ExpressPoseInOtherFrame::Response>
+          resp);
+
+  rclcpp::Service<rcdt_messages::srv::TransformPose>::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<rcdt_messages::srv::TransformPose::Request> req,
+      std::shared_ptr<rcdt_messages::srv::TransformPose::Response> resp);
+
+  rclcpp::Service<rcdt_messages::srv::TransformPose>::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<rcdt_messages::srv::TransformPose::Request> req,
+      std::shared_ptr<rcdt_messages::srv::TransformPose::Response> 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);
+};

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 <gtest/gtest.h>
+
+#include <chrono>
+#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 "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<rcdt_messages::srv::ExpressPoseInOtherFrame>(
+            "pose_manipulator/express_pose_in_other_frame");
+    transform_pose_client_ =
+        this->create_client<rcdt_messages::srv::TransformPose>(
+            "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<rcdt_messages::srv::TransformPose>::FutureAndRequestId
+  sendTransformPoseRequest(
+      std::shared_ptr<rcdt_messages::srv::TransformPose::Request> 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<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_; /**< Client to express pose in
+                                             another coordinate frame */
+  rclcpp::Client<rcdt_messages::srv::TransformPose>::SharedPtr
+      transform_pose_client_; /**< Client to transform a pose using a
+                                     specified transform */
+};

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),

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,
     )