yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Removed old MoveActorToTarget

Browse Source
This commit is contained in:
Bastian Hofmann 2023-03-24 13:52:40 +00:00
parent b8112a36b3
commit 6bda1077c2
4 changed files with 0 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/btree_nodes/CMakeLists.txt
View File

@ -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

2
src/btree_nodes/src/Tree.cpp
View File

@ -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>("GenerateXYPose");
factory.registerNodeType<AmICalled>("AmICalled");
factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
factory.registerNodeType<OffsetPose>("OffsetPose");
factory.registerNodeType<RandomFailure>("RandomFailure");

64
src/btree_nodes/src/nodes/MoveActorToTarget.cpp
View File

@ -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<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
};
}
BT::NodeStatus BT::MoveActorToTarget::onStart() {
std::cout << "started moving" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("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<geometry_msgs::msg::Pose> current;
auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("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<geometry_msgs::msg::Pose>("targetPose", 10);
geometry_msgs::msg::Pose inf;
inf.position.x = HUGE_VAL;
inf.position.y = HUGE_VAL;
publisher->publish(inf);
}

30
src/btree_nodes/src/nodes/MoveActorToTarget.h
View File

@ -1,30 +0,0 @@
//
// Created by bastian on 26.03.22.
//
#ifndef BUILD_MOVEACTORTOTARGET_H
#define BUILD_MOVEACTORTOTARGET_H
#include <behaviortree_cpp_v3/action_node.h>
#include <geometry_msgs/msg/pose.hpp>
#include <rclcpp/rclcpp.hpp>
namespace BT {
class MoveActorToTarget : public StatefulActionNode {
public:
MoveActorToTarget(const std::string &name, const NodeConfiguration &config);
static PortsList providedPorts();
std::shared_ptr<geometry_msgs::msg::Pose> target;
NodeStatus onStart() override;
NodeStatus onRunning() override;
void onHalted() override;
};
}
#endif //BUILD_MOVEACTORTOTARGET_H