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);
}