Esempio n. 1
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;
}
Esempio n. 2
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;
}