コード例 #1
0
//Slot Unwrapper for internet datagram (vision)
void SimulatorCore::UnWrapVisionPacket(SSL_WrapperPacket iPacketTeam){
    SSL_DetectionRobot lPacketRobotBlue;
    SSL_DetectionRobot lPacketRobotYellow;
    SSL_DetectionBall lPacketBall = iPacketTeam.detection().balls(0);
    int lBlueSize = iPacketTeam.detection().robots_blue_size();
    int lYellowSize = iPacketTeam.detection().robots_blue_size();
    mBall->setPosition(lPacketBall.x(),lPacketBall.y());

    for(int i = 0; i < lBlueSize; ++i){
        lPacketRobotBlue = iPacketTeam.detection().robots_blue(i);
        int RobotId = lPacketRobotBlue.robot_id();
        if(RobotId > mNbPlayerPerTeam - 1){
            break;  //TODO clean this with dynamic player allocation
        }
        RobotModel* lRobotBlue = mBlueTeam->GetRobot(RobotId);
        lRobotBlue->setPose(Pose(lPacketRobotBlue.x(),lPacketRobotBlue.y(),
                                 lPacketRobotBlue.orientation()));
        // TODO should be in another function
        lRobotBlue->UpdateSimulationData();
    }

    for(int i = 0; i < lYellowSize; ++i){
        lPacketRobotYellow = iPacketTeam.detection().robots_yellow(i);
        int RobotId = lPacketRobotYellow.robot_id();
        if(RobotId > mNbPlayerPerTeam - 1){
            break;  //TODO clean this with dynamic player allocation
        }
        RobotModel* lRobotYellow = mYellowTeam->GetRobot(RobotId);
        lRobotYellow->setPose(Pose(lPacketRobotYellow.x(),lPacketRobotYellow.y(),
                                 lPacketRobotYellow.orientation()));
    }

    this->sendCommandToGrSim(); //TODO not clear, send back the new command to grsim
}
コード例 #2
0
ファイル: sslField.cpp プロジェクト: roboime/ssl-vision
void SSLField::paint(QPainter *painter, const QStyleOptionGraphicsItem *, QWidget *)
{
    // The field:
    paintField(painter);
    // The robots
    for (int i = 0; i < detection.robots_blue_size(); i++) 
        paintRobot(painter, detection.robots_blue(i), QColor(0,0,255));
    for (int i = 0; i < detection.robots_yellow_size(); i++) 
        paintRobot(painter, detection.robots_yellow(i), QColor(255,255,0));
    // The ball(s)
    for (int i = 0; i < detection.balls_size(); i++) 
    {
        SSL_DetectionBall ball = detection.balls(i);
        painter->setBrush(QColor(255,128,128));
        painter->setPen(QColor(255,128,128));
        painter->drawEllipse(QPoint(ball.x(),-ball.y()),21,21);
    }
    // Fit field in view
    const QSize size(parentView->size());
    int viewWidth = (field_length+2*goal_depth+2*referee_width)*1.2;
    int viewHeight =(field_width+2*referee_width)*1.2;
    float xScale = float(size.width()) / viewWidth;
    float yScale = float(size.height()) / viewHeight;
    float scale = xScale < yScale ? xScale : yScale;
    parentView->setTransform(QTransform(scale, 0, 0, scale, size.width() / 2., size.height() / 2.));
}
コード例 #3
0
ファイル: SSLRealVision.cpp プロジェクト: kfu/metu-ros-pkg
void
SSLRealVision::printBallInfo (const SSL_DetectionBall& ball)
{
  printf ("-Ball : CONF=%4.2f POS=<%9.2f,%9.2f> ", ball.confidence (), ball.x (), ball.y ());
  if (ball.has_z ())
  {
    printf ("Z=%7.2f ", ball.z ());
  }
  else
  {
    printf ("Z=N/A   ");
  }
  printf ("RAW=<%8.2f,%8.2f>\n", ball.pixel_x (), ball.pixel_y ());
}
コード例 #4
0
void Robot::Pass(int Rid, bool ISyellow, float vel){
	//call prepare to aim at/in front of shooter
	SSL_DetectionBall ball = current->balls(0);
	SSL_DetectionRobot robot;
	if(ISyellow){
		robot = current->robots_yellow(Rid);
	}
	else{
		robot = current->robots_blue(Rid);
	}
	float R = distance(robot.x(),robot.y(),x,y);
	float velocityK = R/1500;
	Prepare(ball.x(),ball.y(),vel,velocityK+vel,robot.x(),robot.y());
}
コード例 #5
0
ファイル: SSLRealVision.cpp プロジェクト: kfu/metu-ros-pkg
void
SSLRealVision::cvtToBallState(ssl_msgs::BallState& ball_state, const SSL_DetectionBall& ball)
{
  if(ball.has_area())
    ball_state.area = ball.area();
  if(ball.has_confidence())
    ball_state.confidence = ball.confidence();
  if(ball.has_pixel_x())
    ball_state.pix_coord.x = ball.pixel_x();
  if(ball.has_pixel_y())
    ball_state.pix_coord.y = ball.pixel_y();
  if(ball.has_x())
    ball_state.position.x = ball.x();
  if (ball.has_y())
    ball_state.position.y = ball.y();
  if (ball.has_z())
    ball_state.position.z = ball.z();
}
コード例 #6
0
UpdateBall::UpdateBall(const SSL_DetectionBall& p, double t1, double t2, int camId)
	: Update(t1, t2, camId),
	Point(p.x(), p.y()),
	pimpl(new UpdateBallImpl(p.has_z() ? p.z() : 0.0)) {}
