void
SSLRealVision::printBallInfo (const SSL_DetectionBall& ball)
{
printf ("-Ball : CONF=%4.2f POS=<%9.2f,%9.2f> ", ball.confidence (), ball.x (), ball.y ());
if (ball.has_z ())
{
printf ("Z=%7.2f ", ball.z ());
}
else
{
printf ("Z=N/A ");
}
printf ("RAW=<%8.2f,%8.2f>\n", ball.pixel_x (), ball.pixel_y ());
}
void
SSLRealVision::cvtToBallState(ssl_msgs::BallState& ball_state, const SSL_DetectionBall& ball)
{
if(ball.has_area())
ball_state.area = ball.area();
if(ball.has_confidence())
ball_state.confidence = ball.confidence();
if(ball.has_pixel_x())
ball_state.pix_coord.x = ball.pixel_x();
if(ball.has_pixel_y())
ball_state.pix_coord.y = ball.pixel_y();
if(ball.has_x())
ball_state.position.x = ball.x();
if (ball.has_y())
ball_state.position.y = ball.y();
if (ball.has_z())
ball_state.position.z = ball.z();
}
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 VisionFilter::update(const SSL_WrapperPacket &packet)
{
QMutexLocker locker(&mtx_);
try {
if(packet.has_detection())
{
if( packet.detection().camera_id() < MAX_CAMERA_COUNT )
{
double frame_time = packet.detection().t_capture();
if( frame_time <= cameraLastFrameTime[packet.detection().camera_id()] ) {
throw "Vision: Decayed packet !!!!" ;
}
else
cameraLastFrameTime[packet.detection().camera_id()] = frame_time;
}
if(ParameterManager::getInstance()->get<bool>("vision.camera_0_filtered"))
if( packet.detection().camera_id() == 0 )
throw "Vision: Camera 0 is filtered out";
if(ParameterManager::getInstance()->get<bool>("vision.camera_1_filtered"))
if( packet.detection().camera_id() == 1 )
throw "Vision: Camera 1 is filtered out";
if(ParameterManager::getInstance()->get<bool>("vision.camera_2_filtered"))
if( packet.detection().camera_id() == 2 )
throw "Vision: Camera 2 is filtered out";
if(ParameterManager::getInstance()->get<bool>("vision.camera_3_filtered"))
if( packet.detection().camera_id() == 3 )
throw "Vision: Camera 3 is filtered out";
float diff_time = packet.detection().t_sent() - last_frame_time;
FPS = 1/diff_time;
last_frame_time = packet.detection().t_sent();
OneObjectFrame frame;
frame.camera_id = packet.detection().camera_id();
frame.frame_number = packet.detection().frame_number();
// temp_frame.setToCurrentTimeMilliSec();
frame.timeStampMSec = packet.detection().t_capture() * 1000.0;
if(ParameterManager::getInstance()->get<bool>("vision.filter_blue_robots") == false)
for(int i=0; i < packet.detection().robots_blue_size(); i++)
{
SSL_DetectionRobot Robot = packet.detection().robots_blue(i);
frame.position = Vector3D(Robot.x(), Robot.y(), Robot.orientation());
frame.confidence = Robot.confidence();
robotFilter_cluster[SSL::Blue][Robot.robot_id()]->putNewFrame(frame);
// if(i == ParameterManager::getInstance()->get<int>("skills.under_test_robot"))
robotFilter_kalman[SSL::Blue][Robot.robot_id()]->putNewFrame(frame);
}
if(ParameterManager::getInstance()->get<bool>("vision.filter_yellow_robots") == false)
for(int i=0; i< packet.detection().robots_yellow_size(); i++)
{
SSL_DetectionRobot Robot = packet.detection().robots_yellow(i);
frame.position = Vector3D(Robot.x(), Robot.y(), Robot.orientation());
frame.confidence = Robot.confidence();
robotFilter_cluster[SSL::Yellow][Robot.robot_id()]->putNewFrame(frame);
robotFilter_kalman[SSL::Yellow][Robot.robot_id()]->putNewFrame(frame);
// cout << "Robot Yellow [" << i << "] detected" << endl;
}
vector<OneObjectFrame> balls_vec;
for(int i=0; i< packet.detection().balls_size(); i++)
{
SSL_DetectionBall Ball = packet.detection().balls(i);
frame.position = Vector2D(Ball.x(), Ball.y()).to3D();
frame.confidence = Ball.confidence();
balls_vec.push_back(frame);
// file <<i << " , " << (long)temp_frame.timeStampMilliSec<< " , ";
// file << Ball.x() <<" , " << Ball.y() << endl;
}
if(balls_vec.empty()) {
throw "Warning:: No ball exists in current frame";
}
if(ballFilter->isEmpty()) {
ballFilter->initialize(balls_vec[0]);
throw "Initializing ball filter module";
}
if(balls_vec.size() == 1) {
ballFilter->putNewFrame(balls_vec[0]);
}
else {
ballFilter->putNewFrameWithManyBalls(balls_vec);
throw "Warning:: More than One ball exist in current frame" ;
}
}
if(packet.has_geometry()) {
// SSL_GeometryData geometryData = packet.geometry();
}
}
catch (const char* msg) {
// cout << msg << endl;
mtx_.unlock();
}
}