//Slot Unwrapper for internet datagram (vision) void SimulatorCore::UnWrapVisionPacket(SSL_WrapperPacket iPacketTeam){ SSL_DetectionRobot lPacketRobotBlue; SSL_DetectionRobot lPacketRobotYellow; SSL_DetectionBall lPacketBall = iPacketTeam.detection().balls(0); int lBlueSize = iPacketTeam.detection().robots_blue_size(); int lYellowSize = iPacketTeam.detection().robots_blue_size(); mBall->setPosition(lPacketBall.x(),lPacketBall.y()); for(int i = 0; i < lBlueSize; ++i){ lPacketRobotBlue = iPacketTeam.detection().robots_blue(i); int RobotId = lPacketRobotBlue.robot_id(); if(RobotId > mNbPlayerPerTeam - 1){ break; //TODO clean this with dynamic player allocation } RobotModel* lRobotBlue = mBlueTeam->GetRobot(RobotId); lRobotBlue->setPose(Pose(lPacketRobotBlue.x(),lPacketRobotBlue.y(), lPacketRobotBlue.orientation())); // TODO should be in another function lRobotBlue->UpdateSimulationData(); } for(int i = 0; i < lYellowSize; ++i){ lPacketRobotYellow = iPacketTeam.detection().robots_yellow(i); int RobotId = lPacketRobotYellow.robot_id(); if(RobotId > mNbPlayerPerTeam - 1){ break; //TODO clean this with dynamic player allocation } RobotModel* lRobotYellow = mYellowTeam->GetRobot(RobotId); lRobotYellow->setPose(Pose(lPacketRobotYellow.x(),lPacketRobotYellow.y(), lPacketRobotYellow.orientation())); } this->sendCommandToGrSim(); //TODO not clear, send back the new command to grsim }
void SSLField::paint(QPainter *painter, const QStyleOptionGraphicsItem *, QWidget *) { // The field: paintField(painter); // The robots for (int i = 0; i < detection.robots_blue_size(); i++) paintRobot(painter, detection.robots_blue(i), QColor(0,0,255)); for (int i = 0; i < detection.robots_yellow_size(); i++) paintRobot(painter, detection.robots_yellow(i), QColor(255,255,0)); // The ball(s) for (int i = 0; i < detection.balls_size(); i++) { SSL_DetectionBall ball = detection.balls(i); painter->setBrush(QColor(255,128,128)); painter->setPen(QColor(255,128,128)); painter->drawEllipse(QPoint(ball.x(),-ball.y()),21,21); } // Fit field in view const QSize size(parentView->size()); int viewWidth = (field_length+2*goal_depth+2*referee_width)*1.2; int viewHeight =(field_width+2*referee_width)*1.2; float xScale = float(size.width()) / viewWidth; float yScale = float(size.height()) / viewHeight; float scale = xScale < yScale ? xScale : yScale; parentView->setTransform(QTransform(scale, 0, 0, scale, size.width() / 2., size.height() / 2.)); }
void Robot::Pass(int Rid, bool ISyellow, float vel){ //call prepare to aim at/in front of shooter SSL_DetectionBall ball = current->balls(0); SSL_DetectionRobot robot; if(ISyellow){ robot = current->robots_yellow(Rid); } else{ robot = current->robots_blue(Rid); } float R = distance(robot.x(),robot.y(),x,y); float velocityK = R/1500; Prepare(ball.x(),ball.y(),vel,velocityK+vel,robot.x(),robot.y()); }
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; }
void Environment::sendVision() { SSL_WrapperPacket wrapper; SSL_DetectionFrame* det = wrapper.mutable_detection(); det->set_frame_number(_frameNumber++); det->set_camera_id(0); struct timeval tv; gettimeofday(&tv, nullptr); det->set_t_capture(tv.tv_sec + (double)tv.tv_usec * 1.0e-6); det->set_t_sent(det->t_capture()); for (Robot* robot : _yellow) { if ((rand() % 100) < robot->visibility) { SSL_DetectionRobot* out = det->add_robots_yellow(); convert_robot(robot, out); } } for (Robot* robot : _blue) { if ((rand() % 100) < robot->visibility) { SSL_DetectionRobot* out = det->add_robots_blue(); convert_robot(robot, out); } } for (const Ball* b : _balls) { Geometry2d::Point ballPos = b->getPosition(); // bool occ; // if (ballPos.x < 0) // { // occ = occluded(ballPos, cam0); // } else { // occ = occluded(ballPos, cam1); // } if ((rand() % 100) < ballVisibility) { SSL_DetectionBall* out = det->add_balls(); out->set_confidence(1); out->set_x(ballPos.x * 1000); out->set_y(ballPos.y * 1000); out->set_pixel_x(ballPos.x * 1000); out->set_pixel_y(ballPos.y * 1000); } } std::string buf; wrapper.SerializeToString(&buf); if (sendShared) { _visionSocket.writeDatagram(&buf[0], buf.size(), MulticastAddress, SharedVisionPort); } else { _visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress, SimVisionPort); _visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress, SimVisionPort + 1); } }
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 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::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(); }
UpdateBall::UpdateBall(const SSL_DetectionBall& p, double t1, double t2, int camId) : Update(t1, t2, camId), Point(p.x(), p.y()), pimpl(new UpdateBallImpl(p.has_z() ? p.z() : 0.0)) {}
ProcessResult PluginDetectBalls::process ( FrameData * data, RenderOptions * options ) { ( void ) options; if ( data==0 ) return ProcessingFailed; SSL_DetectionFrame * detection_frame = 0; detection_frame= ( SSL_DetectionFrame * ) data->map.get ( "ssl_detection_frame" ); if ( detection_frame == 0 ) detection_frame= ( SSL_DetectionFrame * ) data->map.insert ( "ssl_detection_frame",new SSL_DetectionFrame() ); int color_id_ball = _lut->getChannelID ( _settings->_color_label->getString() ); if ( color_id_ball == -1 ) { printf ( "Unknown Ball Detection Color Label: '%s'\nAborting Plugin!\n",_settings->_color_label->getString().c_str() ); return ProcessingFailed; } //delete any previous detection results: detection_frame->clear_balls(); //TODO: add a vartype notifier for better performance. //initialize filter: if (vnotify.hasChanged()) { max_balls = _settings->_max_balls->getInt(); filter.setWidth ( _settings->_ball_min_width->getInt(),_settings->_ball_max_width->getInt() ); filter.setHeight ( _settings->_ball_min_height->getInt(),_settings->_ball_max_height->getInt() ); filter.setArea ( _settings->_ball_min_area->getInt(),_settings->_ball_max_area->getInt() ); field_filter.update ( field ); //copy all vartypes to local variables for faster repeated lookup: filter_ball_in_field = _settings->_ball_on_field_filter->getBool(); filter_ball_on_field_filter_threshold = _settings->_ball_on_field_filter_threshold->getDouble(); filter_ball_in_goal = _settings->_ball_in_goal_filter->getBool(); filter_ball_histogram = _settings->_ball_histogram_enabled->getBool(); if ( filter_ball_histogram ) { if ( color_id_ball != color_id_orange ) { printf ( "Warning: ball histogram check is only configured for orange balls!\n" ); printf ( "Please disable the histogram check in the Ball Detection Plugin settings\n" ); } if ( color_id_pink==-1 || color_id_orange==-1 || color_id_yellow==-1 || color_id_field==-1 ) { printf ( "WARNING: some LUT color labels where undefined for the ball detection plugin\n" ); printf ( " Disabling histogram check!\n" ); filter_ball_histogram=false; } } min_greenness = _settings->_ball_histogram_min_greenness->getDouble(); max_markeryness = _settings->_ball_histogram_max_markeryness->getDouble(); //setup values used for the gaussian confidence measurement: filter_gauss = _settings->_ball_gauss_enabled->getBool(); exp_area_min = _settings->_ball_gauss_min->getInt(); exp_area_max = _settings->_ball_gauss_max->getInt(); exp_area_var = sq ( _settings->_ball_gauss_stddev->getDouble() ); z_height= _settings->_ball_z_height->getDouble(); near_robot_filter = _settings->_ball_too_near_robot_enabled->getBool(); near_robot_dist_sq = sq(_settings->_ball_too_near_robot_dist->getDouble()); } const CMVision::Region * reg = 0; //acquire orange region list from data-map: CMVision::ColorRegionList * colorlist; colorlist= ( CMVision::ColorRegionList * ) data->map.get ( "cmv_colorlist" ); if ( colorlist==0 ) { printf ( "error in ball detection plugin: no region-lists were found!\n" ); return ProcessingFailed; } reg = colorlist->getRegionList ( color_id_ball ).getInitialElement(); //acquire color-labeled image from data-map: const Image<raw8> * image = ( Image<raw8> * ) ( data->map.get ( "cmv_threshold" ) ); if ( image==0 ) { printf ( "error in ball detection plugin: no color-thresholded image was found!\n" ); return ProcessingFailed; } int robots_blue_n=0; int robots_yellow_n=0; bool use_near_robot_filter=near_robot_filter; if ( use_near_robot_filter ) { SSL_DetectionFrame * detection_frame = ( SSL_DetectionFrame * ) data->map.get ( "ssl_detection_frame" ); if ( detection_frame==0 ) { use_near_robot_filter=false; } else { robots_blue_n=detection_frame->robots_blue_size(); robots_yellow_n=detection_frame->robots_yellow_size(); if (robots_blue_n==0 && robots_yellow_n==0) use_near_robot_filter=false; } } if ( max_balls > 0 ) { list<BallDetectResult> result; filter.init ( reg ); while ( ( reg = filter.getNext() ) != 0 ) { float conf = 1.0; if ( filter_gauss==true ) { int a = reg->area - bound ( reg->area,exp_area_min,exp_area_max ); conf = gaussian ( a / exp_area_var ); } //TODO: add a plugin for confidence masking... possibly multi-layered. // to replace the commented det.mask.get(...) below: //if (filter_conf_mask) conf*=det.mask.get(reg->cen_x,reg->cen_y)); //convert from image to field coordinates: vector2d pixel_pos ( reg->cen_x,reg->cen_y ); vector3d field_pos_3d; camera_parameters.image2field ( field_pos_3d,pixel_pos,z_height ); vector2d field_pos ( field_pos_3d.x,field_pos_3d.y ); //filter points that are outside of the field: if ( filter_ball_in_field==true && field_filter.isInFieldPlusThreshold ( field_pos, max(0.0,filter_ball_on_field_filter_threshold) ) ==false ) { conf = 0.0; } //filter out points that are deep inside the goal-box if ( filter_ball_in_goal==true && field_filter.isFarInGoal ( field_pos ) ==true ) { conf = 0.0; } //TODO add ball-too-near-robot filter if ( use_near_robot_filter && conf > 0.0 ) { int robots_n=0; for (int team = 0; team < 2; team++) { if (team==0) { robots_n=robots_blue_n; } else { robots_n=robots_yellow_n; } if (robots_n > 0) { ::google::protobuf::RepeatedPtrField< ::SSL_DetectionRobot > * robots; if (team==0) { robots = detection_frame->mutable_robots_blue(); } else { robots = detection_frame->mutable_robots_yellow(); } for (int r = 0; r < robots_n; r++) { const SSL_DetectionRobot & robot = robots->Get(r); if (robot.confidence() > 0.0) { if ((sq((double)(robot.x())-(double)(field_pos.x)) + sq((double)(robot.y())-(double)(field_pos.y))) < near_robot_dist_sq) { conf = 0.0; break; } } } if (conf==0.0) break; } } } // histogram check if enabled if ( filter_ball_histogram && conf > 0.0 && checkHistogram ( image, reg, min_greenness, max_markeryness ) ==false ) { conf = 0.0; } // add filtered region to the region list if(conf > 0) { result.push_back(BallDetectResult(reg,conf)); } } // sort result by confidence and output first max_balls region(s) result.sort(); int num_ball = 0; list<BallDetectResult>::reverse_iterator it; for(it=result.rbegin(); it!=result.rend(); it++) { if(++num_ball > max_balls) break; //update result: SSL_DetectionBall* ball = detection_frame->add_balls(); ball->set_confidence ( it->conf ); vector2d pixel_pos ( it->reg->cen_x,it->reg->cen_y ); vector3d field_pos_3d; camera_parameters.image2field ( field_pos_3d,pixel_pos,z_height ); ball->set_area ( it->reg->area ); ball->set_x ( field_pos_3d.x ); ball->set_y ( field_pos_3d.y ); ball->set_pixel_x ( it->reg->cen_x ); ball->set_pixel_y ( it->reg->cen_y ); } } return ProcessingOk; }
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 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(); } }
void Robot::PathTo(float x1, float y1, float vel){ printf("\nentered pathto %d",id); float R = sqrt(pow(x-x1,2)+pow(y-y1,2)); float x2 = x1; float y2 = y1; //printf("\nA is %f, B is %f, C is %f, R is %f",a,b,c,R); //printf("In the way is %d, Interference is %d",inTheWay(x,x1,-1000,y,y1,-0.001,true),interference(a,b,c,-0.001,-1000,200)); if(R > 0 && 0< current->robots_yellow_size() && 0 < current->robots_blue_size()){ for(int i = 0; i < current->robots_yellow_size(); i++){ SSL_DetectionRobot *robot = const_cast<SSL_DetectionRobot*>(&(current->robots_yellow(i))); if(!(yellow && id==i)){ if(id == 1){ inTheWay(x,x1,robot->x(),y,y1,robot->y(),true); printf("InTheWay? %d, Intefere? %d",inTheWay(x,x1,robot->x(),y,y1,robot->y(),true),interference(x,y,x1,y1,robot->x(),robot->y(),200));} if(interference(x,y,x1,y1,robot->x(),robot->y(),300) && distance(x,robot->x(),y,robot->y()) < R && inTheWay(x,x1,robot->x(),y,y1,robot->y(),false)){ float angle2 = atan2(y-robot->y(),x-robot->x()); angle2 += 1.5; float xUp = 300*cos(angle2); float yUp = 300*sin(angle2); xUp = xUp + robot->x(); yUp = yUp + robot->y(); x2 = xUp; y2 = yUp; R = distance(x,robot->x(),y,robot->y()); printf("\nRobot %d in yellow is in the Way!!",i); }} } for(int i = 0; i < current->robots_blue_size(); i++){ SSL_DetectionRobot * robot = const_cast<SSL_DetectionRobot*>(¤t->robots_blue(i)); if(!(!yellow && id == i)){ if(interference(x,y,x1,y1,robot->x(),robot->y(),300) && distance(x,robot->x(),y,robot->y()) < R && inTheWay(x,x1,robot->x(),y,y1,robot->y(),false)){ float angle2 = atan2(y-robot->y(),x-robot->x()); angle2 += 1.5; float xUp = 300*cos(angle2); float yUp = 300*sin(angle2); xUp = xUp + robot->x(); yUp = yUp + robot->y(); x2 = xUp; y2 = yUp; R = distance(x,robot->x(),y,robot->y()); printf("\nRobot %d in blue is in the Way!!",i); }} } SSL_DetectionBall ball = current->balls(0); if(interference(x,y,x1,y1,ball.x(),ball.y(),150) && distance(x,ball.x(),y,ball.y()) < R && inTheWay(x,x1,ball.x(),y,y1,ball.y(),true)){ float angle2 = atan2(y-ball.y(),x-ball.x()); angle2 += 1.5; float xUp = 200*cos(angle2); float yUp = 200*sin(angle2); xUp = xUp + ball.x(); yUp = yUp + ball.y(); x2 = xUp; y2 = yUp; R = distance(x,ball.x(),y,ball.y()); printf("\nBall is in the WAY!!!"); } //printf("InTheWay? %d, Intefere? %d",inTheWay(x,x1,ball.x(),y,y1,ball.y(),true),interference(x,y,x1,y1,ball.x(),ball.y(),300)); } Move(x2,y2,vel); printf("\nX2 is %f, Y2 is %f",x2,y2); printf("Exited PATHTO!! %d",id); }