/** Send message.
* @param buf buffer with data to send
* @param buf_len length of buffer, all data will be send.
* @param addr addr to send data to.
* @param addr_len length of address
*/
void
Socket::send(void *buf, size_t buf_len,
const struct sockaddr *addr, socklen_t addr_len)
{
int retval = 0;
unsigned int bytes_written = 0;
struct timeval start, now;
gettimeofday(&start, NULL);
do {
retval = ::sendto(sock_fd, (char *)buf + bytes_written, buf_len - bytes_written, 0,
addr, addr_len);
if (retval == -1) {
if (errno != EAGAIN) {
throw SocketException("Could not read data", errno);
} else {
// just to meet loop condition
retval = 0;
}
} else {
bytes_written += retval;
// reset timeout
gettimeofday(&start, NULL);
}
gettimeofday(&now, NULL);
usleep(0);
} while ((bytes_written < buf_len) && (time_diff_sec(now, start) < timeout) );
if ( bytes_written < buf_len) {
throw SocketException("Write timeout");
}
}
/** Write to the socket.
* Write to the socket. This method can only be used on streams.
* @param buf buffer to write
* @param count number of bytes to write from buf
* @exception SocketException if the data could not be written or if a timeout occured.
*/
void
Socket::write(const void *buf, size_t count)
{
int retval = 0;
unsigned int bytes_written = 0;
struct timeval start, now;
gettimeofday(&start, NULL);
do {
retval = ::write(sock_fd, (char *)buf + bytes_written, count - bytes_written);
if (retval == -1) {
if (errno != EAGAIN) {
throw SocketException("Could not write data", errno);
} else {
// just to meet loop condition
retval = 0;
}
} else {
bytes_written += retval;
// reset timeout
gettimeofday(&start, NULL);
}
gettimeofday(&now, NULL);
usleep(0);
} while ((bytes_written < count) && (time_diff_sec(now, start) < timeout) );
if ( bytes_written < count) {
throw SocketException("Write timeout");
}
}
void
VelocityGlobalFromRelative::setRobotPosition(float x, float y, float ori, timeval t)
{
timeval now;
gettimeofday(&now, 0);
robot_ori = ori;
robot_poseage = time_diff_sec(now, t);
}
/** Connect socket.
* If called for a stream socket this will connect to the remote address. If
* you call this on a datagram socket you will tune in to a specific sender and
* receiver.
* @param addr_port struct containing address and port to connect to
* @param struct_size size of addr_port struct
* @exception SocketException thrown if socket cannot connect, check errno for cause
*/
void
Socket::connect(struct sockaddr *addr_port, unsigned int struct_size)
{
if ( sock_fd == -1 ) throw SocketException("Trying to connect invalid socket");
if (timeout == 0.f) {
if ( ::connect(sock_fd, addr_port, struct_size) < 0 ) {
throw SocketException("Could not connect", errno);
}
} else {
struct timeval start, now;
gettimeofday(&start, NULL);
do {
if ( ::connect(sock_fd, addr_port, struct_size) < 0 ) {
if ( (errno != EINPROGRESS) &&
(errno != EALREADY) ) {
throw SocketException("Could not connect", errno);
}
}
gettimeofday(&now, NULL);
} while (time_diff_sec(now, start) < timeout);
}
}
/* Fill */
static GstFlowReturn gst_android_video_source_fill(GstPushSrc * p_pushsrc, GstBuffer * p_buf)
{
GstAndroidVideoSource *p_src;
int vcd_ret;
static struct timeval time_of_day;
static struct timeval window_time;
static struct timeval start_time;
int timeDiffUsec;
static gint frame_count = 0;
static gint frame_count_window = 0;
GstBuffer *p_outbuf;
GstMapInfo mem_info;
gboolean ok;
GA_LOGTRACE("ENTER %s --xx--> thread(%ld)", __FUNCTION__, pthread_self());
p_src = GST_ANDROIDVIDEOSOURCE(p_pushsrc);
if (gst_buffer_get_size(p_buf) != p_src->m_bufSize) {
GA_LOGWARN("%s: WARNING: gst_buffer_get_size(p_buf)==%d != p_src->m_bufSize==%d", __FUNCTION__, gst_buffer_get_size(p_buf), p_src->m_bufSize);
goto fill_error_negotiation;
}
VCD_checkChangeCamera(p_src->m_devHandle);
if (!p_src->vcdStarted) {
AV_CHECK_ERR(VCD_start(p_src->m_devHandle), fill_error_vcd_start);
p_src->vcdStarted = TRUE;
}
if (!frame_count) { // Only first time
gettimeofday(&start_time, NULL);
gettimeofday(&window_time, NULL);
}
frame_count++;
frame_count_window++;
gettimeofday(&time_of_day, NULL);
timeDiffUsec = time_diff_usec(&time_of_day, &window_time);
if (timeDiffUsec > p_src->log_interval || !p_src->log_interval) {
int framerate;
int framerateWindow;
int timeDiffSec;
timeDiffSec = time_diff_sec(&time_of_day, &start_time);
framerate = frame_count / (timeDiffSec > 0 ? timeDiffSec : 1);
framerateWindow = frame_count_window * 1000000 / timeDiffUsec;
GA_LOGVERB("%s ------> has now been called %d times --Create--> framerate since start: %d fps, framerate last %d usec: %d fps", __FUNCTION__, frame_count, framerate, timeDiffUsec, framerateWindow);
gettimeofday(&window_time, NULL);
frame_count_window = 0;
}
g_warn_if_fail(gst_buffer_is_writable(p_buf)); /* g_warn_if_fail() used for internal error (exception to our rules for this special "buffer copying case") */
g_assert(gst_buffer_is_writable(p_buf)); /* this buf should be allocated in the base class and should always be writable */
p_outbuf = gst_buffer_make_writable(p_buf); /* do this cause we never wanna crash in release even if somebody makes a mistake somewhere... */
ok = gst_buffer_map(p_outbuf, &mem_info, GST_MAP_WRITE);
if (!ok) {
goto fill_error_gst_buffer_map;
}
vcd_ret = VCD_read(p_src->m_devHandle, &(mem_info.data), mem_info.size);
if (vcd_ret == VCD_ERR_NO_DATA) {
// This should never happen. There should always be more data from the device.
// In any case, if it happens we don't want to end or lock or anything, we just
// want to go on as if there actually were data. Specifically, we do not want
// to block the streaming thread...
memset(mem_info.data, 0, mem_info.size);
gst_buffer_unmap(p_outbuf, &mem_info);
GST_BUFFER_OFFSET(p_outbuf) = GST_BUFFER_OFFSET_NONE;
GST_BUFFER_OFFSET_END(p_outbuf) = GST_BUFFER_OFFSET_NONE;
GST_BUFFER_PTS(p_outbuf) = GST_CLOCK_TIME_NONE;
GST_BUFFER_DTS(p_outbuf) = GST_CLOCK_TIME_NONE;
GST_BUFFER_DURATION(p_outbuf) = GST_CLOCK_TIME_NONE;
GA_LOGWARN("%s: WARNING: Returning GST_FLOW_OK with a buffer with zeros...", __FUNCTION__);
return GST_FLOW_OK;
}
if (vcd_ret != VCD_NO_ERROR) {
gst_buffer_unmap(p_outbuf, &mem_info);
goto fill_error_read;
}
gst_buffer_unmap(p_outbuf, &mem_info);
set_gstbuf_time_and_offset(p_src, p_outbuf);
GA_LOGTRACE("EXIT %s", __FUNCTION__);
return GST_FLOW_OK;
/*
* propagate unhandled errors
*/
fill_error_read:
{
GA_LOGERROR("%s: Error when reading data from device!", __FUNCTION__);
return GST_FLOW_ERROR;
}
fill_error_negotiation:
{
GA_LOGERROR("%s: ERROR: Strange buffer size. Negotiation not done? Disallowed renegotiation done?", __FUNCTION__);
return GST_FLOW_ERROR;
}
fill_error_gst_buffer_map:
{
GA_LOGERROR("%s: gst_buffer_map() failed!", __FUNCTION__);
return GST_FLOW_ERROR;
}
fill_error_vcd_start:
{
GA_LOGERROR("%s: FATAL ERROR: Could not start the video device!", __FUNCTION__);
return GST_FLOW_ERROR;
}
}
/** Read from socket.
* Read from the socket. This method can only be used on streams.
* @param buf buffer to write from
* @param count length of buffer, number of bytes to write to stream
* @param read_all setting this to true causes a call to read() loop until exactly
* count bytes have been read, if false it will return after the first successful read
* with the number of bytes available then.
* @return number of bytes read.
* @see write
* @exception SocketException thrown for any error during reading
*/
size_t
Socket::read(void *buf, size_t count, bool read_all)
{
int retval = 0;
unsigned int bytes_read = 0;
if ( timeout > 0 ) {
struct timeval start, now;
gettimeofday(&start, NULL);
if ( read_all ) {
do {
retval = ::read(sock_fd, (char *)buf + bytes_read, count - bytes_read);
if (retval == -1) {
if (errno != EAGAIN) {
throw SocketException("Could not read data", errno);
} else {
// just to meet loop condition
retval = 0;
}
} else {
bytes_read += retval;
// reset timeout
gettimeofday(&start, NULL);
}
gettimeofday(&now, NULL);
usleep(0);
} while ((bytes_read < count) && (time_diff_sec(now, start) < timeout) );
} else {
do {
retval = ::read(sock_fd, (char *)buf, count);
if ( (retval == -1) && (errno != EAGAIN) ) {
throw SocketException("Could not read data", errno);
} else {
bytes_read = retval;
}
usleep(0);
} while (retval < 0);
}
} else {
if ( read_all ) {
do {
retval = ::read(sock_fd, (char *)buf + bytes_read, count - bytes_read);
if (retval == -1) {
throw SocketException("Could not read data", errno);
} else if (retval == 0) {
throw SocketException("Could not read any data");
} else {
bytes_read += retval;
}
usleep(0);
} while (bytes_read < count);
} else {
do {
retval = ::read(sock_fd, (char *)buf, count);
if ( (retval == -1) && (errno != EAGAIN) ) {
throw SocketException("Could not read data", errno);
} else {
bytes_read = retval;
}
usleep(0);
} while (retval < 0);
}
}
if ( read_all && (bytes_read < count)) {
throw SocketException("Read timeout");
}
return bytes_read;
}
