Merge pull request #19 from jdlangs/fix_callbacks

Use async tasks to invoke callbacks in main thread

Author: jdlangs

src/RobotOS.jl

@@ -3,13 +3,19 @@ module RobotOS
 using PyCall
 
 #Empty imported modules for valid precompilation
-const __sys__ = PyCall.PyNULL()
+const _py_sys = PyCall.PyNULL()
+const _py_ros_callbacks = PyCall.PyNULL()
 const __rospy__ = PyCall.PyNULL()
 
 function __init__()
     #Put julia's ARGS into python's so remappings will work
-    copy!(__sys__, pyimport("sys"))
-    __sys__["argv"] = ARGS
+    copy!(_py_sys, pyimport("sys"))
+    _py_sys["argv"] = ARGS
+
+    if ! (dirname(@__FILE__) in _py_sys["path"])
+        unshift!(_py_sys["path"], dirname(@__FILE__))
+    end
+    copy!(_py_ros_callbacks, pyimport("ros_callbacks"))
 
     try
         copy!(__rospy__, pyimport("rospy"))
@@ -23,13 +29,12 @@ function __init__()
     end
 end
 
-_threads_enabled() = ccall(:jl_threading_enabled, Cint, ()) != 0
-
 include("debug.jl")
 include("time.jl")
 include("gentypes.jl")
 include("rospy.jl")
 include("pubsub.jl")
 include("services.jl")
+include("callbacks.jl")
 
 end

src/callbacks.jl

@@ -0,0 +1,33 @@
+#This function will run in a new python thread created by rospy.
+#No julia allocation allowed.
+function _callback_notify(handle::Ptr{Void})
+    ccall(:uv_async_send, Cint, (Ptr{Void},), handle)
+end
+
+const CB_NOTIFY_PTR = cfunction(_callback_notify, Cint, (Ptr{Void},))
+
+function _callback_async_loop(rosobj, cond)
+    @debug("Spinning up callback loop...")
+    while ! is_shutdown()
+        wait(cond)
+        _run_callbacks(rosobj)
+    end
+    @debug("Exiting callback loop")
+end
+
+function _run_callbacks{M}(sub::Subscriber{M})
+    while pycall(sub.queue["size"], PyAny) > 0
+        msg = pycall(sub.queue["get"], PyObject)
+        sub.callback(convert(M, msg), sub.callback_args...)
+    end
+end
+
+function _run_callbacks{T}(srv::Service{T})
+    ReqType = _srv_reqtype(T)
+
+    req = pycall(srv.cb_interface["get_request"], PyObject)
+    response = srv.handler(convert(ReqType, req))
+
+    #Python callback is blocking until the response is ready
+    pycall(srv.cb_interface["set_response"], PyAny, convert(PyObject, response))
+end

src/debug.jl

@@ -3,15 +3,15 @@ _debug_output = false
 _debug_indent = 0
 debug(d::Bool) = global _debug_output = d
 macro debug(expr, other...)
-    :(if _debug_output
+    esc(:(if _debug_output
         print(repeat("\t", _debug_indent))
-        println($expr,$(other...)) 
-      end)
+        println($expr,$(other...))
+    end))
 end
 macro debug_addindent()
-    :(global _debug_indent += 1)
+    esc(:(global _debug_indent += 1))
 end
 macro debug_subindent()
-    :(global _debug_indent -= 1)
+    esc(:(global _debug_indent -= 1))
 end
 

src/pubsub.jl

@@ -17,37 +17,31 @@ function publish{MsgType<:MsgT}(p::Publisher{MsgType}, msg::MsgType)
     pycall(p.o["publish"], PyAny, convert(PyObject, msg))
 end
 
-if _threads_enabled() #callbacks are broken
-
-type Subscriber{T}
-end
-Subscriber(args...) = error(
-"""Subscribing to a topic is currently broken on julia v0.5 and above. See
-https://github.com/jdlangs/RobotOS.jl/issues/15 for ongoing efforts to fix this.""")
-
-else #callbacks not broken
-
 type Subscriber{MsgType<:MsgT}
-    o::PyObject
     callback
+    callback_args::Tuple
+    sub_obj::PyObject
+    queue::PyObject
+    async_loop::Task
 
     function Subscriber(topic::AbstractString, cb, cb_args::Tuple=(); kwargs...)
         @debug("Creating <$(string(MsgType))> subscriber on topic: '$topic'")
         rospycls = _get_rospy_class(MsgType)
