(Python) Simple Action Server - register_goal_callback executes unexpectedly
I'm trying to write a simple action server using the goal callback method as described in the tutorial in C++ over here, however I'm trying to rewrite it in Python. If it works, I would definitely suggest it as an addition to the actionlib
tutorials.
I have put together more or less the whole thing, but clearly I'm missing something essential around the registration of the callbacks. As soon as I call the _as.register_goal_callback(self.goalCB())
the program will attempt to execute the callback immediately and then complain that it doesn't have a goal. The same happens for the _as.register_preempt_callback(self.preemptCB())
which preempts itself immediately. All this happens before the server is even started.
I suppose this is not meant to be happening, but I'm not sure what am I doing wrong.
[ERROR] [1523131943.200392]: Attempting to accept the next goal when a new goal is not available
[INFO] [1523131943.200585]: /averaging : Preempted
[ERROR] [1523131943.200728]: Attempt to get a goal id on an uninitialized ServerGoalHandle
[ERROR] [1523131943.200860]: Attempt to get a goal id on an uninitialized ServerGoalHandle
[ERROR] [1523131943.200988]: Attempt to set status on an uninitialized ServerGoalHandle
#! /usr/bin/env python
import rospy
import actionlib
import actionlib_tutorials.msg
from std_msgs.msg import Float32
from math import pow, sqrt, fabs
class AveragingAction(object):
# create messages that are used to publish feedback/result
_feedback = actionlib_tutorials.msg.AveragingActionFeedback()
_result = actionlib_tutorials.msg.AveragingActionResult()
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(name=self._action_name, ActionSpec=actionlib_tutorials.msg.AveragingAction, auto_start=False)
self._as.register_goal_callback(self.goalCB())
self._as.register_preempt_callback(self.preemptCB())
self._as.start()
rospy.loginfo("Starting an ActionServer")
self._sub = rospy.Subscriber("/random_number", Float32, callback=self.analysisCB)
self._msg = Float32()
def goalCB(self):
self._data_count = 0
self._sum = 0
self._sum_sq = 0
self._goal = self._as.accept_new_goal()
def preemptCB(self):
rospy.loginfo("{} : Preempted".format(self._action_name))
self._as.set_preempted()
...
(there is more code, but I don't think that's relevant anymore)
I have gone through the simple_action_callback.py to figure out why it is getting called, but I'm not sure I get what's going on because I'm probably missing out something with the goal registration. All that's written there is this :
## @brief Allows users to register a callback to be invoked when a new goal is available
## @param cb The callback to be invoked
def register_goal_callback(self,cb):
if self.execute_callback:
rospy.logwarn("Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it.");
else:
self.goal_callback = cb;
Which would make me think that it would stay registered and activate only when a new goal arrives. But it activates immediately. Moreover a condition if self._as.is_new_goal_available():
comes out False
when I place it inside the goalCB()
which really confuses me.
I hope this is not too verbose. I can edit it down if needed.
FYI I'm using
- ROS Kinetic distribution
- Ubuntu 16.04 Xenial
- actionlib 1.11.13
- rospy 1.12.13