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(); }