-        jl_cb(msg::PyObject) = cb(convert(MsgType, msg), cb_args...)
-        return new(
-            __rospy__[:Subscriber](ascii(topic), rospycls, jl_cb; kwargs...),
-            jl_cb
-        )
+
+        cond = Base.AsyncCondition()
+        mqueue = _py_ros_callbacks["MessageQueue"](CB_NOTIFY_PTR, cond.handle)
+        subobj = __rospy__[:Subscriber](ascii(topic), rospycls, mqueue["storemsg"]; kwargs...)
+
+        rosobj = new(cb, cb_args, subobj, mqueue)
+        cbloop = Task(() -> _callback_async_loop(rosobj, cond))
+        schedule(cbloop)
+
+        rosobj.async_loop = cbloop
+        return rosobj
     end
 end
 
-Subscriber{MsgType<:MsgT}(
-    topic::AbstractString,
-    ::Type{MsgType},
-    cb,
-    cb_args::Tuple=();
-    kwargs...
-) = Subscriber{MsgType}(ascii(topic), cb, cb_args; kwargs...)
+function Subscriber{MsgType<:MsgT}(topic, ::Type{MsgType}, cb, cb_args::Tuple=(); kwargs...)
+    Subscriber{MsgType}(topic, cb, cb_args; kwargs...)
+end
 
-end #check

src/ros_callbacks.py

@@ -0,0 +1,59 @@
+#Python 2/3 compatibility with 3 style code
+from __future__ import absolute_import, division, print_function, unicode_literals
+__metaclass__ = type
+
+import sys
+import ctypes
+import threading
+try:
+    import queue
+except ImportError:
+    import Queue as queue
+
+class MessageQueue:
+    "Queue up received messages and invoke notification to run callback"
+    def __init__(self, cbptr, notify_handle):
+        CBType = ctypes.CFUNCTYPE(ctypes.c_int, ctypes.c_void_p)
+        self._cb_notify = CBType(cbptr.value)
+        self._notify_handle = notify_handle
+
+        self._queue = queue.Queue()
+
+    def storemsg(self, msg):
+        self._queue.put(msg)
+        self._cb_notify(self._notify_handle)
+
+    def size(self):
+        return self._queue.qsize()
+
+    def get(self):
+        return self._queue.get()
+
+class ServiceCallback:
+    def __init__(self, cbptr, notify_handle):
+        CBType = ctypes.CFUNCTYPE(ctypes.c_int, ctypes.c_void_p)
+        self._cb_notify = CBType(cbptr.value)
+        self._notify_handle = notify_handle
+
+        self._response = None
+        self._hasresponse = threading.Condition()
+
+    def srv_cb(self, srvreq):
+        "Store received service request and block until Julia callback completes"
+        self._hasresponse.acquire()
+        self._request = srvreq
+        self._cb_notify(self._notify_handle)
+
+        #wait for the julia callback
+        self._hasresponse.wait()
+        self._hasresponse.release()
+        return self._response
+
+    def get_request(self):
+        return self._request
+
+    def set_response(self, resp):
+        self._response = resp
+        self._hasresponse.acquire()
+        self._hasresponse.notify()
+        self._hasresponse.release()

src/rospy.jl

@@ -6,11 +6,18 @@ export init_node, is_shutdown, spin,
 #General rospy functions
 init_node(name::AbstractString; args...) =
     __rospy__[:init_node](ascii(name); args...)
-spin()                 = __rospy__[:spin]()
 is_shutdown()          = __rospy__[:is_shutdown]()
 get_published_topics() = __rospy__[:get_published_topics]()
 get_ros_root()         = __rospy__[:get_ros_root]()
 
+function spin()
+    #Have to make sure both Julia tasks and python threads can wake up so
+    #can't just call rospy's spin
+    while ! is_shutdown()
+        rossleep(Duration(0.001))
+    end
+end
+
 #Parameter server API
 function get_param(param_name::AbstractString, def=nothing)
     try

src/services.jl

@@ -32,38 +32,36 @@ end
     resp
 end
 
-if _threads_enabled() #callbacks are broken
-
-type Service{T}
-end
-
-Service(args...) = error(
-"""Providing a service is currently broken on julia v0.5 and above. See
-https://github.com/jdlangs/RobotOS.jl/issues/15 for ongoing efforts to fix this.""")
-
-else #callbacks not broken
-
 type Service{SrvType <: ServiceDefinition}
-    o::PyObject
-    jl_handler
+    handler
+    srv_obj::PyObject
+    cb_interface::PyObject
+    async_loop::Task
 
     function Service(name::AbstractString, handler; kwargs...)
         @debug("Providing <$SrvType> service at '$name'")
         rospycls = _get_rospy_class(SrvType)
