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