75 lines
3.0 KiB
C++
75 lines
3.0 KiB
C++
#include "mg_tree_executor.hpp"
|
|
|
|
#include "behaviortree_cpp/xml_parsing.h"
|
|
#include "tree_nodes/calib.hpp"
|
|
#include "tree_nodes/choose_can.hpp"
|
|
#include "tree_nodes/choose_tactic.hpp"
|
|
#include "tree_nodes/i2c.hpp"
|
|
#include "tree_nodes/move_point.hpp"
|
|
#include "tree_nodes/move_local.hpp"
|
|
#include "tree_nodes/move_global.hpp"
|
|
#include "tree_nodes/look_at.hpp"
|
|
#include "tree_nodes/rotate.hpp"
|
|
#include "tree_nodes/side_pub.hpp"
|
|
#include "tree_nodes/set_pos.hpp"
|
|
#include "tree_nodes/zero.hpp"
|
|
|
|
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
|
#include "tf2/LinearMath/Transform.h"
|
|
#include "tf2/convert.h"
|
|
|
|
namespace mg {
|
|
MgTreeExecutor::MgTreeExecutor(const rclcpp::NodeOptions opts) : BT::TreeExecutionServer(opts) {
|
|
executeRegistration();
|
|
std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
|
|
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
|
|
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
|
|
|
const std::function<std::pair<glm::vec2, double>()> func = std::bind(&MgTreeExecutor::position, this);
|
|
}
|
|
|
|
void MgTreeExecutor::onTreeCreated(BT::Tree& tree) { logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree); }
|
|
|
|
void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
|
|
factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
|
|
factory.registerNodeType<mg::MoveGlobalNode>("MoveGlobal", node());
|
|
factory.registerNodeType<mg::RotateNode>("RotateNode", node());
|
|
factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
|
|
factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
|
|
factory.registerNodeType<mg::SidePub>("SideObstaclePub", node());
|
|
factory.registerNodeType<mg::SendPoseNode>("SendPose", node());
|
|
factory.registerNodeType<mg::LookAtNode>("LookAtNode", node());
|
|
factory.registerNodeType<mg::MoveLocalNode>("MoveLocal", node(), [this]() { return this->position(); });
|
|
factory.registerNodeType<mg::CanChooser>("CanChooser");
|
|
factory.registerNodeType<mg::TacticsChooser>("TacticChooser");
|
|
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
|
|
}
|
|
|
|
std::pair<glm::vec2, double> MgTreeExecutor::position() {
|
|
double x = NAN;
|
|
double y = NAN;
|
|
double theta;
|
|
glm::vec2 pos;
|
|
|
|
auto tmsg = tf_buffer_->lookupTransform("odom", "base-link", tf2::TimePointZero);
|
|
|
|
tf2::Transform t;
|
|
tf2::convert(tmsg.transform, t);
|
|
t.getBasis().getRPY(x, y, theta);
|
|
auto vec3 = tmsg.transform.translation;
|
|
|
|
pos = glm::dvec2(vec3.x, vec3.y);
|
|
return { pos, theta };
|
|
}
|
|
|
|
std::optional<std::string> MgTreeExecutor::onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) {
|
|
(void)status;
|
|
logger_cout_.reset();
|
|
|
|
if (was_cancelled) {
|
|
std::cout << "Behavior tree was cancled" << std::endl;
|
|
}
|
|
|
|
return std::nullopt;
|
|
}
|
|
} |