void Environment::sendVision() {
    SSL_WrapperPacket wrapper;
    SSL_DetectionFrame* det = wrapper.mutable_detection();
    det->set_frame_number(_frameNumber++);
    det->set_camera_id(0);

    struct timeval tv;
    gettimeofday(&tv, nullptr);
    det->set_t_capture(tv.tv_sec + (double)tv.tv_usec * 1.0e-6);
    det->set_t_sent(det->t_capture());

    for (Robot* robot : _yellow) {
        if ((rand() % 100) < robot->visibility) {
            SSL_DetectionRobot* out = det->add_robots_yellow();
            convert_robot(robot, out);
        }
    }

    for (Robot* robot : _blue) {
        if ((rand() % 100) < robot->visibility) {
            SSL_DetectionRobot* out = det->add_robots_blue();
            convert_robot(robot, out);
        }
    }

    for (const Ball* b : _balls) {
        Geometry2d::Point ballPos = b->getPosition();

        // bool occ;
        // if (ballPos.x < 0)
        // {
        // 	occ = occluded(ballPos, cam0);
        // } else {
        // 	occ = occluded(ballPos, cam1);
        // }

        if ((rand() % 100) < ballVisibility) {
            SSL_DetectionBall* out = det->add_balls();
            out->set_confidence(1);
            out->set_x(ballPos.x * 1000);
            out->set_y(ballPos.y * 1000);
            out->set_pixel_x(ballPos.x * 1000);
            out->set_pixel_y(ballPos.y * 1000);
        }
    }

    std::string buf;
    wrapper.SerializeToString(&buf);

    if (sendShared) {
        _visionSocket.writeDatagram(&buf[0], buf.size(), MulticastAddress,
                                    SharedVisionPort);
    } else {
        _visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress,
                                    SimVisionPort);
        _visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress,
                                    SimVisionPort + 1);
    }
}
示例#2
0
SSL_WrapperPacket* SSLWorld::generatePacket()
{
    SSL_WrapperPacket* packet = new SSL_WrapperPacket;
    float x,y,z,dir;
    ball->getBodyPosition(x,y,z);    
    packet->mutable_detection()->set_camera_id(0);
    packet->mutable_detection()->set_frame_number(framenum);    
    double t_elapsed = timer->elapsed()/1000.0;
    packet->mutable_detection()->set_t_capture(t_elapsed);
    packet->mutable_detection()->set_t_sent(t_elapsed);
    float dev_x = cfg->noiseDeviation_x();
    float dev_y = cfg->noiseDeviation_y();
    float dev_a = cfg->noiseDeviation_angle();
    if (cfg->noise()==false) {dev_x = 0;dev_y = 0;dev_a = 0;}
    if ((cfg->vanishing()==false) || (rand0_1() > cfg->ball_vanishing()))
    {
        SSL_DetectionBall* vball = packet->mutable_detection()->add_balls();
        vball->set_x(randn_notrig(x*1000.0f,dev_x));
        vball->set_y(randn_notrig(y*1000.0f,dev_y));
        vball->set_z(z*1000.0f);
        vball->set_pixel_x(x*1000.0f);
        vball->set_pixel_y(y*1000.0f);
        vball->set_confidence(0.9 + rand0_1()*0.1);
    }
    for(int i = 0; i < ROBOT_COUNT; i++){
        if ((cfg->vanishing()==false) || (rand0_1() > cfg->blue_team_vanishing()))
        {
            if (!robots[i]->on) continue;
            SSL_DetectionRobot* rob = packet->mutable_detection()->add_robots_blue();
            robots[i]->getXY(x,y);
            dir = robots[i]->getDir();
            rob->set_robot_id(i);
            rob->set_pixel_x(x*1000.0f);
            rob->set_pixel_y(y*1000.0f);
            rob->set_confidence(1);
            rob->set_x(randn_notrig(x*1000.0f,dev_x));
            rob->set_y(randn_notrig(y*1000.0f,dev_y));
            rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f);
        }
    }
    for(int i = ROBOT_COUNT; i < ROBOT_COUNT*2; i++){
        if ((cfg->vanishing()==false) || (rand0_1() > cfg->yellow_team_vanishing()))
        {
            if (!robots[i]->on) continue;
            SSL_DetectionRobot* rob = packet->mutable_detection()->add_robots_yellow();
            robots[i]->getXY(x,y);
            dir = robots[i]->getDir();
            rob->set_robot_id(i-ROBOT_COUNT);
            rob->set_pixel_x(x*1000.0f);
            rob->set_pixel_y(y*1000.0f);
            rob->set_confidence(1);
            rob->set_x(randn_notrig(x*1000.0f,dev_x));
            rob->set_y(randn_notrig(y*1000.0f,dev_y));
            rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f);
        }
    }
    return packet;
}
ProcessResult PluginDetectBalls::process ( FrameData * data, RenderOptions * options ) {
  ( void ) options;
  if ( data==0 ) return ProcessingFailed;

  SSL_DetectionFrame * detection_frame = 0;

  detection_frame= ( SSL_DetectionFrame * ) data->map.get ( "ssl_detection_frame" );
  if ( detection_frame == 0 ) detection_frame= ( SSL_DetectionFrame * ) data->map.insert ( "ssl_detection_frame",new SSL_DetectionFrame() );

  int color_id_ball = _lut->getChannelID ( _settings->_color_label->getString() );
  if ( color_id_ball == -1 ) {
    printf ( "Unknown Ball Detection Color Label: '%s'\nAborting Plugin!\n",_settings->_color_label->getString().c_str() );
    return ProcessingFailed;
  }

  //delete any previous detection results:
  detection_frame->clear_balls();

  //TODO: add a vartype notifier for better performance.

  //initialize filter:
  if (vnotify.hasChanged()) {
    max_balls = _settings->_max_balls->getInt();
    filter.setWidth ( _settings->_ball_min_width->getInt(),_settings->_ball_max_width->getInt() );
    filter.setHeight ( _settings->_ball_min_height->getInt(),_settings->_ball_max_height->getInt() );
    filter.setArea ( _settings->_ball_min_area->getInt(),_settings->_ball_max_area->getInt() );
    field_filter.update ( field );

    //copy all vartypes to local variables for faster repeated lookup:
    filter_ball_in_field = _settings->_ball_on_field_filter->getBool();
    filter_ball_on_field_filter_threshold = _settings->_ball_on_field_filter_threshold->getDouble();
    filter_ball_in_goal  = _settings->_ball_in_goal_filter->getBool();
    filter_ball_histogram = _settings->_ball_histogram_enabled->getBool();
    if ( filter_ball_histogram ) {
      if ( color_id_ball != color_id_orange ) {
        printf ( "Warning: ball histogram check is only configured for orange balls!\n" );
        printf ( "Please disable the histogram check in the Ball Detection Plugin settings\n" );
      }
      if ( color_id_pink==-1 || color_id_orange==-1 || color_id_yellow==-1 || color_id_field==-1 ) {
        printf ( "WARNING: some LUT color labels where undefined for the ball detection plugin\n" );
        printf ( "         Disabling histogram check!\n" );
        filter_ball_histogram=false;
      }
    }

    min_greenness = _settings->_ball_histogram_min_greenness->getDouble();
    max_markeryness = _settings->_ball_histogram_max_markeryness->getDouble();

    //setup values used for the gaussian confidence measurement:
    filter_gauss = _settings->_ball_gauss_enabled->getBool();
    exp_area_min =  _settings->_ball_gauss_min->getInt();
    exp_area_max = _settings->_ball_gauss_max->getInt();
    exp_area_var = sq ( _settings->_ball_gauss_stddev->getDouble() );
    z_height= _settings->_ball_z_height->getDouble();

    near_robot_filter = _settings->_ball_too_near_robot_enabled->getBool();
    near_robot_dist_sq = sq(_settings->_ball_too_near_robot_dist->getDouble());
  }

  const CMVision::Region * reg = 0;

  //acquire orange region list from data-map:
  CMVision::ColorRegionList * colorlist;
  colorlist= ( CMVision::ColorRegionList * ) data->map.get ( "cmv_colorlist" );
  if ( colorlist==0 ) {
    printf ( "error in ball detection plugin: no region-lists were found!\n" );
    return ProcessingFailed;
  }
  reg = colorlist->getRegionList ( color_id_ball ).getInitialElement();

  //acquire color-labeled image from data-map:
  const Image<raw8> * image = ( Image<raw8> * ) ( data->map.get ( "cmv_threshold" ) );
  if ( image==0 ) {
    printf ( "error in ball detection plugin: no color-thresholded image was found!\n" );
    return ProcessingFailed;
  }

  int robots_blue_n=0;
  int robots_yellow_n=0;
  bool use_near_robot_filter=near_robot_filter;
  if ( use_near_robot_filter ) {
    SSL_DetectionFrame * detection_frame = ( SSL_DetectionFrame * ) data->map.get ( "ssl_detection_frame" );
    if ( detection_frame==0 ) {
      use_near_robot_filter=false;
    } else {
      robots_blue_n=detection_frame->robots_blue_size();
      robots_yellow_n=detection_frame->robots_yellow_size();
      if (robots_blue_n==0 && robots_yellow_n==0) use_near_robot_filter=false;
    }
  }

  if ( max_balls > 0 ) {
    list<BallDetectResult> result;
    filter.init ( reg );
    
    while ( ( reg = filter.getNext() ) != 0 ) {
      float conf = 1.0;

      if ( filter_gauss==true ) {
        int a = reg->area - bound ( reg->area,exp_area_min,exp_area_max );
        conf = gaussian ( a / exp_area_var );
      }

      //TODO: add a plugin for confidence masking... possibly multi-layered.
      //      to replace the commented det.mask.get(...) below:
      //if (filter_conf_mask) conf*=det.mask.get(reg->cen_x,reg->cen_y));

      //convert from image to field coordinates:
      vector2d pixel_pos ( reg->cen_x,reg->cen_y );
      vector3d field_pos_3d;
      camera_parameters.image2field ( field_pos_3d,pixel_pos,z_height );
      vector2d field_pos ( field_pos_3d.x,field_pos_3d.y );

      //filter points that are outside of the field:
      if ( filter_ball_in_field==true && field_filter.isInFieldPlusThreshold ( field_pos, max(0.0,filter_ball_on_field_filter_threshold) ) ==false ) {
        conf = 0.0;
      }

      //filter out points that are deep inside the goal-box
      if ( filter_ball_in_goal==true && field_filter.isFarInGoal ( field_pos ) ==true ) {
        conf = 0.0;
      }

      //TODO add ball-too-near-robot filter
      if ( use_near_robot_filter && conf > 0.0 ) {
        int robots_n=0;
        for (int team = 0; team < 2; team++) {
          if (team==0) {
            robots_n=robots_blue_n;
          } else {
            robots_n=robots_yellow_n;
          }
          if (robots_n > 0) {
            ::google::protobuf::RepeatedPtrField< ::SSL_DetectionRobot > * robots;
            if (team==0) {
              robots = detection_frame->mutable_robots_blue();
            } else {
              robots = detection_frame->mutable_robots_yellow();
            }
            for (int r = 0; r < robots_n; r++) {
              const SSL_DetectionRobot & robot = robots->Get(r);
              if (robot.confidence() > 0.0) {
                if ((sq((double)(robot.x())-(double)(field_pos.x)) + sq((double)(robot.y())-(double)(field_pos.y))) < near_robot_dist_sq) {
                  conf = 0.0;
                  break;
                }
              }
            }
            if (conf==0.0) break;
          }
        }
      }

      // histogram check if enabled
      if ( filter_ball_histogram && conf > 0.0 && checkHistogram ( image, reg, min_greenness, max_markeryness ) ==false ) {
        conf = 0.0;
      }

      // add filtered region to the region list
      if(conf > 0) {
        result.push_back(BallDetectResult(reg,conf));
      }

    }

    // sort result by confidence and output first max_balls region(s)
    result.sort();
    
    int num_ball = 0;
    list<BallDetectResult>::reverse_iterator it;
    for(it=result.rbegin(); it!=result.rend(); it++) {
      if(++num_ball > max_balls)
        break;

      //update result:
      SSL_DetectionBall* ball = detection_frame->add_balls();

      ball->set_confidence ( it->conf );

      vector2d pixel_pos ( it->reg->cen_x,it->reg->cen_y );
      vector3d field_pos_3d;
      camera_parameters.image2field ( field_pos_3d,pixel_pos,z_height );

      ball->set_area ( it->reg->area );
      ball->set_x ( field_pos_3d.x );
      ball->set_y ( field_pos_3d.y );
      ball->set_pixel_x ( it->reg->cen_x );
      ball->set_pixel_y ( it->reg->cen_y );
    }

  }

  return ProcessingOk;

}
示例#4
0
SSL_WrapperPacket* SSLWorld::generatePacket()
{
    SSL_WrapperPacket* packet = new SSL_WrapperPacket;
    dReal x,y,z,dir;
    ball->getBodyPosition(x,y,z);    
    packet->mutable_detection()->set_camera_id(0);
    packet->mutable_detection()->set_frame_number(framenum);    
    dReal t_elapsed = timer->elapsed()/1000.0;
    packet->mutable_detection()->set_t_capture(t_elapsed);
    packet->mutable_detection()->set_t_sent(t_elapsed);
    dReal dev_x = cfg->noiseDeviation_x();
    dReal dev_y = cfg->noiseDeviation_y();
    dReal dev_a = cfg->noiseDeviation_angle();
    if (sendGeomCount++ % cfg->sendGeometryEvery() == 0)
    {
        SSL_GeometryData* geom = packet->mutable_geometry();
        SSL_GeometryFieldSize* field = geom->mutable_field();
        field->set_line_width(CONVUNIT(cfg->Field_Line_Width()));
        field->set_field_length(CONVUNIT(cfg->Field_Length()));
        field->set_field_width(CONVUNIT(cfg->Field_Width()));
        field->set_boundary_width(CONVUNIT(cfg->Field_Margin()));
        field->set_referee_width(CONVUNIT(cfg->Field_Referee_Margin()));
        field->set_goal_width(CONVUNIT(cfg->Goal_Width()));
        field->set_goal_depth(CONVUNIT(cfg->Goal_Depth()));
        field->set_goal_wall_width(CONVUNIT(cfg->Goal_Thickness()));
        field->set_center_circle_radius(CONVUNIT(cfg->Field_Rad()));
        field->set_defense_radius(CONVUNIT(cfg->Field_Defense_Rad()));
        field->set_defense_stretch(CONVUNIT(cfg->Field_Defense_Stretch()));
        field->set_free_kick_from_defense_dist(CONVUNIT(cfg->Field_Free_Kick()));
        //TODO: verify if these fields are correct:
        field->set_penalty_line_from_spot_dist(CONVUNIT(cfg->Field_Penalty_Line()));
        field->set_penalty_spot_from_field_line_dist(CONVUNIT(cfg->Field_Penalty_Point()));
    }
    if (cfg->noise()==false) {dev_x = 0;dev_y = 0;dev_a = 0;}
    if ((cfg->vanishing()==false) || (rand0_1() > cfg->ball_vanishing()))
    {
        SSL_DetectionBall* vball = packet->mutable_detection()->add_balls();
        vball->set_x(randn_notrig(x*1000.0f,dev_x));
        vball->set_y(randn_notrig(y*1000.0f,dev_y));
        vball->set_z(z*1000.0f);
        vball->set_pixel_x(x*1000.0f);
        vball->set_pixel_y(y*1000.0f);
        vball->set_confidence(0.9 + rand0_1()*0.1);
    }
    for(int i = 0; i < ROBOT_COUNT; i++){
        if ((cfg->vanishing()==false) || (rand0_1() > cfg->blue_team_vanishing()))
        {
            if (!robots[i]->on) continue;
            SSL_DetectionRobot* rob = packet->mutable_detection()->add_robots_blue();
            robots[i]->getXY(x,y);
            dir = robots[i]->getDir();
            rob->set_robot_id(i);
            rob->set_pixel_x(x*1000.0f);
            rob->set_pixel_y(y*1000.0f);
            rob->set_confidence(1);
            rob->set_x(randn_notrig(x*1000.0f,dev_x));
            rob->set_y(randn_notrig(y*1000.0f,dev_y));
            rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f);
        }
    }
    for(int i = ROBOT_COUNT; i < ROBOT_COUNT*2; i++){
        if ((cfg->vanishing()==false) || (rand0_1() > cfg->yellow_team_vanishing()))
        {
            if (!robots[i]->on) continue;
            SSL_DetectionRobot* rob = packet->mutable_detection()->add_robots_yellow();
            robots[i]->getXY(x,y);
            dir = robots[i]->getDir();
            rob->set_robot_id(i-ROBOT_COUNT);
            rob->set_pixel_x(x*1000.0f);
            rob->set_pixel_y(y*1000.0f);
            rob->set_confidence(1);
            rob->set_x(randn_notrig(x*1000.0f,dev_x));
            rob->set_y(randn_notrig(y*1000.0f,dev_y));
            rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f);
        }
    }
    return packet;
}