added thread based moveit node, works once for now

src/btree_nodes/CMakeLists.txt

@@ -39,6 +39,7 @@ set(CPP_FILES
         src/nodes/MoveActorToTarget.cpp
         src/helpers/MinimalSubscriber.cpp
         src/nodes/RobotMove.cpp
+        src/nodes/MoveConnection.cpp
         )
 
 add_executable(tree ${CPP_FILES})

src/btree_nodes/robotTree.xml

@@ -4,7 +4,7 @@
     <BehaviorTree ID="BehaviorTree">
         <Sequence>
             <Action ID="RobotMove" target="0.2|0.2|1.1"/>
-            <!--<Action ID="RobotMove" target="-0.2|-0.2|1.1"/>-->
+            <Action ID="RobotMove" target="-0.2|-0.2|1.1"/>
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->

src/btree_nodes/src/Extensions.cpp

@@ -40,7 +40,7 @@ namespace BT {
     }
 
     template<>
-    geometry_msgs::msg::Pose_<std::allocator<void> > convertFromString(StringView str) {
+    std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
 
         std::cout << "[ generating pose from string ]" << std::endl;
 
@@ -49,14 +49,14 @@ namespace BT {
             throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>|<y>|<z>'.");
         }
 
-        geometry_msgs::msg::Pose pose;
+        auto pose = std::make_shared<geometry_msgs::msg::Pose>();
-        pose.orientation.w=0;
+        pose->orientation.w=0;
-        pose.orientation.x=0;
+        pose->orientation.x=0;
-        pose.orientation.y=1;
+        pose->orientation.y=1;
-        pose.orientation.z=0;
+        pose->orientation.z=0;
-        pose.position.x= convertFromString<double>(parts[0]);
+        pose->position.x= convertFromString<double>(parts[0]);
-        pose.position.y= convertFromString<double>(parts[1]);
+        pose->position.y= convertFromString<double>(parts[1]);
-        pose.position.y= convertFromString<double>(parts[2]);
+        pose->position.z= convertFromString<double>(parts[2]);
 
         std::cout << "[ generated pose from string ]" << std::endl;
 

src/btree_nodes/src/Tree.cpp

@@ -8,11 +8,25 @@
 #include "nodes/MoveActorToTarget.h"
 #include "nodes/RobotMove.h"
 #include "helpers/MinimalSubscriber.h"
+#include <chrono>
+#include <thread>
 
-using namespace BT;
 
+//using namespace BT;
 
 int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::NodeOptions node_options;
+    node_options.automatically_declare_parameters_from_overrides(true);
+
+    auto mainNode = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);
+
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(mainNode);
+    std::thread([&executor]() { executor.spin(); }).detach();
+
+    BehaviorTreeFactory factory;
+/*
     std::cout << "Registering nodes." << std::endl;
     NodeBuilder builderGenerateSafeTarget = [](const std::string &name, const NodeConfiguration &config) {
         const Area area = {std::vector<Position2D>({{1, 1},
@@ -34,10 +48,6 @@ int main(int argc, char **argv) {
         return std::make_unique<GenerateTarget2D>(name, config, area);
     };
 
-    rclcpp::init(argc, argv);
-
-    BehaviorTreeFactory factory;
-
     factory.registerBuilder<GenerateTarget2D>("GenerateSafeTarget", builderGenerateSafeTarget);
     factory.registerBuilder<GenerateTarget2D>("GenerateWarningTarget", builderGenerateWarningTarget);
     factory.registerBuilder<GenerateTarget2D>("GenerateUnsafeTarget", builderGenerateUnsafeTarget);
@@ -46,8 +56,15 @@ int main(int argc, char **argv) {
 
     factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
     factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
+     */
 
-    factory.registerNodeType<RobotMove>("RobotMove");
+    auto connection = std::make_shared<MoveConnection>(mainNode,"iisy");
+
+    NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) {
+        return std::make_unique<RobotMove>(name, config, &connection);
+    };
+
+    factory.registerBuilder<RobotMove>("RobotMove",builderIisyMove);
 
     std::cout << "Creating tree." << std::endl;
 
@@ -78,6 +95,8 @@ int main(int argc, char **argv) {
                                                         mutex.unlock();
                                                     }).detach();*/
 
