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);
}
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
}
OjCmpt ojCmptCreate(char *name, JausByte id, double stateFrequencyHz)
{
OjCmpt ojCmpt;
int i;
ojCmpt = (OjCmpt)malloc(sizeof(struct OjCmptStruct));
ojCmpt->jaus = jausComponentCreate();
ojCmpt->jaus->identification = (char *)calloc(strlen(name) + 1, 1);
strcpy(ojCmpt->jaus->identification, name);
ojCmpt->jaus->address->component = id;
ojCmptSetState(ojCmpt, JAUS_UNDEFINED_STATE);
ojCmptSetFrequencyHz(ojCmpt, stateFrequencyHz);
for(i=0; i<OJ_CMPT_MAX_STATE_COUNT; i++)
{
ojCmpt->stateCallback[i] = NULL;
}
ojCmpt->mainCallback = NULL;
ojCmpt->processMessageCallback = NULL;
ojCmpt->messageCallback = NULL;
ojCmpt->messageCallbackCount = 0;
ojCmpt->run = FALSE;
if (!jausServiceAddCoreServices(ojCmpt->jaus->services)) // Add core services
{
free(ojCmpt->jaus->identification);
ojCmpt->jaus->identification = NULL;
jausComponentDestroy(ojCmpt->jaus);
free(ojCmpt);
return NULL;
}
for(i=0; i<OJ_CMPT_MAX_INCOMING_SC_COUNT; i++)
{
ojCmpt->inConnection[i] = NULL;
}
ojCmpt->nmi = nodeManagerOpen(ojCmpt->jaus);
if(ojCmpt->nmi == NULL)
{
free(ojCmpt->jaus->identification);
ojCmpt->jaus->identification = NULL;
jausComponentDestroy(ojCmpt->jaus);
free(ojCmpt);
return NULL;
}
return ojCmpt;
}
void wdStartupState(void)
{
JausService service;
// Populate Core Service
if(!jausServiceAddCoreServices(wd->services))
{
//cError("wd: Addition of Core Services FAILED! Switching to FAILURE_STATE\n");
wd->state = JAUS_FAILURE_STATE;
}
// USER: Add the rest of your component specific service(s) here
// create a new service for waypoint driver message support
service = jausServiceCreate(JAUS_GLOBAL_WAYPOINT_DRIVER);
// add each supported waypoint driver input message
jausServiceAddInputCommand(service, JAUS_SET_TRAVEL_SPEED, NO_PRESENCE_VECTOR);
jausServiceAddInputCommand(service, JAUS_SET_GLOBAL_WAYPOINT, NO_PRESENCE_VECTOR);//actually, there is a PV and it's equal to zero
jausServiceAddInputCommand(service, JAUS_QUERY_WAYPOINT_COUNT, NO_PRESENCE_VECTOR);
jausServiceAddInputCommand(service, JAUS_QUERY_GLOBAL_WAYPOINT, NO_PRESENCE_VECTOR);
// add each supported waypoint driver output message
jausServiceAddOutputCommand(service, JAUS_REPORT_WAYPOINT_COUNT, NO_PRESENCE_VECTOR);
jausServiceAddOutputCommand(service, JAUS_REPORT_GLOBAL_WAYPOINT, NO_PRESENCE_VECTOR);
// add the service to the component's services vector
if(!jausServiceAddService(wd->services, service))
{
//cError("wd: Could not add services");
}
wd->authority = 127;
pd = jausComponentCreate();
pd->address->component = JAUS_PRIMITIVE_DRIVER;
// Setup GPOS Service Connection handle
gposSc = serviceConnectionCreate();
gposSc->requestedUpdateRateHz = GPOS_SC_UPDATE_RATE_HZ;
gposSc->address->component = JAUS_GLOBAL_POSE_SENSOR;
gposSc->presenceVector = GPOS_SC_PRESENCE_VECTOR;
gposSc->commandCode = JAUS_REPORT_GLOBAL_POSE;
gposSc->isActive = JAUS_FALSE;
gposSc->queueSize = GPOS_SC_QUEUE_SIZE;
gposSc->timeoutSec = GPOS_SC_TIMEOUT_SECONDS;
// Setup VSS Service Connection handle
vssSc = serviceConnectionCreate();
vssSc->requestedUpdateRateHz = VSS_SC_UPDATE_RATE_HZ;
vssSc->address->component = JAUS_VELOCITY_STATE_SENSOR;
vssSc->presenceVector = VSS_SC_PRESENCE_VECTOR;
vssSc->commandCode = JAUS_REPORT_VELOCITY_STATE;
vssSc->isActive = JAUS_FALSE;
vssSc->queueSize = VSS_SC_QUEUE_SIZE;
vssSc->timeoutSec = VSS_SC_TIMEOUT_SECONDS;
// Setup PD Service Connection handle
pdWrenchSc = serviceConnectionCreate();
pdWrenchSc->requestedUpdateRateHz = PD_WRENCH_SC_UPDATE_RATE_HZ;
pdWrenchSc->address->component = JAUS_PRIMITIVE_DRIVER;
pdWrenchSc->presenceVector = PD_WRENCH_SC_PRESENCE_VECTOR;
pdWrenchSc->commandCode = JAUS_REPORT_WRENCH_EFFORT;
pdWrenchSc->isActive = JAUS_FALSE;
pdWrenchSc->queueSize = PD_WRENCH_SC_QUEUE_SIZE;
pdWrenchSc->timeoutSec = PD_WRENCH_SC_TIMEOUT_SECONDS;
pdStatusSc = serviceConnectionCreate();
pdStatusSc->requestedUpdateRateHz = PD_STATUS_SC_UPDATE_RATE_HZ;
pdStatusSc->address->component = JAUS_PRIMITIVE_DRIVER;
pdStatusSc->presenceVector = PD_STATUS_SC_PRESENCE_VECTOR;
pdStatusSc->commandCode = JAUS_REPORT_COMPONENT_STATUS;
pdStatusSc->isActive = JAUS_FALSE;
pdStatusSc->queueSize = PD_STATUS_SC_QUEUE_SIZE;
pdStatusSc->timeoutSec = PD_STATUS_SC_TIMEOUT_SECONDS;
// Setup message to be sent to the PD
wdWrench = setWrenchEffortMessageCreate();
jausAddressCopy(wdWrench->source, wd->address);
jausShortPresenceVectorSetBit(&wdWrench->presenceVector, JAUS_WRENCH_PV_PROPULSIVE_LINEAR_X_BIT);
jausShortPresenceVectorSetBit(&wdWrench->presenceVector, JAUS_WRENCH_PV_PROPULSIVE_ROTATIONAL_Z_BIT);
jausShortPresenceVectorSetBit(&wdWrench->presenceVector, JAUS_WRENCH_PV_RESISTIVE_LINEAR_X_BIT);
scManagerAddSupportedMessage(wdNmi, JAUS_REPORT_COMPONENT_STATUS);
wdWaypoints = jausArrayCreate();
vehicleState = vehicleStateCreate();
}