コード例 #7
0
ファイル: visionfilter.cpp プロジェクト: amiryanj/cyrus_ssl
void VisionFilter::update(const SSL_WrapperPacket &packet)
{
    QMutexLocker locker(&mtx_);
    try {
        if(packet.has_detection())
        {
            if( packet.detection().camera_id() < MAX_CAMERA_COUNT )
            {
                double frame_time = packet.detection().t_capture();
                if( frame_time <=  cameraLastFrameTime[packet.detection().camera_id()] ) {
                    throw "Vision: Decayed packet !!!!" ;
                }
                else
                    cameraLastFrameTime[packet.detection().camera_id()] = frame_time;
            }

            if(ParameterManager::getInstance()->get<bool>("vision.camera_0_filtered"))
                if( packet.detection().camera_id() == 0 )
                    throw "Vision: Camera 0 is filtered out";
            if(ParameterManager::getInstance()->get<bool>("vision.camera_1_filtered"))
                if( packet.detection().camera_id() == 1 )
                    throw "Vision: Camera 1 is filtered out";
            if(ParameterManager::getInstance()->get<bool>("vision.camera_2_filtered"))
                if( packet.detection().camera_id() == 2 )
                    throw "Vision: Camera 2 is filtered out";
            if(ParameterManager::getInstance()->get<bool>("vision.camera_3_filtered"))
                if( packet.detection().camera_id() == 3 )
                    throw "Vision: Camera 3 is filtered out";


            float diff_time = packet.detection().t_sent() - last_frame_time;
            FPS = 1/diff_time;
            last_frame_time = packet.detection().t_sent();

            OneObjectFrame frame;

            frame.camera_id = packet.detection().camera_id();
            frame.frame_number = packet.detection().frame_number();

    //        temp_frame.setToCurrentTimeMilliSec();
            frame.timeStampMSec = packet.detection().t_capture() * 1000.0;

            if(ParameterManager::getInstance()->get<bool>("vision.filter_blue_robots") == false)
                for(int i=0; i < packet.detection().robots_blue_size(); i++)
                {
                    SSL_DetectionRobot Robot = packet.detection().robots_blue(i);
                    frame.position = Vector3D(Robot.x(), Robot.y(), Robot.orientation());
                    frame.confidence = Robot.confidence();
                    robotFilter_cluster[SSL::Blue][Robot.robot_id()]->putNewFrame(frame);

//                    if(i == ParameterManager::getInstance()->get<int>("skills.under_test_robot"))
                        robotFilter_kalman[SSL::Blue][Robot.robot_id()]->putNewFrame(frame);

                }

            if(ParameterManager::getInstance()->get<bool>("vision.filter_yellow_robots") == false)
                for(int i=0; i< packet.detection().robots_yellow_size(); i++)
                {
                    SSL_DetectionRobot Robot = packet.detection().robots_yellow(i);
                    frame.position = Vector3D(Robot.x(), Robot.y(), Robot.orientation());
                    frame.confidence = Robot.confidence();
                    robotFilter_cluster[SSL::Yellow][Robot.robot_id()]->putNewFrame(frame);
                    robotFilter_kalman[SSL::Yellow][Robot.robot_id()]->putNewFrame(frame);
//                    cout << "Robot Yellow [" << i << "] detected" << endl;
                }

            vector<OneObjectFrame> balls_vec;
            for(int i=0; i< packet.detection().balls_size(); i++)
            {
                SSL_DetectionBall Ball = packet.detection().balls(i);
                frame.position = Vector2D(Ball.x(), Ball.y()).to3D();
                frame.confidence = Ball.confidence();
                balls_vec.push_back(frame);

            //  file <<i << " , " << (long)temp_frame.timeStampMilliSec<< " , ";
            //  file << Ball.x() <<" , " << Ball.y() << endl;
            }

            if(balls_vec.empty()) {
                throw "Warning:: No ball exists in current frame";
            }
            if(ballFilter->isEmpty()) {
                ballFilter->initialize(balls_vec[0]);
                throw "Initializing ball filter module";
            }
            if(balls_vec.size() == 1) {
                ballFilter->putNewFrame(balls_vec[0]);
            }
            else {
                ballFilter->putNewFrameWithManyBalls(balls_vec);
                throw "Warning:: More than One ball exist in current frame" ;
            }
        }

        if(packet.has_geometry())   {
            // SSL_GeometryData geometryData = packet.geometry();
        }

    }
    catch (const char* msg) {
//        cout << msg << endl;
            mtx_.unlock();
    }
}
コード例 #8
0
void Robot::PathTo(float x1, float y1, float vel){

	printf("\nentered pathto %d",id);
	float R = sqrt(pow(x-x1,2)+pow(y-y1,2));
	float x2 = x1;
	float y2 = y1;
	//printf("\nA is %f, B is %f, C is %f, R is %f",a,b,c,R);
	//printf("In the way is %d, Interference is %d",inTheWay(x,x1,-1000,y,y1,-0.001,true),interference(a,b,c,-0.001,-1000,200));



	if(R > 0 && 0< current->robots_yellow_size()  && 0 < current->robots_blue_size()){
	for(int i = 0; i < current->robots_yellow_size(); i++){
		SSL_DetectionRobot *robot = const_cast<SSL_DetectionRobot*>(&(current->robots_yellow(i)));
		if(!(yellow && id==i)){
		if(id == 1){
			inTheWay(x,x1,robot->x(),y,y1,robot->y(),true);
			printf("InTheWay? %d, Intefere? %d",inTheWay(x,x1,robot->x(),y,y1,robot->y(),true),interference(x,y,x1,y1,robot->x(),robot->y(),200));}
		if(interference(x,y,x1,y1,robot->x(),robot->y(),300) && distance(x,robot->x(),y,robot->y()) < R && inTheWay(x,x1,robot->x(),y,y1,robot->y(),false)){
			float angle2 = atan2(y-robot->y(),x-robot->x());
			angle2 += 1.5;
			float xUp = 300*cos(angle2);
			float yUp = 300*sin(angle2);
			xUp = xUp + robot->x();
			yUp = yUp + robot->y();
			x2 = xUp;
			y2 = yUp;
			R = distance(x,robot->x(),y,robot->y());
			printf("\nRobot %d in yellow is in the Way!!",i);
		}}
	}
	for(int i = 0; i < current->robots_blue_size(); i++){
		SSL_DetectionRobot * robot = const_cast<SSL_DetectionRobot*>(&current->robots_blue(i));
		if(!(!yellow && id == i)){
		if(interference(x,y,x1,y1,robot->x(),robot->y(),300) && distance(x,robot->x(),y,robot->y()) < R && inTheWay(x,x1,robot->x(),y,y1,robot->y(),false)){
			float angle2 = atan2(y-robot->y(),x-robot->x());
			angle2 += 1.5;
			float xUp = 300*cos(angle2);
			float yUp = 300*sin(angle2);
			xUp = xUp + robot->x();
			yUp = yUp + robot->y();
			x2 = xUp;
			y2 = yUp;
			R  = distance(x,robot->x(),y,robot->y());
			printf("\nRobot %d in blue is in the Way!!",i);
		}}
	}
	SSL_DetectionBall ball = current->balls(0);
	if(interference(x,y,x1,y1,ball.x(),ball.y(),150) && distance(x,ball.x(),y,ball.y()) < R && inTheWay(x,x1,ball.x(),y,y1,ball.y(),true)){
			float angle2 = atan2(y-ball.y(),x-ball.x());
			angle2 += 1.5;
			float xUp = 200*cos(angle2);
			float yUp = 200*sin(angle2);
			xUp = xUp + ball.x();
			yUp = yUp + ball.y();
			x2 = xUp;
			y2 = yUp;
			R  = distance(x,ball.x(),y,ball.y());
			printf("\nBall is in the WAY!!!");
		}
	//printf("InTheWay? %d, Intefere? %d",inTheWay(x,x1,ball.x(),y,y1,ball.y(),true),interference(x,y,x1,y1,ball.x(),ball.y(),300));
	}
	Move(x2,y2,vel);
	printf("\nX2 is %f, Y2 is %f",x2,y2);
	printf("Exited PATHTO!! %d",id);
}