def request(self, uavcan_msg, node_id, callback, priority=None, timeout=None):
#pylint: disable=W0613
uavcan_type = uavcan.get_uavcan_data_type(uavcan_msg)
if not uavcan_type.full_name in self.__service_proxies:
self.__service_proxies[uavcan_type.full_name] = Service(uavcan_type.full_name).ServiceProxy()
service_proxy = self.__service_proxies[uavcan_type.full_name]
ros_req = copy_uavcan_ros(service_proxy.request_class(), uavcan_msg, request=True)
setattr(ros_req, uavcan_id_field_name, node_id)
def service_proxy_call():
try:
return service_proxy(ros_req)
except rospy.ServiceException:
return None
def request_finished(fut):
ros_resp = fut.result()
if ros_resp is None:
uavcan_resp = None
else:
uavcan_resp = uavcan_node._Event(self, uavcan_type, ros_resp, uavcan_id=node_id, request=False)
callback(uavcan_resp)
self.__request_executor.submit(service_proxy_call).add_done_callback(request_finished)
评论列表
文章目录