From c976f7242a0008707703ba5085ff11e0b47a1336 Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Tue, 30 Jan 2024 16:06:31 +0100
Subject: [PATCH] grasp_interface.cpp added

---
 CMakeLists.txt                 | 34 +++++++++++++++---
 package.xml                    | 10 ++++--
 src/grasp_interface.cpp        | 65 ++++++++++++++++++++++++++++++++++
 src/grasp_moveit_interface.cpp |  0
 4 files changed, 102 insertions(+), 7 deletions(-)
 create mode 100644 src/grasp_interface.cpp
 delete mode 100644 src/grasp_moveit_interface.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ef001c6..7442b44 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,24 +17,47 @@ option(CHECK_TIDY "Adds clang-tidy tests" OFF)
 find_package(Eigen3 REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+
 find_package(moveit REQUIRED)
-find_package(moveit_msgs REQUIRED)
 find_package(moveit_visual_tools REQUIRED)
 find_package(moveit_ros_planning_interface)
+find_package(moveit_task_constructor_core)
+
+find_package(moveit_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(franka_cps_msgs REQUIRED)
+
 find_package(tf2)
 find_package(tf2_ros)
 
-add_executable(franka_moveit_interface src/moveit_interface.cpp)
+add_executable(franka_iml_interface src/moveit_interface.cpp)
+add_executable(franka_grasp_interface src/grasp_interface.cpp)
 
 ament_target_dependencies(
-  franka_moveit_interface
+  franka_iml_interface
   rclcpp
   moveit
-  moveit_msgs
   moveit_visual_tools
   moveit_ros_planning_interface
+  moveit_msgs
+  std_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+)
+
+ament_target_dependencies(
+  franka_grasp_interface
+  rclcpp
+  rclcpp_action
+  moveit
+  moveit_visual_tools
+  moveit_ros_planning_interface
+  moveit_task_constructor_core
+  moveit_msgs
+  franka_cps_msgs
   std_msgs
   geometry_msgs
   tf2
@@ -42,7 +65,8 @@ ament_target_dependencies(
 )
 
 install(
-  TARGETS franka_moveit_interface
+  TARGETS franka_iml_interface
+  TARGETS franka_grasp_interface
   DESTINATION lib/${PROJECT_NAME}
 )
 
diff --git a/package.xml b/package.xml
index c25a049..8096172 100644
--- a/package.xml
+++ b/package.xml
@@ -10,13 +10,19 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
+
   <depend>moveit</depend>
-  <depend>moveit_msgs</depend>
   <depend>moveit_visual_tools</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>moveit_task_constructor_core</depend>
+
+  <depend>moveit_msgs</depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
-  <depend>moveit_ros_planning_interface</depend>
+  <depend>franka_cps_msgs</depend>
+
   <depend>eigen</depend>
+
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   
diff --git a/src/grasp_interface.cpp b/src/grasp_interface.cpp
new file mode 100644
index 0000000..563d9f4
--- /dev/null
+++ b/src/grasp_interface.cpp
@@ -0,0 +1,65 @@
+#include <functional>
+#include <future>
+#include <memory>
+#include <string>
+#include <sstream>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <franka_cps_msgs/action/grasp_sequence.hpp>
+
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/task_constructor/task.h>
+#include <moveit/task_constructor/solvers.h>
+#include <moveit/task_constructor/stages.h>
+#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#endif
+#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
+#include <tf2_eigen/tf2_eigen.hpp>
+#else
+#include <tf2_eigen/tf2_eigen.h>
+#endif
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("franka_grasp_interface");
+namespace mtc = moveit::task_constructor;
+
+class GraspInterfaceNode
+{
+    public:
+        GraspInterfaceNode(const rclcpp::NodeOptions& options);
+    
+        rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
+    
+        void doTask();
+    
+        void setupPlanningScene();
+
+        rclcpp_action::create_client<franka_cps_msgs::action::GraspSequence>(this, "grasp_interface/grasp");
+    private:
+        mtc::Task createTask();
+        mtc::Task task_;
+        rclcpp::Node::SharedPtr node_;
+}
+
+
+GrapsInterfaceNode::GrapsInterfaceNode(const rclccp::NodeOptions& options)
+    : node_{ std::make_shared<rclcpp::Node>("grasp_interface_node", options)}
+{
+}
+
+rclcpp::node_interfaces::NodeBaseInterface::SharedPtr GraspInterfaceNode::getNodeBaseInterface()
+{
+    return node_->get_node_base_interface();
+}
+
+int main(int argc, char** argv)
+{
+    rclcpp::init(argc, argv);
+
+    rclcpp::shutdown();
+    return 0;
+}
\ No newline at end of file
diff --git a/src/grasp_moveit_interface.cpp b/src/grasp_moveit_interface.cpp
deleted file mode 100644
index e69de29..0000000