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 CGroundStationSim::WriteToRadio() { VecMsgType vecMsg; vecMsg.push_back(PROT_SIMSTAT_ACK); vecMsg.push_back(PROT_SIMSTAT_BROADCAST_MSG); // The radio msg to be broadcasted has to go last (after the ACK)! if (!SendBuffer.empty()) { ToCont(SendBuffer.front(), vecMsg); SendBuffer.pop_front(); } std::cout << "to sim: "; dobots::print(vecMsg.begin(), vecMsg.end()); writeToSim(vecMsg); }
void CSim::CmdLand() { UavIterType itUav; for (itUav=Uavs.begin(); itUav != Uavs.end(); ++itUav) { int id = itUav->UavData.UavData.UavId; VecMsgType vecMsg; vecMsg.push_back(PROT_SIMCMD_LAND); writeRadioCommand(id, vecMsg); } }
void CWpPlanner::Plan() { std::vector<FitnessGaussian2D> coverage; float heightAdjustment(0); std::vector<int> bestChoices; std::vector<WayPoint> bestChoicesWp; float heading; Position pos; WayPointsStruct curWps; int id; UAVState state; UAVState oldState; float batteryTimeLeft; LandingStruct landing; float minHeight; float maxHeight; bool enablePlanner; { // Copy current states boost::interprocess::scoped_lock<MapMutexType> lockSelf(*MutexSelf); heading = MapSelf->UavData.Geom.Heading.angle(); pos = MapSelf->UavData.Geom.Pos; curWps = MapSelf->WayPoints; id = MapSelf->UavData.UavId; state = MapSelf->UavData.State; oldState = MapSelf->PreviousState; batteryTimeLeft = MapSelf->UavData.BatteryTimeLeft; landing = MapSelf->GsCmd.Landing; minHeight = MapSelf->GsCmd.HeightMin; maxHeight = MapSelf->GsCmd.HeightMax; enablePlanner = MapSelf->GsCmd.EnablePlanner; } if (!enablePlanner) { if (config.Debug > 0) std::cout << get_cur_1ms() << " wp planner is disabled" << std::endl; return; } UAVState newState = state; UAVState newOldState = oldState; /* { // Copy relevant gaussians to vector, so we don't lock for too long // Take an area with radius: max_path_radius + 3*sigma coverage // This is oversized, take the heading into account float radius = config.NumWayPointPerPlan * config.WayPointDT * CRUISE_SPEED + 3*FIT_COV_SIGMA_X; boost::interprocess::scoped_lock<MapMutexType> lockCoverage(*MutexCoverage); FitnessCoverage->Get(coverage, pos.x()-radius, pos.x()+radius, pos.y()-radius, pos.y()+radius); } */ if (config.Debug > 1) { std::cout << "Curwp num=" << curWps.WayPointsNum << " pos=[" << pos.transpose() << "]" << " state=" << state << " oldState=" << oldState; } // When flying normally: check current plan for collisions etc. And check if the plan is still long enough. // If no collisions etc. and plan is long enough, we don't need a new plan. bool plan = true; if (newState != UAVSTATE_WAITING_TO_LAND && newState != UAVSTATE_LANDING && newState != UAVSTATE_LANDED && newState != UAVSTATE_TAKING_OFF) { if (CheckCurPlan(heightAdjustment, newState, newOldState, pos, heading, id, curWps)) { plan = false; //return; } if (newState == UAVSTATE_COLLISION_AVOIDING && state != UAVSTATE_COLLISION_AVOIDING) newOldState = state; } if (config.Debug > 1) std::cout << std::endl; // Update state in map self if (state != newState || oldState != newOldState) { boost::interprocess::scoped_lock<MapMutexType> lockSelf(*MutexSelf); MapSelf->UavData.State = newState; MapSelf->PreviousState = newOldState; state = newState; oldState = newOldState; } // When flying normally: check batteries if (newState != UAVSTATE_GOING_HOME && newState != UAVSTATE_WAITING_TO_LAND && newState != UAVSTATE_LANDING && newState != UAVSTATE_LANDED && newState != UAVSTATE_TAKING_OFF) { float distLeft = pos.x() - landing.Pos.x(); distLeft += pos.y() - landing.Pos.y(); float timeNeeded = config.BatteryCriticalTime + distLeft/config.CruiseSpeed; if (batteryTimeLeft < timeNeeded) { if (newState == UAVSTATE_COLLISION_AVOIDING) newOldState = UAVSTATE_GOING_HOME; else newState = UAVSTATE_GOING_HOME; } } // Fly height: default height + adjustment for collision avoidance pos.z() = minHeight + id * (maxHeight-minHeight) / (UAVS_NUM-1); pos.z() += heightAdjustment; switch (newState) { case UAVSTATE_GOING_HOME: { // Fly directly to land circle WayPoint wp; GetLandWp(wp, landing, pos.z()); bestChoicesWp.push_back(wp); // Check if we are at the land circle yet Position dPos(pos); dPos.x() -= wp.to.x(); dPos.y() -= wp.to.y(); dPos.z() = 0; if (dPos.norm() < landing.Radius + 50) // TODO: magic number newState = UAVSTATE_WAITING_TO_LAND; break; } case UAVSTATE_WAITING_TO_LAND: { // Check if we can land: no other uavs below us landing or waiting to land bool land=true; { boost::interprocess::scoped_lock<MapMutexType> lockUavs(*MutexUavs); MapUavIterType it; for (it = MapUavs->begin(); it != MapUavs->end(); ++it) { if ((it->second.data.UavId < id && it->second.data.State == UAVSTATE_WAITING_TO_LAND) || it->second.data.State == UAVSTATE_LANDING) { if (config.Debug > 0) std::cout << "Waiting for uav " << it->second.data.UavId << " to land" << std::endl; land = false; break; } } } if (land) { if (config.Debug > 0) std::cout << "My turn to land" << std::endl; newState = UAVSTATE_LANDING; VecMsgType vecMsg; vecMsg.push_back(PROT_AP_SET_MODE); vecMsg.push_back(AP_PROT_MODE_LAND); writeToAutoPilot(vecMsg); } break; } case UAVSTATE_LANDING: { // Landing is performed by auto pilot // Check if no other UAVs below us are landing, if so: abort landing! bool land=true; { boost::interprocess::scoped_lock<MapMutexType> lockUavs(*MutexUavs); MapUavIterType it; for (it = MapUavs->begin(); it != MapUavs->end(); ++it) { if (it->second.data.State == UAVSTATE_LANDING) { if (config.Debug > 0) std::cout << "Waiting for uav " << it->second.data.UavId << " to land" << std::endl; land = false; break; } } } if (!land) { if (config.Debug > 0) std::cout << "Aborting landing" << std::endl; newState = UAVSTATE_WAITING_TO_LAND; VecMsgType vecMsg; vecMsg.push_back(PROT_AP_SET_MODE); vecMsg.push_back(AP_PROT_MODE_WP); writeToAutoPilot(vecMsg); // TODO: Assume the waypoint is still in autopilot memory or set it again? WayPoint wp; GetLandWp(wp, landing, pos.z()); bestChoicesWp.push_back(wp); } break; } case UAVSTATE_LANDED: { // Can't do much now :) break; } default: { if (plan) // Make a new plan PlanStep(bestChoicesWp, bestChoices, config.NumWayPointPerPlan, config.WayPointDT, pos, heading, coverage); break; } } // Update state in map self if (state != newState || oldState != newOldState) { boost::interprocess::scoped_lock<MapMutexType> lockSelf(*MutexSelf); MapSelf->UavData.State = newState; MapSelf->PreviousState = newOldState; if (newState == UAVSTATE_LANDING) MapSelf->RequestedAPModeByWP = AP_PROT_MODE_LAND; else MapSelf->RequestedAPModeByWP = AP_PROT_MODE_WP; } // When avoiding collision, set max vertical speed if ((newState == UAVSTATE_COLLISION_AVOIDING) && (!bestChoices.empty())) { bestChoicesWp.rbegin()->VerticalSpeed = (heightAdjustment>0) ? config.MaxVertSpeed : -config.MaxVertSpeed; } // Send new waypoints to the autopilot if (!bestChoicesWp.empty()) { WayPointsStruct wps; std::vector<WayPoint>::reverse_iterator itChoicesWp; for (itChoicesWp = bestChoicesWp.rbegin(); itChoicesWp != bestChoicesWp.rend(); ++itChoicesWp) wps.push_back(*itChoicesWp); if (config.Debug > 0) { std::cout << "bestchoices="; dobots::print(bestChoices.rbegin(), bestChoices.rend()); dobots::print(bestChoicesWp.rbegin(), bestChoicesWp.rend()); } VecMsgType vecMsg; vecMsg.push_back(PROT_AP_SET_WAYPOINTS); ToCont(wps, vecMsg); writeToAutoPilot(vecMsg); } }
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::Tick() { // int* cmd = readCommand(false); // if (cmd != NULL) // { // // } VecMsg = readSim(false); if (!VecMsg->empty()) { std::cout << "GroundStation " << ModuleId << " from SIM: "; 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_SIMCMD_RADIO_ROUND_START: { // Our turn to send messages RadioRoundState = RADIO_STATE_ROUND_START; WriteToRadio(); break; } case PROT_SIMCMD_RADIO_ROUND_BROADCAST_MSG: { // Received messages from other uavs RadioRoundState = RADIO_STATE_ROUND_SENDRECEIVE; RadioMsg bmsg; it = FromCont(bmsg, it, VecMsg->end()); std::cout << "GroundStation " << ModuleId << " received bmsg: " << bmsg << std::endl; ReceiveBuffer.push_back(bmsg); break; } case PROT_SIMCMD_RADIO_ROUND_END: { //RadioRoundState = RADIO_STATE_ROUND_END; // At this state we wait for other parts to update before going to IDLE ReadReceiveBuffer(); RadioRoundState = RADIO_STATE_ROUND_IDLE; // At this state the write buffer can be filled with new msgs. VecMsgType vecMsg; vecMsg.push_back(PROT_SIMSTAT_ACK); std::cout << "to sim: "; dobots::print(vecMsg.begin(), vecMsg.end()); writeToSim(vecMsg); // Fill outbuffer with "new" msg WriteToOutBuffer(CmdMsg); break; } } } VecMsg->clear(); // Send ack when we received the msgs from other uavs if (RadioRoundState == RADIO_STATE_ROUND_SENDRECEIVE) { VecMsgType vecMsg; vecMsg.push_back(PROT_SIMSTAT_ACK); std::cout << "to sim: "; dobots::print(vecMsg.begin(), vecMsg.end()); writeToSim(vecMsg); } } VecMsg = readFromGuiInterface(false); if (!VecMsg->empty()) { std::cout << "RADIO " << ModuleId << " from GuiInterface: "; dobots::print(VecMsg->begin(), VecMsg->end()); // Should have more protocol here? VecMsgType::iterator it = VecMsg->begin(); while (it != VecMsg->end()) { int type = *it++; //std::cout << "Type=" << type << std::endl; switch (type) { case RADIO_MSG_RELAY_CMD: { RadioMsgRelayCmd cmdMsg; it = FromCont(cmdMsg, it, VecMsg->end()); CmdMsg.Data.Data[1].Cmd = cmdMsg; break; } } } VecMsg->clear(); } 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(); } }