void PortStream::waitRead(base::Time const& timeout)
{
if (!mPacketRead.data.empty())
return;
uint64_t sleep_time;
int count;
uint64_t full_timeout = timeout.toMicroseconds();
if (full_timeout > 10000)
{
sleep_time = 10000;
count = (full_timeout + sleep_time - 1) / sleep_time;
}
else
{
count = 1;
sleep_time = full_timeout;
}
for (int i = 0; i < count; ++i)
{
if (mIn.read(mPacketRead, false) == RTT::NewData)
return;
usleep(sleep_time);
}
throw TimeoutError(TimeoutError::NONE, "waitRead(): timeout");
}
void FDStream::waitWrite(base::Time const& timeout)
{
fd_set set;
FD_ZERO(&set);
FD_SET(m_fd, &set);
timeval timeout_spec = { static_cast<time_t>(timeout.toSeconds()), timeout.toMicroseconds() % 1000000 };
int ret = select(m_fd + 1, NULL, &set, NULL, &timeout_spec);
if (ret < 0 && errno != EINTR)
throw UnixError("waitWrite(): error in select()");
else if (ret == 0)
throw TimeoutError(TimeoutError::NONE, "waitWrite(): timeout");
}
void ros_convertions::toROS( ros::Time& ros, ::base::Time const& value )
{
ros.fromNSec(value.toMicroseconds() * 1000);
}