Rospy Spin/Callback - Killing running function?

asked 2019-02-11 04:23:26 -0600

NewGuy999 gravatar image

Hi,

I'm not sure how to word this properly, since when it comes to coding I am but a layman. Using rospy, not cpp.

I've implemented an Extended Kalman Filter, where the filter is continually iterating from timestep to timestep. When the user requests it, the filter should be reset to initial values - for this I have created a callback function for a subscriber listening to a ResetEKFObserver signal.

The problem is that most often this signal arrives during the middle of an iteration, with bad results as some class variables are changed during reset. It's gamble, sometimes you can be lucky and the signal arrives just at the end of an iteration, but that's rare and obviously not robust enough.

So, my question is, can I somehow kill the original iteration function thread in my ResetEKFObserver callback function so that it does not continue where it left off after in the iteration function? I do not want it to continue after the filter has been reset, but rather restart the iteration completely after the initial values has been set.

If my verbal description was not clear, please see the code excerpts below.

resetFilter function:

def ResetEKFObserver(self, currentTime, accelerometerData, filterAttitude): 
        EKFObserverReset = 0
        i = 0
        while EKFObserverReset < 0.5 and i < 20:
            if hasattr(self, 'GNSS') and hasattr(self, 'Heading'):
                self.NoPrevState(currentTime, filter_attitude) # Sets  many class variables to initial values
                EKFObserverReset = 1
                print('EKF reset successfully.')
            else:
                i = i+1
                time.sleep(0.05)

        if EKFObserverReset < 0.5:
               print('EKF not reset! Please try again')

Iteration function pseudocode:

def iterate(self, currentTime, accelerometerData, filter_attitude):
     # Really long function without much interesting
     # Uses time and measurements (class variables) as inputs, outputs estimated state values

Main function: def main():

    # initiate node
    rospy.init_node('ObserverNode')
    observer = Observer()
    srv = Server(resetFilter, observer.parameter_callback) #This eventually calls ResetEKFObserver
    rospy.spin()
edit retag flag offensive close merge delete

Comments

Tbh, I don't believe this is a ROS related issue: functions cannot be "killed", and managing long running functions when using async frameworks is something that is a very generic problem/task.

Your current design would also seems to go against the typical advice of "keep callbacks short", ..

gvdhoorn gravatar imagegvdhoorn ( 2019-02-11 04:43:19 -0600 )edit

.. place longer running operations on their own thread or use a work(er) queue.

gvdhoorn gravatar imagegvdhoorn ( 2019-02-11 04:44:16 -0600 )edit

Thanks for your advice, gvdhoorn. Could you point me in a direction where I can learn more about managing long functions in async frameworks such as ROS?

I saw from the roscpp tutorial there were different kinds of spin functions depending on threading. This does not exist for rospy?

NewGuy999 gravatar imageNewGuy999 ( 2019-02-11 04:57:18 -0600 )edit