locusrobotics / robot_navigation

Spiritual successor to ros-planning/navigation.
450 stars 148 forks source link

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

Open cf-zhang opened 5 years ago

cf-zhang commented 5 years ago

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;
  ...
}