void Coach::update(SSL_DetectionFrame newFrame)
{
//update memory
if(_memory.size() < 5) {
_memory.push_back(newFrame);
}
else {
_memory.push_back(newFrame);
_memory.erase(_memory.begin());
//update FieldState
fs.bBots.clear();
fs.yBots.clear();
fs.ball = Movable(newFrame.balls(0).x(), newFrame.balls(0).y(), 0, 0); //velocity calculated in analysis
for(int i = 0; i < _memory[4].robots_blue_size(); i++)
{
fs.bBots.push_back(Robot(_memory[4].robots_blue(i).robot_id(),
_memory[4].robots_blue(i).orientation(),
_memory[4].robots_blue(i).x(),
_memory[4].robots_blue(i).y()));
}
for(int i = 0; i < _memory[4].robots_yellow_size(); i++)
{
fs.yBots.push_back(Robot(_memory[4].robots_yellow(i).robot_id(),
_memory[4].robots_yellow(i).orientation(),
_memory[4].robots_yellow(i).x(),
_memory[4].robots_yellow(i).y()));
}
//call analysis to update velocity
Analysis();
}
}
void SSLVision::parse(SSL_DetectionFrame &pck)
{
// update camera fps
int cid = pck.camera_id();
if(cid == 0) _fpscam0.Pulse();
if(cid == 1) _fpscam1.Pulse();
switch(_camera)
{
case CAMERA_BOTH:
break;
case CAMERA_0:
if (cid==1) return;
break;
case CAMERA_1:
if (cid==0) return;
break;
case CAMERA_NONE:
default:
return;
}
// update vision frame
//_vframe[cid].frame_number = pck.frame_number();
vector<Position> pt;
// Team side Coefficient
float ourSide = (_side == SIDE_RIGHT)? -1.0f : 1.0f;
double time = _time.elapsed(); //pck.t_capture();
// insert balls
int max_balls=min(VOBJ_MAX_NUM, pck.balls_size());
for(int i=0; i<max_balls; ++i)
{
auto b = pck.balls(i);
if(b.has_confidence() && b.has_x() && b.has_y())
if(b.confidence() > MIN_CONF && fabs(b.x()) < FIELD_MAX_X && fabs(b.y()) < FIELD_MAX_Y)
{
Position tp;
tp.loc = Vector2D(b.x()*ourSide, b.y()*ourSide);
pt.push_back(tp);
}
}
_wm->ball.seenAt(pt, time, cid);
if(_color == COLOR_BLUE)
{
APPEND_ROBOTS(blue, our);
APPEND_ROBOTS(yellow, opp);
}
else // _color == COLOR_YELLOW
{
APPEND_ROBOTS(yellow, our);
APPEND_ROBOTS(blue, opp);
}
}
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);
}
}
}