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 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 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 SoccerView::UpdateRobots ( SSL_DetectionFrame &detection )
{
int robots_blue_n = detection.robots_blue_size();
int robots_yellow_n = detection.robots_yellow_size();
int i,j,yellowj=0,bluej=0;
int team=teamBlue;
SSL_DetectionRobot robot;
for ( i = 0; i < robots_blue_n+robots_yellow_n; i++ )
{
if ( i<robots_blue_n )
{
robot = detection.robots_blue ( i );
team = teamBlue;
j=bluej;
}
else
{
robot = detection.robots_yellow ( i-robots_blue_n );
team = teamYellow;
j=yellowj;
}
double x,y,orientation,conf =robot.confidence();
int id=NA;
if ( robot.has_robot_id() )
id = robot.robot_id();
else
id = NA;
x = robot.x();
y = robot.y();
if ( robot.has_orientation() )
orientation = robot.orientation() *180.0/M_PI;
else
orientation = NAOrientation;
//seek to the next robot of the same camera and team colour
while ( j<robots.size() && ( robots[j]->key!=detection.camera_id() || robots[j]->teamID!=team ) )
j++;
if ( j+1>robots.size() )
AddRobot ( new Robot ( x,y,orientation,team,id,detection.camera_id(),conf ) );
robots[j]->SetPose ( x,y,orientation,conf );
QString label;
if ( id!=NA )
label.setNum ( id,16 );
else
label = "?";
label = label.toUpper();
if ( label!=robots[j]->robotLabel )
robots[j]->robotLabel = label;
j++;
if ( i<robots_blue_n )
bluej=j;
else
yellowj=j;
}
for ( j=bluej; j<robots.size(); j++ )
{
if ( robots[j]->key==detection.camera_id() && robots[j]->teamID==teamBlue )
robots[j]->conf=0.0;
}
for ( j=yellowj; j<robots.size(); j++ )
{
if ( robots[j]->key==detection.camera_id() && robots[j]->teamID==teamYellow )
robots[j]->conf=0.0;
}
return;
}
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);
}
}
}
/**
* 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();
}
