// Function: gposStartup
// Access: Public
// Description: This function allows the abstracted component functionality contained in this file to be started from an external source.
// It must be called first before the component state machine and node manager interaction will begin
// Each call to "gposStartup" should be followed by one call to the "gposShutdown" function
OjCmpt gposCreate(void)
{
OjCmpt cmpt;
ReportGlobalPoseMessage message;
JausAddress gposAddr;
cmpt = ojCmptCreate("gpos", JAUS_GLOBAL_POSE_SENSOR, GPOS_THREAD_DESIRED_RATE_HZ);
ojCmptAddService(cmpt, JAUS_GLOBAL_POSE_SENSOR);
ojCmptAddServiceInputMessage(cmpt, JAUS_GLOBAL_POSE_SENSOR, JAUS_QUERY_GLOBAL_POSE, 0xFF);
ojCmptAddServiceOutputMessage(cmpt, JAUS_GLOBAL_POSE_SENSOR, JAUS_REPORT_GLOBAL_POSE, 0xFF);
ojCmptSetStateCallback(cmpt, JAUS_READY_STATE, gposReadyState);
ojCmptSetMessageCallback(cmpt, JAUS_QUERY_GLOBAL_POSE, gposQueryGlobalPoseCallback);
ojCmptAddSupportedSc(cmpt, JAUS_REPORT_GLOBAL_POSE);
message = reportGlobalPoseMessageCreate();
gposAddr = ojCmptGetAddress(cmpt);
jausAddressCopy(message->source, gposAddr);
jausAddressDestroy(gposAddr);
ojCmptSetUserData(cmpt, (void *)message);
ojCmptSetState(cmpt, JAUS_READY_STATE);
if(ojCmptRun(cmpt))
{
ojCmptDestroy(cmpt);
return NULL;
}
return cmpt;
}
OjCmpt pdCreate(void)
{
OjCmpt cmpt;
PdData *data;
JausAddress pdAddr;
cmpt = ojCmptCreate("pd", JAUS_PRIMITIVE_DRIVER, PD_THREAD_DESIRED_RATE_HZ);
ojCmptAddService(cmpt, JAUS_PRIMITIVE_DRIVER);
ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_SET_WRENCH_EFFORT, 0xFF);
ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_SET_DISCRETE_DEVICES, 0xFF);
ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_QUERY_PLATFORM_SPECIFICATIONS, 0xFF);
ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_QUERY_WRENCH_EFFORT, 0xFF);
ojCmptAddServiceOutputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_PLATFORM_SPECIFICATIONS, 0xFF);
ojCmptAddServiceOutputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_WRENCH_EFFORT, 0xFF);
ojCmptAddSupportedSc(cmpt, JAUS_REPORT_WRENCH_EFFORT);
ojCmptSetMessageProcessorCallback(cmpt, pdProcessMessage);
ojCmptSetStateCallback(cmpt, JAUS_STANDBY_STATE, pdStandbyState);
ojCmptSetStateCallback(cmpt, JAUS_READY_STATE, pdReadyState);
ojCmptSetState(cmpt, JAUS_STANDBY_STATE);
pdAddr = ojCmptGetAddress(cmpt);
data = (PdData*)malloc(sizeof(PdData));
data->setWrenchEffort = setWrenchEffortMessageCreate();
data->setDiscreteDevices = setDiscreteDevicesMessageCreate();
data->reportWrenchEffort = reportWrenchEffortMessageCreate();
data->controllerStatus = reportComponentStatusMessageCreate();
data->controllerSc = -1;
jausAddressCopy(data->reportWrenchEffort->source, pdAddr);
jausAddressDestroy(pdAddr);
ojCmptSetUserData(cmpt, (void *)data);
if(ojCmptRun(cmpt))
{
ojCmptDestroy(cmpt);
return NULL;
}
return cmpt;
}
OjCmpt
create_xgv_ccu_component(char *xgv_ccu_name, int xgv_ccu_component_id, double xgv_ccu_state_machine_update_rate)
{
OjCmpt XGV_CCU; // Variable that will store the component reference
XGV_CCU = ojCmptCreate(xgv_ccu_name, xgv_ccu_component_id, xgv_ccu_state_machine_update_rate); // Create the component
if (XGV_CCU == NULL)
{
printf("Error creating %s component\n", xgv_ccu_name);
exit(1);
}
ojCmptAddService(XGV_CCU, xgv_ccu_component_id); // Add XGV_CCU service type
add_xgv_ccu_component_service_messages(XGV_CCU, xgv_ccu_component_id);
//TODO função abaixo foi comentada pois gera 50Htz de mensagens desnecessarias de xgv_ccu_state_machine.
//ojCmptSetStateCallback(XGV_CCU, JAUS_READY_STATE, xgv_ccu_state_machine); // Set ready state callback
//ojCmptSetState(XGV_CCU, JAUS_READY_STATE); // Set the current state to ready
return (XGV_CCU);
}
OjCmpt TutorialComponent::create(std::string prettyName) {
printf("creating\n");
OjCmpt result;
TutorialData *data = NULL;
// it is unbelieveable that I have to do this...what a HACK
char szName[256];
strcpy(szName, prettyName.c_str());
// now, we create it using the global (groan) methods
result = ojCmptCreate(szName, JAUS_EXPERIMENTAL_TUTORIAL_COMPONENT, TUTORIAL_THREAD_DESIRED_RATE_HZ);
if (result == NULL) {
// something bad happened...
std::cout << "Error starting controller...aborting." << std::endl;
return result;
} else {
//<ROS2JAUS>
//First spoof the comand line arguments to ROS
char **argw = (char**)malloc(sizeof(char*)*2);
char *str = new char[8];
strcpy(str,"tutorial");
argw[0] = str;
argw[1] = NULL;
int one = 1;
ros::init(one, argw, "bridge");
//Initialize the data struct and set its values
data = (TutorialData *)malloc(sizeof(TutorialData));
data->gotVelocity = false;
data->gotWrench = false;
data->nodeHandle = new ros::NodeHandle;
data->wrenchPub = new ros::Publisher;
data->wrenchSub = new ros::Subscriber;
data->velocitySub = new ros::Subscriber;
*(data->wrenchPub) = data->nodeHandle->advertise<beginner_tutorials::wrenchData>("setWrench", 1000);
//Set up service connections
ojCmptAddService(result, JAUS_PRIMITIVE_DRIVER);
ojCmptAddService(result, JAUS_VELOCITY_STATE_SENSOR);
ojCmptAddService(result, JAUS_EXPERIMENTAL_MOTION_PROFILE_DRIVER);
ojCmptAddServiceOutputMessage(result, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_WRENCH_EFFORT, 0xFF);
ojCmptAddServiceOutputMessage(result, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_COMPONENT_STATUS, 0xFF);
ojCmptAddServiceOutputMessage(result, JAUS_EXPERIMENTAL_MOTION_PROFILE_DRIVER, JAUS_REPORT_COMPONENT_STATUS, 0xFF);
ojCmptAddServiceOutputMessage(result, JAUS_VELOCITY_STATE_SENSOR, JAUS_REPORT_VELOCITY_STATE, 0xFF);
ojCmptAddSupportedSc(result, JAUS_REPORT_WRENCH_EFFORT);
ojCmptAddSupportedSc(result, JAUS_REPORT_COMPONENT_STATUS);
ojCmptAddSupportedSc(result, JAUS_REPORT_VELOCITY_STATE);
//</ROS2JAUS>
// Register function callback for the process message state
ojCmptSetMessageProcessorCallback(result, TutorialComponent::processMessage);
// Register function callback for the standby state
ojCmptSetStateCallback(result, JAUS_STANDBY_STATE, TutorialComponent::standbyState);
// Register function callback for the ready state
ojCmptSetStateCallback(result, JAUS_READY_STATE, TutorialComponent::readyState);
// Register function callback for the initialize state
ojCmptSetStateCallback(result, JAUS_INITIALIZE_STATE, TutorialComponent::initState);
// Register function callback for the shutdown state
ojCmptSetStateCallback(result, JAUS_SHUTDOWN_STATE, TutorialComponent::shutdownState);
ojCmptSetState(result, JAUS_INITIALIZE_STATE);
data->running = JAUS_TRUE;
ojCmptSetUserData(result, (void *)data);
}
return result;
}
