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