Esempio n. 1
0
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());
}
Esempio n. 2
0
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;
}
Esempio n. 3
0
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;
    }
}
Esempio n. 4
0
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;
}
Esempio n. 5
0
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];
    }
}
Esempio n. 7
0
File: RONet.cpp Progetto: p1tt1/sumo
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;
}
Esempio n. 8
0
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;
        }
    }
}
Esempio n. 9
0
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;
}
Esempio n. 10
0
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;
}
Esempio n. 11
0
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;
}