0
$\begingroup$

Rosanswers logo

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.0 and ROS2 foxy fitzroy.

So I am wondering if it is possible to trigger the client callbacks while the server is waiting for a response and if so, how ?

I hope I asked the questions correctly. Thanks for your help !


Originally posted by Sam_HeraclesRobotics on ROS Answers with karma: 26 on 2022-06-16

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I figured out my mistake. The problem happens because when the action server’s "execute" function ends, the action client is deleted.

I fixed this by putting the pointer to the action client as a class member. Now the callbacks are called and the action succeeds when the result_callback is triggered.


Originally posted by Sam_HeraclesRobotics with karma: 26 on 2022-06-17

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by ljaniec on 2022-06-17:
It is a nice question and an explanation to a bit niche problem, you should accept your own answer as a solution :)

Comment by Sam_HeraclesRobotics on 2022-06-20:
Thanks ! I am new here so I think I couldn't accept it myself. Thanks for accepting it for me !

Comment by ljaniec on 2022-06-20:
No problem, this is a good, detailed question with code snippets and a non-obvious answer, good for future readers :)

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.