コード例 #1
0
void SoccerView::UpdateRobots ( SSL_DetectionFrame &detection )
{
    int robots_blue_n =  detection.robots_blue_size();
    int robots_yellow_n =  detection.robots_yellow_size();
    int i,j,yellowj=0,bluej=0;
    int team=teamBlue;
    SSL_DetectionRobot robot;
    for ( i = 0; i < robots_blue_n+robots_yellow_n; i++ )
    {
        if ( i<robots_blue_n )
        {
            robot = detection.robots_blue ( i );
            team = teamBlue;
            j=bluej;
        }
        else
        {
            robot = detection.robots_yellow ( i-robots_blue_n );
            team = teamYellow;
            j=yellowj;
        }

        double x,y,orientation,conf =robot.confidence();
        int id=NA;
        if ( robot.has_robot_id() )
            id = robot.robot_id();
        else
            id = NA;
        x = robot.x();
        y = robot.y();
        if ( robot.has_orientation() )
            orientation = robot.orientation() *180.0/M_PI;
        else
            orientation = NAOrientation;

        //seek to the next robot of the same camera and team colour
        while ( j<robots.size() && ( robots[j]->key!=detection.camera_id() || robots[j]->teamID!=team ) )
            j++;

        if ( j+1>robots.size() )
            AddRobot ( new Robot ( x,y,orientation,team,id,detection.camera_id(),conf ) );

        robots[j]->SetPose ( x,y,orientation,conf );
        QString label;

        if ( id!=NA )
            label.setNum ( id,16 );
        else
            label = "?";
        label = label.toUpper();
        if ( label!=robots[j]->robotLabel )
            robots[j]->robotLabel = label;

        j++;

        if ( i<robots_blue_n )
            bluej=j;
        else
            yellowj=j;
    }
    for ( j=bluej; j<robots.size(); j++ )
    {
        if ( robots[j]->key==detection.camera_id() && robots[j]->teamID==teamBlue )
            robots[j]->conf=0.0;
    }
    for ( j=yellowj; j<robots.size(); j++ )
    {
        if ( robots[j]->key==detection.camera_id() && robots[j]->teamID==teamYellow )
            robots[j]->conf=0.0;
    }
    return;
}
コード例 #2
0
ファイル: Vision.cpp プロジェクト: brunogouveia/lia-soccer
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");
}
コード例 #3
0
ファイル: SSLRealVision.cpp プロジェクト: kfu/metu-ros-pkg
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);
    }
  }
}