//设定机器人数量,并自动添加和删除机器人对象
bool Simulator::SetNumRobots(bool yellow, int n)
{
if ((n < 0) || (n > MAX_TEAM_ROBOTS))
{
return false;
}
if (yellow)
{
while (num_yellow < n)
{
AddRobot(yellow, ROBOT_TYPE_DIFF);
SetRobotRandom(yellow, num_yellow - 1);
}
if (n < num_yellow)
{
num_yellow = n;
}
}
else
{
while (num_blue < n)
{
AddRobot(yellow, ROBOT_TYPE_DIFF);
SetRobotRandom(yellow, num_blue - 1);
}
if (n < num_blue)
{
num_blue = n;
}
}
return (true);
}
cPluginRobot * cVHPlugin::NewRobot(const string &Nick, int uclass)
{
cPluginRobot *robot = new cPluginRobot(Nick, this, mServer);
//set class before adding to list, otherwise user can't be op
robot->mClass = tUserCl(uclass);
if (AddRobot(robot))
return robot;
else {
delete robot;
return NULL;
}
}
int RobotWorld::LoadRobot(const string& fn)
{
Robot* robot = new Robot;
if(!robot->Load(fn.c_str())) {
delete robot;
return -1;
}
const char* justfn = GetFileName(fn.c_str());
char* buf = new char[strlen(justfn)+1];
strcpy(buf,justfn);
StripExtension(buf);
string name=buf;
delete [] buf;
int i = AddRobot(name,robot);
return i;
}
CRobot::CRobot(void)
{
// Default setting when CRobot is created
mCanDrawTrajectory = false;
mZoom = 1;
SetSensorNum(6); // Default: 6 sensors
SetRadiationCone();
SetSensorRange();
SetNoiseProperties(0.0, 0.0);
SetSimulationTimeStep();
SetSensorSamplingRate();
AddRobot(CPoint(200, 200));
SetActiveRobot(0);
PrepareToRun(); // Init certain variables for simulation
}
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;
}
