void RoboCupField::toProtoBuffer(SSL_GeometryFieldSize& buffer) const {
field_markings_mutex.lockForRead();
buffer.Clear();
buffer.set_field_length(field_length->getDouble());
buffer.set_field_width(field_width->getDouble());
buffer.set_goal_width(goal_width->getDouble());
buffer.set_goal_depth(goal_depth->getDouble());
buffer.set_boundary_width(boundary_width->getDouble());
for (size_t i = 0; i < field_lines.size(); ++i) {
const FieldLine& line = *(field_lines[i]);
SSL_FieldLineSegment proto_line;
proto_line.set_name(line.name->getString());
proto_line.mutable_p1()->set_x(line.p1_x->getDouble());
proto_line.mutable_p1()->set_y(line.p1_y->getDouble());
proto_line.mutable_p2()->set_x(line.p2_x->getDouble());
proto_line.mutable_p2()->set_y(line.p2_y->getDouble());
proto_line.set_thickness(line.thickness->getDouble());
*(buffer.add_field_lines()) = proto_line;
}
for (size_t i = 0; i < field_arcs.size(); ++i) {
const FieldCircularArc& arc = *(field_arcs[i]);
SSL_FieldCicularArc proto_arc;
proto_arc.set_name(arc.name->getString());
proto_arc.mutable_center()->set_x(arc.center_x->getDouble());
proto_arc.mutable_center()->set_y(arc.center_y->getDouble());
proto_arc.set_radius(arc.radius->getDouble());
proto_arc.set_a1(arc.a1->getDouble());
proto_arc.set_a2(arc.a2->getDouble());
proto_arc.set_thickness(arc.thickness->getDouble());
*(buffer.add_field_arcs()) = proto_arc;
}
field_markings_mutex.unlock();
}
void PluginLegacyPublishGeometry::sendGeometry() {
// NOTE: The field dimensions are derived from one half irrespective of
// the exact field half / end in question.
SSL_GeometryFieldSize field;
_field.toProtoBuffer(field);
// Geometry data for double-sized field in old format.
RoboCup2014Legacy::Geometry::SSL_GeometryData ds_geodata_old;
// Field data for double-sized field in old format.
RoboCup2014Legacy::Geometry::SSL_GeometryFieldSize ds_field_old;
ds_field_old.set_field_length(field.field_length());
ds_field_old.set_field_width(field.field_width());
ds_field_old.set_goal_width(field.goal_width());
ds_field_old.set_goal_depth(field.goal_depth());
ds_field_old.set_boundary_width(field.boundary_width());
ds_field_old.set_center_circle_radius(
GetFieldCircularArcRadius("CenterCircle"));
ds_field_old.set_defense_radius(
GetFieldCircularArcRadius("LeftFieldLeftPenaltyArc"));
ds_field_old.set_defense_stretch(GetFieldLineLength("LeftPenaltyStretch"));
// The following fields no longer exist in ssl-vision, and are hard-coded
// from the ssl-vision SVN trunk, r233.
ds_field_old.set_line_width(10);
ds_field_old.set_referee_width(425);
ds_field_old.set_goal_wall_width(20);
ds_field_old.set_free_kick_from_defense_dist(200);
ds_field_old.set_penalty_spot_from_field_line_dist(750);
ds_field_old.set_penalty_line_from_spot_dist(400);
*ds_geodata_old.mutable_field() = ds_field_old;
// Copy over the camera calibrations.
for (unsigned int i = 0; i < params.size(); i++) {
SSL_GeometryCameraCalibration* ds_calib_old = ds_geodata_old.add_calib();
params[i]->toProtoBuffer(*ds_calib_old,i);
}
_ds_udp_server_old->sendLegacyMessage(ds_geodata_old);
}
void SoccerView::LoadFieldGeometry ( SSL_GeometryFieldSize &fieldSize )
{
this->line_width = fieldSize.line_width();
this->field_length = fieldSize.field_length();
this->field_width = fieldSize.field_width();
this->boundary_width = fieldSize.boundary_width();
this->referee_width = fieldSize.referee_width();
this->goal_width = fieldSize.goal_width();
this->goal_depth = fieldSize.goal_depth();
this->goal_wall_width = fieldSize.goal_wall_width();
this->center_circle_radius = fieldSize.center_circle_radius();
this->defense_radius = fieldSize.defense_radius();
this->defense_stretch = fieldSize.defense_stretch();
this->free_kick_from_defense_dist = fieldSize.free_kick_from_defense_dist();
this->penalty_spot_from_field_line_dist = fieldSize.penalty_spot_from_field_line_dist();
this->penalty_line_from_spot_dist = fieldSize.penalty_line_from_spot_dist();
scene->removeItem ( fieldItem );
ConstructField();
fieldItem = scene->addPath ( *field,*fieldLinePen,*fieldBrush );
}
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;
}
void SSLField::loadFieldGeometryFromNetwork(const SSL_GeometryFieldSize &fieldSize)
{
line_width = fieldSize.line_width();
field_length = fieldSize.field_length();
field_width = fieldSize.field_width();
boundary_width = fieldSize.boundary_width();
referee_width = fieldSize.referee_width();
goal_width = fieldSize.goal_width();
goal_depth = fieldSize.goal_depth();
goal_wall_width = fieldSize.goal_wall_width();
center_circle_radius = fieldSize.center_circle_radius();
defense_radius = fieldSize.defense_radius();
defense_stretch = fieldSize.defense_stretch();
free_kick_from_defense_dist = fieldSize.free_kick_from_defense_dist();
penalty_spot_from_field_line_dist = fieldSize.penalty_spot_from_field_line_dist();
penalty_line_from_spot_dist = fieldSize.penalty_line_from_spot_dist();
}
