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