add pose publisher node

rbs_mill_assist/src/CMakeLists.txt

@@ -12,7 +12,7 @@ if("$ENV{GZ_VERSION}" STREQUAL "harmonic")
   find_package(gz-sim8 REQUIRED)
   set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
   message(STATUS "Compiling against Gazebo Harmonic")
-# Default to Garden
+  # Default to Garden
 else()
   find_package(gz-sim7 REQUIRED)
   set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
@@ -68,3 +68,26 @@ install(
   TARGETS planning_scene_publisher
   DESTINATION lib/${PROJECT_NAME}
 )
+
+# publish_ee_pose
+
+find_package(rclcpp_components REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+add_library(publish_ee_pose SHARED publish_ee_pose.cpp)
+target_include_directories(publish_ee_pose
+  PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+          $<INSTALL_INTERFACE:include>)
+target_compile_definitions(publish_ee_pose PRIVATE "PUBLISH_EE_POSE_BUILDING_DLL")
+ament_target_dependencies(publish_ee_pose rclcpp_components tf2_ros std_msgs tf2_geometry_msgs)
+rclcpp_components_register_node(publish_ee_pose PLUGIN "rbs_mill_assist::PublishEePose" EXECUTABLE publish_ee_pose_node)
+
+install(
+  TARGETS
+          publish_ee_pose
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
+

rbs_mill_assist/src/publish_ee_pose.cpp

@@ -0,0 +1,67 @@
+#include "geometry_msgs/msg/pose.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/transform_listener.h"
+#include <chrono>
+#include <string>
+
+namespace rbs_mill_assist {
+class PublishEePose : public rclcpp::Node {
+public:
+  explicit PublishEePose(
+      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : Node("publish_ee_pose", options), tf_buffer_(this->get_clock()),
+        tf_listener_(tf_buffer_) {
+
+    this->declare_parameter<std::string>("ee_link", "ee_link");
+    this->declare_parameter<std::string>("base_link", "base_link");
+    this->get_parameter("ee_link", ee_link_);
+    this->get_parameter("base_link", base_link_);
+
+    publish_pose_ =
+        this->create_publisher<geometry_msgs::msg::Pose>("ee_pose", 10);
+
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(500),
+        std::bind(&PublishEePose::publishMessage, this));
+  }
+
+private:
+  void publishMessage() {
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    try {
+      transform_stamped =
+          tf_buffer_.lookupTransform(base_link_, ee_link_, tf2::TimePointZero);
+
+      geometry_msgs::msg::Pose pose;
+      pose.position.x = transform_stamped.transform.translation.x;
+      pose.position.y = transform_stamped.transform.translation.y;
+      pose.position.z = transform_stamped.transform.translation.z;
+
+      pose.orientation = transform_stamped.transform.rotation;
+
+      // RCLCPP_INFO(this->get_logger(), "Publishing Pose: [x: %.2f, y: %.2f, z:
+      // %.2f]",
+      //             pose.position.x, pose.position.y, pose.position.z);
+
+      publish_pose_->publish(pose);
+    } catch (const tf2::TransformException &ex) {
+      RCLCPP_WARN(this->get_logger(), "Could not transform %s to %s: %s",
+                  ee_link_.c_str(), base_link_.c_str(), ex.what());
+    }
+  }
+
+  std::string ee_link_;
+  std::string base_link_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr publish_pose_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+};
+} // namespace rbs_mill_assist
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_mill_assist::PublishEePose);