Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Created node for checking if in position

Browse Source
This commit is contained in:
Pimpest
2026-04-11 11:13:26 +02:00
parent 7edb62ef34
commit 94e7626fda
5 changed files with 136 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
toid_bt/src/bt_executor.cpp
View File

@@ -6,6 +6,7 @@
#include "rclcpp/rclcpp.hpp"
#include "toid_bt/plugins/empty_srv_action.hpp"
#include "toid_bt/plugins/end_calib_action.hpp"
#include "toid_bt/plugins/in_position_decorator.hpp"
#include "toid_bt/plugins/move_coords_action.hpp"
#include "toid_bt/plugins/rotate_action.hpp"
#include "toid_bt/plugins/rotate_towards_action.hpp"
@@ -32,6 +33,13 @@ TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts)
nav2_util::declare_parameter_if_not_declared(
node(), "global_frame", rclcpp::ParameterValue("map"));
node()->get_parameter("global_frame", global_frame_);
using namespace std::placeholders;
acorn_pose_sub_ = node()->create_subscription<geometry_msgs::msg::PoseStamped>(
"/closest_acorn", rclcpp::QoS(1), std::bind(&TreeExecutor::acorn_pose_cb, this, _1));
on_rotate_sub_ = node()->create_subscription<std_msgs::msg::String>(
"/to_flip", rclcpp::QoS(1), std::bind(&TreeExecutor::to_flip_cb, this, _1));
}
void TreeExecutor::onTreeCreated(BT::Tree & tree)
@@ -41,7 +49,12 @@ void TreeExecutor::onTreeCreated(BT::Tree & tree)
void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
{
FlipFunc get_to_flip = [this](std::string & pose) { pose = this->to_flip_; };
PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); };
PoseFunc get_acorn_pose = [this](geometry_msgs::msg::PoseStamped & pose) {
this->acorn_position(pose);
};
rclcpp::Node::SharedPtr nh = node();
factory.registerNodeType<toid::MovePointNode>(
"MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
@@ -62,16 +75,22 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
factory.registerNodeType<toid::EmptySrvNode>("Seq1", BT::RosNodeParams(nh, "/sequence1"));
BT::RosNodeParams service_params1(nh, "/sequence1");
service_params1.server_timeout = std::chrono::seconds(15);
factory.registerNodeType<toid::EmptySrvNode>("Seq1", service_params1);
BT::RosNodeParams service_params(nh, "/sequence2");
service_params.server_timeout = std::chrono::seconds(15);
factory.registerNodeType<toid::SendTextNode>("Seq2", service_params);
BT::RosNodeParams service_params2(nh, "/sequence2");
service_params2.server_timeout = std::chrono::seconds(20);
factory.registerNodeType<toid::SendTextNode>("Seq2", service_params2, get_to_flip);
factory.registerNodeType<toid::EmptySrvNode>("Seq3", BT::RosNodeParams(nh, "/sequence3"));
BT::RosNodeParams service_params3(nh, "/sequence3");
service_params3.server_timeout = std::chrono::seconds(15);
factory.registerNodeType<toid::EmptySrvNode>("Seq3", service_params3);
factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
factory.registerNodeType<toid::InPositionNode>("InPose", get_acorn_pose);
std::cout << describeCustomNodes() << std::endl;
}
@@ -80,6 +99,23 @@ void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose)
nav2_util::getCurrentPose(pose, *tf_buffer_, global_frame_, base_frame_);
}
void TreeExecutor::acorn_position(geometry_msgs::msg::PoseStamped & pose) { pose = acorn_pose_; }
void TreeExecutor::acorn_pose_cb(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
{
auto node = this->node();
try {
acorn_pose_ = tf_buffer_->transform(*msg, "base_footprint");
} catch (const tf2::TransformException & e) {
RCLCPP_ERROR_THROTTLE(node->get_logger(), *node->get_clock(), 5000, "Transform timeout");
}
}
void TreeExecutor::to_flip_cb(std_msgs::msg::String::ConstSharedPtr msg)
{
this->to_flip_ = msg->data;
}
std::optional<std::string> TreeExecutor::onTreeExecutionCompleted(
BT::NodeStatus status, bool was_cancelled)
{