Publish controller flange frame

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(hardware_interface 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_msgs 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
+  hardware_interface
   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_msgs
   ur_dashboard_msgs
   ur_msgs
   generate_parameter_library
@@ -54,7 +60,13 @@ generate_parameter_library(
   src/scaled_joint_trajectory_controller_parameters.yaml
 )
 
+generate_parameter_library(
+  pose_broadcaster_parameters
+  src/pose_broadcaster_parameters.yaml
+)
+
 add_library(${PROJECT_NAME} SHARED
+  src/pose_broadcaster.cpp
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_broadcaster.cpp
   src/gpio_controller.cpp)
@@ -64,8 +76,9 @@ target_include_directories(${PROJECT_NAME} PRIVATE
 )
 target_link_libraries(${PROJECT_NAME}
   gpio_controller_parameters
-  speed_scaling_state_broadcaster_parameters
+  pose_broadcaster_parameters
   scaled_joint_trajectory_controller_parameters
+  speed_scaling_state_broadcaster_parameters
 )
 ament_target_dependencies(${PROJECT_NAME}
   ${THIS_PACKAGE_INCLUDE_DEPENDS}

ur_controllers/controller_plugins.xml

@@ -14,4 +14,9 @@
       This controller publishes the Tool IO.
     </description>
   </class>
+  <class name="ur_controllers/PoseBroadcaster" type="ur_controllers::PoseBroadcaster" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This controller publishes a tf2 message.
+    </description>
+  </class>
 </library>

ur_controllers/include/ur_controllers/pose_broadcaster.hpp

@@ -0,0 +1,84 @@
+// Copyright 2023, FZI Forschungszentrum Informatik
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Exner <exner@fzi.de>
+ * \date    2023-09-11
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS__POSE_BROADCASTER_HPP
+#define UR_CONTROLLERS__POSE_BROADCASTER_HPP
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "pose_broadcaster_parameters.hpp"
+#include "tf2_msgs/msg/tf_message.hpp"
+
+#include <controller_interface/controller_interface.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include <ur_controllers/sc_pose.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+namespace ur_controllers
+{
+class PoseBroadcaster : public controller_interface::ControllerInterface
+{
+public:
+  PoseBroadcaster();
+  virtual ~PoseBroadcaster() = default;
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+private:
+  std::unique_ptr<ur_controllers::PoseComponent> pose_component_;
+  std::shared_ptr<pose_broadcaster::ParamListener> param_listener_;
+  pose_broadcaster::Params params_;
+
+  using StatePublisher = realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>;
+  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr sensor_state_publisher_;
+  std::unique_ptr<StatePublisher> realtime_publisher_;
+
+  geometry_msgs::msg::Pose pose_;
+};
+}  // namespace ur_controllers
+
+#endif  // ifndef UR_CONTROLLERS__POSE_BROADCASTER_HPP

ur_controllers/include/ur_controllers/sc_pose.hpp

@@ -0,0 +1,119 @@
+// Copyright 2023, FZI Forschungszentrum Informatik
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Exner <exner@fzi.de>
+ * \date    2023-09-11
+ *
+ */
+//----------------------------------------------------------------------
+
+#pragma once
+
+#include <tf2/tf2/LinearMath/Quaternion.h>
+
+#include <limits>
+#include <string>
+#include <vector>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <hardware_interface/loaned_state_interface.hpp>
+#include <semantic_components/semantic_component_interface.hpp>
+
+namespace ur_controllers
+{
+class PoseComponent : public semantic_components::SemanticComponentInterface<geometry_msgs::msg::Pose>
+{
+public:
+  explicit PoseComponent(const std::string& name) : SemanticComponentInterface(name, 6)
+  {
+    interface_names_.emplace_back(name_ + "/" + "position.x");
+    interface_names_.emplace_back(name_ + "/" + "position.y");
+    interface_names_.emplace_back(name_ + "/" + "position.z");
+    interface_names_.emplace_back(name_ + "/" + "orientation.x");
+    interface_names_.emplace_back(name_ + "/" + "orientation.y");
+    interface_names_.emplace_back(name_ + "/" + "orientation.z");
+    interface_names_.emplace_back(name_ + "/" + "orientation.w");
+
+    std::fill(position_.begin(), position_.end(), std::numeric_limits<double>::quiet_NaN());
+    std::fill(orientation_.begin(), orientation_.end(), std::numeric_limits<double>::quiet_NaN());
+  }
+
+  virtual ~PoseComponent() = default;
+
+  /**
+   * Return Pose message with full pose, parent and child frame
+   *
+   * \return Pose from values;
+   */
+  bool get_values_as_message(geometry_msgs::msg::Pose& message)
+  {
+    get_position();
+    get_orientation();
+
+    // update the message values
+    message.position.x = position_[0];
+    message.position.y = position_[1];
+    message.position.z = position_[2];
+
+    message.orientation.x = orientation_[0];
+    message.orientation.y = orientation_[1];
+    message.orientation.z = orientation_[2];
+    message.orientation.w = orientation_[3];
+
+    return true;
+  }
+
+protected:
+  std::array<double, 3> get_position()
+  {
+    size_t interface_counter = 0;
+    for (size_t i = 0; i < 3; ++i) {
+      position_[i] = state_interfaces_[interface_counter].get().get_value();
+      ++interface_counter;
+    }
+    return position_;
+  }
+  std::array<double, 4> get_orientation()
+  {
+    size_t interface_counter = 3;
+    for (size_t i = 0; i < 4; ++i) {
+      orientation_[i] = state_interfaces_[interface_counter].get().get_value();
+      ++interface_counter;
+    }
+    return orientation_;
+  }
+  std::array<double, 3> position_;
+  std::array<double, 4> orientation_;
+};
+
+}  // namespace ur_controllers

ur_controllers/package.xml

@@ -22,6 +22,7 @@
 
   <depend>angles</depend>
   <depend>controller_interface</depend>
+  <depend>hardware_interface</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_msgs</depend>
   <depend>ur_dashboard_msgs</depend>
   <depend>ur_msgs</depend>