diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt index 84dd18c..832cb50 100644 --- a/src/btree_nodes/CMakeLists.txt +++ b/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}) diff --git a/src/btree_nodes/robotTree.xml b/src/btree_nodes/robotTree.xml index 32f5b52..543625a 100644 --- a/src/btree_nodes/robotTree.xml +++ b/src/btree_nodes/robotTree.xml @@ -4,7 +4,7 @@ - + diff --git a/src/btree_nodes/src/Extensions.cpp b/src/btree_nodes/src/Extensions.cpp index d540dfa..5cbcb8e 100644 --- a/src/btree_nodes/src/Extensions.cpp +++ b/src/btree_nodes/src/Extensions.cpp @@ -40,7 +40,7 @@ namespace BT { } template<> - geometry_msgs::msg::Pose_ > convertFromString(StringView str) { + std::shared_ptr 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 '||'."); } - 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]); + auto pose = std::make_shared(); + 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.z= convertFromString(parts[2]); std::cout << "[ generated pose from string ]" << std::endl; diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp index 8d2c7a8..cb24ca1 100644 --- a/src/btree_nodes/src/Tree.cpp +++ b/src/btree_nodes/src/Tree.cpp @@ -8,11 +8,25 @@ #include "nodes/MoveActorToTarget.h" #include "nodes/RobotMove.h" #include "helpers/MinimalSubscriber.h" +#include +#include -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({{1, 1}, @@ -34,10 +48,6 @@ int main(int argc, char **argv) { return std::make_unique(name, config, area); }; - rclcpp::init(argc, argv); - - BehaviorTreeFactory factory; - factory.registerBuilder("GenerateSafeTarget", builderGenerateSafeTarget); factory.registerBuilder("GenerateWarningTarget", builderGenerateWarningTarget); factory.registerBuilder("GenerateUnsafeTarget", builderGenerateUnsafeTarget); @@ -46,8 +56,15 @@ int main(int argc, char **argv) { factory.registerNodeType("MoveActorToTarget"); factory.registerNodeType("WeightedRandom"); + */ - factory.registerNodeType("RobotMove"); + auto connection = std::make_shared(mainNode,"iisy"); + + NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) { + return std::make_unique(name, config, &connection); + }; + + factory.registerBuilder("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; diff --git a/src/btree_nodes/src/helpers/MinimalSubscriber.h b/src/btree_nodes/src/helpers/MinimalSubscriber.h index b80baa4..821d7a4 100644 --- a/src/btree_nodes/src/helpers/MinimalSubscriber.h +++ b/src/btree_nodes/src/helpers/MinimalSubscriber.h @@ -39,7 +39,6 @@ std::thread MinimalSubscriber::createAsThread(const std::string &node_name, c auto subscriber = std::make_shared>(node_name, topic_name, callback); return std::thread([subscriber]() { rclcpp::spin(subscriber); - rclcpp::shutdown(); }); } diff --git a/src/btree_nodes/src/nodes/GenerateTarget2D.cpp b/src/btree_nodes/src/nodes/GenerateTarget2D.cpp index 01792e9..f481e77 100644 --- a/src/btree_nodes/src/nodes/GenerateTarget2D.cpp +++ b/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; diff --git a/src/btree_nodes/src/nodes/GenerateTarget2D.h b/src/btree_nodes/src/nodes/GenerateTarget2D.h index 83360f0..7169e57 100644 --- a/src/btree_nodes/src/nodes/GenerateTarget2D.h +++ b/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(); diff --git a/src/btree_nodes/src/nodes/MoveConnection.cpp b/src/btree_nodes/src/nodes/MoveConnection.cpp new file mode 100644 index 0000000..371d4b5 --- /dev/null +++ b/src/btree_nodes/src/nodes/MoveConnection.cpp @@ -0,0 +1,5 @@ +// +// Created by bastian on 28.03.22. +// + +#include "MoveConnection.h" diff --git a/src/btree_nodes/src/nodes/MoveConnection.h b/src/btree_nodes/src/nodes/MoveConnection.h new file mode 100644 index 0000000..37e7c4b --- /dev/null +++ b/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 +#include +#include + +class MoveConnection { +private: + moveit::planning_interface::MoveGroupInterface moveGroup; + std::thread running; + std::mutex lock; +public: + MoveConnection(const std::shared_ptr& node, const std::string& planningGroup): moveGroup(node,planningGroup) { + } + + void planAndExecute(const std::shared_ptr &pose,const std::function& 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 diff --git a/src/btree_nodes/src/nodes/RobotMove.cpp b/src/btree_nodes/src/nodes/RobotMove.cpp index 45d1ec6..9375693 100644 --- a/src/btree_nodes/src/nodes/RobotMove.cpp +++ b/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 *connection) : 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; + this->connection = connection; } 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; + std::shared_ptr pose; + if(!getInput>("target",pose)){ + std::cout << "no target available." << 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; - } + 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); + asyncResult = NodeStatus::RUNNING; + connection->get()->planAndExecute(pose,[this](bool finished){ + if(finished){ + asyncResult = NodeStatus::SUCCESS; + } else { + asyncResult = NodeStatus::FAILURE; + } + }); return NodeStatus::RUNNING; } +BT::NodeStatus BT::RobotMove::onRunning() { + auto result = asyncResult; + if(asyncResult!=NodeStatus::RUNNING){ + asyncResult=NodeStatus::IDLE; + } + return result; +} + void BT::RobotMove::onHalted() { - moveGroup->stop(); - asyncThread.join(); + connection->get()->stop(); + asyncResult=NodeStatus::IDLE; } BT::PortsList BT::RobotMove::providedPorts() { diff --git a/src/btree_nodes/src/nodes/RobotMove.h b/src/btree_nodes/src/nodes/RobotMove.h index d60e5b1..c37846e 100644 --- a/src/btree_nodes/src/nodes/RobotMove.h +++ b/src/btree_nodes/src/nodes/RobotMove.h @@ -10,20 +10,18 @@ #include #include #include +#include "MoveConnection.h" namespace BT { class RobotMove : public StatefulActionNode { private: - std::shared_ptr moveGroupNode; - std::shared_ptr moveGroup; - std::string planningGroup; - std::thread asyncThread; + std::shared_ptr *connection; 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 *connection); NodeStatus onStart() override; diff --git a/src/btree_robot/CMakeLists.txt b/src/btree_robot/CMakeLists.txt index 35ae210..c8303ad 100644 --- a/src/btree_robot/CMakeLists.txt +++ b/src/btree_robot/CMakeLists.txt @@ -58,11 +58,11 @@ find_package(behaviortree_cpp_v3 REQUIRED) # further dependencies manually. # find_package( REQUIRED) -add_executable(robot_test src/robot_test.cpp) -ament_target_dependencies(robot_test ${THIS_PACKAGE_INCLUDE_DEPENDS} std_msgs behaviortree_cpp_v3) +add_executable(tree src/robot_test.cpp) +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) diff --git a/src/btree_robot/src/robot_test.cpp b/src/btree_robot/src/robot_test.cpp index 883c98b..70d639f 100644 --- a/src/btree_robot/src/robot_test.cpp +++ b/src/btree_robot/src/robot_test.cpp @@ -40,78 +40,49 @@ // 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) -{ - 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); +int main(int argc, char **argv) { + int i; + printf("%d\n", argc); + for (i = 0; i < argc - 1; i++) { + printf("%s\n", argv[i]); + } - // 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(); + 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); - // BEGIN_TUTORIAL - // - // 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"; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_node); + std::thread([&executor]() { executor.spin(); }).detach(); - // The - // :moveit_codedir:`MoveGroupInterface` - // 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); + std::string PLANNING_GROUP = "iisy"; + moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); - // We will use the - // :moveit_codedir:`PlanningSceneInterface` - // class to add and remove collision objects in our "virtual world" scene - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + geometry_msgs::msg::Pose target_pose1; + target_pose1.orientation.w = 0; + target_pose1.orientation.y = 1; - // 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); + target_pose1.position.x = 0.2; + target_pose1.position.y = -0.2; + target_pose1.position.z = 1.2; - // 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"); + // 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. - // .. _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; + //RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); - target_pose1.position.x = 0.2; - target_pose1.position.y = -0.2; - target_pose1.position.z = 1.2; - move_group.setPoseTarget(target_pose1); + 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); + move_group.execute(my_plan); + }).join(); - // 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. - 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); - - rclcpp::shutdown(); - return 0; + rclcpp::shutdown(); + return 0; }