From e8ecafc194f06992bdbe2ac6b51782253abc115f Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Sun, 27 Mar 2022 19:26:14 +0200 Subject: [PATCH] Created a nonfunctional node for robot movement --- src/btree_nodes/CMakeLists.txt | 61 ++++++++++----- src/btree_nodes/{tree.xml => actorTree.xml} | 0 src/btree_nodes/nodes.xml | 10 ++- src/btree_nodes/robotTree.xml | 42 ++++++++++ src/btree_nodes/src/Extensions.cpp | 29 ++++++- src/btree_nodes/src/Tree.cpp | 36 ++++++--- src/btree_nodes/src/nodes/RobotMove.cpp | 78 +++++++++++++++++++ src/btree_nodes/src/nodes/RobotMove.h | 24 +++++- .../src/nodes/WeightedRandomNode.cpp | 1 + src/btree_robot/CMakeLists.txt | 44 +++++------ src/gazebo_ros_actor/CMakeLists.txt | 4 +- .../launch/lbr_move_group.launch.py | 6 +- 12 files changed, 269 insertions(+), 66 deletions(-) rename src/btree_nodes/{tree.xml => actorTree.xml} (100%) create mode 100644 src/btree_nodes/robotTree.xml diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt index 49dd0ca..84dd18c 100644 --- a/src/btree_nodes/CMakeLists.txt +++ b/src/btree_nodes/CMakeLists.txt @@ -8,18 +8,41 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) # find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +set(DEPENDENCIES + ament_cmake + rclcpp + #geometry_msgs + 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( 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) -ament_target_dependencies(tree rclcpp geometry_msgs std_msgs behaviortree_cpp_v3) +set(CPP_FILES + 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) @@ -27,18 +50,18 @@ ament_target_dependencies(tree rclcpp geometry_msgs std_msgs behaviortree_cpp_v3 #ament_target_dependencies(listener geometry_msgs rclcpp) install(TARGETS - tree - DESTINATION lib/${PROJECT_NAME}) + tree + DESTINATION lib/${PROJECT_NAME}) -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 - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # 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() +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 + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # 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 () ament_package() diff --git a/src/btree_nodes/tree.xml b/src/btree_nodes/actorTree.xml similarity index 100% rename from src/btree_nodes/tree.xml rename to src/btree_nodes/actorTree.xml diff --git a/src/btree_nodes/nodes.xml b/src/btree_nodes/nodes.xml index 2b3fdd5..bfa6dd8 100644 --- a/src/btree_nodes/nodes.xml +++ b/src/btree_nodes/nodes.xml @@ -30,8 +30,14 @@ Weights for the children, separated by semicolon. - - + + Bounds to check in + Position of object + Result of check if position is within bounds + + + Target pose of robot. + diff --git a/src/btree_nodes/robotTree.xml b/src/btree_nodes/robotTree.xml new file mode 100644 index 0000000..32f5b52 --- /dev/null +++ b/src/btree_nodes/robotTree.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + called status + + + Generated pose in safe area + + + Generated pose in unsafe area + + + Generated pose in warning area + + + Bounds to check in + Position of object + Result of check if position is within bounds + + + Current actor position + Target to move actor to + + + Target pose of robot. + + + Weights for the children, separated by semicolon. + + + + + diff --git a/src/btree_nodes/src/Extensions.cpp b/src/btree_nodes/src/Extensions.cpp index 4880a7f..d540dfa 100644 --- a/src/btree_nodes/src/Extensions.cpp +++ b/src/btree_nodes/src/Extensions.cpp @@ -3,10 +3,11 @@ // #include "Area.h" +#include 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_ > 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 '||'."); + } + + 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(parts[0]); + pose.position.y= convertFromString(parts[1]); + pose.position.y= convertFromString(parts[2]); + + std::cout << "[ generated pose from string ]" << std::endl; + + return pose; + } } \ No newline at end of file diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp index 057ccac..8d2c7a8 100644 --- a/src/btree_nodes/src/Tree.cpp +++ b/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("GenerateSafeTarget", builderGenerateSafeTarget); factory.registerBuilder("GenerateWarningTarget", builderGenerateWarningTarget); factory.registerBuilder("GenerateUnsafeTarget", builderGenerateUnsafeTarget); @@ -45,39 +47,49 @@ int main(int argc, char **argv) { factory.registerNodeType("MoveActorToTarget"); factory.registerNodeType("WeightedRandom"); + factory.registerNodeType("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("current",init); - tree.rootBlackboard()->set("target",init); - tree.rootBlackboard()->set("called",false); + actorTree.rootBlackboard()->set("current",init); + actorTree.rootBlackboard()->set("target",init); + actorTree.rootBlackboard()->set("called",false); MinimalSubscriber::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::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(); + actorTree.tickRoot(); + mutex.unlock(); + } + //});*/ + while (rclcpp::ok()) { - mutex.lock(); - tree.tickRoot(); - mutex.unlock(); + robotTree.tickRoot(); } std::cout << "End." << std::endl; diff --git a/src/btree_nodes/src/nodes/RobotMove.cpp b/src/btree_nodes/src/nodes/RobotMove.cpp index 12a66eb..45d1ec6 100644 --- a/src/btree_nodes/src/nodes/RobotMove.cpp +++ b/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(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("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("target", description)}; +} \ No newline at end of file diff --git a/src/btree_nodes/src/nodes/RobotMove.h b/src/btree_nodes/src/nodes/RobotMove.h index 4fb742e..d60e5b1 100644 --- a/src/btree_nodes/src/nodes/RobotMove.h +++ b/src/btree_nodes/src/nodes/RobotMove.h @@ -7,15 +7,31 @@ #include #include +#include +#include +#include namespace BT { class RobotMove : public StatefulActionNode { - public: - RobotMove(const std::string &name, const BT::NodeConfiguration &config) - : StatefulActionNode(name, config) { + private: + std::shared_ptr moveGroupNode; + std::shared_ptr 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(); }; } diff --git a/src/btree_nodes/src/nodes/WeightedRandomNode.cpp b/src/btree_nodes/src/nodes/WeightedRandomNode.cpp index a0003d3..2ddac73 100644 --- a/src/btree_nodes/src/nodes/WeightedRandomNode.cpp +++ b/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(); diff --git a/src/btree_robot/CMakeLists.txt b/src/btree_robot/CMakeLists.txt index 478a07f..35ae210 100644 --- a/src/btree_robot/CMakeLists.txt +++ b/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 - moveit_visual_tools +# rviz_visual_tools +# moveit_visual_tools moveit_ros_planning_interface - interactive_markers +# interactive_markers tf2_geometry_msgs moveit_ros_planning - pluginlib - Eigen3 - Boost - control_msgs - moveit_servo +# pluginlib +# Eigen3 +# Boost +# control_msgs +# moveit_servo ) -find_package(Eigen3 REQUIRED) -find_package(Boost REQUIRED system filesystem date_time thread) +#find_package(Eigen3 REQUIRED) +#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_servo REQUIRED) -find_package(interactive_markers REQUIRED) -find_package(rviz_visual_tools REQUIRED) -find_package(moveit_visual_tools REQUIRED) -find_package(geometric_shapes REQUIRED) +#find_package(moveit_ros_perception REQUIRED) +#find_package(moveit_servo REQUIRED) +#find_package(interactive_markers REQUIRED) +#find_package(rviz_visual_tools REQUIRED) +#find_package(moveit_visual_tools 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(pluginlib REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_eigen REQUIRED) +#find_package(rclcpp_action REQUIRED) +#find_package(pluginlib REQUIRED) +#find_package(tf2_ros REQUIRED) +#find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) # find dependencies diff --git a/src/gazebo_ros_actor/CMakeLists.txt b/src/gazebo_ros_actor/CMakeLists.txt index 7d15d78..c10bc38 100644 --- a/src/gazebo_ros_actor/CMakeLists.txt +++ b/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 diff --git a/src/iisy_config/launch/lbr_move_group.launch.py b/src/iisy_config/launch/lbr_move_group.launch.py index 0028a3c..c083042 100644 --- a/src/iisy_config/launch/lbr_move_group.launch.py +++ b/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", - package="btree_robot", - executable="robot_test", + name="btree_nodes", + package="btree_nodes", + executable="tree", output="screen", parameters=[ robot_description,