Created a nonfunctional node for robot movement

src/btree_nodes/CMakeLists.txt

@@ -8,18 +8,41 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 # find dependencies
 
-find_package(ament_cmake REQUIRED)
+set(DEPENDENCIES
-find_package(rclcpp REQUIRED)
+        ament_cmake
-find_package(geometry_msgs REQUIRED)
+        rclcpp
-find_package(std_msgs REQUIRED)
+        #geometry_msgs
-find_package(behaviortree_cpp_v3 REQUIRED)
+        std_msgs
+        behaviortree_cpp_v3
+        rclcpp_action
+        tf2_geometry_msgs
+        moveit_core
+        moveit_ros_planning_interface
+        tf2_geometry_msgs
+        moveit_ros_planning
+        )
+
+foreach(dep IN LISTS DEPENDENCIES)
+    find_package(${dep} REQUIRED)
+endforeach()
 
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-add_executable(tree src/Tree.cpp src/Extensions.cpp src/nodes/WeightedRandomNode.cpp src/nodes/AmICalled.cpp src/nodes/GenerateTarget2D.cpp src/nodes/MoveActorToTarget.cpp src/helpers/MinimalSubscriber.cpp src/nodes/RobotMove.cpp)
+set(CPP_FILES
-ament_target_dependencies(tree rclcpp geometry_msgs std_msgs behaviortree_cpp_v3)
+        src/Tree.cpp
+        src/Extensions.cpp
+        src/nodes/WeightedRandomNode.cpp
+        src/nodes/AmICalled.cpp
+        src/nodes/GenerateTarget2D.cpp
+        src/nodes/MoveActorToTarget.cpp
+        src/helpers/MinimalSubscriber.cpp
+        src/nodes/RobotMove.cpp
+        )
+
+add_executable(tree ${CPP_FILES})
+ament_target_dependencies(tree ${DEPENDENCIES})
 
 #add_executable(talker src/publisher_member_function.cpp)
 #ament_target_dependencies(talker geometry_msgs rclcpp)
@@ -30,7 +53,7 @@ install(TARGETS
         tree
         DESTINATION lib/${PROJECT_NAME})
 
