예제 #1
0
void Coach::update(SSL_DetectionFrame newFrame)
{
    //update memory
    if(_memory.size() < 5) {
        _memory.push_back(newFrame);
    }
    else {
        _memory.push_back(newFrame);
        _memory.erase(_memory.begin());

        //update FieldState
        fs.bBots.clear();
        fs.yBots.clear();
        fs.ball = Movable(newFrame.balls(0).x(), newFrame.balls(0).y(), 0, 0);	//velocity calculated in analysis

        for(int i = 0; i < _memory[4].robots_blue_size(); i++)
        {
            fs.bBots.push_back(Robot(_memory[4].robots_blue(i).robot_id(),
                                     _memory[4].robots_blue(i).orientation(),
                                     _memory[4].robots_blue(i).x(),
                                     _memory[4].robots_blue(i).y()));
        }
        for(int i = 0; i < _memory[4].robots_yellow_size(); i++)
        {
            fs.yBots.push_back(Robot(_memory[4].robots_yellow(i).robot_id(),
                                     _memory[4].robots_yellow(i).orientation(),
                                     _memory[4].robots_yellow(i).x(),
                                     _memory[4].robots_yellow(i).y()));
        }

        //call analysis to update velocity
        Analysis();
    }
}
예제 #2
0
파일: sslvision.cpp 프로젝트: KN2C/KN2C-SSL
void SSLVision::parse(SSL_DetectionFrame &pck)
{

    // update camera fps
    int cid = pck.camera_id();
    if(cid == 0) _fpscam0.Pulse();
    if(cid == 1) _fpscam1.Pulse();

    switch(_camera)
    {
    case CAMERA_BOTH:
        break;
    case CAMERA_0:
        if (cid==1) return;
        break;
    case CAMERA_1:
        if (cid==0) return;
        break;
    case CAMERA_NONE:
    default:
        return;
    }

    // update vision frame
    //_vframe[cid].frame_number =  pck.frame_number();

    vector<Position> pt;

    // Team side Coefficient
    float ourSide = (_side == SIDE_RIGHT)? -1.0f : 1.0f;
    double time = _time.elapsed(); //pck.t_capture();

    // insert balls
    int max_balls=min(VOBJ_MAX_NUM, pck.balls_size());
    for(int i=0; i<max_balls; ++i)
    {
        auto b = pck.balls(i);
        if(b.has_confidence() && b.has_x() && b.has_y())
            if(b.confidence() > MIN_CONF && fabs(b.x()) < FIELD_MAX_X && fabs(b.y()) < FIELD_MAX_Y)
            {
                Position tp;
                tp.loc = Vector2D(b.x()*ourSide, b.y()*ourSide);
                pt.push_back(tp);
            }
    }
    _wm->ball.seenAt(pt, time, cid);

    if(_color == COLOR_BLUE)
    {
        APPEND_ROBOTS(blue, our);
        APPEND_ROBOTS(yellow, opp);
    }
    else // _color == COLOR_YELLOW
    {
        APPEND_ROBOTS(yellow, our);
        APPEND_ROBOTS(blue, opp);
    }

}
예제 #3
0
void Vision::doInBackground() {
	//printf("Vision::doInBackGround - started\n");

	if (client.receive(packet)) {
		//printf("-----Received Wrapper Packet---------------------------------------------\n");
		//see if the packet contains a robot detection frame:

		if (packet.has_detection()) {
			SSL_DetectionFrame detection = packet.detection();
			//Display the contents of the robot detection results:

			int balls_n = detection.balls_size();
			int robots_blue_n = detection.robots_blue_size();
			int robots_yellow_n = detection.robots_yellow_size();

			//Ball info:
			int correctBallId = -1;
			SSL_DetectionBall current;
			SSL_DetectionBall correctBall;
			int ballConfidence = 0.0;
			static int ballMiss = 0;
			for (int i = 0; i < balls_n; i++) {
				current = detection.balls(i);
				if (current.confidence() > ballConfidence) {
					ballConfidence = detection.balls(i).confidence();
					correctBallId = i;
					correctBall = current;
				}
			}

			if (correctBallId != -1) {
				Vision::ball.updateBall(correctBall);
				Vision::ball._confidence = correctBall.confidence();
				ballMiss = 0;
			} else {
				ballMiss++;
			}
			Vision::ball._onField = !(ballMiss > 10);

			//Blue robot info:

			for (int i = 0; i < robots_blue_n; i++) {
				Vision::robots.updateRobot(detection.robots_blue(i));
			}
			for (int i = 0; i < 10; i++) {
				Vision::robots.robots[i]._onField = !(++(Vision::robots.robotsMisses[i]) > 10);
			}

			//Yellow robot info:
			for (int i = 0; i < robots_yellow_n; i++) {
				Vision::opponents.updateRobot(detection.robots_yellow(i));
			}
			for (int i = 0; i < 10; i++) {
				Vision::opponents.robots[i]._onField = !(++(Vision::opponents.robotsMisses[i]) > 10);
			}

		}

		//see if packet contains geometry data:
		if (packet.has_geometry()) {
			const SSL_GeometryData & geom = packet.geometry();
			//printf("-[Geometry Data]-------\n");

			const SSL_GeometryFieldSize & field = geom.field();
			/*printf("Field Dimensions:\n");
			 printf("  -line_width=%d (mm)\n", field.line_width());
			 printf("  -field_length=%d (mm)\n", field.field_length());
			 printf("  -field_width=%d (mm)\n", field.field_width());
			 printf("  -boundary_width=%d (mm)\n", field.boundary_width());
			 printf("  -referee_width=%d (mm)\n", field.referee_width());
			 printf("  -goal_width=%d (mm)\n", field.goal_width());
			 printf("  -goal_depth=%d (mm)\n", field.goal_depth());
			 printf("  -goal_wall_width=%d (mm)\n", field.goal_wall_width());
			 printf("  -center_circle_radius=%d (mm)\n", field.center_circle_radius());
			 printf("  -defense_radius=%d (mm)\n", field.defense_radius());
			 printf("  -defense_stretch=%d (mm)\n", field.defense_stretch());
			 printf("  -free_kick_from_defense_dist=%d (mm)\n", field.free_kick_from_defense_dist());
			 printf("  -penalty_spot_from_field_line_dist=%d (mm)\n", field.penalty_spot_from_field_line_dist());
			 printf("  -penalty_line_from_spot_dist=%d (mm)\n", field.penalty_line_from_spot_dist());*/

			int calib_n = geom.calib_size();
			for (int i = 0; i < calib_n; i++) {
				const SSL_GeometryCameraCalibration & calib = geom.calib(i);
				/*printf("Camera Geometry for Camera ID %d:\n", calib.camera_id());
				 printf("  -focal_length=%.2f\n", calib.focal_length());
				 printf("  -principal_point_x=%.2f\n", calib.principal_point_x());
				 printf("  -principal_point_y=%.2f\n", calib.principal_point_y());
				 printf("  -distortion=%.2f\n", calib.distortion());
				 printf("  -q0=%.2f\n", calib.q0());
				 printf("  -q1=%.2f\n", calib.q1());
				 printf("  -q2=%.2f\n", calib.q2());
				 printf("  -q3=%.2f\n", calib.q3());
				 printf("  -tx=%.2f\n", calib.tx());
				 printf("  -ty=%.2f\n", calib.ty());
				 printf("  -tz=%.2f\n", calib.tz());*/

				if (calib.has_derived_camera_world_tx() && calib.has_derived_camera_world_ty() && calib.has_derived_camera_world_tz()) {
					/*printf("  -derived_camera_world_tx=%.f\n", calib.derived_camera_world_tx());
					 printf("  -derived_camera_world_ty=%.f\n", calib.derived_camera_world_ty());
					 printf("  -derived_camera_world_tz=%.f\n", calib.derived_camera_world_tz());*/
				}

			}
		}
	}

	//printf("Vision::doInBackGround - terminated\n");
}
예제 #4
0
void
SSLRealVision::update ()
{
  //TODO: camera merging has not been done yet
  if (client_.receive (packet_))
  {
    ROS_DEBUG("packet received");

    if (packet_.has_detection ())
    {
      SSL_DetectionFrame detection = packet_.detection ();
      global_state_.header.stamp = ros::Time::now();
      global_state_.header.frame_id = ssl::naming::frame::FIELD;

      int balls_n = detection.balls_size ();
      int robots_blue_n = detection.robots_blue_size ();
      int robots_yellow_n = detection.robots_yellow_size ();

//      std::cout<<balls_n<<std::endl;
//      std::cout<<robots_blue_n<<std::endl;
//      std::cout<<robots_yellow_n<<std::endl;

      //Ball info:
      global_state_.balls.clear();
      for (int i = 0; i < balls_n; i++)
      {
        SSL_DetectionBall ball = detection.balls (i);
        printBallInfo(ball);
        ssl_msgs::BallState ball_state;
        cvtToBallState(ball_state, ball);
        global_state_.balls.push_back(ball_state);
      }

      //Blue robot info:
      //TODO: try to find the most probable id for an unidentified robot
      //uint8_t suspicious_index = -1;
      for (int i = 0; i < robots_blue_n; i++)
      {
        SSL_DetectionRobot robot = detection.robots_blue (i);
        printf ("-Robot(B) (%2d/%2d): ", i + 1, robots_blue_n);
        printRobotInfo (robot);

        ssl_msgs::GlobalRobotState robot_state;
        if(cvtToRobotState(robot_state, robot)){}
          global_state_.blue_team[robot_state.id] = robot_state;
      }
      //if(suspicious_index!= -1){}


      //Yellow robot info:
      for (int i = 0; i < robots_yellow_n; i++)
      {
        SSL_DetectionRobot robot = detection.robots_yellow (i);
        printf ("-Robot(Y) (%2d/%2d): ", i + 1, robots_yellow_n);
        printRobotInfo (robot);
        ssl_msgs::GlobalRobotState robot_state;
        if(cvtToRobotState(robot_state, robot))
          global_state_.yellow_team[robot_state.id] = robot_state;
      }

    }
    //see if packet contains geometry data:
    if (packet_.has_geometry ())
    {
      const SSL_GeometryData & geom = packet_.geometry ();
      field_width_ = geom.field().field_width();
      field_height_= geom.field().field_length();
      field_outer_width_ = geom.field().field_width() + 2* geom.field().boundary_width();
      field_outer_height_= geom.field().field_length()+ 2* geom.field().boundary_width();

      //update parameters
      nh_->setParam("Field/width",field_width_);
      nh_->setParam("Field/height",field_height_);
      nh_->setParam("Field/outer_width",field_outer_width_);
      nh_->setParam("Field/outer_height",field_outer_height_);

      printGeomInfo (geom);
    }
  }
}