bool add(const char *name, yarp::os::Searchable& config) {
//printf("ADDING %s\n", config.toString().c_str());
PolyDriver *pd = new PolyDriver();
YARP_ASSERT (pd!=NULL);
bool result = pd->open(config);
if (!result) {
delete pd;
return false;
}
drivers.push_back(pd);
names.push_back(ConstString(name));
IService *service = NULL;
pd->view(service);
bool backgrounded = true;
if (service!=NULL) {
backgrounded = service->startService();
if (backgrounded) {
// we don't need to poll this, so forget about the
// service interface
printf("group: service backgrounded\n");
service = NULL;
}
}
needDrive.push_back(!backgrounded);
needDriveSummary = needDriveSummary || (!backgrounded);
Drivers::factory().add(new DriverLinkCreator(name,*pd));
return true;
}
extern "C" IFace *RISCV_get_service_iface(const char *servname,
const char *facename) {
IService *iserv = static_cast<IService *>(RISCV_get_service(servname));
if (iserv == NULL) {
return NULL;
}
return iserv->getInterface(facename);
}
extern "C" IFace *RISCV_create_service(IFace *iclass, const char *name,
AttributeType *args) {
IClass *icls = static_cast<IClass *>(iclass);
IService *iobj = icls->createService(name);
iobj->initService(args);
iobj->postinitService();
return iobj;
}
void
CArrayPtrServices::ForEach_DetachFromObjectsAboutBeingDeleted() const
{
IService ** ppServiceStop;
IService ** ppService = PrgpGetServicesStop(OUT &ppServiceStop);
while (ppService != ppServiceStop)
{
IService * pService = *ppService++;
pService->DetachFromObjectsAboutBeingDeleted();
}
}
void update() {
mutex.wait();
PlatformVector<PolyDriver *>& lst = drivers;
for (unsigned int i=0; i<lst.size(); i++) {
if (needDrive[i]) {
IService *service;
lst[i]->view(service);
if (service!=NULL) {
service->updateService();
}
}
}
mutex.post();
}
void update() {
mutex.lock();
std::vector<PolyDriver *>& lst = drivers;
for (unsigned int i=0; i<lst.size(); i++) {
if (needDrive[i]) {
IService *service;
lst[i]->view(service);
if (service!=nullptr) {
service->updateService();
}
}
}
mutex.unlock();
}
extern "C" void RISCV_set_configuration(AttributeType *cfg) {
IClass *icls;
IService *iserv;
Config_.clone(cfg);
if (!Config_.is_dict()) {
RISCV_error("Wrong configuration.", NULL);
return;
}
AttributeType &Services = Config_["Services"];
if (Services.is_list()) {
for (unsigned i = 0; i < Services.size(); i++) {
icls = static_cast<IClass *>(
RISCV_get_class(Services[i]["Class"].to_string()));
if (icls == NULL) {
RISCV_error("Class %s not found",
Services[i]["Class"].to_string());
continue;
}
AttributeType &Instances = Services[i]["Instances"];
for (unsigned n = 0; n < Instances.size(); n++) {
iserv = icls->createService(Instances[n]["Name"].to_string());
iserv->initService(&Instances[n]["Attr"]);
}
}
}
// Post initialization
for (unsigned i = 0; i < listClasses_.size(); i++) {
icls = static_cast<IClass *>(listClasses_[i].to_iface());
icls->postinitServices();
}
RISCV_printf(getInterface(IFACE_SERVICE), 0, "%s",
"\n**********************************************************\n"
" RISC-V debugger\n"
" Author: Sergey Khabarov - [email protected]\n"
" Copyright 2016 GNSS Sensor Ltd. All right reserved.\n"
"**********************************************************");
IHap *ihap;
for (unsigned i = 0; i < listHap_.size(); i++) {
ihap = static_cast<IHap *>(listHap_[i].to_iface());
if (ihap->getType() == HAP_ConfigDone) {
ihap->hapTriggered(HAP_ConfigDone);
}
}
}
/// Add data service
StatusCode PersistencySvc::addCnvService(IConversionSvc* servc) {
if ( 0 != servc ) {
long type = servc->repSvcType();
long def_typ = (m_cnvDefault) ? m_cnvDefault->repSvcType() : 0;
Services::iterator it = m_cnvServices.find( type );
IConversionSvc* cnv_svc = 0;
if ( it != m_cnvServices.end() ) {
cnv_svc = (*it).second.conversionSvc();
}
if ( type == def_typ ) {
m_cnvDefault = servc;
}
if ( cnv_svc != servc ) {
IAddressCreator* icr = 0;
StatusCode status = servc->queryInterface(IAddressCreator::interfaceID(), pp_cast<void>(&icr));
if ( status.isSuccess() ) {
IService* isvc = 0;
status = servc->queryInterface(IService::interfaceID(), pp_cast<void>(&isvc));
if ( status.isSuccess() ) {
if ( 0 != cnv_svc ) {
removeCnvService (type).ignore();
}
std::pair<Services::iterator, bool> p =
m_cnvServices.insert( Services::value_type( type, ServiceEntry(type, isvc, servc, icr)));
if( p.second ) {
info() << "Added successfully Conversion service:" << isvc->name() << endmsg;
servc->addRef();
servc->setAddressCreator(this).ignore();
servc->setDataProvider(m_dataSvc).ignore();
return StatusCode::SUCCESS;
}
info() << "Cannot add Conversion service of type " << isvc->name() << endmsg;
isvc->release();
icr->release();
return StatusCode::FAILURE;
}
icr->release();
}
info() << "Cannot add Conversion service of type " << type << endmsg;
return StatusCode::FAILURE;
}
else {
return StatusCode::SUCCESS;
}
}
return BAD_STORAGE_TYPE;
}
//--------------------------------------------------------------------------------------------
// implementation of IService::reinitialize
//--------------------------------------------------------------------------------------------
StatusCode MTEventLoopMgr::reinitialize() {
// initilaize the base class
StatusCode sc = MinimalEventLoopMgr::reinitialize();
MsgStream log(msgSvc(), name());
if( sc.isFailure() ) {
log << MSG::DEBUG << "Error Initializing base class MinimalEventLoopMgr." << endmsg;
return sc;
}
// Check to see whether a new Event Selector has been specified
setProperty(m_appMgrProperty->getProperty("EvtSel"));
if( m_evtsel != "NONE" || m_evtsel.length() == 0) {
IEvtSelector* theEvtSel;
IService* theSvc;
sc = service( "EventSelector", theEvtSel );
sc = service( "EventSelector", theSvc );
if( sc.isSuccess() && ( theEvtSel != m_evtSelector ) ) {
// Setup Event Selector
m_evtSelector = theEvtSel;
if (theSvc->state() == IService::INITIALIZED) {
sc = theSvc->reinitialize();
if( sc.isFailure() ){
log << MSG::ERROR << "Failure Reinitializing EventSelector "
<< theSvc->name( ) << endmsg;
return sc;
}
} else {
sc = theSvc->initialize();
if( sc.isFailure() ){
log << MSG::ERROR << "Failure Initializing EventSelector "
<< theSvc->name( ) << endmsg;
return sc;
}
}
sc = theEvtSel->createContext(m_evtCtxt);
if( !sc.isSuccess() ) {
log << MSG::ERROR << "Can not create Context "
<< theSvc->name( ) << endmsg;
return sc;
}
log << MSG::INFO << "EventSelector service changed to "
<< theSvc->name( ) << endmsg;
}
}
else {
m_evtSelector = 0;
m_evtCtxt = 0;
}
return StatusCode::SUCCESS;
}
int main(int argc, FCSTR argv[])
{
IService* pService = CreateServerObject();
IServiceLogic* pLogic = new FCLogicWorld;
CCmdLineInfo cmdLine(argc, argv);
char buffer[MAX_PATH];
size_t bufLen = sizeof(buffer);
pService->ISRV_GetBinPath(buffer, bufLen);
#ifdef _WIN32
SetCurrentDirectory(buffer);
#else
#endif
// create the logger
Logging::IDynLogWriter* pWriter = Logging::createLogWriter( Logging::LOGWRITER_TEXTFILE);
((Logging::IDynLogTextFileWriter*)pWriter)->setFilename("FCWorldServer.log");
Logging::getLogger()->addWriter(pWriter);
Logging::getLogger()->setLogLevelFilter(5);
pWriter->release();
if ( pService )
{
pService->ISRV_AttachLogic(pLogic);
// handle any command line options passed in
if ( HandleCommandLine(cmdLine, pService) )
{
// if we get here, we can boot the service/daemon
pService->ISRV_Run(NULL);
}
// time to free up the resources
pLogic->Free();
}
// and release the service/daemon object
ReleaseServerObject(pService);
Logging::destroyLogger();
return 0;
}
extern "C" void RISCV_get_services_with_iface(const char *iname,
AttributeType *list) {
IClass *icls;
IService *iserv;
IFace *iface;
const AttributeType *tlist;
list->make_list(0);
for (unsigned i = 0; i < listClasses_.size(); i++) {
icls = static_cast<IClass *>(listClasses_[i].to_iface());
tlist = icls->getInstanceList();
if (tlist->size()) {
iserv = static_cast<IService *>((*tlist)[0u].to_iface());
iface = iserv->getInterface(iname);
if (iface) {
AttributeType t1(iface);
list->add_to_list(&t1);
}
}
}
}
std::vector< std::string > ScriptComponent::RayQuery( MathVector3 origin, MathVector3 direction, const float& length, const bool& sortByDistance, const int& maxResults )
{
AnyType::AnyTypeMap parameters;
MathVector3 originToDestination = direction - origin;
MathVector3 destination = origin + originToDestination * length;
parameters[ System::Parameters::Origin ] = origin;
parameters[ System::Parameters::Destination ] = destination;
parameters[ System::Parameters::SortByyDistance ] = sortByDistance;
parameters[ System::Parameters::MaxResults ] = maxResults;
/*AnyType::AnyTypeMap debugParameters;
debugParameters[ "origin" ] = origin;
debugParameters[ "destination" ] = destination;
IService* renderService = Management::Get( )->GetServiceManager( )->FindService( System::Types::RENDER );
renderService->Execute( "drawLine", debugParameters );*/
IService* rayService = Management::Get( )->GetServiceManager( )->FindService( System::Types::PHYSICS );
return rayService->Execute( System::Messages::CastRay, parameters ) [ "hits" ].As< std::vector< std::string > >( );
}
char *attribute_to_string(const AttributeType *attr) {
IService *iserv;
AutoBuffer *buf = &strBuffer;
if (attr->is_nil()) {
buf->write_string("None");
} else if (attr->is_int64() || attr->is_uint64()) {
buf->write_uint64(attr->to_uint64());
} else if (attr->is_string()) {
buf->write_string('\'');
buf->write_string(attr->to_string());
buf->write_string('\'');
} else if (attr->is_bool()) {
if (attr->to_bool()) {
buf->write_string("true");
} else {
buf->write_string("false");
}
} else if (attr->is_list()) {
AttributeType list_item;
unsigned list_sz = attr->size();
buf->write_string('[');
for (unsigned i = 0; i < list_sz; i++) {
list_item = (*attr)[i];
attribute_to_string(&list_item);
if (i < (list_sz - 1)) {
buf->write_string(',');
}
}
buf->write_string(']');
} else if (attr->is_dict()) {
AttributeType dict_item;
unsigned dict_sz = attr->size();;
buf->write_string('{');
for (unsigned i = 0; i < dict_sz; i++) {
buf->write_string('\'');
buf->write_string(attr->u_.dict[i].key_.to_string());
buf->write_string('\'');
buf->write_string(':');
const AttributeType &dict_value = (*attr)[i];
attribute_to_string(&dict_value);
if (i < (dict_sz - 1)) {
buf->write_string(',');
}
}
buf->write_string('}');
} else if (attr->is_data()) {
buf->write_string('(');
if (attr->size() > 0) {
for (unsigned n = 0; n < attr->size()-1; n++) {
buf->write_byte((*attr)(n));
buf->write_string(',');
}
buf->write_byte((*attr)(attr->size()-1));
}
buf->write_string(')');
} else if (attr->is_iface()) {
IFace *iface = attr->to_iface();
if (strcmp(iface->getFaceName(), IFACE_SERVICE) == 0) {
iserv = static_cast<IService *>(iface);
buf->write_string('{');
buf->write_string("'Type':'");
buf->write_string(iface->getFaceName());
buf->write_string("','ModuleName':'");
buf->write_string(iserv->getObjName());
buf->write_string("'}");
} else {
RISCV_printf(NULL, LOG_ERROR,
"Not implemented interface to dict. method");
}
}
return buf->getBuffer();
}
void CpuRiscV_Functional::executeInstruction(IInstruction *instr,
uint32_t *rpayload) {
CpuContextType *pContext = getpContext();
if (pContext->reg_trace_file) {
/** Save previous reg values to find modification after exec() */
for (int i = 0; i < Reg_Total; i++) {
iregs_prev[i] = pContext->regs[i];
}
}
instr->exec(cacheline_, pContext);
#if 0
//if (pContext->pc >= 0x10000000) {
//if ((pContext->pc >= 0x100000b4 && pContext->pc <= 0x10000130)
//|| (pContext->pc >= 0x10001ef4)
//)
{
//if (pContext->pc >= 0x10001928 && pContext->pc <= 0x10001960) {
RISCV_debug("[%" RV_PRI64 "d] %08x: %08x \t %4s <prv=%d; mstatus=%016" RV_PRI64 "x; mcause=%016" RV_PRI64 "x; ra=%016" RV_PRI64 "x; sp=%016" RV_PRI64 "x; tp=%016" RV_PRI64 "x>",
getStepCounter(),
static_cast<uint32_t>(pContext->pc),
rpayload[0], instr->name(),
pContext->cur_prv_level,
pContext->csr[CSR_mstatus],
pContext->csr[CSR_mcause],
pContext->regs[Reg_ra],
pContext->regs[Reg_sp],
pContext->regs[Reg_tp]
);
}
#endif
if (pContext->reg_trace_file) {
int sz;
sz = RISCV_sprintf(tstr, sizeof(tstr),"%8I64d [%08x] %08x: ",
pContext->step_cnt, static_cast<uint32_t>(pContext->pc), rpayload[0]);
bool reg_changed = false;
for (int i = 0; i < 32; i++) {
if (iregs_prev[i] != pContext->regs[i]) {
reg_changed = true;
sz += RISCV_sprintf(&tstr[sz], sizeof(tstr) - sz,
"%3s <= %016I64x\n", IREGS_NAMES[i], pContext->regs[i]);
}
}
if (!reg_changed) {
sz += RISCV_sprintf(&tstr[sz], sizeof(tstr) - sz, "%s", "-\n");
}
(*pContext->reg_trace_file) << tstr;
pContext->reg_trace_file->flush();
}
if (generateRegTraceFile_.to_bool()) {
char msg[16];
int msg_len = 0;
IService *uart = NULL;
switch (pContext->step_cnt) {
case 6000:
uart = static_cast<IService *>(RISCV_get_service("uart0"));
msg[0] = 'h';
msg[1] = 'i';
msg_len = 2;
break;
case 6500:
uart = static_cast<IService *>(RISCV_get_service("uart0"));
msg[0] = 'g';
msg[1] = 'h';
msg[2] = 't';
msg_len = 3;
break;
case 8200:
uart = static_cast<IService *>(RISCV_get_service("uart0"));
msg[0] = 'i';
msg[1] = 'c';
msg_len = 2;
break;
case 8300:
uart = static_cast<IService *>(RISCV_get_service("uart0"));
msg[0] = 'k';
msg[1] = 's';
msg[2] = '\r';
msg[3] = '\n';
msg_len = 4;
break;
default:;
}
if (uart) {
ISerial *iserial = static_cast<ISerial *>(
uart->getInterface(IFACE_SERIAL));
//iserial->writeData("pnp\r\n", 6);
//iserial->writeData("highticks\r\n", 11);
iserial->writeData(msg, msg_len);
}
}
if (pContext->regs[0] != 0) {
RISCV_error("Register x0 was modificated (not equal to zero)", NULL);
}
}
int Drivers::yarpdev(int argc, char *argv[]) {
std::signal(SIGINT, handler);
std::signal(SIGTERM, handler);
// get command line options
ResourceFinder rf;
rf.configure(argc, argv); // this will process --from FILE if present
Property options;
// yarpdev will by default try to pass its thread on to the device.
// this is because some libraries need configuration and all
// access methods done in the same thread (e.g. opencv_grabber
// apparently).
options.put("single_threaded", 1);
// interpret command line options as a set of flags
//options.fromCommand(argc,argv,true,false);
options.fromString(rf.toString().c_str(), false);
// check if we're being asked to read the options from file
Value *val;
if (options.check("file",val))
{
// FIXME use argv[0]
yError("*** yarpdev --file is deprecated, please use --from\n");
yError("*** yarpdev --file will be removed in a future version of YARP\n");
std::string fname = val->toString();
options.unput("file");
yDebug("yarpdev: working with config file %s\n", fname.c_str());
options.fromConfigFile(fname,false);
// interpret command line options as a set of flags again
// (just in case we need to override something)
options.fromCommand(argc,argv,true,false);
}
// check if we want to use nested options (less ambiguous)
if (options.check("nested",val)||options.check("lispy",val))
{
std::string lispy = val->toString();
yDebug("yarpdev: working with config %s\n", lispy.c_str());
options.fromString(lispy);
}
if (!options.check("device"))
{
// no device mentioned - maybe user needs help
if (options.check("list"))
{
yInfo("Here are devices listed for your system:");
for (auto& s : split(Drivers::factory().toString(), '\n')) {
yInfo("%s", s.c_str());
}
}
else
{
yInfo("Welcome to yarpdev, a program to create YARP devices\n");
yInfo("To see the devices available, try:\n");
yInfo(" yarpdev --list\n");
yInfo("To create a device whose name you know, call yarpdev like this:\n");
yInfo(" yarpdev --device DEVICENAME --OPTION VALUE ...\n");
yInfo(" For example:\n");
yInfo(" yarpdev --device test_grabber --width 32 --height 16 --name /grabber\n");
yInfo("You can always move options to a configuration file:\n");
yInfo(" yarpdev [--device DEVICENAME] --from CONFIG_FILENAME\n");
yInfo("If you have problems, you can add the \"verbose\" flag to get more information\n");
yInfo(" yarpdev --verbose --device ffmpeg_grabber\n");
if (options.check ("from"))
{
yError("Unable to find --device option in file %s. Closing.", options.find("from").asString().c_str());
}
else
{
yWarning("--device option not specified. Closing.");
}
}
return 0;
}
// ask for a wrapped, remotable device rather than raw device
options.put("wrapped","1");
//YarpDevMonitor monitor;
bool verbose = false;
if (options.check("verbose")) {
verbose = true;
//options.setMonitor(&monitor,"top-level");
}
// we now need network
bool ret=Network::checkNetwork();
if (!ret)
{
yError("YARP network not available, check if yarp server is reachable\n");
return -1;
}
//
// yarpdev initializes the clock only before starting to do real thing.
// This way yarpdev --lish/help will not be affected by network clock.
//
// Shall other devices be affected by network clock ??
// Hereafter the device may need to use the SystemClock or the NetworkClock
// depending by the device, a real or a fake / simulated one.
// Using the YARP_CLOCK_DEFAULT the behaviour will be determined by the
// environment variable.
//
yarp::os::NetworkBase::yarpClockInit(yarp::os::YARP_CLOCK_DEFAULT);
PolyDriver dd(options);
if (verbose) {
toDox(dd,stdout);
}
if (!dd.isValid()) {
yError("yarpdev: ***ERROR*** device not available.\n");
if (argc==1)
{
yInfo("Here are the known devices:\n");
yInfo("%s", Drivers::factory().toString().c_str());
}
else
{
yInfo("Suggestions:\n");
yInfo("+ Do \"yarpdev --list\" to see list of supported devices.\n");
if (!options.check("verbose"))
{
yInfo("+ Or append \"--verbose\" option to get more information.\n");
}
}
return 1;
}
Terminee *terminee = nullptr;
if (dd.isValid()) {
Value *v;
std::string s("/yarpdev/quit");
if (options.check("device", v)) {
if (v->isString()) {
s = "";
s += "/";
s += v->toString();
s += "/quit";
}
}
if (options.check("name", v)) {
s = "";
s += v->toString();
s += "/quit";
}
if (s.find("=") == std::string::npos &&
s.find("@") == std::string::npos) {
terminee = new Terminee(s.c_str());
terminatorKey = s.c_str();
if (terminee == nullptr) {
yError("Can't allocate terminator port\n");
terminatorKey = "";
dd.close();
return 1;
}
if (!terminee->isOk()) {
yError("Failed to create terminator port\n");
terminatorKey = "";
delete terminee;
terminee = nullptr;
dd.close();
return 1;
}
}
}
double dnow = 3;
double startTime = Time::now()-dnow;
IService *service = nullptr;
dd.view(service);
if (service!=nullptr) {
bool backgrounded = service->startService();
if (backgrounded) {
// we don't need to poll this, so forget about the
// service interface
yDebug("yarpdev: service backgrounded\n");
service = nullptr;
}
}
while (dd.isValid() && !(terminated||(terminee&&terminee->mustQuit()))) {
if (service!=nullptr) {
double now = Time::now();
if (now-startTime>dnow) {
yInfo("device active...");
startTime += dnow;
}
// we requested single threading, so need to
// give the device its chance
if(!service->updateService()) {
if(!service->stopService()) {
yWarning("Error while stopping device");
}
terminated = true;
}
} else {
// we don't need to do anything
yInfo("device active in background...");
SystemClock::delaySystem(dnow);
}
}
if (terminee) {
delete terminee;
terminee = nullptr;
}
dd.close();
yInfo("yarpdev is finished.");
return 0;
}
int main()
{
int rc = 0;
// Status wrapper
ThrowStatusWrapper status(master->getStatus());
// Declare pointers to required interfaces
IProvider* prov = master->getDispatcher();
IUtil* utl = master->getUtilInterface();
IService* svc = NULL;
IXpbBuilder *spb1 = NULL;
IXpbBuilder *spb2 = NULL;
try {
printf("** Attaching to service manager...\n");
// Prepare SPB to attach to service manager
spb1 = utl->getXpbBuilder(&status, IXpbBuilder::SPB_ATTACH, NULL, 0);
spb1->insertString(&status, isc_spb_user_name, "sysdba");
spb1->insertString(&status, isc_spb_password, "masterkey");
// In case when your program is expected to be used on a server
// with multiple security database it's very good idea to specify
// DB expected to be used by services (you anyway need separate
// services connectons for databases with different user list location).
spb1->insertString(&status, isc_spb_expected_db, "employee");
// Attach to service manager
svc = prov->attachServiceManager(&status, "service_mgr", spb1->getBufferLength(&status),
spb1->getBuffer(&status));
printf("** Demo of querying information about server version...\n");
// In the simplest case sendItems parameter may be NULL
// Building receiveItems is mostly trivial
const unsigned char receiveItems[] = {isc_info_svc_server_version};
// Output buffer
unsigned char results[1024];
// Query server version
svc->query(&status, 0, NULL, sizeof(receiveItems), receiveItems, sizeof(results), results);
printInfo(results, sizeof(results));
printf("** Demo of running utility using service manager...\n");
// Build service start SPB
spb2 = utl->getXpbBuilder(&status, IXpbBuilder::SPB_START, NULL, 0);
spb2->insertTag(&status, isc_action_svc_db_stats);
spb2->insertString(&status, isc_spb_dbname, "employee");
spb2->insertInt(&status, isc_spb_options, isc_spb_sts_encryption);
// Start service
svc->start(&status, spb2->getBufferLength(&status), spb2->getBuffer(&status));
// Prepare receiveItems block
const unsigned char receiveItems2[] = {isc_info_svc_line};
// Query service output
do
{
svc->query(&status, 0, NULL, sizeof(receiveItems2), receiveItems2, sizeof(results), results);
} while (printInfo(results, sizeof(results)));
printf("** Detaching from service manager...\n");
// Detach from service manager
svc->detach(&status);
svc = NULL;
printf("** Done.\n");
}
catch (const FbException& error)
{
// handle error
rc = 1;
char buf[256];
master->getUtilInterface()->formatStatus(buf, sizeof(buf), error.getStatus());
fprintf(stderr, "%s\n", buf);
}
// release interfaces after error caught
if (svc)
svc->release();
// generic cleanup
prov->release();
status.dispose();
if (spb1)
spb1->dispose();
if (spb2)
spb2->dispose();
return rc;
}
int CServiceDispatcher::Dispatch(CCmd& oCmd) {
m_oCurCmdObj = oCmd;
//printf("++org serno :%d\n",oCmd.iSvcSerialNo);
//printf("--new serno :%d\n",m_oCurCmdObj.iSvcSerialNo);
int iRet = 0;
int iStateFulContinue = 0;
IService* pSvcHandler = NULL;
if (m_oCurCmdObj.iType == RESPONSE) {
//if (oCmd.iSvcSerialNo) {
map<int,IService*>::iterator it = m_mapStatefulSvcQueue.begin();
//printf("stateful-size-%d\n",m_mapStatefulSvcQueue.size());
for (;it != m_mapStatefulSvcQueue.end(); ++it) {
printf("stateful-find-%d\n",it->first);
if (m_oCurCmdObj.iSvcSerialNo == it->first) {
iStateFulContinue = 1;
pSvcHandler = it->second;
printf("++stateful service find:%d\n",pSvcHandler->m_iIndex);
assert(pth_uctx_switch(m_uctx, pSvcHandler->GetUCTX()));
//iRet = pSvcHandler->Execute(oCmd);
//m_mapStatefulSvcQueue.erase(it);
//m_mapStatelessSvcQueue[oCmd.iSvcSerialNo] = pSvcHandler;
//return iRet;
}
}
if (!iStateFulContinue) {
printf("can't find stateful %d in this process %d, stateful queue size:%d\n",m_oCurCmdObj.iSvcSerialNo,getpid(),m_mapStatefulSvcQueue.size());
}
assert(iStateFulContinue);
}
else {
if (m_mapStatelessSvcQueue.size() == 0 && iStateFulContinue == 0) {
return NO_FREE_SVC_HANDLER;
}
else {
map<int,IService*>::iterator it = m_mapStatelessSvcQueue.begin();
pSvcHandler = it->second;
//m_oCurCmdObj.iSvcSerialNo = it->first;
m_mapStatelessSvcQueue.erase(it);
printf("--stateless service find:%d\n",pSvcHandler->m_iIndex);
//printf("--stateless service find:%d\n",pSvcHandler->m_iIndex);
m_mapStatefulSvcQueue[pSvcHandler->m_iIndex] = pSvcHandler;
assert(pth_uctx_switch(m_uctx, pSvcHandler->GetUCTX()));
}
}
printf("****back to main process******,data:%s\n",m_oCurCmdObj.ToString().c_str());
//int pth_uctx_switch(pth_uctx_t uctx_from, pth_uctx_t uctx_to);
//iRet = pSvcHandler->Execute(oCmd);
if (m_oCurCmdObj.iRet != UNFINISH_TASK_RET_FLAG) {
if (pSvcHandler) {
map<int,IService*>::iterator it = m_mapStatefulSvcQueue.find(pSvcHandler->m_iIndex);
if (it != m_mapStatefulSvcQueue.end()) {
m_mapStatefulSvcQueue.erase(it);
}
pSvcHandler->ResetUCTX();//每次ucontext使用完后，因为栈指针等没有复位，必须重新调用pth_uctx_make重置上下文信息，否则可能遭遇SIGSEGV错误
printf("--statful back stateless service :%d\n",pSvcHandler->m_iIndex);
m_mapStatelessSvcQueue[pSvcHandler->m_iIndex] = pSvcHandler;
}
}
oCmd = m_oCurCmdObj;
return oCmd.iRet;
//return iRet;
}
int Drivers::yarpdev(int argc, char *argv[]) {
ACE_OS::signal(SIGINT, (ACE_SignalHandler) handler);
ACE_OS::signal(SIGTERM, (ACE_SignalHandler) handler);
// get command line options
ResourceFinder rf;
rf.configure(argc, argv); // this will process --from FILE if present
Property options;
// yarpdev will by default try to pass its thread on to the device.
// this is because some libraries need configuration and all
// access methods done in the same thread (e.g. opencv_grabber
// apparently).
options.put("single_threaded", 1);
// interpret command line options as a set of flags
//options.fromCommand(argc,argv,true,false);
options.fromString(rf.toString().c_str(), false);
// check if we're being asked to read the options from file
Value *val;
if (options.check("file",val)) {
fprintf(stderr,"*** yarpdev --file is deprecated, please use --from\n");
fprintf(stderr,"*** yarpdev --file will be removed in a future version of YARP\n");
ConstString fname = val->toString();
options.unput("file");
printf("yarpdev: working with config file %s\n", fname.c_str());
options.fromConfigFile(fname,false);
// interpret command line options as a set of flags again
// (just in case we need to override something)
options.fromCommand(argc,argv,true,false);
}
// check if we want to use nested options (less ambiguous)
if (options.check("nested",val)||options.check("lispy",val)) {
ConstString lispy = val->toString();
printf("yarpdev: working with config %s\n", lispy.c_str());
options.fromString(lispy);
}
if (!options.check("device")) {
// no device mentioned - maybe user needs help
if (options.check("list")) {
printf("Here are devices listed for your system:\n");
printf("%s", Drivers::factory().toString().c_str());
} else {
printf("Welcome to yarpdev, a program to create YARP devices\n");
printf("To see the devices available, try:\n");
printf(" yarpdev --list\n");
printf("To create a device whose name you know, call yarpdev like this:\n");
printf(" yarpdev --device DEVICENAME --OPTION VALUE ...\n");
printf("For example:\n");
printf(" yarpdev --device test_grabber --width 32 --height 16 --name /grabber\n");
printf("You can always move options to a configuration file:\n");
printf(" yarpdev [--device DEVICENAME] --from CONFIG_FILENAME\n");
printf("If you have problems, you can add the \"verbose\" flag to get more information\n");
printf(" yarpdev --verbose --device ffmpeg_grabber\n");
}
return 0;
}
// ask for a wrapped, remotable device rather than raw device
options.put("wrapped","1");
//YarpDevMonitor monitor;
bool verbose = false;
if (options.check("verbose")) {
verbose = true;
//options.setMonitor(&monitor,"top-level");
}
// we now need network
bool ret=Network::checkNetwork();
if (!ret)
{
fprintf(stderr, "YARP network not available, check if yarp server is reachable\n");
return -1;
}
PolyDriver dd(options);
if (verbose) {
toDox(dd,stdout);
}
if (!dd.isValid()) {
printf("yarpdev: ***ERROR*** device not available.\n");
if (argc==1) {
printf("Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
} else {
printf("Suggestions:\n");
printf("+ Do \"yarpdev --list\" to see list of supported devices.\n");
if (!options.check("verbose")) {
printf("+ Or append \"--verbose\" option to get more information.\n");
}
}
return 1;
}
Terminee *terminee = 0;
if (dd.isValid()) {
Value *v;
ConstString s("/yarpdev/quit");
if (options.check("device", v)) {
if (v->isString()) {
s = "";
s += "/";
s += v->toString();
s += "/quit";
}
}
if (options.check("name", v)) {
s = "";
s += v->toString();
s += "/quit";
}
if (s.find("=") == ConstString::npos &&
s.find("@") == ConstString::npos) {
terminee = new Terminee(s.c_str());
terminatorKey = s.c_str();
if (terminee == 0) {
printf("Can't allocate terminator port\n");
terminatorKey = "";
dd.close();
return 1;
}
if (!terminee->isOk()) {
printf("Failed to create terminator port\n");
terminatorKey = "";
delete terminee;
terminee = NULL;
dd.close();
return 1;
}
}
}
double dnow = 3;
double startTime = Time::now()-dnow;
IService *service = NULL;
dd.view(service);
if (service!=NULL) {
bool backgrounded = service->startService();
if (backgrounded) {
// we don't need to poll this, so forget about the
// service interface
printf("yarpdev: service backgrounded\n");
service = NULL;
}
}
while (dd.isValid() && !(terminated||(terminee&&terminee->mustQuit()))) {
if (service!=NULL) {
double now = Time::now();
if (now-startTime>dnow) {
if (yarp_show_info()) {
YARP_LOG_INFO("device active...");
}
startTime += dnow;
}
// we requested single threading, so need to
// give the device its chance
service->updateService();
} else {
// we don't need to do anything
if (yarp_show_info()) {
YARP_LOG_INFO("device active in background...");
}
Time::delay(dnow);
}
}
if (terminee) {
delete terminee;
terminee = NULL;
}
dd.close();
if (yarp_show_info()) {
printf("yarpdev is finished.\n");
}
return 0;
}