From 58d1d08d011d2ed5411002ecaad4fe74f858a144 Mon Sep 17 00:00:00 2001
From: Bastian Hofmann <bastian.hofmann@xitaso.com>
Date: Tue, 29 Mar 2022 13:19:40 +0200
Subject: [PATCH] Fixed MoveConnection

---
 src/btree_nodes/src/nodes/MoveConnection.cpp | 44 ++++++++++++++++++++
 src/btree_nodes/src/nodes/MoveConnection.h   | 40 ++----------------
 2 files changed, 48 insertions(+), 36 deletions(-)

diff --git a/src/btree_nodes/src/nodes/MoveConnection.cpp b/src/btree_nodes/src/nodes/MoveConnection.cpp
index 371d4b5..ac0a7fa 100644
--- a/src/btree_nodes/src/nodes/MoveConnection.cpp
+++ b/src/btree_nodes/src/nodes/MoveConnection.cpp
@@ -3,3 +3,47 @@
 //
 
 #include "MoveConnection.h"
+
+void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,
+                                    const std::function<void(bool)> &callback) {
+    lock.lock();
+
+    std::cout << "planning" << std::endl;
+
+    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
+    bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+    if(!success){
+        callback(false);
+        return;
+    }
+
+    std::thread([this,callback,pose](){
+        try {
+            printf("X:%f Y:%f Z:%f\n",pose->position.x,pose->position.y,pose->position.z);
+            printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z);
+
+            moveGroup.setStartStateToCurrentState();
+            moveGroup.setPoseTarget(*pose);
+
+            std::cout << "executing" << std::endl;
+            bool success = this->moveGroup.move() == moveit::planning_interface::MoveItErrorCode::SUCCESS;
+            std::cout << "callback" << std::endl;
+            callback(success);
+            std::cout << "end" << std::endl;
+            lock.unlock();
+        } catch (...) {
+            std::exception_ptr p = std::current_exception();
+            std::clog <<(p ? p.__cxa_exception_type()->name() : "null") << std::endl;
+        }
+    }).detach();
+}
+
+MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const std::string &planningGroup) : moveGroup(node,planningGroup) {
+
+}
+
+void MoveConnection::stop() {
+    std::cout << "stopping move" << std::endl;
+    moveGroup.stop();
+    std::cout << "move stopped." << std::endl;
+}
diff --git a/src/btree_nodes/src/nodes/MoveConnection.h b/src/btree_nodes/src/nodes/MoveConnection.h
index 37e7c4b..fcada04 100644
--- a/src/btree_nodes/src/nodes/MoveConnection.h
+++ b/src/btree_nodes/src/nodes/MoveConnection.h
@@ -8,50 +8,18 @@
 #include <rclcpp/rclcpp.hpp>
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <rclcpp_action/client.hpp>
 
 class MoveConnection {
 private:
     moveit::planning_interface::MoveGroupInterface moveGroup;
-    std::thread running;
     std::mutex lock;
 public:
-    MoveConnection(const std::shared_ptr<rclcpp::Node>& node, const std::string& planningGroup): moveGroup(node,planningGroup) {
-    }
+    MoveConnection(const std::shared_ptr<rclcpp::Node>& node, const std::string& planningGroup);
 
-    void planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,const std::function<void(bool)>& callback){
-        lock.lock();
-        printf("X:%f Y:%f Z:%f\n",pose->position.x,pose->position.y,pose->position.z);
-        printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z);
-        std::cout << "planning" << std::endl;
+    void planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,const std::function<void(bool)> &callback);
 
-        moveGroup.setStartStateToCurrentState();
-        moveGroup.setPoseTarget(*pose);
-
-        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-        bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-        if(!success){
-            callback(false);
-        }
-
-        running = std::thread([this,callback,my_plan](){
-            std::cout << "executing" << std::endl;
-            moveGroup.stop();
-            bool success = moveGroup.execute(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
-            std::cout << "callback" << std::endl;
-            callback(success);
-            std::cout << "end" << std::endl;
-            lock.unlock();
-        });
-    }
-
-    void stop(){
-        std::cout << "stopping move" << std::endl;
-        moveGroup.stop();
-        if(running.joinable()){
-            running.join();
-        }
-        std::cout << "move stopped." << std::endl;
-    }
+    void stop();
 };