std::string Utils::get_config_option(std::string _key)
{
std::string value = "";
TiXmlDocument document(getConfigFile(LOCAL_CONFIG).c_str());
if (!document.LoadFile())
{
cError() << "There was an exception in XML parsing.";
cError() << "Parse error: " << document.ErrorDesc();
cError() << " At line " << document.ErrorRow();
return value;
}
TiXmlHandle docHandle(&document);
TiXmlElement *key_node = docHandle.FirstChildElement("calaos:config").FirstChildElement().ToElement();
if (key_node)
{
for(; key_node; key_node = key_node->NextSiblingElement())
{
if (key_node->ValueStr() == "calaos:option" &&
key_node->Attribute("name") &&
key_node->Attribute("name") == _key &&
key_node->Attribute("value"))
{
value = key_node->Attribute("value");
break;
}
}
}
return value;
}
void gposStartupState(void)
{
JausService service;
// Populate Core Service
if(!jausServiceAddCoreServices(gpos->services))
{
cError("gpos: Addition of Core Services FAILED! Switching to FAILURE_STATE\n");
gpos->state = JAUS_FAILURE_STATE;
}
// USER: Add the rest of your component specific service(s) here
service = jausServiceCreate(gpos->address->component);
if(!service)
{
cError("gpos:%d: Creation of JausService FAILED! Switching to FAILURE_STATE\n", __LINE__);
gpos->state = JAUS_FAILURE_STATE;
}
jausServiceAddService(gpos->services, service);
jausServiceAddInputCommand(service, JAUS_QUERY_GLOBAL_POSE, 0xFF);
jausServiceAddOutputCommand(service, JAUS_REPORT_GLOBAL_POSE, 0xFF);
gposMessage = reportGlobalPoseMessageCreate();
gposMessage->source->id = gpos->address->id;
//add support for ReportGlovalPose Service Connections
scManagerAddSupportedMessage(gposNmi, JAUS_REPORT_GLOBAL_POSE);
}
bool Utils::del_config_option(std::string key)
{
TiXmlDocument document(getConfigFile(LOCAL_CONFIG).c_str());
if (!document.LoadFile())
{
cError() << "There was an exception in XML parsing.";
cError() << "Parse error: " << document.ErrorDesc();
cError() << "In file " << getConfigFile(LOCAL_CONFIG) << " At line " << document.ErrorRow();
return false;
}
TiXmlHandle docHandle(&document);
TiXmlElement *key_node = docHandle.FirstChildElement("calaos:config").FirstChildElement().ToElement();
if (key_node)
{
for(; key_node; key_node = key_node->NextSiblingElement())
{
if (key_node->ValueStr() == "calaos:option" &&
key_node->Attribute("name") &&
key_node->Attribute("name") == key)
{
docHandle.FirstChild("calaos:config").Element()->RemoveChild(key_node);
break;
}
}
document.SaveFile();
}
return true;
}
bool Utils::get_config_options(Params &options)
{
TiXmlDocument document(getConfigFile(LOCAL_CONFIG).c_str());
if (!document.LoadFile())
{
cError() << "There was an exception in XML parsing.";
cError() << "Parse error: " << document.ErrorDesc();
cError() << "In file " << getConfigFile(LOCAL_CONFIG) << " At line " << document.ErrorRow();
return false;
}
TiXmlHandle docHandle(&document);
TiXmlElement *key_node = docHandle.FirstChildElement("calaos:config").FirstChildElement().ToElement();
if (key_node)
{
for(; key_node; key_node = key_node->NextSiblingElement())
{
if (key_node->ValueStr() == "calaos:option" &&
key_node->Attribute("name") &&
key_node->Attribute("value"))
{
options.Add(key_node->Attribute("name"), key_node->Attribute("value"));
}
}
}
return true;
}
ijvType *binReadIJV(FILE* fpr)
{
int n, nnz;
if (!fread(&(n), sizeof(int32), 1, fpr))
cError ("error reading n from file\n");
if (!fread(&(nnz), sizeof(int32), 1, fpr))
cError ("error reading nnz from file\n");
ijvType *ijv;
ijv = makeIJV(nnz);
ijv->n = n;
int x;
int i, j;
float v;
if (!fread(ijv->i, sizeof(int32), nnz, fpr))
cError ("error reading i from file\n");
if (!fread(ijv->j, sizeof(int32), nnz, fpr))
cError ("error reading j from file\n");
if (!fread(ijv->v, sizeof(double), nnz, fpr))
cError ("error reading v from file\n");
for (x = 0; x < nnz; x++) {
ijv->i[x] = ijv->i[x]-1;
ijv->j[x] = ijv->j[x]-1;
}
return ijv;
}
// Function: cmptStartup
// Access: Public
// Description: This function allows the abstracted component functionality contained in this file to be started from an external source.
// It must be called first before the component state machine and node manager interaction will begin
// Each call to "cmptStartup" should be followed by one call to the "cmptShutdown" function
int cmptStartup(void)
{
FILE * propertyFile;
pthread_attr_t attr; // Thread attributed for the component threads spawned in this function
if(cmpt->state == JAUS_SHUTDOWN_STATE) // Execute the startup routines only if the component is not running
{
propertyFile = fopen("./config/cmpt.conf", "r");
if(propertyFile)
{
cmptProperties = propertiesCreate();
cmptProperties = propertiesLoad(cmptProperties, propertyFile);
fclose(propertyFile);
}
else
{
cError("cmpt: Cannot find or open properties file\n");
return CMPT_LOAD_CONFIGURATION_ERROR;
}
// Check in to the Node Manager and obtain Instance, Node and Subsystem IDs
cmptNode = jausNodeCreate();
cmptSubsystem = jausSubsystemCreate();
cmptNode->subsystem = cmptSubsystem;
cmpt->node = cmptNode;
cmpt->services = jausServicesCreate();
cmpt->state = JAUS_INITIALIZE_STATE; // Set the state of the JAUS state machine to INITIALIZE
cmptNmi = nodeManagerOpen(cmpt);
if(cmptNmi == NULL)
{
cError("cmpt: Could not open connection to node manager\n");
return CMPT_NODE_MANAGER_OPEN_ERROR;
}
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
cmptRun = TRUE;
if(pthread_create(&cmptThreadId, &attr, cmptThread, NULL) != 0)
{
cError("cmpt: Could not create cmptThread\n");
cmptShutdown();
pthread_attr_destroy(&attr);
return CMPT_THREAD_CREATE_ERROR;
}
pthread_attr_destroy(&attr);
}
else
{
cError("cmpt: Attempted startup while not shutdown\n");
return CMPT_STARTUP_BEFORE_SHUTDOWN_ERROR;
}
return 0;
}
void Config::LoadConfigRule()
{
std::string file = Utils::getConfigFile(RULES_CONFIG);
if (!Utils::fileExists(file))
{
std::ofstream conf(file.c_str(), std::ofstream::out);
conf << "<?xml version=\"1.0\" encoding=\"UTF-8\" ?>" << std::endl;
conf << "<calaos:rules xmlns:calaos=\"http://www.calaos.fr\">" << std::endl;
conf << "</calaos:rules>" << std::endl;
conf.close();
}
TiXmlDocument document(file);
if (!document.LoadFile())
{
cError() << "There was a parse error in " << file;
cError() << document.ErrorDesc();
cError() << "In file " << file << " At line " << document.ErrorRow();
exit(-1);
}
TiXmlHandle docHandle(&document);
TiXmlElement *rule_node = docHandle.FirstChildElement("calaos:rules").FirstChildElement().ToElement();
if (!rule_node)
{
cError() << "Error, <calaos:rules> node not found in file " << file;
}
for(; rule_node; rule_node = rule_node->NextSiblingElement())
{
if (rule_node->ValueStr() == "calaos:rule" &&
rule_node->Attribute("name") &&
rule_node->Attribute("type"))
{
std::string name, type;
name = rule_node->Attribute("name");
type = rule_node->Attribute("type");
Rule *rule = new Rule(type, name);
rule->LoadFromXml(rule_node);
ListeRule::Instance().Add(rule);
}
}
cInfo() << "Done. " << ListeRule::Instance().size() << " rules loaded.";
}
cMouse::cMouse( HWND hWnd, bool bExclusive )
{
m_pTarget = NULL;
HRESULT hr;
/**
* Crea el device
*/
hr = Input()->GetDInput()->CreateDevice( GUID_SysMouse, &m_pDevice, NULL );
if( FAILED( hr ))
{
throw cError(L"[cMouse::Init]: No se pudo crear el device!\n");
}
/**
* Setea el formato
*/
hr = m_pDevice->SetDataFormat(&c_dfDIMouse);
if( FAILED( hr ))
{
SafeRelease( m_pDevice );
throw cError(L"[cMouse::Init]: SetDataFormat fallo\n");
}
/**
* Setea el nivel de cooperacion
*/
if( bExclusive )
{
hr = m_pDevice->SetCooperativeLevel( hWnd, DISCL_EXCLUSIVE | DISCL_NOWINKEY | DISCL_FOREGROUND );
}
else
{
hr = m_pDevice->SetCooperativeLevel( hWnd, DISCL_NONEXCLUSIVE | DISCL_FOREGROUND);
}
if( FAILED( hr ))
{
SafeRelease( m_pDevice );
throw cError(L"[cMouse::Init]: SetCooperativeLevel fallo\n");
}
m_lastState.lX = 0;
m_lastState.lY = 0;
m_lastState.lZ = 0;
m_lastState.rgbButtons[0] = 0;
m_lastState.rgbButtons[1] = 0;
m_lastState.rgbButtons[2] = 0;
m_lastState.rgbButtons[3] = 0;
}
void Config::LoadConfigIO()
{
std::string file = Utils::getConfigFile(IO_CONFIG);
if (!Utils::fileExists(file))
{
std::ofstream conf(file.c_str(), std::ofstream::out);
conf << "<?xml version=\"1.0\" encoding=\"UTF-8\" ?>" << std::endl;
conf << "<calaos:ioconfig xmlns:calaos=\"http://www.calaos.fr\">" << std::endl;
conf << "<calaos:home></calaos:home>" << std::endl;
conf << "</calaos:ioconfig>" << std::endl;
conf.close();
}
TiXmlDocument document(file);
if (!document.LoadFile())
{
cError() << "There was a parse error";
cError() << document.ErrorDesc();
cError() << "In file " << file << " At line " << document.ErrorRow();
exit(-1);
}
TiXmlHandle docHandle(&document);
TiXmlElement *room_node = docHandle.FirstChildElement("calaos:ioconfig").FirstChildElement("calaos:home").FirstChildElement().ToElement();
for(; room_node; room_node = room_node->NextSiblingElement())
{
if (room_node->ValueStr() == "calaos:room" &&
room_node->Attribute("name") &&
room_node->Attribute("type"))
{
std::string name, type;
int hits = 0;
name = room_node->Attribute("name");
type = room_node->Attribute("type");
if (room_node->Attribute("hits"))
room_node->Attribute("hits", &hits);
Room *room = new Room(name, type, hits);
ListeRoom::Instance().Add(room);
room->LoadFromXml(room_node);
}
}
cInfo() << "Done. ";
}
void wdStandbyState(void)
{
if(!gposSc->isActive || !vssSc->isActive || !pdWrenchSc->isActive || !pdStatusSc->isActive || !wdInControl)
{
wd->state = JAUS_EMERGENCY_STATE;
cError( "wd: Switching to EMERGENCY: GposSc: %s, VssSc: %s, PdWrenchSc: %s, PdStatusSc: %s, Rd: %s\n",
gposSc->isActive? "Active" : "Inactive",
vssSc->isActive? "Active" : "Inactive",
pdWrenchSc->isActive? "Active" : "Inactive",
pdStatusSc->isActive? "Active" : "Inactive",
wdInControl? "In Control" : "Not In Control"
);
return;
}
// Setup Vehicle State
vehicleState->desiredSpeedMps = 0;
vehicleState->desiredPhiEffort = 0;
wdExcecuteControl(vehicleState);
if(pd->state == JAUS_READY_STATE)
{
wd->state = JAUS_READY_STATE;
}
}
/*!
* \fn void cConst::ReAlloc(const cDVector& theVectParam, const uint theNumParam)
* \param const cDVector& theVectParam: the constant value is in theVectParam[0]
* \param const uint theNumParam: not used for cConst class
* \details Here, mvConst = theVectParam[0]
*/
void cConst::ReAlloc(const cDVector& theVectParam, const uint theNumParam)
{
if (theVectParam.GetSize() > 0)
mvConst = theVectParam[0] ;
else
throw cError("Size of 'theVectParam' must be > 0") ;
}
/*!
* \fn double cCondMean::Get(uint theNumMean, uint theIndex, uint theNumParam)
* \param uint theNumMean: index of conditional mean
* \param uint theNumParam: index of parameter
*/
double cCondMean::Get(uint theNumMean, uint theIndex, uint theNumParam)
{
if (theNumMean < GetNMean() )
return mvCondMean.at(theNumMean)->Get(theIndex, theNumParam) ;
else
throw cError("cCondMean::Get bad index");
}
/*!
* \fn void cArch::Set(const cDVector& theVectParam, const uint theNumParam)
* \brief fill the parameters vector
* \param const cDVector& theVectParam: the vector of values
* \param const uint theNumParam: =0, mvConst; =1, mvArch
* \details mvArch = theVectParam or mvConst = theVectParam[0]
*/
void cArch::Set(const cDVector& theVectParam, const uint theNumParam)
{ switch (theNumParam)
{ case 0 :
if (theVectParam.GetSize() > 0)
mvConst = theVectParam[0] ;
else
throw cError("cArch::Set - Size of theVectParam must be > 0") ;
break ;
case 1 :
mvArch = theVectParam ;
break ;
default:
throw cError("cArch::Set - theNumParam must be in 0, 1") ;
break ;
}
}
/*!
* \fn void cArch::Set(const double theValue, const uint theIndex, const uint theNumParam)
* \brief fill the parameters vector
* \param const double theValue: the value of the "theIndex" th lag. Default 0.
* \param const uint theIndex: the index.
* \param const uint theNumParam: =0, mvConst, =1, ARCH parameters
* \details mvArch[theIndex] = theValue or mvConst = theValue
*/
void cArch::Set(const double theValue, const uint theIndex, const uint theNumParam)
{ switch (theNumParam)
{ case 0 :
mvConst = theValue ;
break ;
case 1 :
if (theIndex < mvArch.GetSize())
mvArch[theIndex] = theValue ;
else
throw cError("cArch::Set - wrong index") ;
break ;
default:
throw cError("cArch::Set - theNumParam must be in 0, 1") ;
break ;
}
}
/*!
* \fn cAbstCondMean* cCondMean::GetOneMean(uint theIndex) const
* \param uint theIndex: index of component to be returned
* \\details return mvCondMean[theWhateMean]
*/
cAbstCondMean* cCondMean::GetOneMean(uint theIndex) const
{
if (theIndex < GetNMean())
return mvCondMean.at(theIndex) ;
else
throw cError("cCondMean::GetOneMean bad index") ;
}
// Function: cmptShutdown
// Access: Public
// Description: This function allows the abstracted component functionality contained in this file to be stoped from an external source.
// If the component is in the running state, this function will terminate all threads running in this file
// This function will also close the Jms connection to the Node Manager and check out the component from the Node Manager
int cmptShutdown(void)
{
double timeOutSec;
if(cmpt->state != JAUS_SHUTDOWN_STATE) // Execute the shutdown routines only if the component is running
{
cmptRun = FALSE;
timeOutSec = getTimeSeconds() + CMPT_THREAD_TIMEOUT_SEC;
while(cmptThreadRunning)
{
usleep(100000);
if(getTimeSeconds() >= timeOutSec)
{
pthread_cancel(cmptThreadId);
cmptThreadRunning = FALSE;
cError("cmpt: cmptThread Shutdown Improperly\n");
break;
}
}
nodeManagerClose(cmptNmi); // Close Node Manager Connection
jausSubsystemDestroy(cmpt->node->subsystem);
jausNodeDestroy(cmpt->node);
jausServicesDestroy(cmpt->services);
cmpt->state = JAUS_SHUTDOWN_STATE;
propertiesDestroy(cmptProperties);
}
return 0;
}
void cArch::RegArchParamToVector(cDVector& theDestVect, uint theIndex)
{
uint mySize = GetNParam() ;
if (theDestVect.GetSize() < mySize + theIndex)
throw cError("Wrong size") ;
theDestVect[theIndex] = mvConst ;
mvArch.SetSubVectorWithThis(theDestVect, theIndex+1) ;
}
void cArch::VectorToRegArchParam(const cDVector& theSrcVect, uint theIndex)
{
uint mySize = theSrcVect.GetSize() ;
if (GetNParam() + theIndex > mySize)
throw cError("Wrong size") ;
mvConst = theSrcVect[theIndex] ;
mvArch.SetThisWithSubVector(theSrcVect, theIndex+1) ;
}
bool Utils::set_config_option(std::string key, std::string value)
{
TiXmlDocument document(getConfigFile(LOCAL_CONFIG).c_str());
if (!document.LoadFile())
{
cError() << "There was an exception in XML parsing.";
cError() << "Parse error: " << document.ErrorDesc();
cError() << "In file " << getConfigFile(LOCAL_CONFIG) << " At line " << document.ErrorRow();
return false;
}
TiXmlHandle docHandle(&document);
bool found = false;
TiXmlElement *key_node = docHandle.FirstChildElement("calaos:config").FirstChildElement().ToElement();
if (key_node)
{
for(; key_node; key_node = key_node->NextSiblingElement())
{
if (key_node->ValueStr() == "calaos:option" &&
key_node->Attribute("name") &&
key_node->Attribute("name") == key)
{
key_node->SetAttribute("value", value);
found = true;
break;
}
}
//the option was not found, we create it
if (!found)
{
TiXmlElement *element = new TiXmlElement("calaos:option");
element->SetAttribute("name", key);
element->SetAttribute("value", value);
docHandle.FirstChild("calaos:config").ToElement()->LinkEndChild(element);
}
document.SaveFile();
}
return true;
}
/*!
* \fn void cCondMean::SetOneMean(uint theWhatMean, cAbstCondMean* theAbstCondMean)
* \param uint theWhatMean: index of the conditional mean component
* \param cAbstCondMean* theAbstCondMean: conditional mean component to be copied in the mCondMean array.
* \brief *mvCondMean[theWhatMean] = *theAbstCondMean
*/
void cCondMean::SetOneMean(uint theWhatMean, cAbstCondMean& theAbstCondMean)
{
if (theWhatMean >= GetNMean())
throw cError("cCondMean::GetOneMean bad index") ;
else
{
mvCondMean.at(theWhatMean) = theAbstCondMean.PtrCopy();
}
}
// Function: gposProcessMessage
// Access: Private
// Description: This function is responsible for handling incoming JAUS messages from the Node Manager.
// Incoming messages are processed according to message type.
void gposProcessMessage(JausMessage message)
{
QueryGlobalPoseMessage queryGlobalPose;
JausMessage txMessage;
// This block of code is intended to reject commands from non-controlling components
if(gpos->controller.active && message->source->id != gpos->controller.address->id && jausMessageIsRejectableCommand(message) )
{
cError("gpos: Received command message %s from non-controlling component.\n", jausMessageCommandCodeString(message));
jausMessageDestroy(message); // Ignore this message
return;
}
switch(message->commandCode) // Switch the processing algorithm according to the JAUS message type
{
case JAUS_QUERY_GLOBAL_POSE:
queryGlobalPose = queryGlobalPoseMessageFromJausMessage(message);
if(queryGlobalPose)
{
gposMessage->destination->id = queryGlobalPose->source->id;
gposMessage->presenceVector = queryGlobalPose->presenceVector;
gposMessage->sequenceNumber = 0;
gposMessage->scFlag = 0;
txMessage = reportGlobalPoseMessageToJausMessage(gposMessage);
nodeManagerSend(gposNmi, txMessage);
jausMessageDestroy(txMessage);
queryGlobalPoseMessageDestroy(queryGlobalPose);
}
else
{
cError("gpos: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
default:
defaultJausMessageProcessor(message, gposNmi, gpos);
break;
}
}
string CalaosEvent::typeToString(int type)
{
switch (type)
{
case EventInputAdded: return "input_added";
case EventInputDeleted: return "input_deleted";
case EventInputChanged: return "input_changed";
case EventInputPropertyDelete: return "input_prop_deleted";
case EventOutputAdded: return "output_added";
case EventOutputDeleted: return "output_deleted";
case EventOutputChanged: return "output_changed";
case EventOutputPropertyDelete: return "output_prop_deleted";
case EventRoomAdded: return "room_added";
case EventRoomDeleted: return "room_deleted";
case EventRoomChanged: return "room_changed";
case EventRoomPropertyDelete: return "room_prop_deleted";
case EventTimeRangeChanged: return "timerange_changed";
case EventScenarioAdded: return "scenario_added";
case EventScenarioDeleted: return "scenario_deleted";
case EventScenarioChanged: return "scenario_changed";
case EventAudioSongChanged: return "audio_song_changed";
case EventAudioPlaylistAdd: return "playlist_tracks_added";
case EventAudioPlaylistDelete: return "playlist_tracks_deleted";
case EventAudioPlaylistMove: return "playlist_tracks_moved";
case EventAudioPlaylistReload: return "playlist_reload";
case EventAudioPlaylistCleared: return "playlist_cleared";
case EventAudioStatusChanged: return "audio_status_changed";
case EventAudioVolumeChanged: return "audio_volume_changed";
default: break;
}
cError() << "Unkown string for event " << type;
cError() << "Did you forget to add string representation for that event??";
return "unkown";
}
JausBoolean scManagerReceiveServiceConnection(NodeManagerInterface nmi, ServiceConnection requestSc, JausMessage *message)
{
ServiceConnection prevSc;
ServiceConnection sc = nmi->scm->incommingSc;
prevSc = NULL;
while(sc)
{
if(sc->commandCode == requestSc->commandCode && sc->address->id == requestSc->address->id )
{
if(getTimeSeconds() > (sc->lastSentTime + sc->timeoutSec))
{
// Connection has Timed Out
sc->isActive = JAUS_FALSE;
while(sc->queue->size)
{
jausMessageDestroy(queuePop(sc->queue));
}
// Remove Service Connection
if(prevSc)
{
prevSc->nextSc = sc->nextSc;
sc->nextSc = NULL;
}
else
{
nmi->scm->incommingSc = sc->nextSc;
sc->nextSc = NULL;
}
nmi->scm->incommingScCount--;
return JAUS_FALSE;
}
if(sc->queue->size)
{
*message = (JausMessage)queuePop(sc->queue);
return JAUS_TRUE;
}
else
{
return JAUS_FALSE;
}
}
else
{
prevSc = sc;
sc = sc->nextSc;
}
}
cError( "libnodeManager: scManagerServiceConnectionReceive: Requested SC does not exist\n" );
return JAUS_FALSE;
}
void cmptStartupState(void)
{
// Populate Core Service
if(!jausServiceAddCoreServices(cmpt->services))
{
cError("cmpt: Addition of Core Services FAILED! Switching to FAILURE_STATE\n");
cmpt->state = JAUS_FAILURE_STATE;
}
// USER: Add the rest of your component specific service(s) here
// USER: Insert Code Here that is only run once before the component goes to init state
}
void wdReadyState(void)
{
double waypointHeading = 0;
double headingDelta = 0;
if(!gposSc->isActive || !vssSc->isActive || !pdWrenchSc->isActive || !pdStatusSc->isActive || !wdInControl)
{
wd->state = JAUS_EMERGENCY_STATE;
cError( "wd: Switching to EMERGENCY: GposSc: %s, VssSc: %s, PdWrenchSc: %s, PdStatusSc: %s, Rd: %s\n",
gposSc->isActive? "Active" : "Inactive",
vssSc->isActive? "Active" : "Inactive",
pdWrenchSc->isActive? "Active" : "Inactive",
pdStatusSc->isActive? "Active" : "Inactive",
wdInControl? "In Control" : "Not In Control"
);
return;
}
if(pd->state != JAUS_READY_STATE)
{
wd->state = JAUS_STANDBY_STATE;
return;
}
vehicleState->speedMps = wdReportVss? wdReportVss->velocityXMps : 0;
if(currentWaypointIndex < wdWaypoints->elementCount)
{
SetGlobalWaypointMessage tempGlobalWaypoint;
tempGlobalWaypoint = (SetGlobalWaypointMessage) wdWaypoints->elementData[currentWaypointIndex];
waypointHeading = greatCircleCourse(wdReportGpos->latitudeDegrees * RAD_PER_DEG, wdReportGpos->longitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->latitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->longitudeDegrees * RAD_PER_DEG);
wdWaypointDistance = greatCircleDistance(wdReportGpos->latitudeDegrees * RAD_PER_DEG, wdReportGpos->longitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->latitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->longitudeDegrees * RAD_PER_DEG);
headingDelta = wdAngleSubtract(waypointHeading, wdReportGpos->yawRadians);
//cLog("HeadingDelta: %7.2f\n", headingDelta);
vehicleState->desiredPhiEffort = WHEEL_ROTATION_EFFORT_PER_RAD * headingDelta;
vehicleState->desiredSpeedMps = wdSpeed? wdSpeed->speedMps : 0.0;
//cLog("vehicleState->desiredPhiEffort: %7.2f\n", vehicleState->desiredPhiEffort);
}
else
{
//hang out
vehicleState->desiredSpeedMps = 0;
vehicleState->desiredPhiEffort = 0;
}
wdExcecuteControl(vehicleState);
}
int nodeStartup(void)
{
// USER: Rename cmpt to your individual component
if(cmptStartup())
{
cError("node: cmptStartup failed\n");
nodeShutdown();
return NODE_CMPT_STARTUP_FAILED;
}
return 0;
}
void cAparch::VectorToRegArchParam(const cDVector& theSrcVect, uint theIndex)
{uint mySize = theSrcVect.GetSize(),
myIndex = theIndex ;
if (GetNParam() + theIndex > mySize)
throw cError("Wrong size") ;
mvCste = theSrcVect[myIndex++] ;
mvDelta = theSrcVect[myIndex++] ;
mvArch.SetThisWithSubVector(theSrcVect,myIndex) ;
myIndex += mvArch.GetSize() ;
mvGamma.SetThisWithSubVector(theSrcVect,myIndex) ;
myIndex += mvGamma.GetSize() ;
mvGarch.SetThisWithSubVector(theSrcVect,myIndex) ;
}
cResult clean()
{
std::string op;
utils_docker::pullImage("spotify/docker-gc");
logmsg(kLINFO,"Cleaning.");
CommandLine cl("docker", { "run","--rm","-v","/var/run/docker.sock:/var/run/docker.sock","spotify/docker-gc" });
if (utils::runcommand_stream(cl, kORaw, "", {},NULL) != 0)
return cError("Unable to run spotify/docker-gc to clean docker images.");
logmsg(kLINFO,"Cleaning is complete.");
return kRSuccess;
}
cResult showservices()
{
std::vector<std::string> services;
utils::getAllServices(services);
if (services.size()>0)
{
logmsg(kLINFO,"Installed dServices:");
for (auto const & entry : services)
logmsg(kLINFO," "+entry);
} else
return cError("No dServices are installed.");
return kRSuccess;
}
bool XmasWidget::LoadWidget(std::string, double _x, double _y, std::string _id)
{
std::string witem = "calaos/widget/xmas";
if (!LoadEdje(witem))
{
return false;
}
setLayer(500);
EdjeObject::Show();
clip = evas_object_rectangle_add(evas);
evas_object_show(clip);
edje_object_part_swallow(edje, "widget.swallow", clip);
Resize(1024, 768);
//create some flakes
for (int i = 0;i < MAX_FLAKE;i++)
{
std::string type;
if (i < MAX_FLAKE / 3) type = "flake_small";
else if (i >= MAX_FLAKE / 3 && i < (MAX_FLAKE / 3) * 2) type = "flake_medium";
else type = "flake_large";
EdjeObject *o = new EdjeObject(theme, evas);
if (!o->LoadEdje("calaos/widget/xmas/" + type))
cError() << "Error loading edje group calaos/widget/xmas/" + type;
o->setLayer(500);
evas_object_clip_set(o->getEvasObject(), clip);
Flake *flake = new Flake(o);
int tx = random() % clipw;
int ty = random() % cliph;
flake->Move(tx + clipx, ty + clipy);
flake->Show();
flake->setStart(ecore_time_get() + (double)(random() % (flake->getHeight() * 10)) / (double)flake->getHeight());
if (type == "flake_small") flake->setSpeed(1);
if (type == "flake_medium") flake->setSpeed(2);
if (type == "flake_large") flake->setSpeed(3);
flakes.push_back(flake);
}
return true;
}
