From 7f9b30083f196b42a0bd871c6557a5a36c81ad3f Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Fri, 1 Apr 2022 22:43:19 +0200 Subject: [PATCH] More Nodes for robot interaction, bugfixing, refactoring. probably final version --- src/btree_nodes/CMakeLists.txt | 5 +- src/btree_nodes/actorTree.xml | 41 ++++++--- src/btree_nodes/nodes.xml | 25 ++++-- src/btree_nodes/robotTree.xml | 62 ++++++++----- src/btree_nodes/src/Tree.cpp | 90 ++++++++++--------- src/btree_nodes/src/nodes/GenerateXYPose.cpp | 2 +- .../nodes/{IsInArea.cpp => InAreaTest.cpp} | 16 ++-- .../src/nodes/{IsInArea.h => InAreaTest.h} | 10 +-- .../src/nodes/InterruptableSequence.cpp | 40 +++++++++ .../src/nodes/InterruptableSequence.h | 29 ++++++ src/btree_nodes/src/nodes/MoveConnection.cpp | 37 +++++++- src/btree_nodes/src/nodes/MoveConnection.h | 6 ++ .../src/nodes/SetRobotVelocity.cpp | 27 ++++++ src/btree_nodes/src/nodes/SetRobotVelocity.h | 27 ++++++ .../src/nodes/WeightedRandomNode.cpp | 3 - .../src/nodes/WeightedRandomNode.h | 6 +- 16 files changed, 319 insertions(+), 107 deletions(-) rename src/btree_nodes/src/nodes/{IsInArea.cpp => InAreaTest.cpp} (68%) rename src/btree_nodes/src/nodes/{IsInArea.h => InAreaTest.h} (56%) create mode 100644 src/btree_nodes/src/nodes/InterruptableSequence.cpp create mode 100644 src/btree_nodes/src/nodes/InterruptableSequence.h create mode 100644 src/btree_nodes/src/nodes/SetRobotVelocity.cpp create mode 100644 src/btree_nodes/src/nodes/SetRobotVelocity.h diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt index d384636..cfe7999 100644 --- a/src/btree_nodes/CMakeLists.txt +++ b/src/btree_nodes/CMakeLists.txt @@ -6,6 +6,7 @@ add_compile_options(-Wall -Wextra -Wpedantic) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) + # find dependencies set(DEPENDENCIES @@ -42,7 +43,9 @@ set(CPP_FILES src/nodes/MoveConnection.cpp src/nodes/OffsetPose.cpp src/nodes/RandomFailure.cpp - src/nodes/IsInArea.cpp + src/nodes/InAreaTest.cpp + src/nodes/InterruptableSequence.cpp + src/nodes/SetRobotVelocity.cpp ) add_executable(tree ${CPP_FILES}) diff --git a/src/btree_nodes/actorTree.xml b/src/btree_nodes/actorTree.xml index 8fbb3fb..b8d33b2 100644 --- a/src/btree_nodes/actorTree.xml +++ b/src/btree_nodes/actorTree.xml @@ -2,31 +2,41 @@ - - + + + + + + + + + + + + + + + - - - - - - + + + - + - - called status + + + state to set called to Area to generate pose in Generated pose in area - Bounds to check in - Position of object - Result of check if position is within bounds + Bounds to check in + Position of object Current actor position @@ -38,6 +48,9 @@ generated new pose initial position as Position2D + + Target velocity of robot + Chance to fail from 0 to 1 diff --git a/src/btree_nodes/nodes.xml b/src/btree_nodes/nodes.xml index 7d94d0b..e420f8f 100644 --- a/src/btree_nodes/nodes.xml +++ b/src/btree_nodes/nodes.xml @@ -7,8 +7,11 @@ - - called status + + + + + state to set called to @@ -25,21 +28,27 @@ Weights for the children, separated by semicolon. + + + + + Target velocity of robot + + - Bounds to check in - Position of object - Result of check if position is within bounds + Area to check in + Pose of object Target pose of robot. - - initial position as Position2D + + initial position as Pose offset as a Point rotation of resulting pose as Quaternion - generated new pose + generated new pose diff --git a/src/btree_nodes/robotTree.xml b/src/btree_nodes/robotTree.xml index b5c3938..b36ee65 100644 --- a/src/btree_nodes/robotTree.xml +++ b/src/btree_nodes/robotTree.xml @@ -2,42 +2,53 @@ - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + - - called status + + + state to set called to - - Generated pose in safe area - - - Generated pose in unsafe area - - - Generated pose in warning area + + Area to generate pose in + Generated pose in area - Bounds to check in - Position of object - Result of check if position is within bounds + Bounds to check in + Position of object Current actor position Target to move actor to - + + initial position as Pose offset as a Point rotation of resulting pose as Quaternion - generated new pose - initial position as Position2D + generated new pose Chance to fail from 0 to 1 @@ -45,9 +56,14 @@ Target pose of robot. + + Target velocity of robot + Weights for the children, separated by semicolon. + + diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp index 5451362..6eb6275 100644 --- a/src/btree_nodes/src/Tree.cpp +++ b/src/btree_nodes/src/Tree.cpp @@ -10,13 +10,13 @@ #include "helpers/MinimalSubscriber.h" #include "nodes/OffsetPose.h" #include "nodes/RandomFailure.h" -#include "nodes/IsInArea.h" +#include "nodes/InAreaTest.h" +#include "nodes/InterruptableSequence.h" +#include "nodes/SetRobotVelocity.h" #include #include -//using namespace BT; - int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; @@ -42,22 +42,22 @@ int main(int argc, char **argv) { const std::shared_ptr warningArea = std::make_shared(); warningArea->triangles = {std::vector({ - {-1, 1.5}, - {-1, 2.5}, - {1, 1.5}, - {2, 2.5}, - {1, -1}, - {2, -1} - })}; - - const std::shared_ptr unsafeArea = std::make_shared(); - unsafeArea->triangles = std::vector({ {-1, 2.5}, {-1, 3.5}, {2, 2.5}, {3, 3.5}, {2, -1}, {3, -1} + })}; + + const std::shared_ptr unsafeArea = std::make_shared(); + unsafeArea->triangles = std::vector({ + {-1, 1.5}, + {-1, 2.5}, + {1, 1.5}, + {2, 2.5}, + {1, -1}, + {2, -1} }); const std::shared_ptr negativeYTable = std::make_shared(); @@ -83,69 +83,79 @@ int main(int argc, char **argv) { factory.registerNodeType("WeightedRandom"); factory.registerNodeType("OffsetPose"); factory.registerNodeType("RandomFailure"); - factory.registerNodeType("IsInArea"); + factory.registerNodeType("InAreaTest"); + + factory.registerNodeType("InterruptableSequence"); 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); + NodeBuilder builderIisyVelocity = [&connection](const std::string &name, const NodeConfiguration &config) { + return std::make_unique(name, config, &connection); + }; + factory.registerBuilder("SetRobotVelocity", builderIisyVelocity); + + bool called; + auto IsCalled = [&called](TreeNode& parent_node) -> NodeStatus{ + return called ? NodeStatus::SUCCESS : NodeStatus::FAILURE; + }; + + auto SetCalledTo = [&called](TreeNode& parent_node) -> NodeStatus{ + if(!parent_node.getInput("state",called)){ + std::cout<<"no state supplied."<(); + for (const auto &item : trees){ item->set("safeArea",safeArea); item->set("warningArea",warningArea); item->set("unsafeArea",unsafeArea); item->set("negativeYTable",negativeYTable); item->set("positiveYTable",positiveYTable); - } - std::mutex mutex; + item->set("actorPos", init); + } std::cout << "Starting subscriber." << std::endl; - auto init = std::make_shared(); - actorTree.rootBlackboard()->set("current", init); - actorTree.rootBlackboard()->set("target", init); - actorTree.rootBlackboard()->set("called", false); - MinimalSubscriber::createAsThread("tree_get_currentPose", "currentPose", - [&actorTree, &mutex](geometry_msgs::msg::Pose pose) { + [&trees](geometry_msgs::msg::Pose pose) { auto sharedPose = std::make_shared(pose); - mutex.lock(); - actorTree.rootBlackboard()->set("current", sharedPose); - mutex.unlock(); + for (const auto &item : trees){ + item->set("actorPos", sharedPose); + } }).detach(); - MinimalSubscriber::createAsThread("tree_get_called", "called", - [&actorTree, &mutex](std_msgs::msg::Bool called) { - mutex.lock(); - actorTree.rootBlackboard()->set("called", - (bool) called.data); - mutex.unlock(); - }).detach(); - - std::cout << "Looping." << std::endl; - std::thread actor([&actorTree, &mutex]() { + std::thread actor([&actorTree]() { while (rclcpp::ok()) { - mutex.lock(); actorTree.tickRoot(); - mutex.unlock(); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); } }); while (rclcpp::ok()) { robotTree.tickRoot(); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); } std::cout << "End." << std::endl; diff --git a/src/btree_nodes/src/nodes/GenerateXYPose.cpp b/src/btree_nodes/src/nodes/GenerateXYPose.cpp index 96e00f1..0c8f5bf 100644 --- a/src/btree_nodes/src/nodes/GenerateXYPose.cpp +++ b/src/btree_nodes/src/nodes/GenerateXYPose.cpp @@ -69,7 +69,7 @@ BT::NodeStatus BT::GenerateXYPose::tick() { double triangleRandom = dist(re); unsigned long i = 0; - while (triangleRandom < weights[i + 1] && i < weights.size() - 1) { + while (triangleRandom > weights[i + 1] && i < weights.size() - 1) { i++; } auto pos = getRandomPosInTriangle(area,i); diff --git a/src/btree_nodes/src/nodes/IsInArea.cpp b/src/btree_nodes/src/nodes/InAreaTest.cpp similarity index 68% rename from src/btree_nodes/src/nodes/IsInArea.cpp rename to src/btree_nodes/src/nodes/InAreaTest.cpp index 6b37fd8..42d7d49 100644 --- a/src/btree_nodes/src/nodes/IsInArea.cpp +++ b/src/btree_nodes/src/nodes/InAreaTest.cpp @@ -2,9 +2,9 @@ // Created by bastian on 30.03.22. // -#include "IsInArea.h" +#include "InAreaTest.h" -BT::PortsList BT::IsInArea::providedPorts() { +BT::PortsList BT::InAreaTest::providedPorts() { return { InputPort>("area"), InputPort>("pose") @@ -23,26 +23,28 @@ bool pointInTri(Position2D point, Position2D triA, Position2D triB, Position2D t return !(((d1 < 0) || (d2 < 0) || (d3 < 0)) && ((d1 > 0) || (d2 > 0) || (d3 > 0))); } -BT::IsInArea::IsInArea(const std::string &name, const BT::NodeConfiguration &config) : ConditionNode(name, config) {} +BT::InAreaTest::InAreaTest(const std::string &name, const BT::NodeConfiguration &config) : ConditionNode(name, config) {} -BT::NodeStatus BT::IsInArea::tick() { +BT::NodeStatus BT::InAreaTest::tick() { std::shared_ptr area; if (!getInput>("area", area)) { + std::cout<<"No area given"< pose; if (!getInput>("pose", pose)) { + std::cout<<"No pose given"<position.x, pose->position.y}; for (unsigned long i = 0; i < area->triangles.size() - 2; i++) { - if (!pointInTri(point, area->triangles[i], area->triangles[i + 1], area->triangles[i + 1])) { - return NodeStatus::FAILURE; + if (pointInTri(point, area->triangles[i], area->triangles[i + 1], area->triangles[i + 2])) { + return NodeStatus::SUCCESS; } } - return NodeStatus::SUCCESS; + return NodeStatus::FAILURE; } diff --git a/src/btree_nodes/src/nodes/IsInArea.h b/src/btree_nodes/src/nodes/InAreaTest.h similarity index 56% rename from src/btree_nodes/src/nodes/IsInArea.h rename to src/btree_nodes/src/nodes/InAreaTest.h index f1ab1c2..b90cf20 100644 --- a/src/btree_nodes/src/nodes/IsInArea.h +++ b/src/btree_nodes/src/nodes/InAreaTest.h @@ -2,8 +2,8 @@ // Created by bastian on 30.03.22. // -#ifndef BUILD_ISINAREA_H -#define BUILD_ISINAREA_H +#ifndef BUILD_INAREATEST_H +#define BUILD_INAREATEST_H #include #include @@ -11,9 +11,9 @@ namespace BT { - class IsInArea : public ConditionNode { + class InAreaTest : public ConditionNode { public: - IsInArea(const std::string &name, const NodeConfiguration &config); + InAreaTest(const std::string &name, const NodeConfiguration &config); static PortsList providedPorts(); @@ -22,4 +22,4 @@ namespace BT { } -#endif //BUILD_ISINAREA_H +#endif //BUILD_INAREATEST_H diff --git a/src/btree_nodes/src/nodes/InterruptableSequence.cpp b/src/btree_nodes/src/nodes/InterruptableSequence.cpp new file mode 100644 index 0000000..8d9dab8 --- /dev/null +++ b/src/btree_nodes/src/nodes/InterruptableSequence.cpp @@ -0,0 +1,40 @@ +// +// Created by bastian on 01.04.22. +// + +#include "InterruptableSequence.h" + +BT::InterruptableSequence::InterruptableSequence(const std::string &name, const NodeConfiguration &config) + : ControlNode::ControlNode(name, config) { +} + +void BT::InterruptableSequence::halt() { + ControlNode::halt(); +} + +BT::NodeStatus BT::InterruptableSequence::tick() { + auto result = children_nodes_[position]->executeTick(); + + setStatus(NodeStatus::RUNNING); + + if(result==NodeStatus::FAILURE){ + haltChildren(); + position=0; + return NodeStatus::FAILURE; + } + + if(result==NodeStatus::SUCCESS){ + position++; + if(position>=children_nodes_.size()){ + haltChildren(); + position=0; + return NodeStatus::SUCCESS; + } + } + + return NodeStatus::RUNNING; +} + +BT::PortsList BT::InterruptableSequence::providedPorts() { + return {}; +} \ No newline at end of file diff --git a/src/btree_nodes/src/nodes/InterruptableSequence.h b/src/btree_nodes/src/nodes/InterruptableSequence.h new file mode 100644 index 0000000..bedbee6 --- /dev/null +++ b/src/btree_nodes/src/nodes/InterruptableSequence.h @@ -0,0 +1,29 @@ +// +// Created by bastian on 01.04.22. +// + +#ifndef BUILD_INTERRUPTABLESEQUENCE_H +#define BUILD_INTERRUPTABLESEQUENCE_H + +#include + +namespace BT { + class InterruptableSequence : public ControlNode{ + public: + InterruptableSequence(const std::string &name, const NodeConfiguration &config); + + ~InterruptableSequence() override = default; + + void halt() override; + + static PortsList providedPorts(); + + BT::NodeStatus tick() override; + + private: + unsigned long position=0; + }; +} + + +#endif //BUILD_INTERRUPTABLESEQUENCE_H diff --git a/src/btree_nodes/src/nodes/MoveConnection.cpp b/src/btree_nodes/src/nodes/MoveConnection.cpp index cde8a26..ae87a70 100644 --- a/src/btree_nodes/src/nodes/MoveConnection.cpp +++ b/src/btree_nodes/src/nodes/MoveConnection.cpp @@ -7,6 +7,11 @@ void MoveConnection::planAndExecute(const std::shared_ptr &pose, const std::function &callback) { lock.lock(); + 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); @@ -19,17 +24,45 @@ void MoveConnection::planAndExecute(const std::shared_ptrmoveGroup.move() == moveit::planning_interface::MoveItErrorCode::SUCCESS; - callback(success); + moveit::planning_interface::MoveGroupInterface::Plan plan; + moveGroup.plan(plan); + if(cancelled){ + lastTarget = nullptr; + lock.unlock(); + return; + } + bool success = this->moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS; + if(!cancelled) { + callback(success); + } + lastTarget = nullptr; lock.unlock(); }).detach(); } +void MoveConnection::setSpeedMultiplier(const double speed){ + if(currentSpeed!=speed) { + std::cout<<"Changing speed."< &node, const std::string &planningGroup) : moveGroup(node,planningGroup) { } void MoveConnection::stop() { + std::cout<<"Stopping"< lastTarget = nullptr; + std::function lastCallback = [](bool finished) {}; + bool cancelled=false; + double currentSpeed = 1; std::mutex lock; public: MoveConnection(const std::shared_ptr& node, const std::string& planningGroup); @@ -20,6 +24,8 @@ public: void planAndExecute(const std::shared_ptr &pose,const std::function &callback); void stop(); + + void setSpeedMultiplier(double speed); }; diff --git a/src/btree_nodes/src/nodes/SetRobotVelocity.cpp b/src/btree_nodes/src/nodes/SetRobotVelocity.cpp new file mode 100644 index 0000000..fa4a9a2 --- /dev/null +++ b/src/btree_nodes/src/nodes/SetRobotVelocity.cpp @@ -0,0 +1,27 @@ +// +// Created by bastian on 01.04.22. +// + +#include "SetRobotVelocity.h" + + +BT::NodeStatus BT::SetRobotVelocity::tick() { + double velocity; + if(!getInput("velocity",velocity)){ + std::cout<<"No velocity given."<get()->setSpeedMultiplier(velocity); + return NodeStatus::SUCCESS; +} + +BT::PortsList BT::SetRobotVelocity::providedPorts() { + return { + InputPort("velocity"), + }; +} + +BT::SetRobotVelocity::SetRobotVelocity(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr *connection) : SyncActionNode( + name, config) { + this->connection = connection; +} diff --git a/src/btree_nodes/src/nodes/SetRobotVelocity.h b/src/btree_nodes/src/nodes/SetRobotVelocity.h new file mode 100644 index 0000000..7324050 --- /dev/null +++ b/src/btree_nodes/src/nodes/SetRobotVelocity.h @@ -0,0 +1,27 @@ +// +// Created by bastian on 01.04.22. +// + +#ifndef BUILD_SETROBOTVELOCITY_H +#define BUILD_SETROBOTVELOCITY_H + +#include +#include "MoveConnection.h" + +namespace BT { + class SetRobotVelocity: public SyncActionNode { + private: + std::shared_ptr *connection{}; + + public: + SetRobotVelocity(const std::string &name, const NodeConfiguration &config, + std::shared_ptr *connection); + + static PortsList providedPorts(); + + NodeStatus tick() override; + }; +} + + +#endif //BUILD_SETROBOTVELOCITY_H diff --git a/src/btree_nodes/src/nodes/WeightedRandomNode.cpp b/src/btree_nodes/src/nodes/WeightedRandomNode.cpp index b4c134a..a2a774e 100644 --- a/src/btree_nodes/src/nodes/WeightedRandomNode.cpp +++ b/src/btree_nodes/src/nodes/WeightedRandomNode.cpp @@ -41,7 +41,6 @@ NodeStatus WeightedRandomNode::tick() { } if(weights.size()!=children_count){ - std::cout<< "Number of weights doesn't match child count." <executeTick(); if (child_status != NodeStatus::RUNNING) { select_next = true; - printf("node %zu finished.\n",selected_child_index); } return child_status; diff --git a/src/btree_nodes/src/nodes/WeightedRandomNode.h b/src/btree_nodes/src/nodes/WeightedRandomNode.h index aa7d54c..ecb5819 100644 --- a/src/btree_nodes/src/nodes/WeightedRandomNode.h +++ b/src/btree_nodes/src/nodes/WeightedRandomNode.h @@ -17,9 +17,9 @@ class WeightedRandomNode : public ControlNode public: WeightedRandomNode(const std::string& name, const NodeConfiguration &config); - virtual ~WeightedRandomNode() override = default; + ~WeightedRandomNode() override = default; - virtual void halt() override; + void halt() override; static PortsList providedPorts(); private: @@ -27,7 +27,7 @@ private: bool select_next{}; std::mt19937_64 rng; - virtual BT::NodeStatus tick() override; + BT::NodeStatus tick() override; };