Fix stuff

ur_controllers/CMakeLists.txt

@@ -8,6 +8,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
 find_package(controller_interface REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(joint_trajectory_controller REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
@@ -16,13 +17,16 @@ find_package(rcutils REQUIRED)
 find_package(realtime_tools REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 find_package(ur_dashboard_msgs REQUIRED)
 find_package(ur_msgs REQUIRED)
 find_package(generate_parameter_library REQUIRED)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   angles
   controller_interface
+  geometry_msgs
   joint_trajectory_controller
   lifecycle_msgs
   pluginlib
@@ -31,6 +35,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   realtime_tools
   std_msgs
   std_srvs
+  tf2_geometry_msgs
+  tf2_ros
   ur_dashboard_msgs
   ur_msgs
   generate_parameter_library

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -38,11 +38,14 @@
 #ifndef UR_CONTROLLERS__GPIO_CONTROLLER_HPP_
 #define UR_CONTROLLERS__GPIO_CONTROLLER_HPP_
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 #include <memory>
 #include <string>
 #include <unordered_map>
 #include <vector>
 
+#include "geometry_msgs/msg/pose_stamped.hpp"
 #include "std_srvs/srv/trigger.hpp"
 
 #include "controller_interface/controller_interface.hpp"
@@ -200,6 +203,9 @@ class GPIOController : public controller_interface::ControllerInterface
   std::shared_ptr<rclcpp::Publisher<ur_dashboard_msgs::msg::SafetyMode>> safety_mode_pub_;
   std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>> program_state_pub_;
 
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
+
   ur_msgs::msg::IOStates io_msg_;
   ur_msgs::msg::ToolDataMsg tool_data_msg_;
   ur_dashboard_msgs::msg::RobotMode robot_mode_msg_;

ur_controllers/package.xml

@@ -22,6 +22,7 @@
 
   <depend>angles</depend>
   <depend>controller_interface</depend>
+  <depend>geometry_msgs</depend>
   <depend>joint_trajectory_controller</depend>
   <depend>lifecycle_msgs</depend>
   <depend>pluginlib</depend>
@@ -30,6 +31,8 @@
   <depend>realtime_tools</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
   <depend>ur_dashboard_msgs</depend>
   <depend>ur_msgs</depend>
 

ur_controllers/src/gpio_controller.cpp

@@ -35,9 +35,10 @@
  */
 //----------------------------------------------------------------------
 
-#include "ur_controllers/gpio_controller.hpp"
+#include <tf2/LinearMath/Matrix3x3.h>
 
-#include <string>
+#include "ur_controllers/gpio_controller.hpp"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 namespace ur_controllers
 {
@@ -48,7 +49,6 @@ controller_interface::CallbackReturn GPIOController::on_init()
     // Create the parameter listener and get the parameters
     param_listener_ = std::make_shared<gpio_controller::ParamListener>(get_node());
     params_ = param_listener_->get_params();
-
   } catch (const std::exception& e) {
     fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
     return CallbackReturn::ERROR;
@@ -218,6 +218,9 @@ ur_controllers::GPIOController::on_configure(const rclcpp_lifecycle::State& /*pr
   // get parameters from the listener in case they were updated
   params_ = param_listener_->get_params();
 
+  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_node()->get_clock());
+  tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
+
   return LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
@@ -560,24 +563,52 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r
 bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
                                   ur_msgs::srv::SetForceMode::Response::SharedPtr resp)
 {
-  if (req->task_frame.size() != 6 || req->selection_vector.size() != 6 || req->wrench.size() != 6 ||
-      req->limits.size() != 6) {
-    RCLCPP_WARN(get_node()->get_logger(), "Size of received force mode message is wrong");
-    resp->success = false;
-    return false;
-  }
-
   // reset success flag
+  RCLCPP_INFO(get_node()->get_logger(), "Currently we have %zu command interfaces", command_interfaces_.size());
+
   command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
 
+  // transform task frame into base
+  const std::string tf_prefix = params_.tf_prefix;
+  try {
+    auto task_frame_transformed = tf_buffer_->transform(req->task_frame, tf_prefix + "base");
+    command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(task_frame_transformed.pose.position.x);
+    command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(task_frame_transformed.pose.position.y);
+    command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(task_frame_transformed.pose.position.z);
+
+    tf2::Quaternion quat_tf;
+    tf2::convert(task_frame_transformed.pose.orientation, quat_tf);
+    tf2::Matrix3x3 rot_mat(quat_tf);
+    std::vector<double> rot(3);
+    rot_mat.getRPY(rot[0], rot[1], rot[2]);
+    command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(task_frame_transformed.pose.position.x);
+    command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(task_frame_transformed.pose.position.y);
+    command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(task_frame_transformed.pose.position.z);
+  } catch (const tf2::TransformException& ex) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s",
+                 req->task_frame.header.frame_id.c_str(), ex.what());
+  }
+
+  command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value(req->selection_vector_x);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value(req->selection_vector_y);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value(req->selection_vector_z);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value(req->selection_vector_rx);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value(req->selection_vector_ry);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value(req->selection_vector_rz);
+
+  command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(req->wrench.force.x);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(req->wrench.force.y);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(req->wrench.force.z);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(req->wrench.torque.x);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(req->wrench.torque.y);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(req->wrench.torque.z);
+
   for (size_t i = 0; i < 6; i++) {
-    command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X + i].set_value(req->task_frame[i]);
-    command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X + i].set_value(req->selection_vector[i]);
-    command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X + i].set_value(req->wrench[i]);
     command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X + i].set_value(req->limits[i]);
   }
   command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(unsigned(req->type));
 
+  RCLCPP_INFO(get_node()->get_logger(), "Waiting for force mode to be set.");
   while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
     // Asynchronous wait until the hardware interface has set the force mode
     std::this_thread::sleep_for(std::chrono::milliseconds(50));