RORoute *
RORouteDef_Complete::buildCurrentRoute(SUMOAbstractRouter<ROEdge,ROVehicle> &router,
SUMOTime begin, const ROVehicle &veh) const {
if (myTryRepair) {
const std::vector<const ROEdge*> &oldEdges = myEdges;
if (oldEdges.size()==0) {
MsgHandler *m = OptionsCont::getOptions().getBool("continue-on-unbuild") ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance();
m->inform("Could not repair empty route of vehicle '" + veh.getID() + "'.");
return new RORoute(myID, 0, 1, std::vector<const ROEdge*>(), copyColorIfGiven());
}
std::vector<const ROEdge*> newEdges;
newEdges.push_back(*(oldEdges.begin()));
for (std::vector<const ROEdge*>::const_iterator i=oldEdges.begin()+1; i!=oldEdges.end(); ++i) {
if ((*(i-1))->isConnectedTo(*i)) {
newEdges.push_back(*i);
} else {
std::vector<const ROEdge*> edges;
router.compute(*(i-1), *i, &veh, begin, edges);
if (edges.size()==0) {
return 0;
}
std::copy(edges.begin()+1, edges.end(), back_inserter(newEdges));
}
}
if (myEdges!=newEdges) {
MsgHandler::getWarningInstance()->inform("Repaired route of vehicle '" + veh.getID() + "'.");
}
myEdges = newEdges;
}
SUMOReal costs = router.recomputeCosts(myEdges, &veh, begin);
if (costs<0) {
throw ProcessError("Route '" + getID() + "' (vehicle '" + veh.getID() + "') is not valid.");
}
return new RORoute(myID, 0, 1, myEdges, copyColorIfGiven());
}
bool
ROJTRRouter::compute(const ROEdge* from, const ROEdge* to,
const ROVehicle* const vehicle,
SUMOTime time, ConstROEdgeVector& into) {
const ROJTREdge* current = static_cast<const ROJTREdge*>(from);
SUMOReal timeS = STEPS2TIME(time);
std::set<const ROEdge*> avoidEdges;
// route until a sinks has been found
while (current != 0 && current != to &&
current->getFunc() != ROEdge::ET_SINK &&
(int)into.size() < myMaxEdges) {
into.push_back(current);
if (!myAllowLoops) {
avoidEdges.insert(current);
}
timeS += current->getTravelTime(vehicle, timeS);
current = current->chooseNext(myIgnoreClasses ? 0 : vehicle, timeS, avoidEdges);
assert(myIgnoreClasses || current == 0 || !current->prohibits(vehicle));
}
// check whether no valid ending edge was found
if (current == 0 || (int) into.size() >= myMaxEdges) {
if (myAcceptAllDestination) {
return true;
} else {
MsgHandler* mh = myUnbuildIsWarningOnly ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance();
mh->inform("The route starting at edge '" + from->getID() + "' could not be closed.");
return false;
}
}
// append the sink
if (current != 0) {
into.push_back(current);
}
return true;
}
int main()
{
sout << locker << "\n Console Receiver Demo "
<< "\n =======================\n" << unlocker;
try
{
EndPoint rep("127.0.0.1",8081);
//EndPoint sep("127.0.0.1",2049);
// MsgReceiver_Proc is your receiver's server message handling
// FileReceiver_Proc is your receiver's server file handling
Communicator rcvr(rep);
MsgHandler<MsgReceiver_Proc> rMsgHandler;
rMsgHandler.setCommunicator(&rcvr);
rcvr.attachMsgHandler(&rMsgHandler);
FileHandler<FileReceiver_Proc> rFileHandler;
rFileHandler.setFileDestination(".\\debug");
rFileHandler.setCommunicator(&rcvr);
rcvr.attachFileHandler(&rFileHandler);
rcvr.listen();
sout << "\n\n press key to exit: ";
sout.flush();
_getche();
sout << "\n\n";
rcvr.disconnect();
}
catch(std::exception& ex)
{
sout << locker << "\n " << ex.what() << "\n\n" << unlocker;
}
}
bool IMUCounter::handle_msg(const struct log_Format &f, uint8_t *msg) {
if (strncmp(f.name,"IMU",4)) {
// not an IMU message
return true;
}
if (handler->field_value(msg, "TimeUS", last_imu_timestamp)) {
} else if (handler->field_value(msg, "TimeMS", last_imu_timestamp)) {
last_imu_timestamp *= 1000;
} else {
::printf("Unable to find timestamp in IMU message");
}
return true;
}
bool IMUCounter::handle_msg(const struct log_Format &f, uint8_t *msg) {
if (strncmp(f.name,"PARM",4) == 0) {
// gather parameter values to check for SCHED_LOOP_RATE
parm_handler->field_value(msg, "Name", last_parm_name, sizeof(last_parm_name));
parm_handler->field_value(msg, "Value", last_parm_value);
return true;
}
if (strncmp(f.name,"IMU",4) &&
strncmp(f.name,"IMT",4)) {
// not an IMU message
return true;
}
if (handler->field_value(msg, "TimeUS", last_clock_timestamp)) {
} else if (handler->field_value(msg, "TimeMS", last_clock_timestamp)) {
last_clock_timestamp *= 1000;
} else {
::printf("Unable to find timestamp in message");
}
return true;
}
void
RORouteDef::repairCurrentRoute(SUMOAbstractRouter<ROEdge, ROVehicle>& router,
SUMOTime begin, const ROVehicle& veh) const {
const std::vector<const ROEdge*>& oldEdges = myAlternatives[0]->getEdgeVector();
if (oldEdges.size() == 0) {
MsgHandler* m = OptionsCont::getOptions().getBool("ignore-errors") ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance();
m->inform("Could not repair empty route of vehicle '" + veh.getID() + "'.");
return;
}
std::vector<const ROEdge*> newEdges;
if (oldEdges.size() == 1) {
/// should happen with jtrrouter only
router.compute(oldEdges.front(), oldEdges.front(), &veh, begin, newEdges);
} else {
newEdges.push_back(*(oldEdges.begin()));
for (std::vector<const ROEdge*>::const_iterator i = oldEdges.begin() + 1; i != oldEdges.end(); ++i) {
if ((*(i - 1))->isConnectedTo(*i)) {
newEdges.push_back(*i);
} else {
std::vector<const ROEdge*> edges;
router.compute(*(i - 1), *i, &veh, begin, edges);
if (edges.size() == 0) {
return;
}
std::copy(edges.begin() + 1, edges.end(), back_inserter(newEdges));
}
}
}
if (myAlternatives[0]->getEdgeVector() != newEdges) {
WRITE_MESSAGE("Repaired route of vehicle '" + veh.getID() + "'.");
myNewRoute = true;
RGBColor* col = myAlternatives[0]->getColor() != 0 ? new RGBColor(*myAlternatives[0]->getColor()) : 0;
myPrecomputed = new RORoute(myID, 0, 1, newEdges, col);
} else {
myPrecomputed = myAlternatives[0];
}
}
bool
RONet::computeRoute(OptionsCont& options, SUMOAbstractRouter<ROEdge, ROVehicle>& router,
const ROVehicle* const veh) {
MsgHandler* mh = (OptionsCont::getOptions().getBool("ignore-errors") ?
MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance());
std::string noRouteMsg = "The vehicle '" + veh->getID() + "' has no valid route.";
RORouteDef* const routeDef = veh->getRouteDefinition();
// check if the route definition is valid
if (routeDef == 0) {
mh->inform(noRouteMsg);
return false;
}
// check whether the route was already saved
if (routeDef->isSaved()) {
return true;
}
//
RORoute* current = routeDef->buildCurrentRoute(router, veh->getDepartureTime(), *veh);
if (current == 0 || current->size() == 0) {
delete current;
mh->inform(noRouteMsg);
return false;
}
// check whether we have to evaluate the route for not containing loops
if (options.getBool("remove-loops")) {
current->recheckForLoops();
// check whether the route is still valid
if (current->size() == 0) {
delete current;
mh->inform(noRouteMsg + " (after removing loops)");
return false;
}
}
// add built route
routeDef->addAlternative(router, veh, current, veh->getDepartureTime());
return true;
}
void
RORouteDef::preComputeCurrentRoute(SUMOAbstractRouter<ROEdge, ROVehicle>& router,
SUMOTime begin, const ROVehicle& veh) const {
myNewRoute = false;
const OptionsCont& oc = OptionsCont::getOptions();
assert(myAlternatives[0]->getEdgeVector().size() > 0);
MsgHandler* mh = (OptionsCont::getOptions().getBool("ignore-errors") ?
MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance());
if (myAlternatives[0]->getFirst()->prohibits(&veh) && (!oc.getBool("repair.from")
// do not try to reassign starting edge for trip input
|| myMayBeDisconnected || myAlternatives[0]->getEdgeVector().size() < 2)) {
mh->inform("Vehicle '" + veh.getID() + "' is not allowed to depart on edge '" +
myAlternatives[0]->getFirst()->getID() + "'.");
return;
} else if (myAlternatives[0]->getLast()->prohibits(&veh) && (!oc.getBool("repair.to")
// do not try to reassign destination edge for trip input
|| myMayBeDisconnected || myAlternatives[0]->getEdgeVector().size() < 2)) {
// this check is not strictly necessary unless myTryRepair is set.
// However, the error message is more helpful than "no connection found"
mh->inform("Vehicle '" + veh.getID() + "' is not allowed to arrive on edge '" +
myAlternatives[0]->getLast()->getID() + "'.");
return;
}
if (myTryRepair || myUsingJTRR) {
ConstROEdgeVector newEdges;
if (repairCurrentRoute(router, begin, veh, myAlternatives[0]->getEdgeVector(), newEdges)) {
if (myAlternatives[0]->getEdgeVector() != newEdges) {
if (!myMayBeDisconnected) {
WRITE_WARNING("Repaired route of vehicle '" + veh.getID() + "'.");
}
myNewRoute = true;
RGBColor* col = myAlternatives[0]->getColor() != 0 ? new RGBColor(*myAlternatives[0]->getColor()) : 0;
myPrecomputed = new RORoute(myID, 0, myAlternatives[0]->getProbability(), newEdges, col, myAlternatives[0]->getStops());
} else {
myPrecomputed = myAlternatives[0];
}
}
return;
}
if (RouteCostCalculator<RORoute, ROEdge, ROVehicle>::getCalculator().skipRouteCalculation()
|| OptionsCont::getOptions().getBool("remove-loops")) {
myPrecomputed = myAlternatives[myLastUsed];
} else {
// build a new route to test whether it is better
ConstROEdgeVector oldEdges;
oldEdges.push_back(myAlternatives[0]->getFirst());
oldEdges.push_back(myAlternatives[0]->getLast());
ConstROEdgeVector edges;
repairCurrentRoute(router, begin, veh, oldEdges, edges);
// check whether the same route was already used
int cheapest = -1;
for (int i = 0; i < (int)myAlternatives.size(); i++) {
if (edges == myAlternatives[i]->getEdgeVector()) {
cheapest = i;
break;
}
}
if (cheapest >= 0) {
myPrecomputed = myAlternatives[cheapest];
} else {
RGBColor* col = myAlternatives[0]->getColor() != 0 ? new RGBColor(*myAlternatives[0]->getColor()) : 0;
myPrecomputed = new RORoute(myID, 0, 1, edges, col, myAlternatives[0]->getStops());
myNewRoute = true;
}
}
}
bool
RORouteDef::repairCurrentRoute(SUMOAbstractRouter<ROEdge, ROVehicle>& router,
SUMOTime begin, const ROVehicle& veh,
ConstROEdgeVector oldEdges, ConstROEdgeVector& newEdges) const {
MsgHandler* mh = (OptionsCont::getOptions().getBool("ignore-errors") ?
MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance());
const int initialSize = (int)oldEdges.size();
if (initialSize == 1) {
if (myUsingJTRR) {
/// only ROJTRRouter is supposed to handle this type of input
router.compute(oldEdges.front(), 0, &veh, begin, newEdges);
} else {
newEdges = oldEdges;
}
} else {
if (oldEdges.front()->prohibits(&veh)) {
// option repair.from is in effect
const std::string& frontID = oldEdges.front()->getID();
for (ConstROEdgeVector::iterator i = oldEdges.begin(); i != oldEdges.end();) {
if ((*i)->prohibits(&veh) || (*i)->isInternal()) {
i = oldEdges.erase(i);
} else {
WRITE_MESSAGE("Changing invalid starting edge '" + frontID
+ "' to '" + (*i)->getID() + "' for vehicle '" + veh.getID() + "'.");
break;
}
}
}
if (oldEdges.size() == 0) {
mh->inform("Could not find new starting edge for vehicle '" + veh.getID() + "'.");
return false;
}
if (oldEdges.back()->prohibits(&veh)) {
// option repair.to is in effect
const std::string& backID = oldEdges.back()->getID();
// oldEdges cannot get empty here, otherwise we would have left the stage when checking "from"
while (oldEdges.back()->prohibits(&veh) || oldEdges.back()->isInternal()) {
oldEdges.pop_back();
}
WRITE_MESSAGE("Changing invalid destination edge '" + backID
+ "' to edge '" + oldEdges.back()->getID() + "' for vehicle '" + veh.getID() + "'.");
}
ConstROEdgeVector mandatory = veh.getMandatoryEdges(oldEdges.front(), oldEdges.back());
assert(mandatory.size() >= 2);
// removed prohibited
for (ConstROEdgeVector::iterator i = oldEdges.begin(); i != oldEdges.end();) {
if ((*i)->prohibits(&veh) || (*i)->isInternal()) {
// no need to check the mandatories here, this was done before
i = oldEdges.erase(i);
} else {
++i;
}
}
// reconnect remaining edges
if (mandatory.size() > oldEdges.size() && initialSize > 2) {
WRITE_MESSAGE("There are stop edges which were not part of the original route for vehicle '" + veh.getID() + "'.");
}
const ConstROEdgeVector& targets = mandatory.size() > oldEdges.size() ? mandatory : oldEdges;
newEdges.push_back(*(targets.begin()));
ConstROEdgeVector::iterator nextMandatory = mandatory.begin() + 1;
int lastMandatory = 0;
for (ConstROEdgeVector::const_iterator i = targets.begin() + 1;
i != targets.end() && nextMandatory != mandatory.end(); ++i) {
if ((*(i - 1))->isConnectedTo(*i, &veh)) {
newEdges.push_back(*i);
} else {
if (initialSize > 2) {
// only inform if the input is (probably) not a trip
WRITE_MESSAGE("Edge '" + (*(i - 1))->getID() + "' not connected to edge '" + (*i)->getID() + "' for vehicle '" + veh.getID() + "'.");
}
const ROEdge* const last = newEdges.back();
newEdges.pop_back();
if (!router.compute(last, *i, &veh, begin, newEdges)) {
// backtrack: try to route from last mandatory edge to next mandatory edge
// XXX add option for backtracking in smaller increments
// (i.e. previous edge to edge after *i)
// we would then need to decide whether we have found a good
// tradeoff between faithfulness to the input data and detour-length
ConstROEdgeVector edges;
if (lastMandatory >= (int)newEdges.size() || last == newEdges[lastMandatory] || !router.compute(newEdges[lastMandatory], *nextMandatory, &veh, begin, edges)) {
mh->inform("Mandatory edge '" + (*i)->getID() + "' not reachable by vehicle '" + veh.getID() + "'.");
return false;
}
while (*i != *nextMandatory) {
++i;
}
newEdges.erase(newEdges.begin() + lastMandatory + 1, newEdges.end());
std::copy(edges.begin() + 1, edges.end(), back_inserter(newEdges));
}
}
if (*i == *nextMandatory) {
nextMandatory++;
lastMandatory = (int)newEdges.size() - 1;
}
}
}
return true;
}
void MyCacheWorker::run()
{
LOG4CXX_INFO(logger, "[" << name_ << "] worker started");
// Tell backend we're ready for work
socket_->send("READY", 5);
while (!s_interrupted)
{
try
{
// Read and save all frames until we get an empty frame
// In this example there is only 1 but it could be more
zmq::message_t client_addr_msg;
socket_->recv(&client_addr_msg);
// Empty Frame
{
zmq::message_t empty;
socket_->recv(&empty);
}
// Get request, send reply
zmq::message_t request;
socket_->recv(&request);
//std::cout << "Worker: " << (char*)request.data() << std::endl;
// Do the Request
std::string reply;
uint64_t id = 0;
uint8_t cmd = 0;
std::string msgbody;
std::string clientid((char*)client_addr_msg.data(), client_addr_msg.size());
if (MyCache::parseBinMsg(request, id, cmd, msgbody))
{
#if 0
LOG4CXX_INFO(logger, "[" << name_ << "] [" << clientid << "] "
<< (char)cmd << " : "
<< id << " : "
<< msgbody.size());
#endif
MsgHandler* handler = MsgHandler::createByType(cmd);
if (handler)
{
handler->worker = this;
handler->doMsg(reply, msgbody);
delete handler;
}
}
else
{
LOG4CXX_WARN(logger, "[" << name_ << "] [" << clientid << "] "
<< (char)cmd << " : "
<< id << " : parse msg failed");
}
//reply = "FUCKYOU";
// Reply
{
zmq::message_t empty;
socket_->send(client_addr_msg, ZMQ_SNDMORE);
socket_->send(empty, ZMQ_SNDMORE);
zmq::message_t replymsg;
MyCache::packBinMsg(replymsg, id, cmd, reply);
socket_->send(replymsg);
}
}
catch(const std::exception& err)
{
LOG4CXX_ERROR(logger, "[" << name_ << "] worker error : " << err.what());
break;
}
}
LOG4CXX_INFO(logger, "[" << name_ << "] worker finished");
delete this;
}
bool LogReader::update(uint8_t &type)
{
uint8_t hdr[3];
if (::read(fd, hdr, 3) != 3) {
return false;
}
if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
printf("bad log header\n");
return false;
}
if (hdr[2] == LOG_FORMAT_MSG) {
struct log_Format f;
memcpy(&f, hdr, 3);
if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
return false;
}
memcpy(&formats[f.type], &f, sizeof(formats[f.type]));
type = f.type;
char name[5];
memset(name, '\0', 5);
memcpy(name, f.name, 4);
::printf("Defining log format for type (%d) (%s)\n", f.type, name);
// map from format name to a parser subclass:
if (streq(name, "PARM")) {
parameter_handler = new MsgHandler_PARM(formats[f.type], dataflash,
last_timestamp_usec);
msgparser[f.type] = parameter_handler;
} else if (streq(name, "GPS")) {
msgparser[f.type] = new MsgHandler_GPS(formats[f.type],
dataflash,
last_timestamp_usec,
gps, ground_alt_cm,
rel_altitude);
} else if (streq(name, "GPS2")) {
msgparser[f.type] = new MsgHandler_GPS2(formats[f.type], dataflash,
last_timestamp_usec,
gps, ground_alt_cm,
rel_altitude);
} else if (streq(name, "MSG")) {
msgparser[f.type] = new MsgHandler_MSG(formats[f.type], dataflash,
last_timestamp_usec,
vehicle, ahrs);
} else if (streq(name, "IMU")) {
msgparser[f.type] = new MsgHandler_IMU(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "IMU2")) {
msgparser[f.type] = new MsgHandler_IMU2(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "IMU3")) {
msgparser[f.type] = new MsgHandler_IMU3(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "SIM")) {
msgparser[f.type] = new MsgHandler_SIM(formats[f.type], dataflash,
last_timestamp_usec,
sim_attitude);
} else if (streq(name, "BARO")) {
msgparser[f.type] = new MsgHandler_BARO(formats[f.type], dataflash,
last_timestamp_usec, baro);
} else if (streq(name, "AHR2")) {
msgparser[f.type] = new MsgHandler_AHR2(formats[f.type], dataflash,
last_timestamp_usec,
ahr2_attitude);
} else if (streq(name, "ATT")) {
// this parser handles *all* attitude messages - the common one,
// and also the rover/copter/plane-specific (old) messages
msgparser[f.type] = new MsgHandler_ATT(formats[f.type], dataflash,
last_timestamp_usec,
ahr2_attitude);
} else if (streq(name, "MAG")) {
msgparser[f.type] = new MsgHandler_MAG(formats[f.type], dataflash,
last_timestamp_usec, compass);
} else if (streq(name, "NTUN")) {
// the label "NTUN" is used by rover, copter and plane -
// and they all look different! creation of a parser is
// deferred until we receive a MSG log entry telling us
// which vehicle type to use. Sucks.
memcpy(&deferred_formats[f.type], &formats[f.type],
sizeof(struct log_Format));
} else if (streq(name, "ARSP")) { // plane-specific(?!)
msgparser[f.type] = new MsgHandler_ARSP(formats[f.type], dataflash,
last_timestamp_usec,
airspeed);
} else {
::printf(" No parser for (%s)\n", name);
}
return true;
}
const struct log_Format &f = formats[hdr[2]];
if (f.length == 0) {
// can't just throw these away as the format specifies the
// number of bytes in the message
::printf("No format defined for type (%d)\n", hdr[2]);
exit(1);
}
uint8_t msg[f.length];
memcpy(msg, hdr, 3);
if (::read(fd, &msg[3], f.length-3) != f.length-3) {
return false;
}
type = f.type;
MsgHandler *p = msgparser[type];
if (p == NULL) {
// I guess this wasn't as self-describing as it could have been....
// ::printf("No format message received for type %d; ignoring message\n",
// type);
return true;
}
p->process_message(msg);
maybe_install_vehicle_specific_parsers();
return true;
}