Nav2 Simple Action Server always returning ABORTED

asked 2023-08-11 02:24:14 -0600

PatoInso gravatar image

updated 2023-08-11 03:55:31 -0600

Hello, I'm trying to use the SimpleActionServer class from nav2_utils. To test it I'm using ros2 action send_goal from the CLI.

My call back function get called. However, the send_goal commands exits immediately after and output Goal finished with status: ABORTED,

Goal accepted with ID: 54a3fde88a714c0d948aefe47635a5df
Result:
    {}
Goal finished with status: ABORTED

while I expected it to hang endlessly as I never explicitly terminate the action, return success or error nor abort in my callback. What am I missing ?

#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "my_msgs/action/my_action.hpp"

class AtlerEstimator
{
public:

    AtlerEstimator()
    {
        m_node = rclcpp::Node::make_shared("alter_estimator");

        m_server = std::make_unique<nav2_util::SimpleActionServer<my_msgs::action::MyAction>>(m_node, "my_action", std::bind(&AtlerEstimator::MyActionRun, this), nullptr, 500ms, false);
        m_server->activate();
   }

private:
    rclcpp::Node::SharedPtr m_node;
    std::shared_ptr<const my_msgs::action::MyAction::Goal> m_goal;
    std::unique_ptr<nav2_util::SimpleActionServer<my_msgs::action::MyAction>> m_server;

void MyActionRun()
{
    m_goal = m_server->get_current_goal();
    RCLCPP_WARN(m_node->get_logger(), "Hello");
}

EDIT: After experimenting, adding this

auto aa = std::shared_ptr<my_msgs::action::MyAction_Feedback>();
m_server->publish_feedback(aa);

gives the expected behavior. With some other experiment, I suspect that if the pointer to the goal handle (not accessible outside the SimpleActionServer class) get destroyed, then it abort the action. Publishing a feedback may somehow "store it"...

edit retag flag offensive close merge delete