////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void CameraV4L2::Main()
{
struct timeval time;
long sumTime=0;
int fpsFrames = 0;
struct timeval timePrev;
GlobalTime->GetTime(&timePrev);
int frameno;
frameno = 0;
while (true)
{
// Go to sleep for a while (this is a polling loop).
//usleep(50000);
// Test if we are supposed to cancel this thread.
pthread_testcancel();
// Process any pending requests.
HandleRequests();
// Get the time
GlobalTime->GetTime(&time);
sumTime += (time.tv_sec-timePrev.tv_sec)*1000000 + (time.tv_usec-timePrev.tv_usec);
timePrev = time;
fpsFrames++;
if (sumTime > 2000000){
if (showFPS>=0)
PLAYER_MSG2(showFPS, "Fps: %f (%d)", 1000000.0*fpsFrames/sumTime, fpsFrames);
sumTime = 0;
fpsFrames = 0;
}
if (sumTime<0){
sumTime = 0;
fpsFrames = 0;
}
this->tsec = time.tv_sec;
this->tusec = time.tv_usec;
//printf("Time between
// Write data to server
this->WriteData();
frameno++;
}
}
////////////////////////////////////////////////////////////////////////////////
/// Process incoming requests
int CameraV4L2::HandleRequests()
{
void *client;
char request[PLAYER_MAX_REQREP_SIZE];
char outBuf[PLAYER_MAX_REQREP_SIZE];
int len;
while ((len = GetConfig(&client, &request, sizeof(request),NULL)) > 0)
{
int ret = -1;
PLAYER_MSG2(2,"Got Reguest %c (size: %d)", request[0], len);
if (len>1 && (request[0]=='g' || request[0]=='G')){
// pobranie jakiejs wartosci
PLAYER_MSG0(2,"Get type request");
ret = handleCommand(request, len, outBuf, PLAYER_MAX_REQREP_SIZE-1);
if (ret==0) {
outBuf[PLAYER_MAX_REQREP_SIZE-1] = '\0';
PLAYER_MSG1(2,"Sending Ack: %s", outBuf);
if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, outBuf, strlen(outBuf)+1,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
} else {
PLAYER_MSG0(2,"Sendinf NACK");
if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
}
} else {
if (len>0) {
ret = handleCommand(request, len);
}
if (ret == 0){
PLAYER_MSG0(2,"Sending Ack");
if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
} else {
PLAYER_MSG0(2,"Sending Nack");
if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
PLAYER_ERROR("PutReply() failed");
}
}
// if framegrabber is slow(1-10s), it is worth handling all commands at once
//usleep(30000);
}
return 0;
}
void
TE::ProcessCommand(player_msghdr_t*, player_position2d_cmd_vel_t* cmd) {
if (!cmd->state) {
PutPositionCmd(0.0, 0.0);
active_goal = false;
} else {
PLAYER_MSG2(2, "TE::ProcessCommand Stopped by velocity command (%.3f %.3f)",
cmd->vel.px, RTOD(cmd->vel.pa));
// TODO: bylo wylaczone w ostatniej wersji, ale wtedy nie zatrzymuje sie
PutPositionCmd(cmd->vel.px, cmd->vel.pa);
active_goal = false;
}
if (cmd->vel.px < 0)
dir = -1;
else
dir = 1;
}
