ROS2 action client callbacks not triggered
Hello,
I am trying to call an action from an action server. However, the action client callbacks do not seem to trigger.
Here is a minimal example I wrote based on the Fibonacci action tutorial. This action server calls the action_tutorials_cpp Fibonacci action.
#include <functional>
#include <memory>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "action_tutorials_interfaces/action/fibonacci.hpp"
class FibonacciActionServer : public rclcpp::Node {
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using ServerGoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
using ClientGoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
explicit FibonacciActionServer(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options) {
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
this,
"master_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_;
std::shared_ptr<ServerGoalHandleFibonacci> called_action_goal_handle;
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<ServerGoalHandleFibonacci> 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<ServerGoalHandleFibonacci> 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<ServerGoalHandleFibonacci> goal_handle) {
this->called_action_goal_handle = goal_handle;
RCLCPP_INFO(this->get_logger(), "Creating Action client and sending goal to Tutorial Fibonacci Action Server");
auto action_client = rclcpp_action::create_client<Fibonacci>(shared_from_this(),
"fibonacci");
Fibonacci::Goal goal_msg;
goal_msg.order = goal_handle->get_goal()->order;
using std::placeholders::_1;
using std::placeholders::_2;
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionServer::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionServer::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionServer::result_callback, this, _1);
action_client->async_send_goal(goal_msg, send_goal_options);
}
void goal_response_callback(std::shared_future<ClientGoalHandleFibonacci::SharedPtr> future) {
auto goal_handle = future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
void feedback_callback(
ClientGoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback) {
std::stringstream ss;
ss << "Next number in sequence received: ";
for (auto number: feedback->partial_sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}
void result_callback(const ClientGoalHandleFibonacci::WrappedResult &result) {
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
std::stringstream ss;
ss << "Result received: ";
for (auto number: result.result->sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
this->called_action_goal_handle->succeed(result.result);
}
}; // class FibonacciActionServer
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<FibonacciActionServer>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
I expected the callbacks to be called and the goal handle to succeed once the tutorials Fibonacci action server succeeds. However, only the "goal_response_callback" is called, not the feedback_callback nor the result_callback...
I tried using a SingleThreadedExecutor and a MultiThreadedExecutor to spin the node but the behavior doesn't change.
I am running Ubuntu 20.04.1 on an x86, with kernel 5.13 ...