+
+
     std::cout << "Looping." << std::endl;
 
     /*std::thread actor([&actorTree, &mutex](){
@@ -90,6 +109,7 @@ int main(int argc, char **argv) {
 
     while (rclcpp::ok()) {
         robotTree.tickRoot();
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
     }
 
     std::cout << "End." << std::endl;

src/btree_nodes/src/helpers/MinimalSubscriber.h

@@ -39,7 +39,6 @@ std::thread MinimalSubscriber<T>::createAsThread(const std::string &node_name, c
     auto subscriber = std::make_shared<MinimalSubscriber<T>>(node_name, topic_name, callback);
     return std::thread([subscriber]() {
         rclcpp::spin(subscriber);
-        rclcpp::shutdown();
     });
 }
 

src/btree_nodes/src/nodes/GenerateTarget2D.cpp

@@ -36,7 +36,7 @@ Position2D BT::GenerateTarget2D::getRandomPosInTriangle(unsigned long id) {
     return {a.x + (u1 * ab.x + u2 * ac.x), a.y + (u1 * ab.y + u2 * ac.y)};
 }
 
-BT::GenerateTarget2D::GenerateTarget2D(const std::string &name, const BT::NodeConfiguration &config, const Area area) :
+BT::GenerateTarget2D::GenerateTarget2D(const std::string &name, const BT::NodeConfiguration &config, const Area& area) :
         SyncActionNode(name, config) {
     this->area = area;
     unsigned long triangleCount = area.triangles.size() - 2;

src/btree_nodes/src/nodes/GenerateTarget2D.h

@@ -28,7 +28,7 @@ private:
     Position2D getRandomPosInTriangle(unsigned long id);
 
 public:
-    GenerateTarget2D(const std::string &name, const NodeConfiguration &config, const Area area);
+    GenerateTarget2D(const std::string &name, const NodeConfiguration &config, const Area& area);
 
     static PortsList providedPorts();
 

src/btree_nodes/src/nodes/MoveConnection.cpp

@@ -0,0 +1,5 @@
+//
+// Created by bastian on 28.03.22.
+//
+
+#include "MoveConnection.h"

src/btree_nodes/src/nodes/MoveConnection.h

@@ -0,0 +1,58 @@
+//
+// Created by bastian on 28.03.22.
+//
+
+#ifndef BUILD_MOVECONNECTION_H
+#define BUILD_MOVECONNECTION_H
+
+#include <rclcpp/rclcpp.hpp>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+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) {
+    }
+
+    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;
+
+        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;
+    }
+};
+
+
+#endif //BUILD_MOVECONNECTION_H

src/btree_nodes/src/nodes/RobotMove.cpp

@@ -4,76 +4,42 @@
 
 #include "RobotMove.h"
 
-BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &config)
+BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &config,
+                         std::shared_ptr<MoveConnection> *connection)
         : StatefulActionNode(name, config) {
-    std::cout << "[ constructing RobotMove ]" << std::endl;
+    this->connection = connection;
-    rclcpp::NodeOptions node_options;
-    node_options.automatically_declare_parameters_from_overrides(true);
-
-    char buffer[60];
-    sprintf(buffer,"RobotMove_%ld",random());
-
-    moveGroupNode = rclcpp::Node::make_shared(buffer, node_options);
-
-    rclcpp::executors::SingleThreadedExecutor executor;
-    executor.add_node(moveGroupNode);
-    std::thread([&executor]() { executor.spin(); }).detach();
-
-    planningGroup="iisy";
-    moveGroup = std::make_shared<moveit::planning_interface::MoveGroupInterface>(moveGroupNode, planningGroup);
-    std::cout << "[ constructing RobotMove end ]" << std::endl;
 }
 
 BT::NodeStatus BT::RobotMove::onStart() {
-    std::cout << "[ start RobotMove ]" << std::endl;
+    std::shared_ptr<geometry_msgs::msg::Pose> pose;
-    geometry_msgs::msg::Pose pose;
+    if(!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target",pose)){
-    if(!getInput<geometry_msgs::msg::Pose>("target",pose)){
+        std::cout << "no target available." << std::endl;
-        std::cout << "[ no target position given ]" << std::endl;
         return NodeStatus::FAILURE;
     }
-    std::cout << "[ what the shit ]" << std::endl;
+    printf("X:%f Y:%f Z:%f\n",pose->position.x,pose->position.y,pose->position.z);
-    moveGroup->setPoseTarget(pose);
+    printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z);
-
+    asyncResult = NodeStatus::RUNNING;
-    std::cout << "[ planning ]" << std::endl;
+    connection->get()->planAndExecute(pose,[this](bool finished){
-
+        if(finished){
-    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-    bool success = (moveGroup->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-
-    std::cout << "[ planned ]" << std::endl;
-
-    if(success) {
-        std::cout << "[ stopping async move ]" << std::endl;
-        moveGroup->stop();
-        std::cout << "[ stopped async move ]" << std::endl;
-        asyncThread.join();
-        asyncFinished = false;
-        asyncThread=std::thread([&my_plan, this](){
-            std::cout << "[ starting async move ]" << std::endl;
-            auto result = moveGroup->execute(my_plan);
-            std::cout << "[ finished async move ]" << std::endl;
-            if(result==moveit_msgs::msg::MoveItErrorCodes::SUCCESS){
             asyncResult = NodeStatus::SUCCESS;
         } else {
             asyncResult = NodeStatus::FAILURE;
         }
-            asyncFinished = true;
     });
     return NodeStatus::RUNNING;
-    }else{
-        return NodeStatus::FAILURE;
-    }
 }
 
 BT::NodeStatus BT::RobotMove::onRunning() {
-    if(asyncFinished){
+    auto result = asyncResult;
-        return asyncResult;
+    if(asyncResult!=NodeStatus::RUNNING){
+        asyncResult=NodeStatus::IDLE;
     }
-    return NodeStatus::RUNNING;
+    return result;
 }
 
 void BT::RobotMove::onHalted() {
-    moveGroup->stop();
+    connection->get()->stop();
-    asyncThread.join();
+    asyncResult=NodeStatus::IDLE;
 }
 
 BT::PortsList BT::RobotMove::providedPorts() {

src/btree_nodes/src/nodes/RobotMove.h

@@ -10,20 +10,18 @@
 #include <rclcpp/rclcpp.hpp>
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include "MoveConnection.h"
 
 namespace BT {
 
     class RobotMove : public StatefulActionNode {
     private:
-        std::shared_ptr<rclcpp::Node> moveGroupNode;
+        std::shared_ptr<MoveConnection> *connection;
-        std::shared_ptr<moveit::planning_interface::MoveGroupInterface> moveGroup;
-        std::string planningGroup;
-        std::thread asyncThread;
         NodeStatus asyncResult = NodeStatus::FAILURE;
-        bool asyncFinished = false;
 
     public:
-        RobotMove(const std::string &name, const BT::NodeConfiguration &config);
+        RobotMove(const std::string &name, const BT::NodeConfiguration &config,
+                  std::shared_ptr<MoveConnection> *connection);
 
         NodeStatus onStart() override;
 

src/btree_robot/CMakeLists.txt

@@ -58,11 +58,11 @@ find_package(behaviortree_cpp_v3 REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-add_executable(robot_test src/robot_test.cpp)
+add_executable(tree src/robot_test.cpp)
-ament_target_dependencies(robot_test ${THIS_PACKAGE_INCLUDE_DEPENDS} std_msgs behaviortree_cpp_v3)
+ament_target_dependencies(tree ${THIS_PACKAGE_INCLUDE_DEPENDS} std_msgs behaviortree_cpp_v3)
 
 install(TARGETS
-  robot_test
+  tree
   DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)

src/btree_robot/src/robot_test.cpp

@@ -40,57 +40,27 @@
 // All source files that use ROS logging should define a file-specific
 // static const rclcpp::Logger named LOGGER, located at the top of the file
 // and inside the namespace with the narrowest scope (if there is one)
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo");
+//static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo");
+
+int main(int argc, char **argv) {
+    int i;
+    printf("%d\n", argc);
+    for (i = 0; i < argc - 1; i++) {
+        printf("%s\n", argv[i]);
+    }
 
-int main(int argc, char** argv)
-{
     rclcpp::init(argc, argv);
     rclcpp::NodeOptions node_options;
     node_options.automatically_declare_parameters_from_overrides(true);
     auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);
 
-  // We spin up a SingleThreadedExecutor for the current state monitor to get information
-  // about the robot's state.
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(move_group_node);
     std::thread([&executor]() { executor.spin(); }).detach();
 
-  // BEGIN_TUTORIAL
+    std::string PLANNING_GROUP = "iisy";
-  //
-  // Setup
-  // ^^^^^
-  //
-  // MoveIt operates on sets of joints called "planning groups" and stores them in an object called
-  // the ``JointModelGroup``. Throughout MoveIt, the terms "planning group" and "joint model group"
-  // are used interchangeably.
-  static const std::string PLANNING_GROUP = "iisy";
-
-  // The
-  // :moveit_codedir:`MoveGroupInterface<moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h>`
-  // class can be easily set up using just the name of the planning group you would like to control and plan for.
     moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
 
-  // We will use the
-  // :moveit_codedir:`PlanningSceneInterface<moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h>`
-  // class to add and remove collision objects in our "virtual world" scene
-  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-
-  // Raw pointers are frequently used to refer to the planning group for improved performance.
-  const moveit::core::JointModelGroup* joint_model_group =
-      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
-
-  // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
-
-  // Start the demo
-  // ^^^^^^^^^^^^^^^^^^^^^^^^^
-  // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
-
-  // .. _move_group_interface-planning-to-pose-goal:
-  //
-  // Planning to a Pose goal
-  // ^^^^^^^^^^^^^^^^^^^^^^^
-  // We can plan a motion for this group to a desired pose for the
-  // end-effector.
     geometry_msgs::msg::Pose target_pose1;
     target_pose1.orientation.w = 0;
     target_pose1.orientation.y = 1;
@@ -98,19 +68,20 @@ int main(int argc, char** argv)
     target_pose1.position.x = 0.2;
     target_pose1.position.y = -0.2;
     target_pose1.position.z = 1.2;
-  move_group.setPoseTarget(target_pose1);
+
 
     // Now, we call the planner to compute the plan and visualize it.
     // Note that we are just planning, not asking move_group
     // to actually move the robot.
+
+    //RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
+
+    std::thread([&move_group, &target_pose1]() {
+        move_group.setPoseTarget(target_pose1);
         moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-
         bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-
-  RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
-
-  move_group.asyncExecute(my_plan.trajectory_);
         move_group.execute(my_plan);
+    }).join();
 
     rclcpp::shutdown();
     return 0;