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;
}
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");
}
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);
}
}
}