-if(BUILD_TESTING)
+if (BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
     # the following line skips the linter which checks for copyrights
     # uncomment the line when a copyright and license is not present in all source files
@@ -39,6 +62,6 @@ if(BUILD_TESTING)
     # uncomment the line when this package is not in a git repo
     #set(ament_cmake_cpplint_FOUND TRUE)
     ament_lint_auto_find_test_dependencies()
-endif()
+endif ()
 
 ament_package()

src/btree_nodes/nodes.xml

@@ -30,8 +30,14 @@
             <input_port name="weights">Weights for the children, separated by semicolon.</input_port>
         </Control>
 
-        <Condition ID="InTest">
+        <Condition ID="InAreaTest">
-
+            <input_port name="bounds">Bounds to check in</input_port>
+            <input_port name="position">Position of object</input_port>
+            <output_port name="result">Result of check if position is within bounds</output_port>
         </Condition>
+
+        <Action ID="RobotMove">
+            <input_port name="target">Target pose of robot.</input_port>
+        </Action>
     </TreeNodesModel>
 </root>

src/btree_nodes/robotTree.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <Sequence>
+            <Action ID="RobotMove" target="0.2|0.2|1.1"/>
+            <!--<Action ID="RobotMove" target="-0.2|-0.2|1.1"/>-->
+        </Sequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="AmICalled">
+            <input_port name="called">called status</input_port>
+        </Action>
+        <Action ID="GenerateSafeTarget">
+            <output_port name="target">Generated pose in safe area</output_port>
+        </Action>
+        <Action ID="GenerateUnsafeTarget">
+            <output_port name="target">Generated pose in unsafe area</output_port>
+        </Action>
+        <Action ID="GenerateWarningTarget">
+            <output_port name="target">Generated pose in warning area</output_port>
+        </Action>
+        <Condition ID="InAreaTest">
+            <input_port name="bounds">Bounds to check in</input_port>
+            <input_port name="position">Position of object</input_port>
+            <output_port name="result">Result of check if position is within bounds</output_port>
+        </Condition>
+        <Action ID="MoveActorToTarget">
+            <input_port name="current">Current actor position</input_port>
+            <input_port name="target">Target to move actor to</input_port>
+        </Action>
+        <Action ID="RobotMove">
+            <input_port name="target">Target pose of robot.</input_port>
+        </Action>
+        <Control ID="WeightedRandom">
+            <input_port name="weights">Weights for the children, separated by semicolon.</input_port>
+        </Control>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
+

src/btree_nodes/src/Extensions.cpp

@@ -3,10 +3,11 @@
 //
 
 #include "Area.h"
+#include <geometry_msgs/msg/pose.hpp>
 
 namespace BT {
     template<>
-    inline Position2D convertFromString(StringView str) {
+    Position2D convertFromString(StringView str) {
         printf("Converting string: \"%s\"\n", str.data());
 
         auto split = splitString(str, ';');
@@ -21,7 +22,7 @@ namespace BT {
     }
 
     template<>
-    inline Area convertFromString(StringView str) {
+    Area convertFromString(StringView str) {
         printf("Converting string: \"%s\"\n", str.data());
 
         auto parts = splitString(str, '|');
@@ -37,4 +38,28 @@ namespace BT {
         output.triangles = array;
         return output;
     }
+
+    template<>
+    geometry_msgs::msg::Pose_<std::allocator<void> > convertFromString(StringView str) {
+
+        std::cout << "[ generating pose from string ]" << std::endl;
+
+        auto parts = splitString(str, '|');
+        if (parts.size() != 3) {
+            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>|<y>|<z>'.");
+        }
+
+        geometry_msgs::msg::Pose pose;
+        pose.orientation.w=0;
+        pose.orientation.x=0;
+        pose.orientation.y=1;
+        pose.orientation.z=0;
+        pose.position.x= convertFromString<double>(parts[0]);
+        pose.position.y= convertFromString<double>(parts[1]);
+        pose.position.y= convertFromString<double>(parts[2]);
+
+        std::cout << "[ generated pose from string ]" << std::endl;
+
+        return pose;
+    }
 }

src/btree_nodes/src/Tree.cpp

@@ -6,6 +6,7 @@
 #include "nodes/AmICalled.h"
 #include "nodes/GenerateTarget2D.h"
 #include "nodes/MoveActorToTarget.h"
+#include "nodes/RobotMove.h"
 #include "helpers/MinimalSubscriber.h"
 
 using namespace BT;
@@ -36,6 +37,7 @@ int main(int argc, char **argv) {
     rclcpp::init(argc, argv);
 
     BehaviorTreeFactory factory;
+
     factory.registerBuilder<GenerateTarget2D>("GenerateSafeTarget", builderGenerateSafeTarget);
     factory.registerBuilder<GenerateTarget2D>("GenerateWarningTarget", builderGenerateWarningTarget);
     factory.registerBuilder<GenerateTarget2D>("GenerateUnsafeTarget", builderGenerateUnsafeTarget);
@@ -45,40 +47,50 @@ int main(int argc, char **argv) {
     factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
     factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
 
+    factory.registerNodeType<RobotMove>("RobotMove");
+
     std::cout << "Creating tree." << std::endl;
 
-    Tree tree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/tree.xml");
+    //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");
 
+    /*
     std::mutex mutex;
 
     std::cout << "Starting subscriber." << std::endl;
 
     geometry_msgs::msg::Pose init;
-    tree.rootBlackboard()->set<geometry_msgs::msg::Pose>("current",init);
+    actorTree.rootBlackboard()->set<geometry_msgs::msg::Pose>("current",init);
-    tree.rootBlackboard()->set<geometry_msgs::msg::Pose>("target",init);
+    actorTree.rootBlackboard()->set<geometry_msgs::msg::Pose>("target",init);
-    tree.rootBlackboard()->set<bool>("called",false);
+    actorTree.rootBlackboard()->set<bool>("called",false);
 
     MinimalSubscriber<geometry_msgs::msg::Pose>::createAsThread("tree_get_currentPose", "currentPose",
-                                                    [&tree, &mutex](geometry_msgs::msg::Pose pose) {
+                                                    [&actorTree, &mutex](geometry_msgs::msg::Pose pose) {
                                                         mutex.lock();
-                                                        tree.rootBlackboard()->set("current", pose);
+                                                        actorTree.rootBlackboard()->set("current", pose);
                                                         mutex.unlock();
                                                     }).detach();
 
     MinimalSubscriber<std_msgs::msg::Bool>::createAsThread("tree_get_called", "called",
-                                                    [&tree, &mutex](std_msgs::msg::Bool called) {
+                                                    [&actorTree, &mutex](std_msgs::msg::Bool called) {
                                                         mutex.lock();
-                                                        tree.rootBlackboard()->set("called", (bool) called.data);
+                                                        actorTree.rootBlackboard()->set("called", (bool) called.data);
                                                         mutex.unlock();
-                                                    }).detach();
+                                                    }).detach();*/
 
     std::cout << "Looping." << std::endl;
 
+    /*std::thread actor([&actorTree, &mutex](){
         while (rclcpp::ok()) {
             mutex.lock();
-        tree.tickRoot();
+            actorTree.tickRoot();
             mutex.unlock();
         }
+    //});*/
+
+    while (rclcpp::ok()) {
+        robotTree.tickRoot();
+    }
 
     std::cout << "End." << std::endl;
 

src/btree_nodes/src/nodes/RobotMove.cpp

@@ -3,3 +3,81 @@
 //
 
 #include "RobotMove.h"
+
+BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &config)
+        : StatefulActionNode(name, config) {
+    std::cout << "[ constructing RobotMove ]" << std::endl;
+    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;
+    geometry_msgs::msg::Pose pose;
+    if(!getInput<geometry_msgs::msg::Pose>("target",pose)){
+        std::cout << "[ no target position given ]" << std::endl;
+        return NodeStatus::FAILURE;
+    }
+    std::cout << "[ what the shit ]" << std::endl;
+    moveGroup->setPoseTarget(pose);
+
+    std::cout << "[ planning ]" << std::endl;
+
+    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){
+        return asyncResult;
+    }
+    return NodeStatus::RUNNING;
+}
+
+void BT::RobotMove::onHalted() {
+    moveGroup->stop();
+    asyncThread.join();
+}
+
+BT::PortsList BT::RobotMove::providedPorts() {
+    // Optionally, a port can have a human readable description
+    const char *description = "Generates a random position in an area defined by a triangle strip";
+    return {InputPort<geometry_msgs::msg::Pose>("target", description)};
+}

src/btree_nodes/src/nodes/RobotMove.h

@@ -7,15 +7,31 @@
 
 #include <behaviortree_cpp_v3/action_node.h>
 #include <geometry_msgs/msg/pose.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
 
 namespace BT {
 
     class RobotMove : public StatefulActionNode {
-    public:
+    private:
-        RobotMove(const std::string &name, const BT::NodeConfiguration &config)
+        std::shared_ptr<rclcpp::Node> moveGroupNode;
-            : StatefulActionNode(name, config) {
+        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);
+
+        NodeStatus onStart() override;
+
+        NodeStatus onRunning() override;
+
+        void onHalted() override;
+
+        static PortsList providedPorts();
     };
 }
 

src/btree_nodes/src/nodes/WeightedRandomNode.cpp

@@ -57,6 +57,7 @@ NodeStatus WeightedRandomNode::tick() {
         select_next = false;
     }
 
+    std::cout<< "Running child " << selected_child_index << std::endl;
     TreeNode *current_child_node = children_nodes_[selected_child_index];
     const NodeStatus child_status = current_child_node->executeTick();
 

src/btree_robot/CMakeLists.txt

@@ -11,42 +11,42 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
   rclcpp_action
   tf2_geometry_msgs
-  tf2_ros
+#  tf2_ros
   moveit_core
-  rviz_visual_tools
+#  rviz_visual_tools
-  moveit_visual_tools
+#  moveit_visual_tools
   moveit_ros_planning_interface
-  interactive_markers
+#  interactive_markers
   tf2_geometry_msgs
   moveit_ros_planning
-  pluginlib
+#  pluginlib
-  Eigen3
+#  Eigen3
-  Boost
+#  Boost
-  control_msgs
+#  control_msgs
-  moveit_servo
+#  moveit_servo
 )
 
-find_package(Eigen3 REQUIRED)
+#find_package(Eigen3 REQUIRED)
-find_package(Boost REQUIRED system filesystem date_time thread)
+#find_package(Boost REQUIRED system filesystem date_time thread)
 find_package(ament_cmake REQUIRED)
-find_package(control_msgs REQUIRED)
+#find_package(control_msgs REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
-find_package(moveit_ros_perception REQUIRED)
+#find_package(moveit_ros_perception REQUIRED)
-find_package(moveit_servo REQUIRED)
+#find_package(moveit_servo REQUIRED)
-find_package(interactive_markers REQUIRED)
+#find_package(interactive_markers REQUIRED)
-find_package(rviz_visual_tools REQUIRED)
+#find_package(rviz_visual_tools REQUIRED)
-find_package(moveit_visual_tools REQUIRED)
+#find_package(moveit_visual_tools REQUIRED)
-find_package(geometric_shapes REQUIRED)
+#find_package(geometric_shapes REQUIRED)
 #find_package(pcl_ros REQUIRED)
 #find_package(pcl_conversions REQUIRED)
 #find_package(rosbag REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rclcpp_action REQUIRED)
+#find_package(rclcpp_action REQUIRED)
-find_package(pluginlib REQUIRED)
+#find_package(pluginlib REQUIRED)
-find_package(tf2_ros REQUIRED)
+#find_package(tf2_ros REQUIRED)
-find_package(tf2_eigen REQUIRED)
+#find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
 # find dependencies

src/gazebo_ros_actor/CMakeLists.txt

@@ -8,7 +8,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rospy REQUIRED)
-find_package(geometry_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(gazebo REQUIRED)
 
 link_directories(${GAZEBO_LIBRARY_DIRS})
@@ -20,7 +20,7 @@ include_directories(
 
 add_library(RosActorPlugin SHARED src/RosActorPlugin.cc)
 target_link_libraries(RosActorPlugin ${GAZEBO_LIBRARIES})
-ament_target_dependencies(RosActorPlugin geometry_msgs rclcpp)
+ament_target_dependencies(RosActorPlugin tf2_geometry_msgs rclcpp)
 
 install(
         TARGETS

src/iisy_config/launch/lbr_move_group.launch.py

@@ -141,9 +141,9 @@ def launch_setup(context, *args, **kwargs):
 
     # MoveGroupInterface demo executable
     move_group_demo = Node(
-        name="btree_robot",
+        name="btree_nodes",
-        package="btree_robot",
+        package="btree_nodes",
-        executable="robot_test",
+        executable="tree",
         output="screen",
         parameters=[
             robot_description,