bool Rad2Degree::OnStartUp()
{
AppCastingMOOSApp::OnStartUp();
STRING_LIST sParams;
m_MissionReader.EnableVerbatimQuoting(false);
if(!m_MissionReader.GetConfiguration(GetAppName(), sParams))
reportConfigWarning("No config block found for " + GetAppName());
STRING_LIST::iterator p;
sParams.reverse();
for(p = sParams.begin() ; p != sParams.end() ; p++)
{
string orig = *p;
string line = *p;
string param = toupper(biteStringX(line, '='));
string value = line;
bool handled = false;
if(param == "FOO")
{
handled = true;
}
else if(param == "BAR")
{
handled = true;
}
if(!handled)
reportUnhandledConfigWarning(orig);
}
registerVariables();
return true;
}
bool OpiDetection::OnStartUp()
{
AppCastingMOOSApp::OnStartUp();
setlocale(LC_ALL, "C");
STRING_LIST sParams;
m_MissionReader.EnableVerbatimQuoting(false);
if(!m_MissionReader.GetConfiguration(GetAppName(), sParams))
reportConfigWarning("No config block found for " + GetAppName());
STRING_LIST::iterator p;
sParams.reverse();
for(p = sParams.begin() ; p != sParams.end() ; p++)
{
string orig = *p;
string line = *p;
string param = toupper(biteStringX(line, '='));
string value = line;
bool handled = false;
if(param == "VARIABLE_IMAGE_NAME")
{
image_name = value;
handled = true;
}
else if(param == "VARIABLE_WHEN_FOUND")
{
message_name = value;
handled = true;
}
else if(param == "SHOW_PROCESS")
{
show_process = (value == "true");
handled = true;
}
else if(param == "IMAGE_NAME_PATTERN")
{
image_name_pattern = "/" + value;
handled = true;
}
else if(param == "FOLDER_NAME_PATTERN")
{
folder_name_pattern = "/" + value;
handled = true;
}
else if(param == "SAVE_IN_FOLDER")
{
path_save = value;
handled = true;
}
if(!handled)
reportUnhandledConfigWarning(orig);
}
char folder_name[80];
time_t now = time(0);
struct tm tstruct;
tstruct = *localtime(&now);
// Visit http://en.cppreference.com/w/cpp/chrono/c/strftime
// for more information about date/time format
strftime(folder_name, sizeof(folder_name), folder_name_pattern.c_str(), &tstruct);
path_save += folder_name;
mkdir(path_save.c_str(),S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
registerVariables();
return true;
}
bool XSensINS::OnStartUp() {
AppCastingMOOSApp::OnStartUp();
// XSens Configuration Array
XsOutputConfigurationArray configArray;
// MOOS parser
STRING_LIST sParams;
m_MissionReader.EnableVerbatimQuoting(false);
if (!m_MissionReader.GetConfiguration(GetAppName(), sParams))
reportConfigWarning("No config block found for " + GetAppName());
STRING_LIST::iterator p;
sParams.reverse();
for (p = sParams.begin() ; p != sParams.end() ; p++) {
string orig = *p;
string line = *p;
string param = toupper(biteStringX(line, '='));
string value = line;
bool handled = false;
if (param == "UART_BAUD_RATE") {
m_uart_baud_rate = atoi(value.c_str());
handled = true;
}
else if (param == "SERIAL_PORT") {
m_uart_port = value;
handled = true;
}
else if (param == "XDI_EULERANGLES"){
XsOutputConfiguration config(XDI_EulerAngles, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
else if (param == "XDI_ACCELERATION"){
XsOutputConfiguration config(XDI_Acceleration, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
else if (param == "XDI_RATEOFTURN"){
XsOutputConfiguration config(XDI_RateOfTurn, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
else if (param == "XDI_MAGNETICFIELD"){
XsOutputConfiguration config(XDI_MagneticField, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
else if (param == "XDI_LATLON"){
XsOutputConfiguration config(XDI_LatLon, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
else if (param == "XDI_VELOCITYXYZ"){
XsOutputConfiguration config(XDI_VelocityXYZ, atoi(value.c_str()));
configArray.push_back(config);
handled = true;
}
if(!handled)
reportUnhandledConfigWarning(orig);
}
registerVariables();
//------ OPEN INS ---------------//
XsPortInfo mtPort(m_uart_port, XsBaud::numericToRate(m_uart_baud_rate));
if (!m_device.openPort(mtPort)) {
cout << "CANNOT OPEN THE PORT : " << m_uart_port << '\n';
reportRunWarning("Could not open the COM port" + m_uart_port);
}
//------ CONFIGURE INS ---------------//
// Put the device into configuration mode before configuring the device
if (!m_device.gotoConfig()) {
reportRunWarning("Could not begin the config mode");
}
// Save INS Config
if (!m_device.setOutputConfiguration(configArray)) {
reportRunWarning("Could not save config");
}
//------ START INS ---------------//
if (!m_device.gotoMeasurement()) {
reportRunWarning("Could not start the INS");
}
return true;
}
bool CMOOSLogger::ConfigureLogging()
{
//figure out what we are required to log....
//here we read in what we want to log from the mission file..
STRING_LIST Params;
bool bHasMissionFile = true;
if(m_MissionReader.GetConfiguration(m_sAppName,Params))
{
//this will make columns in sync log in order they
//were declared in *.moos file
Params.reverse();
STRING_LIST::iterator p;
for(p=Params.begin();p!=Params.end();p++)
{
string sParam = *p;
string sWhat = MOOSChomp(sParam,"=");
if(MOOSStrCmp(sWhat,"LOG"))
{
std::string sNewVar;
HandleLogRequest(sParam,sNewVar);
}
}
}
else
{
bHasMissionFile = false;
MOOSTrace("Warning:\n\tNo Configuration block was read - unusual but not terminal\n");
}
//are we allowing dynamic logging of variables is via PLOGGER_CMD message?
int nNumDynamicVariables = m_MissionReader.IsOpen() ? 0 : 10;
//this won't touch nNumDynamicVariables if mission file isn't open.
m_MissionReader.GetConfigurationParam("DynamicSyncLogColumns",nNumDynamicVariables);
if(nNumDynamicVariables>0)
{
MOOSTrace("Comment:\n\tReserving space for %d dynamic variables in slog\n",nNumDynamicVariables);
for(int i = 0; i<nNumDynamicVariables;i++)
{
m_UnusedDynamicVariables.push_back(MOOSFormat("DYNAMIC_%d",i));
}
}
//and generally turn on command message filtering at the CMOOSApp level
EnableCommandMessageFiltering(true);
//do we want wildcard logging - ie have the logger log every change...
m_bWildCardLogging = false;
if(bHasMissionFile)
{
m_MissionReader.GetConfigurationParam("WildcardLogging",m_bWildCardLogging);
}
else
{
m_bWildCardLogging = true;
}
//what sort of things do we want to wild card log
if(m_bWildCardLogging )
{
//we never want to log mission files sent between communities - this is done elsewhere
m_sWildCardOmitted.push_back("MISSION_FILE");
//there was a request to allow multiple statements of the these patterns...hence the
//more complicated parsing here
STRING_LIST sList;
if(m_MissionReader.GetConfiguration(GetAppName(), sList))
{
STRING_LIST::iterator q;
for(q = sList.begin();q!=sList.end();q++)
{
//are we being told exactly what accept and what not to accept
std::string sTok,sVal;
if(!CMOOSFileReader::GetTokenValPair(*q, sTok,sVal))
continue;
if(MOOSStrCmp("WildCardPattern",sTok))
{
while(!sVal.empty())
{
m_sWildCardAccepted.push_back(MOOSChomp(sVal,","));
}
}
else if(MOOSStrCmp("WildCardOmitPattern",sTok))
{
while(!sVal.empty())
{
m_sWildCardOmitted.push_back(MOOSChomp(sVal));
}
}
}
}
m_bAsynchronousLog = true;
}
//ok so now lets register our interest in all these MOOS vars!
if(!RegisterMOOSVariables())
MOOSDebugWrite("Variable subscription is still pending - not terminal, but unusual");
return true;
}
bool Joystick::OnStartUp()
{
AppCastingMOOSApp::OnStartUp();
setlocale(LC_ALL, "C");
STRING_LIST sParams;
m_MissionReader.EnableVerbatimQuoting(false);
if(!m_MissionReader.GetConfiguration(GetAppName(), sParams))
reportConfigWarning("No config block found for " + GetAppName());
int inputId = -1;
int inputType = -1;
float ratio = 1.;
float axis_min = -1.;
float axis_max = 1.;
ControlMode inputMode = Value;
STRING_LIST::iterator p;
sParams.reverse();
for(p = sParams.begin() ; p != sParams.end() ; p++)
{
string orig = *p;
string line = *p;
string param = toupper(biteStringX(line, '='));
string value = line;
bool handled = false;
if(param == "DEVICE_NAME")
{
mJoystickControl.mDeviceName = value;
handled = true;
}
else if(param == "MODE")
{
if (tolower(value) == "value")
inputMode = Value;
else if (tolower(value) == "increment")
inputMode = Increment;
else if (tolower(value) == "switch")
inputMode = Switch;
handled = true;
}
else if(param == "MIN")
{
if(!MOOSIsNumeric(value))
reportConfigWarning(orig);
else
axis_min = atof(value.c_str());
handled = true;
}
else if(param == "MAX")
{
if(!MOOSIsNumeric(value))
reportConfigWarning(orig);
else
axis_max = atof(value.c_str());
handled = true;
}
else if(param == "BUTTON")
{
if(!MOOSIsNumeric(value))
reportConfigWarning(orig);
else
{
inputId = atoi(value.c_str());
inputType = JS_EVENT_BUTTON;
}
handled = true;
}
else if(param == "AXIS")
{
if(!MOOSIsNumeric(value))
reportConfigWarning(orig);
else
{
inputId = atoi(value.c_str());
inputType = JS_EVENT_AXIS;
}
handled = true;
}
else if(param == "SCALE")
{
if(!MOOSIsNumeric(value))
reportConfigWarning(orig);
else
{
axis_max = atof(value.c_str());
axis_min = -axis_max;
}
handled = true;
}
else if(param == "SCALE_RATIO")
{
if(!MOOSIsNumeric(value))
reportConfigWarning(orig);
else
ratio = atof(value.c_str());
handled = true;
}
else if(param == "MOOS_DEST_VAR")
{
value = toupper(value);
if(inputId >= 0)
{
mControlsVariables[make_pair(inputType, inputId)] = pair<string, JoystickControlState>(value, JoystickControlState());
double offset = ratio*axis_min;
double gain = (ratio*axis_max - offset);
mJoystickControl.setState(inputType, inputId, inputMode, gain, offset);
handled = true;
}
}
if(!handled)
reportUnhandledConfigWarning(orig);
}
if(!mJoystickControl.OpenPort())
MOOSFail("Cannot open the joystick device '%s'.\n", mJoystickControl.mDeviceName.c_str());
registerVariables();
return(true);
}
bool XSensINS::OnStartUp() {
AppCastingMOOSApp::OnStartUp();
STRING_LIST sParams;
m_MissionReader.EnableVerbatimQuoting(false);
if (!m_MissionReader.GetValue("XSENSINS_SERIAL_PORT",m_uart_port))
reportConfigWarning("No XSENSINS_SERIAL_PORT config found for " + GetAppName());
if (!m_MissionReader.GetConfiguration(GetAppName(), sParams))
reportConfigWarning("No config block found for " + GetAppName());
STRING_LIST::iterator p;
sParams.reverse();
for (p = sParams.begin() ; p != sParams.end() ; p++) {
string orig = *p;
string line = *p;
string param = toupper(biteStringX(line, '='));
string value = line;
bool handled = false;
if (param == "m_uart_baud_rate") {
m_uart_baud_rate = atoi(value.c_str());
handled = true;
}
else if (param == "YAW_DECLINATION") {
m_yaw_declination = atoi(value.c_str());
handled = true;
}
if(!handled)
reportUnhandledConfigWarning(orig);
}
registerVariables();
//------ OPEN INS ---------------//
XsPortInfo mtPort("/dev/xsens", XsBaud::numericToRate(115200));
if (!m_device.openPort(mtPort)) {
cout << "CANNOT OPEN THE PORT" << '\n';
reportRunWarning("Could not open the COM port" + m_uart_port);
}
//------ CONFIGURE INS ---------------//
// Put the device into configuration mode before configuring the device
if (!m_device.gotoConfig()) {
reportRunWarning("Could not begin the config mode");
}
XsOutputConfiguration euler(XDI_EulerAngles, 25);
XsOutputConfiguration acceleration(XDI_Acceleration, 25);
XsOutputConfiguration rateOfTurn(XDI_RateOfTurn, 25);
XsOutputConfiguration magnetic(XDI_MagneticField, 25);
XsOutputConfiguration latlon(XDI_LatLon, 25);
XsOutputConfigurationArray configArray;
configArray.push_back(euler);
configArray.push_back(acceleration);
configArray.push_back(rateOfTurn);
configArray.push_back(magnetic);
configArray.push_back(latlon);
// Save INS Config
if (!m_device.setOutputConfiguration(configArray)) {
reportRunWarning("Could not save config");
}
//------ START INS ---------------//
if (!m_device.gotoMeasurement()) {
reportRunWarning("Could not start the INS");
}
return true;
}
bool LatLon2LocalGrid::OnStartUp() {
AppCastingMOOSApp::OnStartUp();
bool geodesy_origin_param = true;
if(!m_MissionReader.GetValue("LatOrigin", lat_origin))
{
reportConfigWarning("No LatOrigin in *.moos file");
geodesy_origin_param = false;
}
if(!m_MissionReader.GetValue("LongOrigin", long_origin))
{
reportConfigWarning("No LongOrigin in *.moos file");
geodesy_origin_param = false;
}
if(geodesy_origin_param)
geodesy.Initialise(lat_origin, long_origin);
STRING_LIST sParams;
m_MissionReader.EnableVerbatimQuoting(false);
if (!m_MissionReader.GetConfiguration(GetAppName(), sParams))
reportConfigWarning("No config block found for " + GetAppName());
STRING_LIST::iterator p;
sParams.reverse();
for (p = sParams.begin(); p != sParams.end(); p++) {
string orig = *p;
string line = *p;
string param = toupper(biteStringX(line, '='));
string value = line;
bool handled = false;
if (param == "NORTHING_PUBLICATION_NAME") {
NORTHING_PUBLICATION_NAME = value;
handled = true;
} else if (param == "EASTING_PUBLICATION_NAME") {
EASTING_PUBLICATION_NAME = value;
handled = true;
} else if (param == "LAT_SUBSCRIPTION_NAME") {
LAT_SUBSCRIPTION_NAME = value;
handled = true;
} else if (param == "LON_SUBSCRIPTION_NAME") {
LON_SUBSCRIPTION_NAME = value;
handled = true;
} else if (param == "CUSTOM_SHIFT_X") {
m_custom_shift_x = atof(value.c_str());
handled = true;
} else if (param == "CUSTOM_SHIFT_Y") {
m_custom_shift_y = atof(value.c_str());
handled = true;
}
if (!handled)
reportUnhandledConfigWarning(orig);
}
registerVariables();
return true;
}
