-
Notifications
You must be signed in to change notification settings - Fork 146
Open
Description
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;
...
}
Metadata
Metadata
Assignees
Labels
No labels