Open
Description
I'm running in a weird behavior when trying to halting an action node.
Setup:
BehaviorTree.ROS2 branch humble bb0d510
BehaviorTree.CPP branch master acbc707
Structure:
ws
|- BehaviorTree.ROS2
|- BehaviorTree.CPP
|- test
|- CMakeLists.txt
|- package.xml
|- src
|- bt_act_test.cpp
CMakeLists.txt:
cmake_minimum_required(VERSION 3.8)
project(bt_act_test)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED )
find_package(rclcpp_action REQUIRED )
find_package(behaviortree_cpp REQUIRED )
find_package(behaviortree_ros2 REQUIRED )
find_package(example_interfaces REQUIRED)
add_executable(bt_act_test src/bt_act_test.cpp)
target_include_directories(bt_act_test PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
ament_target_dependencies(bt_act_test
rclcpp
rclcpp_action
behaviortree_cpp
behaviortree_ros2
example_interfaces
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
install(TARGETS
bt_act_test
DESTINATION lib/${PROJECT_NAME})
ament_package()
package.xml:
...
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>behaviortree_ros2</depend>
<depend>behaviortree_cpp</depend>
<depend>example_interfaces</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
...
bt_act_test.cpp:
#include <stdio.h>
#include <string>
#include <chrono>
#include <thread>
#include <memory>
#include <filesystem>
#include <vector>
#include <example_interfaces/action/fibonacci.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <behaviortree_ros2/bt_action_node.hpp>
using vec_int = std::vector<int32_t>;
using Fibonacci = example_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
void sleep(int ms=10){
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}
class FibonacciActionServer : public rclcpp::Node {
public:
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("server_node", options)
{
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
this,
"fibonacci",
std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
std::bind(&FibonacciActionServer::handle_cancel, this, _1),
std::bind(&FibonacciActionServer::handle_accepted, this, _1)
);
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
using namespace std::placeholders;
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
}
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
rclcpp::Rate loop_rate(1);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<Fibonacci::Feedback>();
auto & sequence = feedback->sequence;
sequence.push_back(0);
sequence.push_back(1);
auto result = std::make_shared<Fibonacci::Result>();
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
// Check if there is a cancel request
if (goal_handle->is_canceling()) {
result->sequence = sequence;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
// Update sequence
sequence.push_back(sequence[i] + sequence[i - 1]);
// Publish feedback
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "Publish feedback");
loop_rate.sleep();
}
// Check if goal is done
if (rclcpp::ok()) {
result->sequence = sequence;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
};
using namespace BT;
class FibonacciAction: public RosActionNode<Fibonacci>
{
public:
FibonacciAction(const std::string& name,
const NodeConfig& conf,
const RosNodeParams& params)
: RosActionNode<Fibonacci>(name, conf, params)
{}
// The specific ports of this Derived class
// should be merged with the ports of the base class,
// using RosActionNode::providedBasicPorts()
static PortsList providedPorts()
{
return providedBasicPorts({InputPort<unsigned>("order"), OutputPort<vec_int>("result")});
}
// This is called when the TreeNode is ticked and it should
// send the request to the action server
bool setGoal(RosActionNode::Goal& goal) override
{
// get "order" from the Input port
getInput("order", goal.order);
// return true, if we were able to set the goal correctly.
return true;
}
// Callback executed when the reply is received.
// Based on the reply you may decide to return SUCCESS or FAILURE.
NodeStatus onResultReceived(const WrappedResult& wr) override
{
std::stringstream ss;
ss << "Result received: ";
for (auto number : wr.result->sequence) {
ss << number << " ";
}
RCLCPP_INFO(node_->get_logger(), ss.str().c_str());
this->setOutput<vec_int>("result", wr.result->sequence);
return NodeStatus::SUCCESS;
}
// Callback invoked when there was an error at the level
// of the communication between client and server.
// This will set the status of the TreeNode to either SUCCESS or FAILURE,
// based on the return value.
// If not overridden, it will return FAILURE by default.
virtual NodeStatus onFailure(ActionNodeErrorCode error) override
{
RCLCPP_ERROR(node_->get_logger(), "Error: %d", error);
return NodeStatus::FAILURE;
}
// we also support a callback for the feedback, as in
// the original tutorial.
// Usually, this callback should return RUNNING, but you
// might decide, based on the value of the feedback, to abort
// the action, and consider the TreeNode completed.
// In that case, return SUCCESS or FAILURE.
// The Cancel request will be send automatically to the server.
NodeStatus onFeedback(const std::shared_ptr<const Feedback> feedback)
{
std::stringstream ss;
ss << "Next number in sequence received: ";
for (auto number : feedback->sequence) {
ss << number << " ";
}
RCLCPP_INFO(node_->get_logger(), ss.str().c_str());
this->setOutput<vec_int>("result", feedback->sequence);
return NodeStatus::RUNNING;
}
};
template<typename T>
class GetBBvalueNode : public BT::SyncActionNode{
public:
GetBBvalueNode (
const std::string& name, const BT::NodeConfig& config,
std::function<BT::NodeStatus(T)> callback,
std::function<BT::NodeStatus(BT::Expected<T>)> no_value_callback = [](BT::Expected<T> input){
throw BT::RuntimeError("missing required input [input]: ", input.error() );
return BT::NodeStatus::FAILURE; //needed to pass contructor checks
}
) :
BT::SyncActionNode(name, config),
_callback(callback),
_no_value_callback(no_value_callback) {}
BT::NodeStatus tick() override{
auto input = getInput<T>("input"); //get value from port "input"
// Check if expected is valid. If not, throw its error
if (!input) return this->_no_value_callback(input);
else return this->_callback(input.value());
}
/**
* @brief set port input as <T> input
*/
static BT::PortsList providedPorts(){
return{ BT::InputPort<T>("input", "read value")};
}
private:
std::function<BT::NodeStatus(T)> _callback;
std::function<BT::NodeStatus(BT::Expected<T>)> _no_value_callback;
};
int main(int argc, char *argv[]){
rclcpp::init(argc, argv);
static const char* xml_text = R"(
<root BTCPP_format="4" >
<BehaviorTree ID="MainTree">
<ReactiveSequence>
<GetVec input="{res}"/>
<Fibonacci order="5"
result="{res}"/>
</ReactiveSequence>
</BehaviorTree>
</root>
)";
std::vector<vec_int> res_vecs;
auto get_vec = [&res_vecs](vec_int input){
if ( std::find(res_vecs.begin(), res_vecs.end(), input) == res_vecs.end() ){
res_vecs.push_back(input);
for(auto el : res_vecs.back()) std::cout << el << "; ";
std::cout << std::endl;
}
if (res_vecs.size() == 2) return BT::NodeStatus::FAILURE;
else return BT::NodeStatus::SUCCESS;
};
auto on_err = [](BT::Expected<vec_int> input){
(void)input;
return BT::NodeStatus::SUCCESS;
};
BehaviorTreeFactory factory;
// provide the ROS node and the name of the action service
RosNodeParams params;
auto node = std::make_shared<rclcpp::Node>("fibonacci_action_client");
params.nh = node;
params.default_port_value = "fibonacci";
factory.registerNodeType<FibonacciAction>("Fibonacci", params);
factory.registerNodeType<GetBBvalueNode<vec_int>>("GetVec", get_vec, on_err);
auto server_node = std::make_shared<FibonacciActionServer>();
auto spin_callback = [](rclcpp::Node::SharedPtr node){rclcpp::spin(node);};
std::thread{spin_callback, server_node}.detach();
sleep();
// tick the tree
auto tree = factory.createTreeFromText(xml_text);
tree.tickWhileRunning();
rclcpp::shutdown();
return 0;
}
The code is essentially copied-pasted from the tutorials BT ros2 integration and ROS2 action server.
When I try to run the executable I get the following:
root@f1e379732b0f:~/projects# ros2 run bt_act_test bt_act_test
[INFO] [1687792324.400457200] [server_node]: Received goal request with order 5
[INFO] [1687792324.401037500] [server_node]: Executing goal
[INFO] [1687792324.401250200] [server_node]: Publish feedback
[INFO] [1687792324.421203700] [fibonacci_action_client]: Goal accepted by server, waiting for result
[INFO] [1687792325.401689100] [server_node]: Publish feedback
[INFO] [1687792325.407532400] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2
0; 1; 1; 2;
[INFO] [1687792326.401572200] [server_node]: Publish feedback
[INFO] [1687792326.405681700] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3
0; 1; 1; 2; 3;
[INFO] [1687792326.406163200] [server_node]: Received request to cancel goal
terminate called after throwing an instance of 'std::runtime_error'
what(): Asked to publish result for goal that does not exist
[ros2run]: Aborted
Apologies for the long message, but I hope it will make the error reproduction easier.
Thanks in advance for the support