void CSim::CmdStart()
{
VecMsgType vecMsg;
UavIterType it;
int id=0;
for (it=Uavs.begin(); it != Uavs.end(); ++it, ++id)
{
//it->UavData.UavData.UavId = id; // Should be set at init
vecMsg.clear();
it->UavData.UavData.Geom.Init();
it->UavData.UavData.Geom.Pos.x() = config.GroundStationX;
it->UavData.UavData.Geom.Pos.y() = config.GroundStationY;
vecMsg.push_back(PROT_SIMCMD_SET_GEOM);
//geom.Pos << (id+0)*50, (id+0)*50, (id+0)*10;
ToCont(it->UavData.UavData.Geom, vecMsg);
//vecMsg.push_back(PROT_SIMCMD_SET_BATTERY);
//vecMsg.push_back(40*60 + rand()%5); // 40 - 44 minutes battery time
//vecMsg.push_back((i+35)*60);
it->UavData.UavData.State = UAVSTATE_LANDED;
vecMsg.push_back(PROT_SIMCMD_SET_STATE);
vecMsg.push_back(it->UavData.UavData.State);
if (it->AutoPilotSim)
{
writeAutoPilotCommand(id, vecMsg);
it->WaitForReplyAutoPilot = true;
}
else
it->WaitForReplyAutoPilot = false;
// Each 2 minutes 2 uavs liftoff (with 20s in between)
//it->TakeOffTime = int(id/2)*120.0 + id*20 + rand()%10;
it->TakeOffTime = id*5.0;
if (id == UAVS_NUM)
it->TakeOffTime = 0;
//it->TakeOffTime = 0;
it->RadioWorks = false;
it->WaitForReplyRadio = false;
// Add a dummy wp
WayPoint wp;
wp.to << config.GroundStationX, config.GroundStationY, 0;
it->UavData.WayPoints.push_back(wp);
BroadcastMsgs[id].clear();
}
RadioRoundState = RADIO_STATE_ROUND_IDLE;
LastRadioRoundStartTime = 0;
Running = true;
StartTimeStamp = get_cur_1ms();
LastTimeStepTimeStamp = get_cur_1ms();
}
void CSim::Tick()
{
IntMsg = readCommand(false);
if (IntMsg != NULL)
{
switch(*IntMsg)
{
case CMD_INIT:
CmdInit();
break;
case CMD_START:
CmdStart();
break;
case CMD_STOP:
CmdLand();
break;
}
}
// ------------------------------------------------------------------------------
// Read radio
// ------------------------------------------------------------------------------
UavIterType itUav;
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
int id = itUav->UavData.UavData.UavId;
VecMsg = readRadioState(id, false);
if (!VecMsg->empty())
{
std::cout << "SIM from Radio " << id << ": ";
dobots::print(VecMsg->begin(), VecMsg->end());
VecMsgType::iterator it = VecMsg->begin();
while (it != VecMsg->end())
{
int type = *it++;
//std::cout << "Type=" << type << std::endl;
switch (type)
{
case PROT_SIMSTAT_ACK:
{
itUav->WaitForReplyRadio = false;
break;
}
case PROT_SIMSTAT_BROADCAST_MSG:
{
// This only works when we assume that the broadcast msg is at the end of vecmsg
//UavIterType itUav2;
if (it != VecMsg->end() && itUav->RadioWorks)
{
// Only add the msg to connected neighbours
//for (itUav2 = Uavs.begin(); itUav2 != Uavs.end(); ++itUav2)
for (int i = 0; i < itUav->UavData.NeighBours.ConnectedNeighboursNum; ++i)
{
//int id2 = itUav2->UavData.UavData.UavId;
int id2 = itUav->UavData.NeighBours.ConnectedNeighbours[i];
BroadcastMsgs[id2].push_back(PROT_SIMCMD_RADIO_ROUND_BROADCAST_MSG);
BroadcastMsgs[id2].insert(BroadcastMsgs[id2].end(), it, VecMsg->end());
}
// Debug test
RadioMsg bmsg;
it = FromCont(bmsg, it, VecMsg->end());
if (it != VecMsg->end())
std::cout << "ERROR! " << bmsg << std::endl;
std::cout << "Adding bmsg: " << bmsg << std::endl;
}
break;
}
}
}
VecMsg->clear();
}
}
// ------------------------------------------------------------------------------
// Read new auto pilot state (uav position etc)
// ------------------------------------------------------------------------------
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
if (!itUav->AutoPilotSim)
continue;
int id = itUav->UavData.UavData.UavId;
VecMsg = readAutoPilotState(id, false);
if (!VecMsg->empty())
{
std::cout << "SIM from AP " << id << ": ";
dobots::print(VecMsg->begin(), VecMsg->end());
// if (Uavs[i].UavData.UavData.State == UAVSTATE_LANDED)
// return; // Nothing to read here :)
VecMsgType::iterator it = VecMsg->begin();
while (it != VecMsg->end())
{
int type = *it++;
//std::cout << "Type=" << type << std::endl;
switch (type)
{
case PROT_SIMSTAT_ACK:
{
itUav->WaitForReplyAutoPilot = false;
break;
}
case PROT_SIMSTAT_GEOM:
{
//UavGeomStruct geom;
//FromCont(geom, it, VecMsg->end());
it = FromCont(itUav->UavData.UavData.Geom, it, VecMsg->end());
break;
}
// case PROT_SIMSTAT_GEOM_EXTRA:
// {
// itUav->UavData.Heading = *it++;
// itUav->UavData.RollAngle = *it++;
// break;
// }
case PROT_SIMSTAT_BATTERY:
{
itUav->UavData.UavData.BatteryTimeLeft = *it++;
break;
}
case PROT_SIMSTAT_WP:
{
it = FromCont(itUav->UavData.WayPoints[0], it, VecMsg->end());
break;
}
}
}
VecMsg->clear();
}
}
if (Running)
{
// --------------------------------------------------------------------------
// Check if can take the next step in the radio round
// --------------------------------------------------------------------------
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
if (itUav->WaitForReplyRadio)
{
// std::cout << "waiting for radio reply " << itUav->UavData.UavData.UavId << std::endl;
return;
}
}
// One radio round
// state = idle, waiting for time
// ==> start --- state = idle -> start
// <== bmsg, ack --- state = start -> send/receive
// ==> bmsgs --- state = send/receive
// <== ack --- state = send/receive -> end
// ==> end --- state = end
// <== ack --- state = end -> idle, wait for time
if (RadioRoundState != RADIO_STATE_ROUND_IDLE)
std::cout << "RadioRoundState=" << RadioRoundState << std::endl;
switch (RadioRoundState)
{
case RADIO_STATE_ROUND_START:
{
// Radios received the start command and replied with their msg
// Send msgs to all uavs with working radio
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
int id = itUav->UavData.UavData.UavId;
if (itUav->RadioWorks && !BroadcastMsgs[id].empty())
{
itUav->WaitForReplyRadio = true;
writeRadioCommand(id, BroadcastMsgs[id]);
}
BroadcastMsgs[id].clear();
}
RadioRoundState = RADIO_STATE_ROUND_SENDRECEIVE;
return; // return, not break, since we have to wait for replies
}
case RADIO_STATE_ROUND_SENDRECEIVE:
{
// Tell all uavs that the radio round is over
VecMsgType vecMsg;
vecMsg.push_back(PROT_SIMCMD_RADIO_ROUND_END);
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
itUav->WaitForReplyRadio = true;
writeRadioCommand(itUav->UavData.UavData.UavId, vecMsg);
}
RadioRoundState = RADIO_STATE_ROUND_END;
return; // return, not break, since we have to wait for replies
}
case RADIO_STATE_ROUND_END:
{
// All uavs received the end of the round, radio goes idle
RadioRoundState = RADIO_STATE_ROUND_IDLE;
break;
}
}
VecMsgType vecMsg;
// --------------------------------------------------------------------------
// After some time waiting it's time for a new radio round
// --------------------------------------------------------------------------
if (LastRadioRoundStartTime+config.RadioRoundTime < CurTime)
{
LastRadioRoundStartTime = CurTime;
RadioRoundState = RADIO_STATE_ROUND_START;
vecMsg.push_back(PROT_SIMCMD_RADIO_ROUND_START);
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
itUav->WaitForReplyRadio = true;
writeRadioCommand(itUav->UavData.UavData.UavId, vecMsg);
}
return; // return, since we have to wait for replies
}
// --------------------------------------------------------------------------
// Check if we can take the next time step: shouldn't be waiting for any ACK (radio or autopilot)
// --------------------------------------------------------------------------
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
if (itUav->WaitForReplyAutoPilot || itUav->WaitForReplyRadio)
{
// std::cout << "waiting for reply " << itUav->UavData.UavData.UavId << std::endl;
return;
}
}
#ifdef SIM_REALTIME
// Simulation should go real time
if (get_duration(LastTimeStepTimeStamp, get_cur_1ms()) < 1000*config.TimeStep)
return;
LastTimeStepTimeStamp = get_cur_1ms();
#endif
// If we arrived here, all uavs have taken their time step and reported back the new state
// Update the neighbours of each UAV
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
itUav->UavData.NeighBours.clear();
// if (!itUav->AutoPilotSim)
// {
// // UAV has unknown position, let's just say it's connected to everything
// UavIterType itUav2;
// for (itUav2=Uavs.begin(); itUav2 != Uavs.end(); ++itUav2)
// if (itUav != itUav2)
// itUav->UavData.NeighBours.push_back(itUav2->UavData.UavData.UavId);
// continue;
// }
Position p1(itUav->UavData.UavData.Geom.Pos);
if (itUav->UavData.UavData.UavId == UAVS_NUM)
p1 << config.GroundStationX, config.GroundStationY, 0;
Position p2, diff;
UavIterType itUav2;
for (itUav2=Uavs.begin(); itUav2 != Uavs.end(); ++itUav2)
{
// Uav can't listen to it's own msgs
if (itUav == itUav2)
continue;
// Position unknown of one of the UAVs: assume connected
if ((!itUav->AutoPilotSim && itUav->UavData.UavData.UavId != UAVS_NUM)
|| (!itUav2->AutoPilotSim && itUav2->UavData.UavData.UavId != UAVS_NUM))
{
itUav->UavData.NeighBours.push_back(itUav2->UavData.UavData.UavId);
}
// Check distance
else
{
if (itUav2->UavData.UavData.UavId == UAVS_NUM)
p2 << config.GroundStationX, config.GroundStationY, 0;
else
p2 = itUav2->UavData.UavData.Geom.Pos;
diff = p1 - p2;
if (diff.dot(diff) <= config.RadioRange*config.RadioRange)
{
itUav->UavData.NeighBours.push_back(itUav2->UavData.UavData.UavId);
}
}
}
//std::cout << "uav " << it->UavData.UavData.UavId << " neighbours: ";
//dobots::print(it->UavData.NeighBours.ConnectedNeighbours, it->UavData.NeighBours.ConnectedNeighbours + it->UavData.NeighBours.ConnectedNeighboursNum);
}
// Write current states of uavs to output file
FileOut << get_cur_1ms() << " ";
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
if (itUav->UavData.UavData.UavId == UAVS_NUM)
{
itUav->UavData.UavData.Geom.Pos << config.GroundStationX, config.GroundStationY, 0;
itUav->UavData.WayPoints[0].to << config.GroundStationX, config.GroundStationY, 0;
}
else if (!itUav->AutoPilotSim)
continue;
FileOut << itUav->UavData.UavData.UavId << " ";
FileOut << itUav->UavData.UavData.Geom.Pos.x() << " ";
FileOut << itUav->UavData.UavData.Geom.Pos.y() << " ";
FileOut << itUav->UavData.UavData.Geom.Pos.z() << " ";
FileOut << itUav->UavData.UavData.Geom.Heading.angle() << " ";
FileOut << itUav->UavData.WayPoints[0].to.x() << " ";
FileOut << itUav->UavData.WayPoints[0].to.y() << " ";
FileOut << itUav->UavData.WayPoints[0].to.z() << " ";
//file << autoPilotSim->RollAngle.angle();
int neighbours = 0;
for (int i=0; i<itUav->UavData.NeighBours.ConnectedNeighboursNum; ++i)
neighbours |= 1 << itUav->UavData.NeighBours.ConnectedNeighbours[i];
FileOut << neighbours << " ";
}
FileOut << std::endl;
// Check for end of simulation time
if (CurTime > SimTime)
{
std::cout << std::endl << "----- End of simulation -----" << std::endl;
std::cout << SimTime << "s simulated in " << get_duration(StartTimeStamp, get_cur_1ms())/1000 << "s" << std::endl;
exit(1);
}
#ifndef SIM_REALTIME
// Give UAVs some time to calculate
// Wp planner takes about 25 ms
// Should have planning at at least 4 Hz, so 1 plan per 25 timesteps
// So each timestep should at least take 1ms
usleep(1*1000);
//usleep(10*1000); // 10ms = 0.01s = 0.1*timestep
//usleep(50*1000); // 50ms = 0.05s = 0.5*timestep
#endif
// Now that we've written all the latest states, we can go to the next time step
std::cout << std::endl << "----- Next time step " << CurTime << " -----" << std::endl;
// Command autopilot to take a time step
for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav)
{
vecMsg.clear();
int id = itUav->UavData.UavData.UavId;
if (itUav->UavData.UavData.State == UAVSTATE_LANDED)
{
std::cout << id << " takeofftime=" << itUav->TakeOffTime << std::endl;
if (CurTime > itUav->TakeOffTime)
{
itUav->UavData.UavData.State = UAVSTATE_FLYING;
itUav->RadioWorks = true;
vecMsg.push_back(PROT_SIMCMD_SET_STATE);
vecMsg.push_back(UAVSTATE_FLYING);
}
}
if (!itUav->AutoPilotSim)
continue;
vecMsg.push_back(PROT_SIMCMD_TIMESTEP);
vecMsg.push_back(config.TimeStep);
itUav->WaitForReplyAutoPilot = true;
writeAutoPilotCommand(id, vecMsg);
}
// for (int i=0; i<Uavs.size(); ++i)
// {
// writeAutoPilotCommand(i, vecMsg);
// }
// Increase time
CurTime += config.TimeStep;
}
usleep(config.TickTime);
}
void CGroundStationSim::ReadReceiveBuffer()
{
UavStruct uav;
while (!ReceiveBuffer.empty())
{
for (int i=0; i<RADIO_NUM_RELAY_PER_MSG; ++i)
{
switch (ReceiveBuffer.front().Data.Data[i].MessageType)
{
case RADIO_MSG_RELAY_POS:
{
uav.UavId = ReceiveBuffer.front().Data.Data[i].Pos.UavId -1;
// Ignore invalid uav IDs and own messages
if ((uav.UavId > -1) && (uav.UavId != UavId))
{
uav.FromRadioMsg(ReceiveBuffer.front().Data.Data[i].Pos);
std::cout << "Received: " << ReceiveBuffer.front().Data.Data[i] << " === " << uav << std::endl;
VecMsgType vecMsg;
vecMsg.push_back(PROT_RADIO_MSG_RELAY);
ToCont(ReceiveBuffer.front().Data.Data[i], vecMsg);
writeToMapUavs(vecMsg);
vecMsg.clear();
vecMsg.push_back(PROT_RADIO_MSG_RELAY_POS);
ToCont(ReceiveBuffer.front().Data.Data[i].Pos, vecMsg);
writeToGuiInterface(vecMsg);
}
break;
}
case RADIO_MSG_RELAY_FIRE:
{
FireStruct fire;
VecMsgType vecMsg;
fire.FromRadioMsg(ReceiveBuffer.front().Data.Data[i].Fires.Fire[0]);
if (fire.UavId > -1)
{
std::cout << "Received: " << ReceiveBuffer.front().Data.Data[i].Fires.Fire[0] << std::endl;
vecMsg.push_back(PROT_FIRE_STRUCT);
ToCont(fire, vecMsg);
writeToMapFire(vecMsg);
vecMsg.clear();
vecMsg.push_back(PROT_RADIO_MSG_RELAY_FIRE);
ToCont(ReceiveBuffer.front().Data.Data[i].Fires.Fire[0], vecMsg);
writeToGuiInterface(vecMsg);
}
fire.FromRadioMsg(ReceiveBuffer.front().Data.Data[i].Fires.Fire[1]);
if (fire.UavId > -1)
{
std::cout << "Received: " << ReceiveBuffer.front().Data.Data[i].Fires.Fire[1] << std::endl;
vecMsg.clear();
vecMsg.push_back(PROT_FIRE_STRUCT);
ToCont(fire, vecMsg);
writeToMapFire(vecMsg);
vecMsg.clear();
vecMsg.push_back(PROT_RADIO_MSG_RELAY_FIRE);
ToCont(ReceiveBuffer.front().Data.Data[i].Fires.Fire[1], vecMsg);
writeToGuiInterface(vecMsg);
}
break;
}
case RADIO_MSG_RELAY_CMD:
{
// Ignore
break;
}
}
}
ReceiveBuffer.pop_front();
}
}