-        ReqType = _srv_reqtype(SrvType)
-        jl_hndl(req::PyObject) =
-            convert(PyObject, handler(convert(ReqType,req)))
-        try
-            new(__rospy__[:Service](ascii(name), rospycls, jl_hndl; kwargs...),
-                jl_hndl
-            )
+
+        cond = Base.AsyncCondition()
+        pysrv = _py_ros_callbacks["ServiceCallback"](CB_NOTIFY_PTR, cond.handle)
+
+        srvobj = try
+            __rospy__[:Service](ascii(name), rospycls, pysrv["srv_cb"]; kwargs...)
         catch err
             if isa(err, PyCall.PyError)
                 error("Problem during service creation: $(err.val[:args][1])")
             else
                 rethrow(err)
             end
         end
+
+        rosobj = new(handler, srvobj, pysrv)
+
+        cbloop = Task(() -> _callback_async_loop(rosobj, cond))
+        schedule(cbloop)
+
+        rosobj.async_loop = cbloop
+        return rosobj
     end
 end
 function Service{SrvType<:ServiceDefinition}(
@@ -75,8 +73,6 @@ function Service{SrvType<:ServiceDefinition}(
     Service{SrvType}(ascii(name), handler; kwargs...)
 end
 
-end #check
-
 function wait_for_service(service::AbstractString; kwargs...)
     try
         __rospy__[:wait_for_service](ascii(service); kwargs...)

src/time.jl

@@ -79,11 +79,12 @@ end
 now() = get_rostime()
 
 function rossleep(td::Duration)
+    #Busy sleep loop needed to allow both julia and python async activity
     tnsecs = to_nsec(td)
     t0 = time_ns()
     while time_ns()-t0 < tnsecs
-        yield()
-        __rospy__[:sleep](0.001)
+        yield()                   #Allow julia callback loops to run
+        __rospy__[:sleep](0.001)  #Allow rospy comm threads to run
     end
 end
 rossleep(t::Real) = rossleep(Duration(t))

test/pubsub.jl

@@ -36,23 +36,13 @@ function pose_cb(msg::PoseStamped, msgs::Vector{PoseStamped})
 end
 pose_cb(PoseStamped(), msgs) #warm up run
 
-if RobotOS._threads_enabled() #callbacks are broken
-
-warn("Not testing subscriber!")
-
-publish_messages(ros_pub, refs, 20.0)
-rossleep(Duration(5.0))
-Nreceived = get_param("/num_received_messages")
-@test Nreceived == length(refs)
-set_param("/num_received_messages", 0)
-
-else #callbacks not broken
-
 const ros_sub = Subscriber("poses", PoseStamped, pose_cb, (msgs,), queue_size = 10)
 
 #First message doesn't go out for some reason
 publish(ros_pub, Vector3(1.1,2.2,3.3))
 rossleep(Duration(1.0))
+
+#Test messages
 publish_messages(ros_pub, refs, 20.0)
 rossleep(Duration(1.0))
 
@@ -65,5 +55,3 @@ for i=1:Nmsgs
     @test_approx_eq msgs[i].pose.position.z refs[i].z
 end
 empty!(msgs)
-
-end #check

test/services.jl

@@ -8,6 +8,9 @@ const srvcall = ServiceProxy("callme", SetBool)
 println("Waiting for 'callme' service...")
 wait_for_service("callme")
 
+const flag = Bool[false]
+const Nposes = 5
+
 function srv_cb(req::GetPlanRequest)
     println("GetPlan call received")
     @test_approx_eq(req.start.pose.position.x, 1.0)
@@ -24,23 +27,6 @@ function srv_cb(req::GetPlanRequest)
     return resp
 end
 
-if RobotOS._threads_enabled() #callbacks are broken
-
-warn("Not testing service provider!")
-
-println("Calling service...")
-srvcall(SetBoolRequest(false))
-rossleep(Duration(5.0))
-
-didcall = get_param("/received_service_call")
-@test didcall
-set_param("/received_service_call", false)
-
-else #callbacks not broken
-
-const flag = Bool[false]
-const Nposes = 5
-
 const srvlisten = Service("getplan", GetPlan, srv_cb)
 
 println("Calling service...")
@@ -64,8 +50,6 @@ if flag[1]
 end
 empty!(msgs)
 
-end #check
-
 #Test error handling
 @test_throws ErrorException wait_for_service("fake_srv", timeout=1.0)
 @test_throws ArgumentError srvcall(SetBoolResponse())