From 912f21e0e54dde110df0500f135b0e2bd7835dfd Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Tue, 12 Sep 2023 15:02:26 +0200
Subject: [PATCH] All components for Inverse Control implemented and tested +
 publishing the current endeffector state

---
 CMakeLists.txt           |  4 ++++
 package.xml              |  2 ++
 src/moveit_interface.cpp | 50 ++++++++++++++++++++++++++++++++++------
 3 files changed, 49 insertions(+), 7 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3d638c5..ef001c6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,6 +23,8 @@ find_package(moveit_visual_tools REQUIRED)
 find_package(moveit_ros_planning_interface)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(tf2)
+find_package(tf2_ros)
 
 add_executable(franka_moveit_interface src/moveit_interface.cpp)
 
@@ -35,6 +37,8 @@ ament_target_dependencies(
   moveit_ros_planning_interface
   std_msgs
   geometry_msgs
+  tf2
+  tf2_ros
 )
 
 install(
diff --git a/package.xml b/package.xml
index caa7472..c25a049 100644
--- a/package.xml
+++ b/package.xml
@@ -17,6 +17,8 @@
   <depend>geometry_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>eigen</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
   
 
   <test_depend>ament_lint_auto</test_depend>
diff --git a/src/moveit_interface.cpp b/src/moveit_interface.cpp
index 60411d8..5c5b74c 100644
--- a/src/moveit_interface.cpp
+++ b/src/moveit_interface.cpp
@@ -6,23 +6,26 @@
 #include <Eigen/Geometry>
 
 #include <rclcpp/rclcpp.hpp>
+
 #include "moveit_msgs/msg/robot_trajectory.hpp"
 #include "geometry_msgs/msg/pose_array.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-
+#include "geometry_msgs/msg/transform_stamped.h"
 #include "std_msgs/msg/empty.hpp"
 
 #include "moveit/move_group_interface/move_group_interface.h"
 #include "moveit_visual_tools/moveit_visual_tools.h"
 #include "moveit/robot_model_loader/robot_model_loader.h"
 
+#include "tf2_ros/transform_listener.h"
+
 using namespace std::chrono_literals;
 
 class MoveitInterface : public rclcpp::Node
 {
   public:
     MoveitInterface()
-    : Node("moveit_interface"), trajectory_set(false)
+    : Node("moveit_interface"), tf_buffer_(this->get_clock()),tf_listener_(tf_buffer_), trajectory_set(false)
     {
       publisher_ = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("moveit_interface/joint_space_trajectory", 10);
       pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("moveit_interface/current_pose", 10);
@@ -35,20 +38,50 @@ class MoveitInterface : public rclcpp::Node
       // Connecting to moveit
       static const std::string PLANNING_GROUP = "panda_arm";
       move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(std::shared_ptr<rclcpp::Node>(shared_this), PLANNING_GROUP);
-
+      move_group_->setStartStateToCurrentState();
       // Connection to Rviz
       visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(std::shared_ptr<rclcpp::Node>(shared_this), "panda_link0", "move_group", move_group_->getRobotModel());
       Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
       text_pose.translation().z()=1.0;
-      visual_tools_->publishText(text_pose, "MoveGroupInterface_Demo", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
+      visual_tools_->publishText(text_pose, "Franka_IML_Experiment", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
+
+      // TF2
+      tf2_ros::Buffer tf_buffer_{this->get_clock()};
+      tf2_ros::TransformListener tf_listener_{tf_buffer_};
+      tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&MoveitInterface::compute_endeffector_pose, this));
+
     }
 
   private:
+    void compute_endeffector_pose()
+    {
+      try
+      {
+        geometry_msgs::msg::TransformStamped transform_stamped;
+        transform_stamped = tf_buffer_.lookupTransform("panda_link1", "panda_link7", rclcpp::Time(0));
+        
+        geometry_msgs::msg::PoseStamped current_pose;
+        current_pose.header.frame_id = "panda_link8";
+        current_pose.header.stamp = transform_stamped.header.stamp;
+        //Position
+        current_pose.pose.position.x = transform_stamped.transform.translation.x;
+        current_pose.pose.position.y = transform_stamped.transform.translation.y;
+        current_pose.pose.position.z = transform_stamped.transform.translation.z;
+        // Orientation
+        current_pose.pose.orientation.x = transform_stamped.transform.rotation.x;
+        current_pose.pose.orientation.y = transform_stamped.transform.rotation.y;
+        current_pose.pose.orientation.z = transform_stamped.transform.rotation.z;
+        current_pose.pose.orientation.w = transform_stamped.transform.rotation.w;
+        pose_publisher_->publish(current_pose);
+      }
+      catch (tf2::TransformException &ex)
+      {
+        RCLCPP_WARN(this->get_logger(), "%s", ex.what());
+      }
+    }
+
     void task_space_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg)
     {
-      geometry_msgs::msg::PoseStamped current_pose = move_group_->getCurrentPose();
-      pose_publisher_->publish(current_pose);
-
       std::vector<geometry_msgs::msg::Pose> waypoints = msg->poses;
 
       
@@ -84,6 +117,9 @@ class MoveitInterface : public rclcpp::Node
     rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr execute_sub_;
     std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
     std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;
+    tf2_ros::Buffer tf_buffer_;
+    tf2_ros::TransformListener tf_listener_;
+    rclcpp::TimerBase::SharedPtr tf2_timer_;
 
     Eigen::Isometry3d text_pose;
     moveit_msgs::msg::RobotTrajectory trajectory;