Tree working again, just robot simulation for now

src/btree_nodes/CMakeLists.txt

@@ -22,6 +22,7 @@ set(DEPENDENCIES
         tf2_geometry_msgs
         moveit_ros_planning
         pluginlib
+        ros_actor_action_server_msgs
         )
 
 foreach(dep IN LISTS DEPENDENCIES)
@@ -48,6 +49,7 @@ set(CPP_FILES
         src/nodes/InAreaTest.cpp
         src/nodes/InterruptableSequence.cpp
         src/nodes/SetRobotVelocity.cpp
+        src/nodes/ActorMovement.cpp
         )
 
 add_library(tree_plugins_base src/Factory.cpp)

src/btree_nodes/package.xml

@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
+  <depend>ros_actor_action_server_msgs</depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/btree_nodes/src/Tree.cpp

@@ -13,6 +13,7 @@
 #include "nodes/InAreaTest.h"
 #include "nodes/InterruptableSequence.h"
 #include "nodes/SetRobotVelocity.h"
+#include "nodes/ActorMovement.h"
 #include <chrono>
 #include <thread>
 
@@ -23,7 +24,8 @@ int main(int argc, char **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);
+    auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options);
+    auto moveNode = rclcpp::Node::make_shared("tree_moveit_node", node_options);
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(mainNode);
@@ -33,8 +35,8 @@ int main(int argc, char **argv) {
 
     const std::shared_ptr<Area> safeArea = std::make_shared<Area>();
     safeArea->triangles = {std::vector<Position2D>({
-            {-1, 3.5},
-            {-1, 7},
+            {   1,     3.5 },
+            {   1,     7   },
             {   3,     3.5 },
             {   7,     7   },
             {   3,    -1   },
@@ -80,6 +82,7 @@ int main(int argc, char **argv) {
     factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
     factory.registerNodeType<AmICalled>("AmICalled");
 
+    factory.registerNodeType<ActorMovement>("ActorMovement");
     factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
     factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
     factory.registerNodeType<OffsetPose>("OffsetPose");
@@ -88,7 +91,7 @@ int main(int argc, char **argv) {
 
     factory.registerNodeType<InterruptableSequence>("InterruptableSequence");
 
-    auto connection = std::make_shared<MoveConnection>(mainNode, "iisy");
+    auto connection = std::make_shared<MoveConnection>(moveNode, "arm");
 
     NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) {
         return std::make_unique<RobotMove>(name, config, &connection);
@@ -119,11 +122,13 @@ int main(int argc, char **argv) {
 
     std::cout << "Creating tree." << std::endl;
 
-    Tree actorTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/actorTree.xml");
-    Tree robotTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/robotTree.xml");
+    Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml");
+    Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml");
 
     auto trees = {actorTree.rootBlackboard(), robotTree.rootBlackboard()};
     auto init = std::make_shared<geometry_msgs::msg::Pose>();
+    init->position.x = 6.0;
+    init->position.y = 6.0;
 
     for (const auto &item : trees){
         item->set("safeArea",safeArea);
@@ -147,12 +152,12 @@ int main(int argc, char **argv) {
 
     std::cout << "Looping." << std::endl;
 
-    std::thread actor([&actorTree]() {
+    /*std::thread actor([&actorTree]() {
         while (rclcpp::ok()) {
             actorTree.tickRoot();
             std::this_thread::sleep_for(std::chrono::milliseconds(20));
         }
-    });
+    });*/
 
     while (rclcpp::ok()) {
         robotTree.tickRoot();

src/btree_nodes/src/nodes/ActorMovement.cpp

@@ -0,0 +1,64 @@
+//
+// Created by bastian on 26.03.22.
+//
+
+#include "ActorMovement.h"
+
+BT::ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config)
+        : StatefulActionNode(name, config) {}
+
+BT::PortsList BT::ActorMovement::providedPorts() {
+    return {
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
+    };
+}
+
+BT::NodeStatus BT::ActorMovement::onStart() {
+    std::cout << "started moving" << std::endl;
+
+    rclcpp::Node node("targetPublisherNode");
+    auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
+    auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
+
+    if (!res) {
+        std::cout << "[ no target available ]" << std::endl;
+        std::cout << res.error() << std::endl;
+        return NodeStatus::FAILURE;
+    }
+
+    publisher->publish(*target);
+    return NodeStatus::RUNNING;
+}
+
+BT::NodeStatus BT::ActorMovement::onRunning() {
+    std::shared_ptr<geometry_msgs::msg::Pose> current;
+
+    auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("current", current);
+    if (!res) {
+        std::cout << "[ no current position available ]" << std::endl;
+        std::cout << res.error() << std::endl;
+        return NodeStatus::FAILURE;
+    }
+
+    double dX = target->position.x - current->position.x;
+    double dY = target->position.y - current->position.y;
+
+    auto distance = sqrt(dX * dX + dY * dY);
+
+    if (distance < 0.3) {
+        return NodeStatus::SUCCESS;
+    } else {
+        return NodeStatus::RUNNING;
+    }
+}
+
+void BT::ActorMovement::onHalted() {
+    std::cout << "halted move" << std::endl;
+    rclcpp::Node node("targetPublisherNode");
+    auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
+    geometry_msgs::msg::Pose inf;
+    inf.position.x = HUGE_VAL;
+    inf.position.y = HUGE_VAL;
+    publisher->publish(inf);
+}

src/btree_nodes/src/nodes/ActorMovement.h

@@ -0,0 +1,30 @@
+//
+// Created by bastian on 26.03.22.
+//
+
+#ifndef BUILD_ACTORMOVEMENT_H
+#define BUILD_ACTORMOVEMENT_H
+
+#include <behaviortree_cpp_v3/action_node.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace BT {
+    class ActorMovement : public StatefulActionNode {
+    public:
+        ActorMovement(const std::string &name, const NodeConfiguration &config);
+
+        static PortsList providedPorts();
+
+        std::shared_ptr<geometry_msgs::msg::Pose> target;
+
+        NodeStatus onStart() override;
+
+        NodeStatus onRunning() override;
+
+        void onHalted() override;
+    };
+}
+
+
+#endif //BUILD_ACTORMOVEMENT_H

src/btree_nodes/src/nodes/MoveConnection.cpp

@@ -6,32 +6,39 @@
 
 void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,
                                     const std::function<void(bool)> &callback) {
+    std::cout<<"planAndExecute."<<std::endl;
     lock.lock();
+    std::cout<<"got lock."<<std::endl;
     lastTarget = pose;
     lastCallback = callback;
 
     moveGroup.setMaxVelocityScalingFactor(currentSpeed);
     moveGroup.setMaxAccelerationScalingFactor(1);
 
-    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-    bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-    if(!success){
-        callback(false);
-        return;
-    }
+    std::cout<<"Starting planning"<<std::endl;
 
     std::thread([this,callback,pose](){
-        moveGroup.setStartStateToCurrentState();
+        
         moveGroup.setPoseTarget(*pose);
+        moveGroup.setGoalJointTolerance(0.01);
+        moveGroup.setGoalOrientationTolerance(0.01);
+
+        std::cout<<"Parameters set."<<std::endl;
 
         moveit::planning_interface::MoveGroupInterface::Plan plan;
-        moveGroup.plan(plan);
+        if(moveGroup.plan(plan)==moveit::planning_interface::MoveItErrorCode::SUCCESS){
+            std::cout<<"Planned a path."<<std::endl;
+        }else{
+            std::cout<<"Error during planning."<<std::endl;
+            lock.unlock();
+            callback(false);
+        }
         if(cancelled){
             lastTarget = nullptr;
             lock.unlock();
             return;
         }
-        bool success = this->moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
+        bool success = moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
         if(!cancelled) {
             callback(success);
         }

src/iisy_config_2/config/moveit_controllers.yaml

@@ -1,4 +1,6 @@
 # MoveIt uses this configuration for controller management
+trajectory_execution:
+  allowed_execution_duration_scaling: 1.2 # Wait for moves to finish without timeout
 
 moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
 

src/moveit_test/src/test.cpp

@@ -21,14 +21,18 @@ int main(int argc, char * argv[])
     // Set a target Pose
     auto const target_pose = []{
         geometry_msgs::msg::Pose msg;
-        msg.orientation.w = 1.0;
-        msg.position.x = 0.25;
-        msg.position.y = 0.25;
-        msg.position.z = 1.25;
+        msg.orientation.w = 0.0;
+        msg.orientation.x = 0.0;
+        msg.orientation.y = 1.0;
+        msg.orientation.z = 0.0;
+        msg.position.x = -0.043357;
+        msg.position.y = -0.281366;
+        msg.position.z = 1.1;
         return msg;
     }();
 
-    move_group_interface.setGoalOrientationTolerance(100000);
+    move_group_interface.setGoalOrientationTolerance(0.01);
+    move_group_interface.setGoalPositionTolerance(0.01);
     move_group_interface.setPoseTarget(target_pose);
 
     //std::map<std::string,double> states = {