void *ApoloPort::handleConnections(void *server)
{
Socket *aux_sock= (Socket*) server;
Socket socket=*aux_sock;
PositionableEntity *pos;
RobotSim *robot;
WheeledBaseSim *wb;
static int valid=0, total=0;
while(1)
{
Socket *temp=socket.acceptConnection();
while(temp->IsConnected())
{
char buffer[10000];
int size=0;
if(0<(size=temp->Receive(buffer,10000,-1)))
{
ApoloMessage *m=0;
char *pbuffer=buffer;//recorrepbuffer
while(m=ApoloMessage::getApoloMessage(&pbuffer,size))
{
size-=m->getSize();
total++;
char *nworld=m->getWorld();
char *name=m->getObjectName();
int worldindex=0;
PositionableEntity *element=getElement(nworld,name,&worldindex);
switch(m->getType())
{
case AP_CHECKJOINTS:
case AP_SETJOINTS:
if(element){
robot=dynamic_cast<RobotSim*>(element);
int numJoints=m->getCharAt(0);
for(int i=0;i<numJoints;i++)
if(robot)robot->setJointValue(i,m->getDoubleAt(1+8*i));
if(m->getType()==AP_CHECKJOINTS){
bool res=false;
if(robot){
res=robot->checkRobotColision();
}
char resp[50];
int tam=ApoloMessage::writeBOOL(resp,res);
temp->Send(resp,tam);
}
valid++;
}
break;
case AP_GETLOCATION:
case AP_GETLOCATION_WB:
if(element){
double d[6];
Vector3D p=element->getRelativePosition();
double o[3];
element->getRelativeOrientation(d[3],d[4],d[5]);
for(int i=0;i<3;i++)d[i]=p[i];
char resp[70];
int tam;
if(m->getType()==AP_GETLOCATION)tam=ApoloMessage::writeDoubleVector(resp,6,d);
else {
d[3]=d[5];
tam=ApoloMessage::writeDoubleVector(resp,4,d);
}
temp->Send(resp,tam);
valid++;
}
break;
case AP_PLACE:
if(element){
double d[6];
for(int i=0;i<6;i++)d[i]=m->getDoubleAt(8*i);
element->setAbsoluteT3D(Transformation3D(d[0],d[1],d[2],d[3],d[4],d[5]));
valid++;
}
break;
case AP_PLACE_WB: //place a wheeled based robot. computes the grounded location and collision
if(element){
wb=dynamic_cast<WheeledBaseSim *>(element);
bool res=false;
double d[4];
for(int i=0;i<4;i++)d[i]=m->getDoubleAt(8*i);
Transformation3D taux(d[0],d[1],d[2],mr::Z_AXIS,d[3]);
if(wb)res=wb->dropWheeledBase(taux);
if(res){
res=!(wb->getWorld()->checkCollisionWith(*wb));
}
char resp[50];
int tam=ApoloMessage::writeBOOL(resp,res);
temp->Send(resp,tam);
valid++;
}
break;
case AP_MOVE_WB:
if(element){
wb=dynamic_cast<WheeledBaseSim *>(element);
double d[3];
for(int i=0;i<3;i++)d[i]=m->getDoubleAt(8*i);
wb->move(d[0],d[1]);
wb->simulate(d[2]);
wb->move(0,0);
valid++;
}
break;
case AP_UPDATEWORLD:
if(worldindex>=0){//updates one world
(*world)[worldindex]->getChild()->RefreshChild();
}else //updates all worlds
{
for(int i=0; i<world->size();i++)
(*world)[i]->getChild()->RefreshChild();
}
valid++;
break;
case AP_LINK_TO_ROBOT_TCP:
if(element){
robot=dynamic_cast<RobotSim*>(element);
char *objectname=m->getStringAt(0);
PositionableEntity *target=getElement(nworld,objectname,&worldindex);
if(target&&robot){
target->LinkTo(robot->getTcp());
}
valid++;
}
break;
}
delete m;//aseguro limpieza
}
char messageLog[150];
sprintf(messageLog,"Commands Executed: %d/%d",valid,total);
wxLogStatus(messageLog);
//temp->Send(request,50);
}
}
temp->close();
delete temp;
}
}
bool Homography::extract(cv::Mat &H, irr::core::vector2di *corners, irr::core::vector3df *position, irr::core::vector3df *angles, int refine) {
if ( matches.size() > 3 && objectKeyPoints.size() < sceneKeyPoints.size() )
{
std::vector<cv::Point2f> objectPoints;
std::vector<cv::Point2f> scenePoints;
// get the keypoints from the goodmatches
for( int i = 0; i < matches.size(); i++ )
{
objectPoints.push_back( objectKeyPoints[ matches[i].queryIdx ].pt );
scenePoints.push_back( sceneKeyPoints[ matches[i].trainIdx ].pt );
}
// find the homography of the keypoints.
H = cv::findHomography( objectPoints, scenePoints, CV_RANSAC );
std::vector<cv::Point2f> obj_corners(4);;
std::vector<cv::Point2f> scene_corners(4);
obj_corners[0] = cvPoint( 0, 0 );
obj_corners[1] = cvPoint( objectSize.width, 0 );
obj_corners[2] = cvPoint( objectSize.width, objectSize.height );
obj_corners[3] = cvPoint( 0, objectSize.height );
// get the 2D points for the homography corners
perspectiveTransform( obj_corners, scene_corners, H);
if (refine > 0) {
cv::Mat sceneCopyCopy = sceneCopy.clone();
cv::warpPerspective(sceneCopy, sceneCopy, H, objectSize, cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
cv::Mat H2;
analyze(sceneCopy);
if (extract(H2, NULL, NULL, NULL, refine - 1)) {
H *= H2;
perspectiveTransform( obj_corners, scene_corners, H);
}
}
// give the caller the corners of the 2D plane
if (corners != NULL)
for (int i = 0; i < 4; i++) {
corners[i] = irr::core::vector2di(scene_corners[i].x, scene_corners[i].y);
}
// init the rotation and translation vectors
cv::Mat raux(3, 1, CV_64F), taux(3, 1, CV_64F);
// calculating 3D points
float maxSize = std::max(objectSize.width, objectSize.height);
float unitW = objectSize.width / maxSize;
float unitH = objectSize.height / maxSize;
// get the rotation and translation vectors
std::vector<cv::Point3f> scene_3d_corners(4);
scene_3d_corners[0] = cv::Point3f(-unitW, -unitH, 0);
scene_3d_corners[1] = cv::Point3f( unitW, -unitH, 0);
scene_3d_corners[2] = cv::Point3f( unitW, unitH, 0);
scene_3d_corners[3] = cv::Point3f(-unitW, unitH, 0);
cv::solvePnP(scene_3d_corners, scene_corners, getCamIntrinsic(), cv::Mat(), raux, taux);
// give the caller the 3D plane position and angle
if (position != NULL)
position->set(taux.at<double>(0, 0), -taux.at<double>(1, 0), taux.at<double>(2, 0));
if (angles != NULL)
angles->set(-raux.at<double>(0, 0) * irr::core::RADTODEG, raux.at<double>(1, 0) * irr::core::RADTODEG, -raux.at<double>(2, 0) * irr::core::RADTODEG);
return true;
}
return false;
}
