예제 #1
0
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;
}