diff --git a/rtmbuild/scripts/idl2srv.py b/rtmbuild/scripts/idl2srv.py index 549ebc9bb..86fbe2fb7 100755 --- a/rtmbuild/scripts/idl2srv.py +++ b/rtmbuild/scripts/idl2srv.py @@ -525,6 +525,9 @@ def replaceline(src, dest, ref): compsrc = open(tmpdir + '/' + module_name + '.cpp').read() port_name_src = """ nh = ros::NodeHandle("~"); + int threads = 1; + nh.getParam("threads", threads); + spinner = ros::AsyncSpinner(threads); std::string port_name = "service0"; nh.getParam("service_port", port_name);""" compsrc = addline(port_name_src, compsrc, 'Set service consumers to Ports') @@ -532,7 +535,7 @@ def replaceline(src, dest, ref): compsrc += """ RTC::ReturnCode_t %s::onExecute(RTC::UniqueId ec_id) { - ros::spinOnce(); + //ros::spinOnce(); return RTC::RTC_OK; }\n\n""" % module_name @@ -540,6 +543,7 @@ def replaceline(src, dest, ref): for i in range(len(operations)): name = operations[i].identifier() compsrc += ' _srv%d = nh.advertiseService("%s", &%s::%s, this);\n' % (i, name, module_name, name) + compsrc +=" spinner.start();\n" compsrc +=" return RTC::RTC_OK;\n}\n\n""" compsrc += "RTC::ReturnCode_t %s::onDeactivated(RTC::UniqueId ec_id) {\n" % module_name @@ -547,6 +551,7 @@ def replaceline(src, dest, ref): name = operations[i].identifier() srvinst = ' _srv%d.shutdown();' % i compsrc = addline(srvinst, compsrc, 'Unadvertise service') + compsrc +=" spinner.stop();\n" compsrc +=" return RTC::RTC_OK;\n}\n\n""" compsrc += convert_functions + self.convertFunctionCode(interface) @@ -576,6 +581,7 @@ def replaceline(src, dest, ref): compsrc = addline(srvfunc, compsrc, 'public:') defsrv = " ros::NodeHandle nh;\n"; + defsrv += " ros::AsyncSpinner spinner = ros::AsyncSpinner(1);\n"; defsrv += " ros::ServiceServer " + ', '.join(['_srv%d' % i for i in range(len(operations))]) + ';' compsrc = addline(defsrv, compsrc, 'private:')