Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[LocomotorActionServer] A callback function should be provided for cancelRequest #43

Open
cf-zhang opened this issue May 17, 2019 · 0 comments

Comments

@cf-zhang
Copy link

In LocomotorActionServer's constructor function, navigate_action_server_.registerPreemptCallback(std::bind(&LocomotorActionServer::preemptCallback, this)); is called for register callback to deal with cancel request.
but in preemptCallback() can not stop the move_base's plan_loop_timer_ and control_loop_timer_.
and this will caused the action server is preempted, but the move_base is still runing the planners, robot is moving.

To solve this problem, I think we should provide a callback function in the LocomotorActionServer like this:
In locomotor_action_server.h

namespace locomotor
{
    ...
    using CancelTaskCallback = std::function<void ()>;
    class LocomotorActionServer
    {
        ...
        NewGoalCallback goal_cb_;
        CancelTaskCallback ctcb_;
    };
}  // namespace locomotor

In locomotor_action_server.cpp

MoveBaseActionServer::MoveBaseActionServer(const ros::NodeHandle nh, NewGoalCallback cb, CancelTaskCallback ctcb, const std::string name)
        : navigate_action_server_(nh, name, false), goal_cb_(cb), ctcb_(ctcb)
{
    ...
}

void MoveBaseActionServer::preemptCallback()
{
  ctcb_();
  if (!navigate_action_server_.isActive()) return;
  ...
}

DLu added a commit that referenced this issue Jul 2, 2020
@DLu DLu mentioned this issue Jul 2, 2020
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant