diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt index 6afedf1..61c8d61 100644 --- a/src/btree_nodes/CMakeLists.txt +++ b/src/btree_nodes/CMakeLists.txt @@ -41,7 +41,6 @@ set(CPP_FILES src/nodes/WeightedRandomNode.cpp src/nodes/AmICalled.cpp src/nodes/GenerateXYPose.cpp - src/nodes/MoveActorToTarget.cpp src/nodes/RobotMove.cpp src/nodes/MoveConnection.cpp src/nodes/OffsetPose.cpp diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp index 78c3e9c..f3fa6fa 100644 --- a/src/btree_nodes/src/Tree.cpp +++ b/src/btree_nodes/src/Tree.cpp @@ -10,7 +10,6 @@ #include "nodes/WeightedRandomNode.h" #include "nodes/AmICalled.h" #include "nodes/GenerateXYPose.h" -#include "nodes/MoveActorToTarget.h" #include "nodes/RobotMove.h" #include "nodes/OffsetPose.h" #include "nodes/RandomFailure.h" @@ -43,7 +42,6 @@ int main(int argc, char **argv) { factory.registerNodeType("GenerateXYPose"); factory.registerNodeType("AmICalled"); - factory.registerNodeType("MoveActorToTarget"); factory.registerNodeType("WeightedRandom"); factory.registerNodeType("OffsetPose"); factory.registerNodeType("RandomFailure"); diff --git a/src/btree_nodes/src/nodes/MoveActorToTarget.cpp b/src/btree_nodes/src/nodes/MoveActorToTarget.cpp deleted file mode 100644 index 4c4d968..0000000 --- a/src/btree_nodes/src/nodes/MoveActorToTarget.cpp +++ /dev/null @@ -1,64 +0,0 @@ -// -// Created by bastian on 26.03.22. -// - -#include "MoveActorToTarget.h" - -BT::MoveActorToTarget::MoveActorToTarget(const std::string &name, const BT::NodeConfiguration &config) - : StatefulActionNode(name, config) {} - -BT::PortsList BT::MoveActorToTarget::providedPorts() { - return { - InputPort>("current"), - InputPort>("target") - }; -} - -BT::NodeStatus BT::MoveActorToTarget::onStart() { - std::cout << "started moving" << std::endl; - - rclcpp::Node node("targetPublisherNode"); - auto publisher = node.create_publisher("targetPose", 10); - auto res = getInput>("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::MoveActorToTarget::onRunning() { - std::shared_ptr current; - - auto res = getInput>("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::MoveActorToTarget::onHalted() { - std::cout << "halted move" << std::endl; - rclcpp::Node node("targetPublisherNode"); - auto publisher = node.create_publisher("targetPose", 10); - geometry_msgs::msg::Pose inf; - inf.position.x = HUGE_VAL; - inf.position.y = HUGE_VAL; - publisher->publish(inf); -} diff --git a/src/btree_nodes/src/nodes/MoveActorToTarget.h b/src/btree_nodes/src/nodes/MoveActorToTarget.h deleted file mode 100644 index af011a1..0000000 --- a/src/btree_nodes/src/nodes/MoveActorToTarget.h +++ /dev/null @@ -1,30 +0,0 @@ -// -// Created by bastian on 26.03.22. -// - -#ifndef BUILD_MOVEACTORTOTARGET_H -#define BUILD_MOVEACTORTOTARGET_H - -#include -#include -#include - -namespace BT { - class MoveActorToTarget : public StatefulActionNode { - public: - MoveActorToTarget(const std::string &name, const NodeConfiguration &config); - - static PortsList providedPorts(); - - std::shared_ptr target; - - NodeStatus onStart() override; - - NodeStatus onRunning() override; - - void onHalted() override; - }; -} - - -#endif //BUILD_MOVEACTORTOTARGET_H