YARP_SSIZE_T BayerCarrier::read(const yarp::os::Bytes& b) {
// copy across small stuff - the image header
if (consumed<sizeof(header)) {
size_t len = b.length();
if (len>sizeof(header)-consumed) {
len = sizeof(header)-consumed;
}
memcpy(b.get(),((char*)(&header))+consumed,len);
consumed += len;
return (YARP_SSIZE_T) len;
}
// sane client will want to read image into correct-sized block
if (b.length()==image_data_len) {
// life is good!
processDirect(b);
consumed += image_data_len;
return image_data_len;
}
// funky client, fall back on image copy
processBuffered();
if (consumed<sizeof(header)+out.getRawImageSize()) {
size_t len = b.length();
if (len>sizeof(header)+out.getRawImageSize()-consumed) {
len = sizeof(header)+out.getRawImageSize()-consumed;
}
memcpy(b.get(),out.getRawImage()+consumed-sizeof(header),len);
consumed += len;
return (YARP_SSIZE_T) len;
}
return -1;
}
void yarp::os::ModifyingCarrier::getHeader(const yarp::os::Bytes &header) {
if (header.length()==8) {
ConstString target = "ohbehave";
for (int i=0; i<8; i++) {
header.get()[i] = target[i];
}
}
}
void yarp::os::impl::HttpTwoWayStream::write(const yarp::os::Bytes& b) { // throws
if (chunked) {
delegate->getOutputStream().write(b);
} else {
for (size_t i=0; i<b.length(); i++) {
apply(b.get()[i]);
}
}
}
virtual void write(const yarp::os::Bytes& b) {
outputCache.append(b.get(),b.length());
while (outputCache.find("\n")!=std::string::npos) {
size_t idx = outputCache.find("\n");
std::string show;
show.append(outputCache.c_str(),idx);
yInfo() << "*** TYPE THIS ON THE OTHER TERMINAL: " << show;
outputCache = outputCache.substr(idx+1);
yarp::os::Time::delay(1);
}
}
bool yarp::os::impl::NameserCarrier::checkHeader(const yarp::os::Bytes& header) {
if (header.length()==8) {
String target = getSpecifierName();
for (int i=0; i<8; i++) {
if (!(target[i]==header.get()[i])) {
return false;
}
}
return true;
}
return false;
}
virtual bool checkHeader(const yarp::os::Bytes& header) {
if (header.length()!=8) {
return false;
}
const char *target = "HUMANITY";
for (int i=0; i<8; i++) {
if (header.get()[i] != target[i]) {
return false;
}
}
return true;
}
yarp::conf::ssize_t WireTwiddlerReader::readMapped(yarp::os::InputStream& is,
yarp::os::Bytes& b,
const WireTwiddlerGap& gap) {
if (gap.load_external) {
int v = 0;
if (gap.var_name[0]=='=') {
Bottle bot;
bot.fromString(gap.var_name.substr(1,gap.var_name.length()));
v = bot.get(0).asInt32();
} else {
v = prop.find(gap.var_name).asInt32();
}
dbg_printf("Read %s: %d\n", gap.var_name.c_str(), v);
for (size_t i=0; i<b.length(); i++) {
b.get()[i] = 0;
}
NetInt32 *nn = reinterpret_cast<NetInt32 *> (b.get());
if (b.length()>=4) {
*nn = v;
}
return gap.unit_length;
}
if (gap.wire_unit_length==gap.unit_length) {
return is.read(b);
}
for (size_t i=0; i<b.length(); i++) {
b.get()[i] = 0;
}
size_t len = gap.wire_unit_length;
if (len>b.length()) {
len = b.length();
}
Bytes b2(b.get(),len);
yarp::conf::ssize_t r = is.readFull(b2);
if (r==(yarp::conf::ssize_t)len) {
if (gap.flavor==BOTTLE_TAG_FLOAT64) {
if (gap.wire_unit_length==4 &&
gap.unit_length==8) {
NetFloat32 x = *(reinterpret_cast<NetFloat32 *> (b2.get()));
NetFloat64 *y = reinterpret_cast<NetFloat64 *> (b2.get());
*y = x;
}
}
int len2 = gap.wire_unit_length-b.length();
if (len2>0) {
dump.allocateOnNeed(len2,len2);
Bytes b3(dump.get(),len2);
is.readFull(b3);
}
return gap.unit_length;
}
return -1;
}
void AbstractCarrier::createYarpNumber(int x, const yarp::os::Bytes& header)
{
if (header.length()!=8) {
return;
}
char *base = header.get();
base[0] = 'Y';
base[1] = 'A';
base[6] = 'R';
base[7] = 'P';
yarp::os::Bytes code(base+2, 4);
NetType::netInt(x, code);
}
int AbstractCarrier::interpretYarpNumber(const yarp::os::Bytes& b)
{
if (b.length()==8) {
char *base = b.get();
if (base[0]=='Y' &&
base[1]=='A' &&
base[6]=='R' &&
base[7]=='P') {
yarp::os::Bytes b2(b.get()+2, 4);
int x = NetType::netInt(b2);
return x;
}
}
return -1;
}
bool BayerCarrier::processDirect(const yarp::os::Bytes& bytes) {
if (have_result) {
memcpy(bytes.get(),out.getRawImage(),bytes.length());
return true;
}
//printf("Copyless conversion\n");
ImageOf<PixelRgb> wrap;
wrap.setQuantum(out.getQuantum());
wrap.setExternal(bytes.get(),out.width(),out.height());
if (half) {
debayerHalf(in,wrap);
} else {
debayerFull(in,wrap);
}
return true;
}
virtual ssize_t read(yarp::os::Bytes& b) {
if (interrupting) { return -1; }
while (inputCache.size() < b.length()) {
yInfo() <<"*** CHECK OTHER TERMINAL FOR SOMETHING TO TYPE:";
char buf[1000];
needInterrupt = true; // should be mutexed, in real implementation
std::cin.getline(buf,1000);
needInterrupt = false;
if (interrupting) { return -1; }
inputCache += buf;
inputCache += "\r\n";
yInfo() << "Thank you";
}
memcpy(b.get(),inputCache.c_str(),b.length());
inputCache = inputCache.substr(b.length());
return b.length();
}
bool BufferedConnectionWriter::addPool(const yarp::os::Bytes& data)
{
if (pool != nullptr) {
if (data.length() + poolIndex > pool->length()) {
pool = nullptr;
}
}
if (pool == nullptr && data.length() < poolLength) {
bool add = false;
if (*target_used < target->size()) {
yarp::os::ManagedBytes*& bytes = (*target)[*target_used];
if (bytes->length() < poolLength) {
delete bytes;
bytes = new yarp::os::ManagedBytes(poolLength);
}
pool = bytes;
if (pool == nullptr) {
return false;
}
} else {
pool = new yarp::os::ManagedBytes(poolLength);
if (pool == nullptr) {
return false;
}
add = true;
}
(*target_used)++;
poolCount++;
poolIndex = 0;
if (poolLength < 65536) {
poolLength *= 2;
}
pool->setUsed(0);
if (add)
target->push_back(pool);
}
if (pool != nullptr) {
memcpy(pool->get() + poolIndex, data.get(), data.length());
poolIndex += data.length();
pool->setUsed(poolIndex);
return true;
}
return false;
}
bool ShmemOutputStreamImpl::write(const yarp::os::Bytes& b)
{
if (!m_bOpen) {
return false;
}
m_pAccessMutex->acquire();
if (!m_bOpen) {
return false;
}
if (m_pHeader->close) {
m_pAccessMutex->release();
close();
return false;
}
if ((int)m_pHeader->size - (int)m_pHeader->avail < (int)b.length()) {
yarp::conf::ssize_t required = m_pHeader->size + 2 * b.length();
Resize((int)required);
}
if ((int)m_pHeader->head + (int)b.length() <= (int)m_pHeader->size) {
memcpy(m_pData + m_pHeader->head, b.get(), b.length());
} else {
int first_block_size = m_pHeader->size - m_pHeader->head;
memcpy(m_pData + m_pHeader->head, b.get(), first_block_size);
memcpy(m_pData, b.get() + first_block_size, b.length() - first_block_size);
}
m_pHeader->avail += (int)b.length();
m_pHeader->head += (int)b.length();
m_pHeader->head %= m_pHeader->size;
while (m_pHeader->waiting > 0) {
--m_pHeader->waiting;
m_pWaitDataMutex->release();
}
m_pAccessMutex->release();
return true;
}
yarp::conf::ssize_t ShmemInputStreamImpl::read(yarp::os::Bytes& b)
{
m_ReadSerializerMutex.lock();
if (!m_bOpen) {
m_ReadSerializerMutex.unlock();
return -1;
}
char* data = b.get();
char* buf;
size_t len = b.length();
yarp::conf::ssize_t ret;
while (!(ret = read(data, (int)len))) {
#ifdef _ACE_USE_SV_SEM
yarp::os::impl::YARP_timeval tv = ACE_OS::gettimeofday();
tv.sec(tv.sec() + 1);
#else
yarp::os::impl::YARP_timeval tv(1);
#endif
m_pWaitDataMutex->acquire(tv);
if (!m_pSock->recv(&buf, 1)) {
//yDebug("STREAM IS BROKEN");
close();
m_ReadSerializerMutex.unlock();
return -1;
}
}
m_ReadSerializerMutex.unlock();
return ret;
}
yarp::conf::ssize_t yarp::os::impl::LocalCarrierStream::read(yarp::os::Bytes& b)
{
yAssert(false);
return b.length();
}
virtual void getHeader(const yarp::os::Bytes& header) {
const char *target = "HUMANITY";
for (int i=0; i<8 && i<header.length(); i++) {
header.get()[i] = target[i];
}
}
