//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 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);
}
}
//Slot Send noised camera output to the internet (vision)
void SimulatorCore::SendCameraFrame(){
SSL_WrapperPacket lPacket;
lPacket.mutable_detection()->set_camera_id(0);
lPacket.mutable_detection()->set_frame_number(0);
lPacket.mutable_detection()->set_t_capture(0);
lPacket.mutable_detection()->set_t_sent(0);
for(int i = 0; i < mNbPlayerPerTeam; ++i){
mCameraModel->insertRobotWithNoise(&lPacket, mYellowTeam->GetRobot(i));
mCameraModel->insertRobotWithNoise(&lPacket, mBlueTeam->GetRobot(i));
}
mCameraModel->insertBallWithNoise(&lPacket,mBall);
mCameraOutput->SendUDPVisionDatagram(lPacket);
}
void SSLVision::readPendingPacket(QByteArray data, QString ip, int port)
{
// check for server ip (& port)
if(ip=="" && port==0) return;
// unpack sslvision packet
SSL_WrapperPacket packet;
bool ans=packet.ParseFromArray(data.data(), data.size());
if(!ans) return;
if(packet.has_detection()==false) return;
SSL_DetectionFrame pck = packet.detection();
// parse detection frame
parse(pck);
}
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;
}
SSL_WrapperPacket Net::receivePacket(){
SSL_WrapperPacket packet;
datagramReceive.resize(udpsocketReceive.pendingDatagramSize());
udpsocketReceive.readDatagram(datagramReceive.data(), datagramReceive.size());
packet.ParseFromArray(datagramReceive.data(), datagramReceive.size());
//SSL_DetectionRobot robot;
//robot = packet.detection().robots_blue(0);
//cout << robot.orientation() << endl;
// debug all
/*string text_str;
google::protobuf::TextFormat::PrintToString(packet, &text_str);
cout << text_str << endl;*/
return packet;
}
void RoboCupSSLClient::recieveData()
{
while(udpSocket->hasPendingDatagrams())
{
SSL_WrapperPacket *packet = new SSL_WrapperPacket();
QByteArray datagram;
datagram.resize(udpSocket->pendingDatagramSize());
udpSocket->readDatagram(datagram.data(),datagram.size());
mutex.lock();
if(packet->ParseFromArray(datagram.data(),datagram.size()))
{
packetList.append(packet);
}
mutex.unlock();
}
}
void SSLWorld::sendVisionBuffer()
{
int t = timer->elapsed();
sendQueue.push_back(new SendingPacket(generatePacket(),t));
while (t - sendQueue.front()->t>=cfg->sendDelay())
{
SSL_WrapperPacket *packet = sendQueue.front()->packet;
SSL_WrapperPacket pac = *packet;
string message;
packet->SerializeToString(&message);
sendBlue(&message);
delete sendQueue.front();
sendQueue.pop_front();
visionServer->send(*packet);
delete packet;
if (sendQueue.isEmpty()) break;
}
}
int main(int argc, char *argv[])
{
if (argc != 3)
{
printf("Usage: %s <tcpdump.cap> <output.log>\n", argv[0]);
return 1;
}
char errbuf[PCAP_ERRBUF_SIZE];
pcap_t *pcap = pcap_open_offline(argv[1], errbuf);
if (!pcap)
{
printf("pcap_open_offline: %s\n", errbuf);
return 1;
}
int out_fd = creat(argv[2], 0666);
if (out_fd < 0)
{
printf("Can't create %s: %m\n", argv[2]);
pcap_close(pcap);
return 1;
}
LogFrame frame;
bool needWrite = false;
// Pretend we were reading packets in a fixed-frequency loop (like Processor).
// endTime is the time that a simulated processing loop iteration ends.
uint64_t endTime = 0;
bool first = true;
while (true)
{
struct pcap_pkthdr header;
const u_char *data = pcap_next(pcap, &header);
if (!data)
{
break;
}
// Verify ethertype for IPv4
if (data[12] != 8 || data[13] != 0)
{
continue;
}
// Require IPv4 with no options
if (data[14] != 0x45)
{
continue;
}
// FIXME - Checksum
uint64_t timestamp = header.ts.tv_usec + header.ts.tv_sec * 1000000;
if (!endTime)
{
// Find the time to write the first frame
endTime = timestamp + FramePeriod;
}
if (timestamp >= endTime)
{
// We're about to handle a packet received after this iteration,
// so write everything we have.
// Calculate the time this iteration would have started
frame.set_command_time(endTime - FramePeriod);
// Advance endTime to the next iteration.
endTime += FramePeriod;
// Add software information
if (first)
{
first = false;
LogConfig *logConfig = frame.mutable_log_config();
logConfig->set_generator("convert_tcpdump");
logConfig->set_git_version_hash(git_version_hash);
logConfig->set_git_version_dirty(git_version_dirty);
}
// Write this frame
needWrite = false;
if (!writeFrame(out_fd, frame))
{
break;
}
// Clear for the next iteration
frame.Clear();
}
// Check for data we're interested in
if (!memcmp(data + 30, referee_addr, 4) && !memcmp(data + 36, referee_port, 2))
{
// Referee packet
frame.add_raw_referee(data + 42, header.caplen - 42);
needWrite = true;
} else if (!memcmp(data + 30, vision_addr, 4) && !memcmp(data + 36, vision_port, 2))
{
// Vision packet
SSL_WrapperPacket *packet = frame.add_raw_vision();
if (!packet->ParseFromArray(data + 42, header.caplen - 42))
{
printf("Failed to parse: %s\n", packet->InitializationErrorString().c_str());
}
needWrite = true;
}
}
if (needWrite)
{
writeFrame(out_fd, frame);
}
close(out_fd);
pcap_close(pcap);
return 0;
}
int main(int argc, char *argv[])
{
int framePeriod = 1000000 / 60;
QString logFile;
// Determine log file name
if (argc == 2)
{
logFile = argv[1];
}
if (logFile.isNull())
{
if (!QDir("logs").exists())
{
printf("No logs directory and no log file specified\n");
return 1;
}
logFile = QString("logs/") + QDateTime::currentDateTime().toString("yyyyMMdd-hhmmss.log");
}
// Create vision socket
QUdpSocket visionSocket;
if (!visionSocket.bind(SharedVisionPort, QUdpSocket::ShareAddress))
{
printf("Can't bind to shared vision port");
return 1;
}
multicast_add(&visionSocket, SharedVisionAddress);
// Create referee socket
QUdpSocket refereeSocket;
if (!refereeSocket.bind(LegacyRefereePort, QUdpSocket::ShareAddress))
{
printf("Can't bind to referee port");
return 1;
}
multicast_add(&refereeSocket, RefereeAddress);
// Create log file
int fd = creat(logFile.toAscii(), 0666);
if (fd < 0)
{
printf("Can't create %s: %m\n", (const char *)logFile.toAscii());
return 1;
}
printf("Writing to %s\n", (const char *)logFile.toAscii());
// Main loop
LogFrame logFrame;
bool first = true;
while (true)
{
uint64_t startTime = timestamp();
logFrame.Clear();
logFrame.set_command_time(startTime);
// Check for user input (to exit)
struct pollfd pfd;
pfd.fd = 0;
pfd.events = POLLIN;
if (poll(&pfd, 1, 0) > 0)
{
// Enter pressed
break;
}
// Read vision data
while (visionSocket.hasPendingDatagrams())
{
string buf;
unsigned int n = visionSocket.pendingDatagramSize();
buf.resize(n);
visionSocket.readDatagram(&buf[0], n);
SSL_WrapperPacket *packet = logFrame.add_raw_vision();
if (!packet->ParseFromString(buf))
{
printf("Bad vision packet of %d bytes\n", n);
continue;
}
}
// Read referee data
while (refereeSocket.hasPendingDatagrams())
{
unsigned int n = refereeSocket.pendingDatagramSize();
string str(6, 0);
refereeSocket.readDatagram(&str[0], str.size());
// Check the size after receiving to discard bad packets
if (n != str.size())
{
printf("Bad referee packet of %d bytes\n", n);
continue;
}
logFrame.add_raw_referee(str);
}
if (first)
{
first = false;
LogConfig *logConfig = logFrame.mutable_log_config();
logConfig->set_generator("simple_logger");
logConfig->set_git_version_hash(git_version_hash);
logConfig->set_git_version_dirty(git_version_dirty);
}
uint32_t size = logFrame.ByteSize();
if (write(fd, &size, sizeof(size)) != sizeof(size))
{
printf("Failed to write size: %m\n");
break;
} else if (!logFrame.SerializeToFileDescriptor(fd))
{
printf("Failed to write frame: %m\n");
break;
}
uint64_t endTime = timestamp();
int lastFrameTime = endTime - startTime;
if (lastFrameTime < framePeriod)
{
usleep(framePeriod - lastFrameTime);
} else {
printf("Processor took too long: %d us\n", lastFrameTime);
}
}
// Discard input on stdin
tcflush(0, TCIFLUSH);
printf("Done.\n");
return 0;
}
/**
* program loop
*/
void Processor::run() {
vision.start();
// Create radio socket
_radio =
_simulation ? (Radio*)new SimRadio(_blueTeam) : (Radio*)new USBRadio();
Status curStatus;
bool first = true;
// main loop
while (_running) {
RJ::Time startTime = RJ::timestamp();
int delta_us = startTime - curStatus.lastLoopTime;
_framerate = 1000000.0 / delta_us;
curStatus.lastLoopTime = startTime;
_state.timestamp = startTime;
if (!firstLogTime) {
firstLogTime = startTime;
}
////////////////
// Reset
// Make a new log frame
_state.logFrame = std::make_shared<Packet::LogFrame>();
_state.logFrame->set_timestamp(RJ::timestamp());
_state.logFrame->set_command_time(startTime + Command_Latency);
_state.logFrame->set_use_our_half(_useOurHalf);
_state.logFrame->set_use_opponent_half(_useOpponentHalf);
_state.logFrame->set_manual_id(_manualID);
_state.logFrame->set_blue_team(_blueTeam);
_state.logFrame->set_defend_plus_x(_defendPlusX);
if (first) {
first = false;
Packet::LogConfig* logConfig =
_state.logFrame->mutable_log_config();
logConfig->set_generator("soccer");
logConfig->set_git_version_hash(git_version_hash);
logConfig->set_git_version_dirty(git_version_dirty);
logConfig->set_simulation(_simulation);
}
for (OurRobot* robot : _state.self) {
// overall robot config
switch (robot->hardwareVersion()) {
case Packet::RJ2008:
robot->config = robotConfig2008;
break;
case Packet::RJ2011:
robot->config = robotConfig2011;
break;
case Packet::RJ2015:
robot->config = robotConfig2015;
case Packet::Unknown:
robot->config =
robotConfig2011; // FIXME: defaults to 2011 robots
break;
}
// per-robot configs
robot->status = robotStatuses.at(robot->shell());
}
////////////////
// Inputs
// Read vision packets
vector<const SSL_DetectionFrame*> detectionFrames;
vector<VisionPacket*> visionPackets;
vision.getPackets(visionPackets);
for (VisionPacket* packet : visionPackets) {
SSL_WrapperPacket* log = _state.logFrame->add_raw_vision();
log->CopyFrom(packet->wrapper);
curStatus.lastVisionTime = packet->receivedTime;
if (packet->wrapper.has_detection()) {
SSL_DetectionFrame* det = packet->wrapper.mutable_detection();
// FIXME - Account for network latency
double rt = packet->receivedTime / 1000000.0;
det->set_t_capture(rt - det->t_sent() + det->t_capture());
det->set_t_sent(rt);
// Remove balls on the excluded half of the field
google::protobuf::RepeatedPtrField<SSL_DetectionBall>* balls =
det->mutable_balls();
for (int i = 0; i < balls->size(); ++i) {
float x = balls->Get(i).x();
// FIXME - OMG too many terms
if ((!_state.logFrame->use_opponent_half() &&
((_defendPlusX && x < 0) ||
(!_defendPlusX && x > 0))) ||
(!_state.logFrame->use_our_half() &&
((_defendPlusX && x > 0) ||
(!_defendPlusX && x < 0)))) {
balls->SwapElements(i, balls->size() - 1);
balls->RemoveLast();
--i;
}
}
// Remove robots on the excluded half of the field
google::protobuf::RepeatedPtrField<SSL_DetectionRobot>*
robots[2] = {det->mutable_robots_yellow(),
det->mutable_robots_blue()};
for (int team = 0; team < 2; ++team) {
for (int i = 0; i < robots[team]->size(); ++i) {
float x = robots[team]->Get(i).x();
if ((!_state.logFrame->use_opponent_half() &&
((_defendPlusX && x < 0) ||
(!_defendPlusX && x > 0))) ||
(!_state.logFrame->use_our_half() &&
((_defendPlusX && x > 0) ||
(!_defendPlusX && x < 0)))) {
robots[team]->SwapElements(
i, robots[team]->size() - 1);
robots[team]->RemoveLast();
--i;
}
}
}
detectionFrames.push_back(det);
}
}
// Read radio reverse packets
_radio->receive();
for (const Packet::RadioRx& rx : _radio->reversePackets()) {
_state.logFrame->add_radio_rx()->CopyFrom(rx);
curStatus.lastRadioRxTime = rx.timestamp();
// Store this packet in the appropriate robot
unsigned int board = rx.robot_id();
if (board < Num_Shells) {
// We have to copy because the RX packet will survive past this
// frame but LogFrame will not (the RadioRx in LogFrame will be
// reused).
_state.self[board]->radioRx().CopyFrom(rx);
_state.self[board]->radioRxUpdated();
}
}
_radio->clear();
_loopMutex.lock();
for (Joystick* joystick : _joysticks) {
joystick->update();
}
runModels(detectionFrames);
for (VisionPacket* packet : visionPackets) {
delete packet;
}
// Update gamestate w/ referee data
_refereeModule->updateGameState(blueTeam());
_refereeModule->spinKickWatcher();
string yellowname, bluename;
if (blueTeam()) {
bluename = _state.gameState.OurInfo.name;
yellowname = _state.gameState.TheirInfo.name;
} else {
yellowname = _state.gameState.OurInfo.name;
bluename = _state.gameState.TheirInfo.name;
}
_state.logFrame->set_team_name_blue(bluename);
_state.logFrame->set_team_name_yellow(yellowname);
// Run high-level soccer logic
_gameplayModule->run();
// recalculates Field obstacles on every run through to account for
// changing inset
if (_gameplayModule->hasFieldEdgeInsetChanged()) {
_gameplayModule->calculateFieldObstacles();
}
/// Collect global obstacles
Geometry2d::ShapeSet globalObstacles =
_gameplayModule->globalObstacles();
Geometry2d::ShapeSet globalObstaclesWithGoalZones = globalObstacles;
Geometry2d::ShapeSet goalZoneObstacles =
_gameplayModule->goalZoneObstacles();
globalObstaclesWithGoalZones.add(goalZoneObstacles);
// Build a plan request for each robot.
std::map<int, Planning::PlanRequest> requests;
for (OurRobot* r : _state.self) {
if (r && r->visible) {
if (_state.gameState.state == GameState::Halt) {
r->setPath(nullptr);
continue;
}
// Visualize local obstacles
for (auto& shape : r->localObstacles().shapes()) {
_state.drawShape(shape, Qt::black, "LocalObstacles");
}
auto& globalObstaclesForBot =
(r->shell() == _gameplayModule->goalieID() ||
r->isPenaltyKicker)
? globalObstacles
: globalObstaclesWithGoalZones;
// create and visualize obstacles
Geometry2d::ShapeSet fullObstacles =
r->collectAllObstacles(globalObstaclesForBot);
requests[r->shell()] = Planning::PlanRequest(
Planning::MotionInstant(r->pos, r->vel),
r->motionCommand()->clone(), r->motionConstraints(),
std::move(r->angleFunctionPath.path),
std::make_shared<ShapeSet>(std::move(fullObstacles)));
}
}
// Run path planner and set the path for each robot that was planned for
auto pathsById = _pathPlanner->run(std::move(requests));
for (auto& entry : pathsById) {
OurRobot* r = _state.self[entry.first];
auto& path = entry.second;
path->draw(&_state, Qt::magenta, "Planning");
r->setPath(std::move(path));
r->angleFunctionPath.angleFunction =
angleFunctionForCommandType(r->rotationCommand());
}
// Visualize obstacles
for (auto& shape : globalObstacles.shapes()) {
_state.drawShape(shape, Qt::black, "Global Obstacles");
}
// Run velocity controllers
for (OurRobot* robot : _state.self) {
if (robot->visible) {
if ((_manualID >= 0 && (int)robot->shell() == _manualID) ||
_state.gameState.halt()) {
robot->motionControl()->stopped();
} else {
robot->motionControl()->run();
}
}
}
////////////////
// Store logging information
// Debug layers
const QStringList& layers = _state.debugLayers();
for (const QString& str : layers) {
_state.logFrame->add_debug_layers(str.toStdString());
}
// Add our robots data to the LogFram
for (OurRobot* r : _state.self) {
if (r->visible) {
r->addStatusText();
Packet::LogFrame::Robot* log = _state.logFrame->add_self();
*log->mutable_pos() = r->pos;
*log->mutable_world_vel() = r->vel;
*log->mutable_body_vel() = r->vel.rotated(2 * M_PI - r->angle);
//*log->mutable_cmd_body_vel() = r->
// *log->mutable_cmd_vel() = r->cmd_vel;
// log->set_cmd_w(r->cmd_w);
log->set_shell(r->shell());
log->set_angle(r->angle);
if (r->radioRx().has_kicker_voltage()) {
log->set_kicker_voltage(r->radioRx().kicker_voltage());
}
if (r->radioRx().has_kicker_status()) {
log->set_charged(r->radioRx().kicker_status() & 0x01);
log->set_kicker_works(
!(r->radioRx().kicker_status() & 0x90));
}
if (r->radioRx().has_ball_sense_status()) {
log->set_ball_sense_status(
r->radioRx().ball_sense_status());
}
if (r->radioRx().has_battery()) {
log->set_battery_voltage(r->radioRx().battery());
}
log->mutable_motor_status()->Clear();
log->mutable_motor_status()->MergeFrom(
r->radioRx().motor_status());
if (r->radioRx().has_quaternion()) {
log->mutable_quaternion()->Clear();
log->mutable_quaternion()->MergeFrom(
r->radioRx().quaternion());
} else {
log->clear_quaternion();
}
for (const Packet::DebugText& t : r->robotText) {
log->add_text()->CopyFrom(t);
}
}
}
// Opponent robots
for (OpponentRobot* r : _state.opp) {
if (r->visible) {
Packet::LogFrame::Robot* log = _state.logFrame->add_opp();
*log->mutable_pos() = r->pos;
log->set_shell(r->shell());
log->set_angle(r->angle);
*log->mutable_world_vel() = r->vel;
*log->mutable_body_vel() = r->vel.rotated(2 * M_PI - r->angle);
}
}
// Ball
if (_state.ball.valid) {
Packet::LogFrame::Ball* log = _state.logFrame->mutable_ball();
*log->mutable_pos() = _state.ball.pos;
*log->mutable_vel() = _state.ball.vel;
}
////////////////
// Outputs
// Send motion commands to the robots
sendRadioData();
// Write to the log
_logger.addFrame(_state.logFrame);
_loopMutex.unlock();
// Store processing loop status
_statusMutex.lock();
_status = curStatus;
_statusMutex.unlock();
////////////////
// Timing
RJ::Time endTime = RJ::timestamp();
int lastFrameTime = endTime - startTime;
if (lastFrameTime < _framePeriod) {
// Use system usleep, not QThread::usleep.
//
// QThread::usleep uses pthread_cond_wait which sometimes fails to
// unblock.
// This seems to depend on how many threads are blocked.
::usleep(_framePeriod - lastFrameTime);
} else {
// printf("Processor took too long: %d us\n", lastFrameTime);
}
}
vision.stop();
}
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();
}
}
