void TutorialComponent::run(OjCmpt cmpt) {
ojCmptRun(cmpt);
TutorialData* data = (TutorialData *)ojCmptGetUserData(cmpt);
//<ROS2JAUS>
ROSComponent roscomp;
roscomp.data = data;
*(data->subscriber) = data->nodeHandle->subscribe<std_msgs::Int32>("chatter", 1000, &ROSComponent::chatterCallback, &roscomp);
while (data->running == JAUS_TRUE) {
// doing nothing...
ros::spinOnce();
//</ROS2JAUS>
ojSleepMsec(100);
}
}
// Function: vehicleSimShutdown
// 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 vehicleSimShutdown(void)
{
double timeOutSec;
vehicleSimRun = FALSE;
timeOutSec = ojGetTimeSec() + VEHICLE_SIM_THREAD_TIMEOUT_SEC;
while(vehicleSimThreadRunning)
{
ojSleepMsec(100);
if(ojGetTimeSec() >= timeOutSec)
{
pthread_cancel(vehicleSimThreadId);
vehicleSimThreadRunning = FALSE;
//cError("vehicleSim: vehicleSimThread Shutdown Improperly\n");
break;
}
}
propertiesDestroy(vehicleSimProperties);
return 0;
}
// Function: vehicleSimThread
// Access: Private
// Description: All core component functionality is contained in this thread.
// All of the JAUS component state machine code can be found here.
void *vehicleSimThread(void *threadData)
{
double time, prevTime, dt;
double steeringRate;
double motorForce;
double brakeForce;
double dragForce;
double turningCurvature;
double deltaH;
vehicleSimThreadRunning = TRUE;
// Initialize the UTM engine
utmConversionInit(vehiclePosLla);
vehiclePosUtm = pointLlaToPointUtm(vehiclePosLla);
time = ojGetTimeSec();
while(vehicleSimRun) // Execute state machine code while not in the SHUTDOWN state
{
prevTime = time;
time = ojGetTimeSec();
dt = time - prevTime;
vehicleSimThreadHz = 1.0/dt; // Compute the update rate of this thread
steeringRate = K_STEERING * (vehicleDesiredRotationalEffort - vehicleRotationalEffort);
if(steeringRate > MAX_STEERING_RATE)
{
steeringRate = MAX_STEERING_RATE;
}
else if(steeringRate < -MAX_STEERING_RATE)
{
steeringRate = -MAX_STEERING_RATE;
}
vehicleRotationalEffort += steeringRate * dt;
turningCurvature = -vehicleRotationalEffort / 100.0 / MIN_TURNING_RADIUS_M;
vehicleLinearEffortX = vehicleDesiredLinearEffortX;
motorForce = K_THROTTLE * vehicleLinearEffortX + IDLE_FORCE_N;
vehicleResistiveEffortX += BRAKE_TAU * (vehicleDesiredResistiveEffortX - vehicleResistiveEffortX);
if(vehicleSpeed > 0.005)
{
brakeForce = K_DYNAMIC_BRAKE * vehicleResistiveEffortX;
}
else
{
brakeForce = K_STATIC_BRAKE * vehicleResistiveEffortX;
if(brakeForce > motorForce)
{
brakeForce = motorForce + 1.0;
}
}
dragForce = DRAG_COEFF_N_PER_MPS_SQ * vehicleSpeed * vehicleSpeed;
vehicleSpeed += ( motorForce - brakeForce - dragForce) / VEHICLE_MASS_KG * dt;
if(vehicleSpeed < 0)
{
vehicleSpeed = 0;
}
deltaH = turningCurvature * vehicleSpeed * dt;
vehiclePosUtm->xMeters += cos(vehicleH + deltaH / 2.0) * vehicleSpeed * dt;
vehiclePosUtm->yMeters += sin(vehicleH + deltaH / 2.0) * vehicleSpeed * dt;
vehicleH += deltaH;
vehicleH = fmod(vehicleH, 2*M_PI);
if(vehiclePosLla)
{
pointLlaDestroy(vehiclePosLla);
}
vehiclePosLla = pointUtmToPointLla(vehiclePosUtm);
ojSleepMsec(25);
}
ojSleepMsec(50); // Sleep for 50 milliseconds and then exit
vehicleSimThreadRunning = FALSE;
return NULL;
}
int main(int argCount, char **argString)
{
char keyPressed = FALSE;
int keyboardLock = FALSE;
double keyboardLockTime = ojGetTimeSec() + KEYBOARD_LOCK_TIMEOUT_SEC;
time_t timeStamp;
//Get and Format Time String
time(&timeStamp);
strftime(timeString, DEFAULT_STRING_LENGTH-1, "%m-%d-%Y %X", localtime(&timeStamp));
system(CLEAR);
//cDebug(1, "main: Starting Up %s Node Software\n", simulatorGetName());
// if(simulatorStartup())
// {
// printf("main: %s Node Startup failed\n", simulatorGetName());
// //cDebug(1, "main: Exiting %s Node Software\n", simulatorGetName());
//#if defined(WIN32)
// system("pause");
//#else
// printf("Press ENTER to exit\n");
// getch();
//#endif
// return 0;
// }
vehicleSimStartup();
pd = pdCreate();
gpos = gposCreate();
vss = vssCreate();
wd = wdCreate();
setupTerminal();
mainRunning = TRUE;
while(mainRunning)
{
keyPressed = getUserInput();
if(keyPressed)
{
keyboardLockTime = ojGetTimeSec() + KEYBOARD_LOCK_TIMEOUT_SEC;
}
else if(ojGetTimeSec() > keyboardLockTime)
{
keyboardLock = TRUE;
}
//if(verbose)
//{
// choice = getc(stdin);
//}
//else // Not in verbose mode
//{
// choice = getch(); // Get the key that the user has selected
// updateScreen(keyboardLock, choice);
//}
ojSleepMsec(100);
}
cleanupConsole();
//cDebug(1, "main: Shutting Down %s Node Software\n", simulatorGetName());
wdDestroy(wd);
pdDestroy(pd);
gposDestroy(gpos);
vssDestroy(vss);
vehicleSimShutdown();
if(logFile != NULL)
{
fclose(logFile);
}
return 0;
}
