From 090cbb45b0c831f7368083f4c7feab7618c7540f Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Tue, 5 Sep 2023 16:00:55 +0200
Subject: [PATCH] moveit_interface.cpp works

---
 src/moveit_interface.cpp | 25 ++++++++-----------------
 1 file changed, 8 insertions(+), 17 deletions(-)

diff --git a/src/moveit_interface.cpp b/src/moveit_interface.cpp
index fe1a6ce..60411d8 100644
--- a/src/moveit_interface.cpp
+++ b/src/moveit_interface.cpp
@@ -8,6 +8,7 @@
 #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 "std_msgs/msg/empty.hpp"
 
@@ -24,7 +25,8 @@ class MoveitInterface : public rclcpp::Node
     : Node("moveit_interface"), trajectory_set(false)
     {
       publisher_ = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("moveit_interface/joint_space_trajectory", 10);
-      subcriber_ = this->create_subscription<geometry_msgs::msg::PoseArray>("moveit_interface/task_space_trajectory", 10, std::bind(&MoveitInterface::task_space_callback, this, std::placeholders::_1));
+      pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("moveit_interface/current_pose", 10);
+      subscriber_ = this->create_subscription<geometry_msgs::msg::PoseArray>("moveit_interface/task_space_trajectory", 10, std::bind(&MoveitInterface::task_space_callback, this, std::placeholders::_1));
       execute_sub_ = this->create_subscription<std_msgs::msg::Empty>("moveit_interface/execution", 10, std::bind(&MoveitInterface::execution_callback, this, std::placeholders::_1));
       
       // Create a shared instance of the current node
@@ -32,17 +34,7 @@ class MoveitInterface : public rclcpp::Node
       
       // Connecting to moveit
       static const std::string PLANNING_GROUP = "panda_arm";
-
-//      robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>(std::shared_ptr<rclcpp::Node>(shared_this), "robot_description");
-//      const moveit::core::RobotModelPtr& robot_model = robot_model_loader->getModel();
-//      robot_state = std::make_shared<moveit::core::RobotState>(robot_model);
-//      const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
-//
-//      planning_scene = std::make_shared<planning_scene::PlanningScene>(robot_model);
-//      planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
-
       move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(std::shared_ptr<rclcpp::Node>(shared_this), PLANNING_GROUP);
-      // const moveit::core::JointModelGroup* joint_model_group = move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP);
 
       // 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());
@@ -54,8 +46,9 @@ class MoveitInterface : public rclcpp::Node
   private:
     void task_space_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg)
     {
-      // moveit::core::RobotState start_state(*move_group_->getCurrentState());
-      // move_group_->setStartState(start_state);
+      geometry_msgs::msg::PoseStamped current_pose = move_group_->getCurrentPose();
+      pose_publisher_->publish(current_pose);
+
       std::vector<geometry_msgs::msg::Pose> waypoints = msg->poses;
 
       
@@ -86,13 +79,11 @@ class MoveitInterface : public rclcpp::Node
       }
     }
     rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr publisher_;
-    rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr subcriber_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
+    rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr subscriber_;
     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_;
-    std::shared_ptr<robot_model_loader::RobotModelLoader> robot_model_loader;
-    std::shared_ptr<moveit::core::RobotState> robot_state;
-    std::shared_ptr<planning_scene::PlanningScene> planning_scene;
 
     Eigen::Isometry3d text_pose;
     moveit_msgs::msg::RobotTrajectory trajectory;