LargeMessageList lmListCreate(void)
{
LargeMessageList msgList = (LargeMessageList)malloc(sizeof(LargeMessageListStruct));
msgList->commandCode = 0;
msgList->source = jausAddressCreate();
msgList->messages = jausArrayCreate();
return msgList;
}
// Services Vector Constructor
JausArray jausServicesCreate(void)
{
JausArray jausServices;
jausServices = jausArrayCreate();
if(jausServices)
{
return jausServices;
}
else
{
return NULL;
}
}
LargeMessageHandler lmHandlerCreate(void)
{
LargeMessageHandler lmh = (LargeMessageHandler)malloc( sizeof(LargeMessageHandlerStruct) );
if(lmh)
{
lmh->messageLists = jausArrayCreate();
return lmh;
}
else
{
return NULL;
}
}
JausSubsystem jausSubsystemCreate(void)
{
JausSubsystem subsystem;
subsystem = (JausSubsystem)malloc( sizeof(JausSubsystemStruct) );
if(subsystem == NULL)
{
return NULL;
}
//Init Values
subsystem->id = 0;
subsystem->identification = NULL;
subsystem->nodes = jausArrayCreate();
jausSubsystemUpdateTimestamp(subsystem);
return subsystem;
}
JausArray jausServicesClone(JausArray sourceServices)
{
JausArray destinationServices;
JausService sourceService, destinationService;
JausCommand sourceCommand;
int i = 0;
destinationServices = jausArrayCreate();
if(!destinationServices) return NULL;
for(i = 0; i < sourceServices->elementCount; i++)
{
sourceService = (JausService) sourceServices->elementData[i];
destinationService = jausServiceCreate(sourceService->type);
jausServiceAddService(destinationServices, destinationService);
// Duplicate Input Commands from sourceService to destinationService
sourceCommand = sourceService->inputCommandList;
while(sourceCommand)
{
if(!jausServiceAddInputCommand(destinationService, sourceCommand->commandCode, sourceCommand->presenceVector))
{
jausServicesDestroy(destinationServices);
return NULL;
}
sourceCommand = sourceCommand->next;
}
// Duplicate Output Commands from sourceService to destinationService
sourceCommand = sourceService->outputCommandList;
while(sourceCommand)
{
if(!jausServiceAddOutputCommand(destinationService, sourceCommand->commandCode, sourceCommand->presenceVector))
{
jausServicesDestroy(destinationServices);
return NULL;
}
sourceCommand = sourceCommand->next;
}
}
return destinationServices;
}
JausNode jausNodeCreate(void)
{
JausNode node;
node = (JausNode)malloc( sizeof(JausNodeStruct) );
if(node == NULL)
{
return NULL;
}
//Init Values
node->identification = NULL;
node->id = 0;
node->components = jausArrayCreate();
node->subsystem = NULL;
jausNodeUpdateTimestamp(node);
return node;
}
JausNode jausNodeClone(JausNode node)
{
JausNode clone;
JausComponent tempCmpt;
int i;
size_t stringLength;
clone = (JausNode)malloc( sizeof(JausNodeStruct) );
if(clone == NULL)
{
return NULL;
}
//Init Values
if(node->identification)
{
stringLength = strlen(node->identification) + 1;
clone->identification = (char *) malloc(stringLength);
sprintf(clone->identification, "%s", node->identification);
}
else
{
clone->identification = NULL;
}
clone->id = node->id;
clone->components = jausArrayCreate();
for( i = 0; i < node->components->elementCount; i++)
{
tempCmpt = jausComponentClone(node->components->elementData[i]);
jausArrayAdd(clone->components, tempCmpt);
}
clone->subsystem = node->subsystem;
jausNodeUpdateTimestamp(clone);
return clone;
}
JausSubsystem jausSubsystemClone(JausSubsystem subsystem)
{
JausSubsystem clone;
JausNode tempNode;
size_t stringLength;
int i;
clone = (JausSubsystem)malloc( sizeof(JausSubsystemStruct) );
if(clone == NULL)
{
return NULL;
}
//Init Values
clone->id = subsystem->id;
if(subsystem->identification)
{
stringLength = strlen(subsystem->identification) + 1;
clone->identification = (char *) malloc(stringLength);
sprintf(clone->identification, "%s", subsystem->identification);
}
else
{
clone->identification = NULL;
}
clone->nodes = jausArrayCreate();
for( i = 0; i < subsystem->nodes->elementCount; i++)
{
tempNode = jausNodeClone(subsystem->nodes->elementData[i]);
jausArrayAdd(clone->nodes, tempNode);
}
jausSubsystemUpdateTimestamp(clone);
return clone;
}
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();
}
