def write(self, data):
if self._directMode:
return self.PORT_OK
# data -> (conversion) -> CDR stream
cdr_data = None
if self._endian is not None:
cdr_data = cdrMarshal(any.to_any(data).typecode(), data, self._endian)
else:
self._rtcout.RTC_ERROR("write(): endian %s is not support.",self._endian)
return self.UNKNOWN_ERROR
if self._buffer:
if self._sync_readwrite:
self._readready_worker._cond.acquire()
while not self._readready_worker._completed:
self._readready_worker._cond.wait()
self._readready_worker._cond.release()
self._buffer.write(cdr_data)
if self._sync_readwrite:
self._writecompleted_worker._completed = True
self._writecompleted_worker._cond.acquire()
self._writecompleted_worker._cond.notify()
self._writecompleted_worker._cond.release()
self._readcompleted_worker._cond.acquire()
while not self._readcompleted_worker._completed:
self._readcompleted_worker._cond.wait()
self._readcompleted_worker._cond.release()
self._writecompleted_worker._completed = False
else:
return self.UNKNOWN_ERROR
return self.PORT_OK
def write(self, data):
self._rtcout.RTC_TRACE("write()")
if self._directInPort is not None:
if self._directInPort.isNew():
#self._listeners.connectorData_[OpenRTM_aist.ConnectorDataListenerType.ON_BUFFER_OVERWRITE].notify(self._profile, data)
#self._inPortListeners.connectorData_[OpenRTM_aist.ConnectorDataListenerType.ON_BUFFER_OVERWRITE].notify(self._profile, data)
#self._listeners.connectorData_[OpenRTM_aist.ConnectorDataListenerType.ON_RECEIVER_FULL].notify(self._profile, data)
#self._inPortListeners.connectorData_[OpenRTM_aist.ConnectorDataListenerType.ON_RECEIVER_FULL].notify(self._profile, data)
self._rtcout.RTC_TRACE("ONBUFFER_OVERWRITE(InPort,OutPort), ")
self._rtcout.RTC_TRACE("ON_RECEIVER_FULL(InPort,OutPort) ")
self._rtcout.RTC_TRACE("callback called in direct mode.")
#self._listeners.connectorData_[OpenRTM_aist.ConnectorDataListenerType.ON_BUFFER_WRITE].notify(self._profile, data)
#self._inPortListeners.connectorData_[OpenRTM_aist.ConnectorDataListenerType.ON_BUFFER_WRITE].notify(self._profile, data)
self._rtcout.RTC_TRACE("ON_BUFFER_WRITE(InPort,OutPort), ")
self._rtcout.RTC_TRACE("callback called in direct mode.")
self._directInPort.write(data)
#self._listeners.connectorData_[OpenRTM_aist.ConnectorDataListenerType.ON_RECEIVED].notify(self._profile, data)
#self._inPortListeners.connectorData_[OpenRTM_aist.ConnectorDataListenerType.ON_RECEIVED].notify(self._profile, data)
self._rtcout.RTC_TRACE("ON_RECEIVED(InPort,OutPort), ")
self._rtcout.RTC_TRACE("callback called in direct mode.")
return self.PORT_OK
# data -> (conversion) -> CDR stream
cdr_data = None
if self._endian is not None:
cdr_data = cdrMarshal(any.to_any(data).typecode(), data, self._endian)
else:
self._rtcout.RTC_ERROR("write(): endian %s is not support.",self._endian)
return self.UNKNOWN_ERROR
return self._publisher.write(cdr_data, -1, 0)
def test_SharedMemory(self):
sh_read = OpenRTM_aist.SharedMemory()
sh_read_var = sh_read._this()
sh_write = OpenRTM_aist.SharedMemory()
sh_write_var = sh_write._this()
sh_write.setInterface(sh_read_var)
memsize = sh_write.string_to_MemorySize("1")
self.assertEqual(memsize, 1)
memsize = sh_write.string_to_MemorySize("1k")
self.assertEqual(memsize, 1024)
memsize = sh_write.string_to_MemorySize("1M")
self.assertEqual(memsize, 1024 * 1024)
sh_write.create_memory(1000, "test")
sh_write.setEndian(True)
data_cdr = cdrMarshal(CORBA.TC_ulong, 100, True)
sh_write.write(data_cdr)
data_cdr = sh_read.read()
data = cdrUnmarshal(CORBA.TC_ulong, data_cdr, True)
self.assertEqual(data, 100)
if platform.system() == "Windows":
pass
else:
self.assertTrue(os.path.exists("/dev/shm/test"))
sh_write.close_memory(True)
if platform.system() == "Windows":
pass
else:
self.assertFalse(os.path.exists("/dev/shm/test"))
def serialize(self, data):
if self._endian is not None:
try:
cdr = cdrMarshal(
any.to_any(data).typecode(), data, self._endian)
return OpenRTM_aist.ByteDataStreamBase.SERIALIZE_OK, cdr
except:
if sys.version_info[0] == 3:
return OpenRTM_aist.ByteDataStreamBase.SERIALIZE_ERROR, b""
else:
return OpenRTM_aist.ByteDataStreamBase.SERIALIZE_ERROR, ""
else:
if sys.version_info[0] == 3:
return OpenRTM_aist.ByteDataStreamBase.SERIALIZE_NOT_SUPPORT_ENDIAN, b""
else:
return OpenRTM_aist.ByteDataStreamBase.SERIALIZE_NOT_SUPPORT_ENDIAN, ""
def put(self, data):
try:
ref_ = self.getObject()
if ref_:
inportcdr = ref_._narrow(OpenRTM.InPortCdr)
with open(self.file_path, "wb") as fout:
fout.write(data)
mar_file_path = cdrMarshal(CORBA.TC_string, self.file_path)
return self.convertReturnCode(inportcdr.put(mar_file_path))
return self.CONNECTION_LOST
except:
self._rtcout.RTC_ERROR(OpenRTM_aist.Logger.print_exception())
return self.CONNECTION_LOST
return self.convertReturn(ret, data)
def get(self):
try:
if self._buffer.empty():
return (OpenRTM.BUFFER_EMPTY, None)
cdr = [None]
ret = self._buffer.read(cdr)
with open(self.file_path, "wb") as fout:
fout.write(cdr[0])
except:
self._rtcout.RTC_TRACE(OpenRTM_aist.Logger.print_exception())
return (OpenRTM.UNKNOWN_ERROR, None)
mar_file_path = cdrMarshal(CORBA.TC_string, self.file_path)
return self.convertReturn(ret, mar_file_path)
def write(self, data):
self._rtcout.RTC_TRACE("write()")
if self._shmem:
data_size = len(data)
if data_size + SharedMemory.default_size > self._memory_size:
self._memory_size = data_size + SharedMemory.default_size
if self._smInterface is not None:
self._smInterface.close_memory(False)
self.close_memory(True)
self.create_memory(self._memory_size, self._shm_address)
data_size_cdr = cdrMarshal(CORBA.TC_ulonglong, data_size,
self._endian)
self._shmem.seek(os.SEEK_SET)
self._shmem.write(data_size_cdr)
self._shmem.write(data)
def toString(ior):
global endian
data = cdrMarshal(any.to_any(ior).typecode(), ior, endian)
result = [0] * (12 + len(data) * 2)
result[0] = "I"
result[1] = "O"
result[2] = "R"
result[3] = ":"
result[4] = "0"
if endian:
result[5] = "1"
else:
result[5] = "0"
result[6] = "0"
result[7] = "0"
result[8] = "0"
result[9] = "0"
result[10] = "0"
result[11] = "0"
for i,datai in enumerate(data):
d = ord(datai)
j = 12 + i * 2
v = (d & 0xf0)
v = v >> 4
if v < 10:
result[j] = chr(ord('0') + v)
else:
result[j] = chr(ord('a') + (v - 10))
v = ((d & 0xf))
if v < 10:
result[j + 1] = chr(ord('0') + v)
else:
result[j + 1] = chr(ord('a') + (v - 10))
iorstr = "".join(result)
return iorstr
def replaceEndpoint(iorstr, endpoint):
global endian
ior = toIOR(iorstr)
for p in ior.profiles:
# TAG_INTERNET_IOP
if p.tag == IOP.TAG_INTERNET_IOP:
if sys.version_info[0] == 2:
pbody = cdrUnmarshal(_0__GlobalIDL._tc_ProfileBody,
"".join(p.profile_data), endian)
else:
pbody = cdrUnmarshal(_0__GlobalIDL._tc_ProfileBody,
p.profile_data, endian)
pbody.address_.host = endpoint
p.profile_data = cdrMarshal(
any.to_any(pbody).typecode(), pbody, endian)
# TAG_MULTIPLE_COMPONENTS
elif p.tag == IOP.TAG_MULTIPLE_COMPONENTS:
pass
else:
print("Other Profile")
iorstr = toString(ior)
return iorstr
if datasize < 300:
data.height = 1
else:
data.height = 100
data.width = int(datasize/data.height/3)
data.pixels = b" " * int(data.height * data.width * 3)
data.format = "rgb8"
#tm = time.time()
#tm_f = float(tm - int(tm))
#data.tm.sec = int(float(tm) - float(tm_f))
#data.tm.nsec = int(float(tm_f) * float(1000000000))
OpenRTM_aist.setTimestamp(data)
cdr = cdrMarshal(
any.to_any(data).typecode(), data, True)
to.put(cdr)
count += 1
if count % datacount == 0:
print(data.height*data.width*3)
datasize = int(
datasize*logmul[int(count / datacount) % 3])
time.sleep(0.01)
orb.shutdown()
def write(self, data):
# self.ref.put(CORBA.Any(self.data_tc,
# self.data_class(RTC.Time(0,0),data)))
self.ref.put(
cdrMarshal(self.data_tc, self.data_class(RTC.Time(0, 0), data), 1))
def data2cdr(data):
return cdrMarshal(any.to_any(data).typecode(), data, True)
def write(self, data):
self.ref.put(
cdrMarshal(self.data_tc, self.data_class(RTC.Time(0, 0), data), 1))
def data2cdr(data):
return cdrMarshal(any.to_any(data).typecode(), data, True)
def write(self,data) :
self.ref.put(cdrMarshal(self.data_tc,
self.data_class(RTC.Time(0,0),data), 1))
if tm.sec + tm.nsec*1e-9 > lasttm.sec + lasttm.nsec*1e-9:
pos = v
if linalg.norm(pos-lastpos) < 10:
return a
elif linalg.norm(pos) > 1:
lastpos = pos
lasttm = tm
time.sleep(0.1)
return None
if sensor == 'rhandcam':
hrecog = self.hrhandrecog
else:
hrecog = self.hlhandrecog
return read_stable_result(hrecog)
import omniORB
if __name__ == '__main__':
rr = MyHiroNxSystem(portdefs)
rr.connect()
while True:
v = rr.jstt_port.read()
bytes = omniORB.cdrMarshal(rr.hlogger.data_tc, v, 1)
rr.hlogger.ref.put(bytes)
time.sleep(0.01)
def write(self,data) :
# self.ref.put(CORBA.Any(self.data_tc,
# self.data_class(RTC.Time(0,0),data)))
self.ref.put(cdrMarshal(self.data_tc,
self.data_class(RTC.Time(0,0),data), 1))
def inject_data(cmd_path, full_path, options, data, tree):
path, port = parse_path(full_path)
if not port:
# Need a port to inject to
print >>sys.stderr, '{0}: Cannot access {1}: Not a port'.format(\
sys.argv[0], cmd_path)
return 1
trailing_slash = False
if not path[-1]:
# There was a trailing slash
print >>sys.stderr, '{0}: {1}: Not a port'.format(\
sys.argv[0], cmd_path)
return 1
if not tree:
tree = create_rtctree(paths=path)
if not tree:
return tree
comp = tree.get_node(path)
if not comp or not comp.is_component:
print >>sys.stderr, '{0}: Cannot access {1}: No such \
component.'.format(sys.argv[0], cmd_path)
return 1
port = comp.get_port_by_name(port)
if not port:
print >>sys.stderr, '{0}: Cannot access {1}: No such \
port'.format(sys.argv[0], cmd_path)
return 1
if not port.porttype == 'DataInPort':
print >>sys.stderr, '{0}: Can only inject to DataInPort port \
type.'.format(sys.argv[0])
return 1
# Create a dummy connector on the port
datatype = str(data.__class__).rpartition('.')[2]
props = []
props.append(SDOPackage.NameValue('dataport.dataflow_type',
any.to_any('push')))
props.append(SDOPackage.NameValue('dataport.interface_type',
any.to_any('corba_cdr')))
props.append(SDOPackage.NameValue('dataport.subscription_type',
any.to_any('flush')))
props.append(SDOPackage.NameValue('dataport.data_type',
any.to_any(datatype)))
profile = RTC.ConnectorProfile(path[-1] + '.inject_conn',
path[-1] + '.temp', [port.object], props)
return_code, profile = port.object.connect(profile)
port.reparse_connections()
if return_code != RTC.RTC_OK:
print >>sys.stderr, '{0}: Failed to create local connection. Check \
your data type matches the port.'.format(sys.argv[0])
return 1
# Get the connection's IOR and narrow it to an InPortCdr object
conn = port.get_connection_by_name(path[-1] + '.inject_conn')
ior = conn.properties['dataport.corba_cdr.inport_ior']
object = comp.orb.string_to_object(ior)
if CORBA.is_nil(object):
print >>sys.stderr, '{0}: Failed to get inport object.'.format(\
sys.argv[0])
return 1
object = object._narrow(InPortCdr)
# Inject the data
cdr = cdrMarshal(any.to_any(data).typecode(), data, True)
if object.put(cdr) != PORT_OK:
print >>sys.stderr, '{0}: Failed to inject data.'.format(sys.argv[0])
return 1
# Remove the dummy connection
conn.disconnect()
return 0
