static void test_err (void)
{
int i;
const char *dds_errors [] = {
"no error",
"Generic unspecified error",
"Unsupported operation",
"Invalid parameter value",
"Precondition not met",
"Not enough memory",
"Not enabled yet",
"Immutable policy",
"Inconsistent policy",
"Object not found",
"Timeout occurred",
"No data available",
"Illegal operation"
};
v_printf (" - Test error strings.\r\n");
for (i = -1; i <= DDS_RETCODE_ILLEGAL_OPERATION + 2; i++)
if (i >= 0 && i <= DDS_RETCODE_ILLEGAL_OPERATION)
fail_unless (!strcmp (DDS_error (i), dds_errors [i]));
else
fail_unless (!strcmp (DDS_error (i), "unknown"));
}
void dr_on_statistic_data_available(DDS_DataReaderListener *self, DDS_DataReader dataReader)
{
acceptance_high_end_StatisticPtrSeq samples;
DDS_SampleInfoSeq sampleInfos;
DDS_ReturnCode_t retCode;
int i;
DDS_SEQ_INIT(samples);
DDS_SEQ_INIT(sampleInfos);
retCode = DDS_DataReader_take(dataReader, (DDS_DataSeq *)&samples, &sampleInfos, -1, DDS_ANY_SAMPLE_STATE, DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE);
if (retCode == DDS_RETCODE_NO_DATA) {
return;
}
else if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error taking sample (%s).\r\n", DDS_error(retCode));
return;
}
for (i = 0; i < DDS_SEQ_LENGTH (samples); i++)
if (DDS_SEQ_ITEM (sampleInfos, i)->valid_data)
nofConfigSamples++;
retCode = DDS_DataReader_return_loan(dataReader, (DDS_DataSeq *)&samples, &sampleInfos);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error returning loan (%s).\r\n", DDS_error(retCode));
return;
}
}
static void remove_state_writer ()
{
DDS_ReturnCode_t error;
DDS_DataWriter_set_listener (dw, NULL, 0);
if ((error = DDS_Publisher_delete_datawriter (pub, dw)))
fatal ("DDS_Publisher_delete_datawriter() failed (%s)!", DDS_error (error));
if ((error = DDS_Publisher_delete_contained_entities (pub)))
fatal ("DDS_Publisher_delete_contained_entities () failed (%s)!", DDS_error (error));
}
static void remove_state_reader ()
{
DDS_ReturnCode_t error;
error = DDS_DataReader_delete_contained_entities (dr);
if (error)
fatal ("DDS_DataReader_delete_contained_entities() failed (%s)!", DDS_error (error));
DDS_DataReader_set_listener (dr, NULL, 0);
error = DDS_Subscriber_delete_datareader (sub, dr);
if (error)
fatal ("DDS_Subscriber_delete_datareader() failed (%s)!", DDS_error (error));
}
void read_data (DDS_DataReaderListener *l, DDS_DataReader dr)
{
static DDS_DynamicDataSeq drx_sample = DDS_SEQ_INITIALIZER (void *);
static DDS_SampleInfoSeq rx_info = DDS_SEQ_INITIALIZER (DDS_SampleInfo *);
DDS_SampleStateMask ss = DDS_NOT_READ_SAMPLE_STATE;
DDS_ViewStateMask vs = DDS_ANY_VIEW_STATE;
DDS_InstanceStateMask is = DDS_ANY_INSTANCE_STATE;
DDS_SampleInfo *info;
DDS_ReturnCode_t error;
DDS_DynamicData dd;
Topic *tp;
int secure = (cert_path != NULL);
tp = (Topic *) l->cookie;
for (;;) {
error = DDS_DynamicDataReader_take (dr, &drx_sample, &rx_info, 1, ss, vs, is);
if (error) {
if (error != DDS_RETCODE_NO_DATA)
printf ("Unable to read samples: error = %s!\r\n", DDS_error (error));
return;
}
if (DDS_SEQ_LENGTH (rx_info)) {
info = DDS_SEQ_ITEM (rx_info, 0);
if (info->valid_data) {
dd = DDS_SEQ_ITEM (drx_sample, 0);
if (!dd)
fatal ("Empty dynamic sample!");
tp->ndata++;
if (!paused) {
dbg_printf ("%s: ", tp->topic_name);
DDS_Debug_dump_dynamic (0, tp->ts, dd, 0, secure, 1);
dbg_printf ("\r\n");
}
}
else if (info->instance_handle && info->instance_state != DDS_ALIVE_INSTANCE_STATE) {
if (info->instance_state == DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE)
tp->ndispose++;
else if (info->instance_state == DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE)
tp->nnowriter++;
if (!paused) {
dd = DDS_DynamicDataFactory_create_data (tp->dtype);
DDS_DynamicDataReader_get_key_value (dr, dd, info->instance_handle);
dbg_printf ("%s: ", tp->topic_name);
DDS_Debug_dump_dynamic (0, tp->ts, dd, 1, secure, 1);
if (info->instance_state == DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE)
dbg_printf (": Not alive - disposed.");
else if (info->instance_state == DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE)
dbg_printf (": Not alive - no writers.");
dbg_printf ("\r\n");
}
}
DDS_DynamicDataReader_return_loan (dr, &drx_sample, &rx_info);
}
else
return;
}
}
static void make_state_reader ()
{
DDS_DataReaderQos rd_qos;
DDS_ReturnCode_t error;
printf ("Create Reader\r\n");
/* === Create a subscriber === */
sub = DDS_DomainParticipant_create_subscriber (part, 0, NULL, 0);
if (!sub) {
fatal ("DDS_DomainParticipant_create_subscriber () returned an error!");
DDS_DomainParticipantFactory_delete_participant (part);
}
/* Test get_qos() fynctionality. */
if ((error = DDS_Subscriber_get_qos (sub, &sqos)) != DDS_RETCODE_OK)
fatal ("DDS_Subscriber_get_qos () failed (%s)!", DDS_error (error));
/* Setup reader QoS parameters. */
DDS_Subscriber_get_default_datareader_qos (sub, &rd_qos);
/* === create the reader === */
rd_qos.history.kind = DDS_KEEP_LAST_HISTORY_QOS;
rd_qos.history.depth = 1;
rd_qos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
rd_qos.durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
rd_qos.ownership.kind = DDS_EXCLUSIVE_OWNERSHIP_QOS;
dr = DDS_Subscriber_create_datareader (sub, topic_desc, &rd_qos, NULL, sm);
if (!dr)
fatal ("DDS_DomainParticipant_create_datareader () returned an error!");
DDS_DataReaderListener *l = DDS_DataReader_get_listener(dr);
if (NULL != l) {
l->on_data_available = dr_listener_data_available;
error = DDS_DataReader_set_listener(dr, l, DDS_DATA_AVAILABLE_STATUS);
}
error = DDS_DataReader_enable(dr);
/* === end program === */
if (test == 3)
return;
if (!writer)
while (1)
sleep (1);
}
static void *imu_reader (void *args)
{
DDS_DynamicDataReader dr;
DDS_WaitSet ws;
DDS_SampleStateMask ss = DDS_NOT_READ_SAMPLE_STATE;
DDS_ViewStateMask vs = DDS_ANY_VIEW_STATE;
DDS_InstanceStateMask is = DDS_ANY_INSTANCE_STATE;
DDS_ReadCondition rc;
DDS_ConditionSeq conds = DDS_SEQ_INITIALIZER (DDS_Condition);
DDS_Duration_t to;
DDS_ReturnCode_t ret;
dr = args;
ws = DDS_WaitSet__alloc ();
if (!ws)
fatal ("Unable to allocate a WaitSet!");
if (verbose)
printf ("DDS Waitset allocated.\r\n");
rc = DDS_DataReader_create_readcondition (dr, ss, vs, is);
if (!rc)
fatal ("DDS_DataReader_create_readcondition () returned an error!");
if (verbose)
printf ("DDS Readcondition created.\r\n");
ret = DDS_WaitSet_attach_condition (ws, rc);
if (ret)
fatal ("Unable to attach condition to a WaitSet!");
while (!aborting) {
to.sec = 0;
to.nanosec = 200000000; /* Timeout after 200ms. */
ret = DDS_WaitSet_wait (ws, &conds, &to);
if (ret == DDS_RETCODE_TIMEOUT)
continue;
read_msg (NULL, dr);
}
ret = DDS_WaitSet_detach_condition (ws, rc);
if (ret)
fatal ("Unable to detach condition from WaitSet (%s)!", DDS_error (ret));
DDS_WaitSet__free (ws);
return (NULL);
}
static void enable_security (void)
{
DDS_Credentials credentials;
DDS_ReturnCode_t error;
#ifdef MSECPLUG_WITH_SECXML
/*int dhandle, thandle;*/
#endif
error = DDS_SP_set_policy ();
if (error)
fatal ("DDS_SP_set_policy() returned error (%s)!", DDS_error (error));
#ifdef MSECPLUG_WITH_SECXML
if (DDS_SP_parse_xml ("security.xml"))
fatal ("SP: no DDS security rules in 'security.xml'!\r\n");
#else
DDS_SP_add_domain();
if (!realm_name)
DDS_SP_add_participant ();
else
DDS_SP_set_participant_access (DDS_SP_add_participant (), strcat(realm_name, "*"), 2, 0);
#endif
if (!cert_path || !key_path)
fatal ("Error: you must provide a valid certificate path and a valid private key path\r\n");
if (engine_id) {
DDS_SP_init_engine (engine_id, init_engine_fs);
credentials.credentialKind = DDS_ENGINE_BASED;
credentials.info.engine.engine_id = engine_id;
credentials.info.engine.cert_id = cert_path;
credentials.info.engine.priv_key_id = key_path;
}
else {
credentials.credentialKind = DDS_FILE_BASED;
credentials.info.filenames.private_key_file = key_path;
credentials.info.filenames.certificate_chain_file = cert_path;
}
error = DDS_Security_set_credentials ("Technicolor Chatroom", &credentials);
}
void do_dds_shell (DDS_DataWriter dw)
{
Vector3_t m;
DDS_InstanceHandle_t h;
char buf [256];
#if !defined (NUTTX_RTOS)
tty_init ();
DDS_Handle_attach (tty_stdin,
POLLIN | POLLPRI | POLLERR | POLLHUP | POLLNVAL,
tty_input,
NULL);
#endif
#if 0
printf ("Welcome to the Technicolor chatroom.\r\n");
printf ("Anything you type will be sent to all chatroom attendees.\r\n");
printf ("Type '!help' for chatroom options.\r\n");
m.chatroom = chatroom;
m.from = user_name;
#endif
h = 0;
while (!aborting) {
#if defined (NUTTX_RTOS)
/* Take into account that fgets reads the "\n" character at the end
of each line. Code should consider this aspect in every case */
fgets(buf, 256, stdin);
#else
tty_gets (sizeof (buf), buf, 0, 1);
#endif
if (buf [0] == '!') {
#if defined (NUTTX_RTOS)
if (!strcmp (buf + 1, "quit\n") ||
(buf [1] == 'q' && buf [2] == '\n')) {
#else
if (!strcmp (buf + 1, "quit") ||
(buf [1] == 'q' && buf [2] == '\0')) {
#endif
aborting = 1;
break;
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "list\n"))
#else
else if (!strcmp (buf + 1, "list"))
#endif
printf ("Attendees:\r\n\t%s\r\n", user_name);
#if defined (NUTTX_RTOS)
else if (!memcmp (buf + 1, "user\n", 4)) {
#else
else if (!memcmp (buf + 1, "user", 4)) {
#endif
if (h) {
Vector3_signal (dw, h, 1);
h = 0;
}
strcpy (user_name, buf + 6);
printf ("You are now: %s\r\n", user_name);
}
#if defined (NUTTX_RTOS)
else if (!memcmp (buf + 1, "room\n", 4)) {
#else
else if (!memcmp (buf + 1, "room", 4)) {
#endif
if (h) {
Vector3_signal (dw, h, 1);
h = 0;
}
strcpy (chatroom, buf + 6);
printf ("Switched to chatroom: %s\r\n", chatroom);
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "info\n"))
#else
else if (!strcmp (buf + 1, "info"))
#endif
printf ("Chatroom: %s, Username: %s\r\n",
chatroom, user_name);
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "busy\n"))
#else
else if (!strcmp (buf + 1, "busy"))
#endif
Vector3_signal (dw, h, 0);
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "away\n")) {
#else
else if (!strcmp (buf + 1, "away")) {
#endif
if (h) {
Vector3_signal (dw, h, 1);
h = 0;
}
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "help\n") ||
(buf [1] == 'h' && buf [2] == '\n\0') ||
(buf [1] == '?' && buf [2] == '\n\0')) {
#else
else if (!strcmp (buf + 1, "help") ||
(buf [1] == 'h' && buf [2] == '\0') ||
(buf [1] == '?' && buf [2] == '\0')) {
#endif
printf ("Commands:\r\n");
printf (" !room <room_name> -> set the chatroom name.\r\n");
printf (" !user <user_name> -> set the user name.\r\n");
printf (" !list -> list the attendees.\r\n");
printf (" !info -> show chatroom and user.\r\n");
printf (" !busy -> momentarily not involved.\r\n");
printf (" !away -> gone away.\r\n");
printf (" !help or !h or !? -> Show this info.\r\n");
printf (" !quit or !q -> Quit the chatroom.\r\n");
printf (" !!<command> -> DDS debug command.\r\n");
/* printf (" !$<command> -> Shell command.\r\n"); */
}
else if (buf [1] == '!')
DDS_Debug_command (buf + 2);
/* else if (buf [1] == '$')
system (buf + 2); */
else
printf ("?%s\r\n", buf + 1);
continue;
} else {
printf("Writing to the other endpoints not allowed.\n");
#if 0
if (!h)
h = ChatMsg_register (dw, &m);
//sprintf(buf_t, "Embedded says %s\n", buf);
fetch_imu();
//m.message = buf_t;
m.message = buf;
ChatMsg_write (dw, &m, h);
#endif
printf ("Commands:\r\n");
printf (" !list -> list the attendees.\r\n");
printf (" !busy -> momentarily not involved.\r\n");
printf (" !away -> gone away.\r\n");
printf (" !help or !h or !? -> Show this info.\r\n");
printf (" !quit or !q -> Quit the chatroom.\r\n");
printf (" !!<command> -> DDS debug command.\r\n");
}
}
}
static void *dds_send_imu (void *args)
{
Vector3_t m;
DDS_InstanceHandle_t h;
h = 0;
for (;;){
sleep (1); // sleep 0.5 seconds
//sprintf(buf_t, "Embedded says %s\n", buf);
m.x_ = 1;
m.y_ = 2;
m.z_ = 3;
#if 0
/* According to https://github.com/brunodebus/tinq-core/issues/7#issuecomment-63740498:
the Vector3 shouldn't be registered if it doesn't contain a @key attribute
*/
if (!h)
h = Vector3_register (dw, &m);
#endif
Vector3_write (dw, &m, h);
}
}
void read_msg (DDS_DataReaderListener *l, DDS_DataReader dr)
{
Vector3_t msg;
DDS_InstanceStateKind kind;
int valid;
DDS_ReturnCode_t ret;
ARG_NOT_USED (l)
memset (&msg, 0, sizeof (msg));
ret = Vector3_read_or_take (dr, &msg, DDS_NOT_READ_SAMPLE_STATE,
DDS_ANY_VIEW_STATE,
DDS_ANY_INSTANCE_STATE, 1,
&valid, &kind);
if (ret == DDS_RETCODE_OK)
do {
#if 0
/* Use a from field within the message to avoid to read the messages that this
node sent.
*/
#ifndef DISPLAY_SELF
if (!strcmp (msg.from, user_name) &&
!strcmp (msg.chatroom, chatroom))
break;
#endif
#endif
if (valid)
printf ("IMU accel message: x=%f, y=%f and z=%f\r\n", msg.x_, msg.y_, msg.z_);
else if (kind == DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE)
printf ("DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE!\r\n");
else if (kind == DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE)
printf ("DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE!\r\n");
}
while (0);
Vector3_cleanup (&msg);
}
#ifdef WAITSETS
static void *imu_reader (void *args)
{
DDS_DynamicDataReader dr;
DDS_WaitSet ws;
DDS_SampleStateMask ss = DDS_NOT_READ_SAMPLE_STATE;
DDS_ViewStateMask vs = DDS_ANY_VIEW_STATE;
DDS_InstanceStateMask is = DDS_ANY_INSTANCE_STATE;
DDS_ReadCondition rc;
DDS_ConditionSeq conds = DDS_SEQ_INITIALIZER (DDS_Condition);
DDS_Duration_t to;
DDS_ReturnCode_t ret;
dr = args;
ws = DDS_WaitSet__alloc ();
if (!ws)
fatal ("Unable to allocate a WaitSet!");
if (verbose)
printf ("DDS Waitset allocated.\r\n");
rc = DDS_DataReader_create_readcondition (dr, ss, vs, is);
if (!rc)
fatal ("DDS_DataReader_create_readcondition () returned an error!");
if (verbose)
printf ("DDS Readcondition created.\r\n");
ret = DDS_WaitSet_attach_condition (ws, rc);
if (ret)
fatal ("Unable to attach condition to a WaitSet!");
while (!aborting) {
to.sec = 0;
to.nanosec = 200000000; /* Timeout after 200ms. */
ret = DDS_WaitSet_wait (ws, &conds, &to);
if (ret == DDS_RETCODE_TIMEOUT)
continue;
read_msg (NULL, dr);
}
ret = DDS_WaitSet_detach_condition (ws, rc);
if (ret)
fatal ("Unable to detach condition from WaitSet (%s)!", DDS_error (ret));
DDS_WaitSet__free (ws);
return (NULL);
}
static void start_imu_reader (DDS_DynamicDataReader dr)
{
thread_create (rt, imu_reader, dr);
}
int main (int argc, char *argv [])
{
int domain_id = 128;
DDS_ReturnCode_t error;
do_switches (argc, argv);
#ifdef DDS_DEBUG
DDS_Debug_start ();
#endif
DDS_set_generate_callback (calculate_member_id);
sem_init(&_sync, 0, 0);
part = DDS_DomainParticipantFactory_create_participant (domain_id, NULL, NULL, 0);
if (!part)
fatal ("DDS_DomainParticipantFactory_create_participant () failed!");
if (tsm_type == 0) {
error = register_HelloWorldData_type (part);
if (error) {
DDS_DomainParticipantFactory_delete_participant (part);
fatal ("DDS_DomainParticipant_register_type ('HelloWorldData') failed (%s)!", DDS_error (error));
}
sm = DDS_INCONSISTENT_TOPIC_STATUS;
topic = DDS_DomainParticipant_create_topic (part, "HelloWorld", "HelloWorldData",
NULL, NULL, sm);
if (!topic) {
DDS_DomainParticipantFactory_delete_participant (part);
fatal ("DDS_DomainParticipant_create_topic ('HelloWorld') failed!");
}
topic_desc = DDS_DomainParticipant_lookup_topicdescription (part, "HelloWorld");
if (!topic_desc) {
DDS_DomainParticipantFactory_delete_participant (part);
fatal ("Unable to create topic description for 'HelloWorld'!");
}
} else {
_tsm_types[0].flags |= TSMFLAG_KEY;
error = register_TestType_type (part);
if (error) {
DDS_DomainParticipantFactory_delete_participant (part);
fatal ("DDS_DomainParticipant_register_type ('HelloWorldData') failed (%s)!", DDS_error (error));
}
sm = DDS_INCONSISTENT_TOPIC_STATUS;
topic = DDS_DomainParticipant_create_topic (part, _tsm_types->name, _tsm_types->name,
NULL, NULL, sm);
if (!topic) {
DDS_DomainParticipantFactory_delete_participant (part);
fatal ("DDS_DomainParticipant_create_topic ('%s') failed!", _tsm_types->name);
}
topic_desc = DDS_DomainParticipant_lookup_topicdescription (part, _tsm_types->name);
if (!topic_desc) {
DDS_DomainParticipantFactory_delete_participant (part);
fatal ("Unable to create topic description for '%s'!", _tsm_types->name);
}
}
if (test == 3) {
/* interop test */
make_state_reader ();
make_state_writer (key);
} else {
if (reader)
make_state_reader ();
if (writer)
make_state_writer (key);
}
if (reader)
remove_state_reader ();
if (writer)
remove_state_writer ();
if (test == 3) {
remove_state_reader ();
remove_state_writer ();
}
error = DDS_DomainParticipant_delete_topic (part, topic);
if (error)
fatal ("DDS_DomainParticipant_delete_topic () failed (%s)!", DDS_error (error));
free_HelloWorldData_type (part);
error = DDS_DomainParticipant_delete_contained_entities (part);
if (error)
fatal ("DDS_DomainParticipant_delete_contained_entities () failed (%s)!", DDS_error (error));
error = DDS_DomainParticipantFactory_delete_participant (part);
if (error)
fatal ("DDS_DomainParticipantFactory_delete_participant () failed (%s)!", DDS_error (error));
return (0);
}
static void make_state_writer (int id)
{
DDS_DataWriterQos wr_qos;
DDS_ReturnCode_t error;
static DDS_InstanceHandle_t handle;
MsgData_t data;
printf ("Create Writer\r\n");
/* === create publisher === */
pub = DDS_DomainParticipant_create_publisher (part, NULL, NULL, 0);
if (!pub) {
fatal ("DDS_DomainParticipant_create_publisher () failed!");
DDS_DomainParticipantFactory_delete_participant (part);
}
/* Test get_qos() fynctionality. */
if ((error = DDS_Publisher_get_qos (pub, &pqos)) != DDS_RETCODE_OK)
fatal ("DDS_Publisher_get_qos () failed (%s)!", DDS_error (error));
/* Setup writer QoS parameters. */
DDS_Publisher_get_default_datawriter_qos (pub, &wr_qos);
/* === create the writer === */
wr_qos.history.kind = DDS_KEEP_LAST_HISTORY_QOS;
wr_qos.history.depth = 1;
wr_qos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
wr_qos.durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
wr_qos.ownership.kind = DDS_EXCLUSIVE_OWNERSHIP_QOS;
dw = DDS_Publisher_create_datawriter (pub, topic, &wr_qos, NULL, sm);
if (!dw) {
fatal ("Unable to create a writer \r\n");
DDS_DomainParticipantFactory_delete_participant (part);
}
/* === While true, add sample, remove sample === */
if (tsm_type == 0) {
data.key = id;
data.counter = 100 + id;
strcpy (data.message, "Hi folks!");
if (test == 3) {
printf ("Register instance\r\n");
DDS_DataWriter_register_instance (dw, &data);
printf ("Write data\r\n");
if ((error = DDS_DataWriter_write (dw, &data, DDS_HANDLE_NIL)))
printf ("Error writing data\r\n");
sleep (5);
sem_wait (&_sync);
sem_wait (&_sync);
printf ("Unregister instance\r\n");
if ((error = DDS_DataWriter_unregister_instance(dw, &data, DDS_HANDLE_NIL)))
printf ("Error unregistering instance\r\n");
sleep (5);
sem_wait (&_sync);
sem_wait (&_sync);
return;
}
} else {
if (test == 3) {
_types1.i8 = key;
printf ("Register instance\r\n");
DDS_DataWriter_register_instance (dw, &_types1);
printf ("Write data\r\n");
if ((error = DDS_DataWriter_write (dw, &_types1, DDS_HANDLE_NIL)))
printf ("Error writing data\r\n");
sleep (5);
sem_wait (&_sync);
sem_wait (&_sync);
printf ("Unregister instance\r\n");
if ((error = DDS_DataWriter_unregister_instance(dw, &_types1, DDS_HANDLE_NIL)))
printf ("Error unregistering instance\r\n");
sleep (5);
sem_wait (&_sync);
sem_wait (&_sync);
return;
}
}
if (test == 0) {
printf ("Register instance\r\n");
DDS_DataWriter_register_instance (dw, &data);
printf ("Write data\r\n");
if ((error = DDS_DataWriter_write (dw, &data, DDS_HANDLE_NIL)))
printf ("Error writing data\r\n");
sleep (2);
printf ("Unregister instance\r\n");
if ((error = DDS_DataWriter_unregister_instance(dw, &data, DDS_HANDLE_NIL)))
printf ("Error unregistering instance\r\n");
sleep (2);
if (reader)
remove_state_reader ();
} else if (test >= 1) {
while (1) {
printf ("Register instance\r\n");
handle = DDS_DataWriter_register_instance (dw, &data);
printf ("Write data\r\n");
if ((error = DDS_DataWriter_write (dw, &data, handle)))
printf ("Error writing data\r\n");
sleep (1);
if (test == 1) {
printf ("Unregister instance\r\n");
if ((error = DDS_DataWriter_unregister_instance(dw, &data, handle)))
printf ("Error unregistering instance\r\n");
} else {
printf ("Dispose instance\r\n");
if ((error = DDS_DataWriter_dispose (dw, &data, handle)))
printf ("Error disposing instance\r\n");
}
sleep (1);
}
}
}
int main (int argc, const char *argv [])
{
DDS_DomainParticipant part;
unsigned sw, sr, dw, dr, i;
int error;
DDS_entity_name ("Technicolor Interop test");
printf ("Test 1: dynamic type first, then static.\r\n");
printf ("----------------------------------------\r\n");
/* Create a domain participant. */
part = DDS_DomainParticipantFactory_create_participant (
domain_id, NULL, NULL, 0);
if (!part)
fatal ("DDS_DomainParticipantFactory_create_participant () failed!");
printf ("DDS Domain Participant created.\r\n");
/* Register the dynamic shape topic type. */
register_dynamic_type (part);
printf ("DDS dynamic Topic type registered.\r\n");
/* Register the static topic type. */
register_static_type (part);
printf ("DDS static Topic type registered.\r\n");
dw = dynamic_writer_create ("Square");
dr = dynamic_reader_create ("Square");
sw = static_writer_create ("Square");
sr = static_reader_create ("Square");
for (i = 0; i < 10; i++) {
dynamic_writer_write (dw, "Red", i * 9 + 5, i * 11 + 5);
static_writer_write (sw, "Yellow", i * 10, i * 12);
usleep (100000);
}
dynamic_writer_delete (dw);
dynamic_reader_delete (dr);
static_writer_delete (sw);
static_reader_delete (sr);
unregister_dynamic_type ();
unregister_static_type ();
error = DDS_DomainParticipant_delete_contained_entities (part);
if (error)
fatal ("DDS_DomainParticipant_delete_contained_entities () failed (%s)!", DDS_error (error));
printf ("DDS Entities deleted\r\n");
error = DDS_DomainParticipantFactory_delete_participant (part);
if (error)
fatal ("DDS_DomainParticipantFactory_delete_participant () failed (%s)!", DDS_error (error));
printf ("DDS Participant deleted\r\n\r\n");
printf ("Test 2: static type first, then dynamic.\r\n");
printf ("----------------------------------------\r\n");
/* Create a domain participant. */
part = DDS_DomainParticipantFactory_create_participant (
domain_id, NULL, NULL, 0);
if (!part)
fatal ("DDS_DomainParticipantFactory_create_participant () failed!");
printf ("DDS Domain Participant created.\r\n");
/* Register the static topic type. */
register_static_type (part);
printf ("DDS static Topic type registered.\r\n");
/* Register the dynamic shape topic type. */
register_dynamic_type (part);
printf ("DDS dynamic Topic type registered.\r\n");
sw = static_writer_create ("Circle");
sr = static_reader_create ("Circle");
dw = dynamic_writer_create ("Circle");
dr = dynamic_reader_create ("Circle");
for (i = 0; i < 10; i++) {
static_writer_write (sw, "Green", i * 11, i * 13);
dynamic_writer_write (dw, "Magenta", i * 10 + 5, i * 12 + 5);
usleep (100000);
}
static_writer_delete (sw);
static_reader_delete (sr);
dynamic_writer_delete (dw);
dynamic_reader_delete (dr);
unregister_static_type ();
unregister_dynamic_type ();
error = DDS_DomainParticipant_delete_contained_entities (part);
if (error)
fatal ("DDS_DomainParticipant_delete_contained_entities () failed (%s)!", DDS_error (error));
printf ("DDS Entities deleted\r\n");
error = DDS_DomainParticipantFactory_delete_participant (part);
if (error)
fatal ("DDS_DomainParticipantFactory_delete_participant () failed (%s)!", DDS_error (error));
printf ("DDS Participant deleted\r\n\r\n");
return (0);
}
void do_chat (DDS_DataWriter dw)
{
ChatMsg_t m;
DDS_InstanceHandle_t h;
char buf [256];
#if !defined (NUTTX_RTOS)
tty_init ();
#endif
DDS_Handle_attach (tty_stdin,
POLLIN | POLLPRI | POLLERR | POLLHUP | POLLNVAL,
tty_input,
NULL);
printf ("Welcome to the ROS 2.0 DDS chatroom.\r\n");
printf ("Anything you type will be sent to all chatroom attendees.\r\n");
printf (" (Please write messages of less than 255 characters).\r\n");
printf ("Type '!help' for chatroom options.\r\n");
m.chatroom = chatroom;
m.from = user_name;
h = 0;
while (!aborting) {
#if defined (NUTTX_RTOS)
/* Take into account that fgets reads the "\n" character at the end
of each line. Code should consider this aspect in every case */
fgets(buf, 256, stdin);
#else
tty_gets (sizeof (buf), buf, 0, 1);
#endif
if (buf [0] == '!') {
#if defined (NUTTX_RTOS)
if (!strcmp (buf + 1, "quit\n") ||
(buf [1] == 'q' && buf [2] == '\n')) {
#else
if (!strcmp (buf + 1, "quit") ||
(buf [1] == 'q' && buf [2] == '\0')) {
#endif
aborting = 1;
break;
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "list\n"))
#else
else if (!strcmp (buf + 1, "list"))
#endif
printf ("Attendees:\r\n\t%s\r\n", user_name);
#if defined (NUTTX_RTOS)
else if (!memcmp (buf + 1, "user\n", 4)) {
#else
else if (!memcmp (buf + 1, "user", 4)) {
#endif
if (h) {
ChatMsg_signal (dw, h, 1);
h = 0;
}
strcpy (user_name, buf + 6);
printf ("You are now: %s\r\n", user_name);
}
#if defined (NUTTX_RTOS)
else if (!memcmp (buf + 1, "room\n", 4)) {
#else
else if (!memcmp (buf + 1, "room", 4)) {
#endif
if (h) {
ChatMsg_signal (dw, h, 1);
h = 0;
}
strcpy (chatroom, buf + 6);
printf ("Switched to chatroom: %s\r\n", chatroom);
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "info\n"))
#else
else if (!strcmp (buf + 1, "info"))
#endif
printf ("Chatroom: %s, Username: %s\r\n",
chatroom, user_name);
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "busy\n"))
#else
else if (!strcmp (buf + 1, "busy"))
#endif
ChatMsg_signal (dw, h, 0);
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "away\n")) {
#else
else if (!strcmp (buf + 1, "away")) {
#endif
if (h) {
ChatMsg_signal (dw, h, 1);
h = 0;
}
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "help\n") ||
(buf [1] == 'h' && buf [2] == '\n\0') ||
(buf [1] == '?' && buf [2] == '\n\0')) {
#else
else if (!strcmp (buf + 1, "help") ||
(buf [1] == 'h' && buf [2] == '\0') ||
(buf [1] == '?' && buf [2] == '\0')) {
#endif
printf ("Commands:\r\n");
printf (" !room <room_name> -> set the chatroom name.\r\n");
printf (" !user <user_name> -> set the user name.\r\n");
printf (" !list -> list the attendees.\r\n");
printf (" !info -> show chatroom and user.\r\n");
printf (" !busy -> momentarily not involved.\r\n");
printf (" !away -> gone away.\r\n");
printf (" !help or !h or !? -> Show this info.\r\n");
printf (" !quit or !q -> Quit the chatroom.\r\n");
printf (" !!<command> -> DDS debug command.\r\n");
/* printf (" !$<command> -> Shell command.\r\n"); */
}
else if (buf [1] == '!')
DDS_Debug_command (buf + 2);
/* else if (buf [1] == '$')
system (buf + 2); */
else
printf ("?%s\r\n", buf + 1);
continue;
}
if (!h)
h = ChatMsg_register (dw, &m);
m.message = buf;
ChatMsg_write (dw, &m, h);
}
}
void read_msg (DDS_DataReaderListener *l, DDS_DataReader dr)
{
ChatMsg_t msg;
DDS_InstanceStateKind kind;
int valid;
DDS_ReturnCode_t ret;
ARG_NOT_USED (l)
memset (&msg, 0, sizeof (msg));
ret = ChatMsg_read_or_take (dr, &msg, DDS_NOT_READ_SAMPLE_STATE,
DDS_ANY_VIEW_STATE,
DDS_ANY_INSTANCE_STATE, 1,
&valid, &kind);
if (ret == DDS_RETCODE_OK)
do {
#ifndef DISPLAY_SELF
if (!strcmp (msg.from, user_name) &&
!strcmp (msg.chatroom, chatroom))
break;
#endif
if (valid)
printf ("%s: %s\r\n", msg.from, msg.message);
else if (kind == DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE)
printf ("%s is busy!\r\n", msg.from);
else if (kind == DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE)
printf ("%s has left!\r\n", msg.from);
}
while (0);
ChatMsg_cleanup (&msg);
}
#ifdef WAITSETS
static void *chat_reader (void *args)
{
DDS_DynamicDataReader dr;
DDS_WaitSet ws;
DDS_SampleStateMask ss = DDS_NOT_READ_SAMPLE_STATE;
DDS_ViewStateMask vs = DDS_ANY_VIEW_STATE;
DDS_InstanceStateMask is = DDS_ANY_INSTANCE_STATE;
DDS_ReadCondition rc;
DDS_ConditionSeq conds = DDS_SEQ_INITIALIZER (DDS_Condition);
DDS_Duration_t to;
DDS_ReturnCode_t ret;
dr = args;
ws = DDS_WaitSet__alloc ();
if (!ws)
fatal ("Unable to allocate a WaitSet!");
if (verbose)
printf ("DDS Waitset allocated.\r\n");
rc = DDS_DataReader_create_readcondition (dr, ss, vs, is);
if (!rc)
fatal ("DDS_DataReader_create_readcondition () returned an error!");
if (verbose)
printf ("DDS Readcondition created.\r\n");
ret = DDS_WaitSet_attach_condition (ws, rc);
if (ret)
fatal ("Unable to attach condition to a WaitSet!");
while (!aborting) {
to.sec = 0;
to.nanosec = 200000000; /* Timeout after 200ms. */
ret = DDS_WaitSet_wait (ws, &conds, &to);
if (ret == DDS_RETCODE_TIMEOUT)
continue;
read_msg (NULL, dr);
}
ret = DDS_WaitSet_detach_condition (ws, rc);
if (ret)
fatal ("Unable to detach condition from WaitSet (%s)!", DDS_error (ret));
DDS_WaitSet__free (ws);
return (NULL);
}
static void start_chat_reader (DDS_DynamicDataReader dr)
{
thread_create (rt, chat_reader, dr);
}
static void stop_chat_reader (DDS_DynamicDataReader dr)
{
ARG_NOT_USED (dr)
thread_wait (rt, NULL);
}
#else
static DDS_DataReaderListener msg_listener = {
NULL, /* Sample rejected. */
NULL, /* Liveliness changed. */
NULL, /* Requested Deadline missed. */
NULL, /* Requested incompatible QoS. */
read_msg, /* Data available. */
NULL, /* Subscription matched. */
NULL, /* Sample lost. */
NULL /* Cookie */
};
#endif
#ifdef DDS_SECURITY
#define fail_unless assert
static void enable_security (void)
{
DDS_Credentials credentials;
DDS_ReturnCode_t error;
#ifdef MSECPLUG_WITH_SECXML
/*int dhandle, thandle;*/
#endif
error = DDS_SP_set_policy ();
if (error)
fatal ("DDS_SP_set_policy() returned error (%s)!", DDS_error (error));
#ifdef MSECPLUG_WITH_SECXML
if (DDS_SP_parse_xml ("security.xml"))
fatal ("SP: no DDS security rules in 'security.xml'!\r\n");
#else
DDS_SP_add_domain();
if (!realm_name)
DDS_SP_add_participant ();
else
DDS_SP_set_participant_access (DDS_SP_add_participant (), strcat(realm_name, "*"), 2, 0);
#endif
if (!cert_path || !key_path)
fatal ("Error: you must provide a valid certificate path and a valid private key path\r\n");
if (engine_id) {
DDS_SP_init_engine (engine_id, init_engine_fs);
credentials.credentialKind = DDS_ENGINE_BASED;
credentials.info.engine.engine_id = engine_id;
credentials.info.engine.cert_id = cert_path;
credentials.info.engine.priv_key_id = key_path;
}
else {
credentials.credentialKind = DDS_FILE_BASED;
credentials.info.filenames.private_key_file = key_path;
credentials.info.filenames.certificate_chain_file = cert_path;
}
error = DDS_Security_set_credentials ("Technicolor Chatroom", &credentials);
}
int main(int argc, char* argv[])
{
int startTime = mstime();
DDS_PoolConstraints reqs;
DDS_DomainParticipant domainParticipant;
DDS_Publisher publisher;
DDS_Subscriber subscriber;
DDS_ReturnCode_t retCode;
char *configTopicName[nofConfigTopics];
DDS_Topic configTopic[nofConfigTopics];
DDS_Topic stateTopic[nofStateTopics];
DDS_Topic statisticTopic[nofStatisticTopics];
DDS_DataWriterQos dataWriterQos;
DDS_DataWriter configDataWriter[nofConfigTopics];
DDS_DataWriter stateDataWriter[nofStateTopics];
DDS_DataWriter statisticDataWriter[nofStatisticTopics];
DDS_DataReaderQos dataReaderQos;
DDS_DataReader configDataReader[nofConfigTopics];
acceptance_high_end_Config config;
acceptance_high_end_State state;
acceptance_high_end_Statistic statistic;
DDS_InstanceHandle_t configInstance[nofConfigTopics][nofConfigInstances];
DDS_InstanceHandle_t stateInstance[nofStateTopics][nofStateInstances];
DDS_InstanceHandle_t statisticInstance[nofStatisticTopics][nofStatisticInstances];
int i = 0;
int j = 0;
int firstConfigTopic = (argc > 1) ? atoi(argv[1]) : 0;
int firstStateTopic = (argc > 2) ? atoi(argv[2]) : 0;
int firstStatisticTopic = (argc > 3) ? atoi(argv[3]) : 0;
int stagedLoading = (argc > 4) ? atoi(argv[4]) : 0;
printf("[0] Config topics start at %d...\r\n", firstConfigTopic);
printf("[0] State topics start at %d...\r\n", firstStateTopic);
printf("[0] Statistic topics start at %d...\r\n", firstStatisticTopic);
printf("[0] Staged loading %s.\r\n", stagedLoading ? "on" : "off");
if (stagedLoading) {
char file_name[24];
sprintf(file_name, "/tmp/acceptStageLoad%d", firstConfigTopic);
struct stat buf;
if (stat(file_name, &buf)) {
printf ("[%d] Waiting for %s\r\n", mstime() - startTime, file_name);
do {
sleep(1);
}
while (stat(file_name, &buf));
printf ("[%d] Got %s!\r\n", mstime() - startTime, file_name);
}
}
DDS_program_name (&argc, argv);
DDS_entity_name ("Technicolor Limits Component");
DDS_get_default_pool_constraints(&reqs, ~0, 100);
#ifdef MINIMAL_POOLS
configure_minimal_pool_constraints (&reqs);
#else
configure_optimal_pool_constraints (&reqs);
#endif
DDS_set_pool_constraints(&reqs);
#ifdef DDS_DEBUG
DDS_Debug_start ();
#endif
/* Create domain participant... */
domainParticipant = DDS_DomainParticipantFactory_create_participant(0, NULL, NULL, 0);
if (!domainParticipant) {
fprintf(stderr, "Error creating domain participant.\r\n");
return -1;
}
printf("[%d] Created domain participant.\r\n", mstime() - startTime);
sleep(1);
/* Create publisher... */
publisher = DDS_DomainParticipant_create_publisher(domainParticipant, NULL, NULL, 0);
if (!publisher) {
fprintf(stderr, "Error creating publisher.\r\n");
return -1;
}
printf("[%d] Created publisher.\r\n", mstime() - startTime);
sleep(1);
/* Create subscriber... */
subscriber = DDS_DomainParticipant_create_subscriber(domainParticipant, NULL, NULL, 0);
if (!subscriber) {
fprintf(stderr, "Error creating subscriber.\r\n");
return -1;
}
printf("[%d] Created subscriber.\r\n", mstime() - startTime);
sleep(1);
/* Register types... */
retCode = acceptance_high_end_ConfigTypeSupport_register_type(domainParticipant, NULL);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error registering type (%s).\r\n", DDS_error(retCode));
return -1;
}
printf("[%d] Registered config type.\r\n", mstime() - startTime);
retCode = acceptance_high_end_StateTypeSupport_register_type(domainParticipant, NULL);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error registering type (%s).\r\n", DDS_error(retCode));
return -1;
}
printf("[%d] Registered state type.\r\n", mstime() - startTime);
retCode = acceptance_high_end_StatisticTypeSupport_register_type(domainParticipant, NULL);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error registering type (%s).\r\n", DDS_error(retCode));
return -1;
}
printf("[%d] Registered statistic type.\r\n", mstime() - startTime);
sleep(1);
/* Create topics... */
for (i = 0; i < nofConfigTopics; i++) {
char topicName[32];
sprintf(topicName, "ConfigTopic%d", firstConfigTopic + i);
configTopicName[i] = strdup (topicName);
configTopic[i] = DDS_DomainParticipant_create_topic(domainParticipant, topicName, acceptance_high_end_ConfigTypeSupport_get_type_name(), NULL, NULL, 0);
if (!configTopic[i]) {
fprintf(stderr, "Error creating topic.\r\n");
return -1;
}
}
printf("[%d] Created %d config topics.\r\n", mstime() - startTime, nofConfigTopics);
for (i = 0; i < nofStateTopics; i++) {
char topicName[32];
sprintf(topicName, "StateTopic%d", firstStateTopic + i);
stateTopic[i] = DDS_DomainParticipant_create_topic(domainParticipant, topicName, acceptance_high_end_StateTypeSupport_get_type_name(), NULL, NULL, 0);
if (!stateTopic[i]) {
fprintf(stderr, "Error creating topic.\r\n");
return -1;
}
}
printf("[%d] Created %d state topics.\r\n", mstime() - startTime, nofStateTopics);
for (i = 0; i < nofStatisticTopics; i++) {
char topicName[32];
sprintf(topicName, "StatisticTopic%d", firstStatisticTopic + i);
statisticTopic[i] = DDS_DomainParticipant_create_topic(domainParticipant, topicName, acceptance_high_end_StatisticTypeSupport_get_type_name(), NULL, NULL, 0);
if (!statisticTopic[i]) {
fprintf(stderr, "Error creating topic.\r\n");
return -1;
}
}
printf("[%d] Created %d statistic topics.\r\n", mstime() - startTime, nofStatisticTopics);
sleep(1);
/* Create data writers... */
DDS_Publisher_get_default_datawriter_qos(publisher, &dataWriterQos);
dataWriterQos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
dataWriterQos.durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
for (i = 0; i < nofConfigTopics; i++) {
configDataWriter[i] = DDS_Publisher_create_datawriter(publisher, configTopic[i], &dataWriterQos, NULL, 0);
if (!configDataWriter[i]) {
fprintf(stderr, "Error creating data writer.\r\n");
return -1;
}
}
printf("[%d] Created %d config data writers.\r\n", mstime() - startTime, nofConfigTopics);
DDS_Publisher_get_default_datawriter_qos(publisher, &dataWriterQos);
dataWriterQos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
dataWriterQos.durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
for (i = 0; i < nofStateTopics; i++) {
stateDataWriter[i] = DDS_Publisher_create_datawriter(publisher, stateTopic[i], &dataWriterQos, NULL, 0);
if (!stateDataWriter[i]) {
fprintf(stderr, "Error creating data writer.\r\n");
return -1;
}
}
printf("[%d] Created %d state data writers.\r\n", mstime() - startTime, nofStateTopics);
DDS_Publisher_get_default_datawriter_qos(publisher, &dataWriterQos);
for (i = 0; i < nofStatisticTopics; i++) {
statisticDataWriter[i] = DDS_Publisher_create_datawriter(publisher, statisticTopic[i], &dataWriterQos, NULL, 0);
if (!statisticDataWriter[i]) {
fprintf(stderr, "Error creating data writer.\r\n");
return -1;
}
}
printf("[%d] Created %d statistic data writers.\r\n", mstime() - startTime, nofStatisticTopics);
sleep(1);
/* Create data readers... */
DDS_Subscriber_get_default_datareader_qos(subscriber, &dataReaderQos);
dataReaderQos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
dataReaderQos.durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
for (i = 0; i < nofConfigTopics; i++) {
configDataReader[i] = DDS_Subscriber_create_datareader(subscriber, DDS_DomainParticipant_lookup_topicdescription(domainParticipant, configTopicName[i]), &dataReaderQos, NULL, 0);
if (!configDataReader[i]) {
fprintf(stderr, "Error creating data reader.\r\n");
return -1;
}
}
printf("[%d] Created %d config data readers.\r\n", mstime() - startTime, nofConfigTopics);
sleep(1);
/* Register instances... */
for (i = 0; i < nofConfigTopics; i++) {
for (j = 0; j < nofConfigInstances; j++) {
config.key = j;
configInstance[i][j] = DDS_DataWriter_register_instance(configDataWriter[i], &config);
if (!configInstance[i][j]) {
fprintf(stderr, "Error registering instance.\r\n");
break;
}
}
}
printf("[%d] Registered %d config instances.\r\n", mstime() - startTime, nofConfigTopics * nofConfigInstances);
for (i = 0; i < nofStateTopics; i++) {
for (j = 0; j < nofStateInstances; j++) {
state.key = j;
stateInstance[i][j] = DDS_DataWriter_register_instance(stateDataWriter[i], &state);
if (!stateInstance[i][j]) {
fprintf(stderr, "Error registering instance.\r\n");
break;
}
}
}
printf("[%d] Registered %d state instances.\r\n", mstime() - startTime, nofStatisticTopics * nofStatisticInstances);
for (i = 0; i < nofStatisticTopics; i++) {
for (j = 0; j < nofStatisticInstances; j++) {
statistic.key = j;
statisticInstance[i][j] = DDS_DataWriter_register_instance(statisticDataWriter[i], &statistic);
if (!statisticInstance[i][j]) {
fprintf(stderr, "Error registering instance.\r\n");
break;
}
}
}
printf("[%d] Registered %d statistic instances.\r\n", mstime() - startTime, nofStatisticTopics * nofStatisticInstances);
sleep(1);
/* Publish samples... */
for (i = 0; i < nofConfigTopics; i++) {
for (j = 0; j < nofConfigInstances; j++) {
config.key = j;
retCode = DDS_DataWriter_write(configDataWriter[i], &config, configInstance[i][j]);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error publishing sample (%s).\r\n", DDS_error(retCode));
break;
}
}
}
printf("[%d] Published %d config samples.\r\n", mstime() - startTime, nofConfigTopics * nofConfigInstances);
for (i = 0; i < nofStateTopics; i++) {
for (j = 0; j < nofStateInstances; j++) {
state.key = j;
retCode = DDS_DataWriter_write(stateDataWriter[i], &state, stateInstance[i][j]);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error publishing sample (%s).\r\n", DDS_error(retCode));
break;
}
}
}
printf("[%d] Published %d state samples.\r\n", mstime() - startTime, nofStateTopics * nofStateInstances);
for (i = 0; i < nofStatisticTopics; i++) {
for (j = 0; j < nofStatisticInstances; j++) {
statistic.key = j;
retCode = DDS_DataWriter_write(statisticDataWriter[i], &statistic, statisticInstance[i][j]);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error publishing sample (%s).\r\n", DDS_error(retCode));
break;
}
}
}
printf("[%d] Published %d statistic samples.\r\n", mstime() - startTime, nofStatisticTopics * nofStatisticInstances);
sleep(1);
if (stagedLoading) {
char file_name[24];
sprintf(file_name, "/tmp/acceptStageLoad%d", firstConfigTopic + nofConfigTopics);
FILE *f = fopen(file_name, "w");
fclose(f);
}
// dbg_printf ("Zzzzzzz ... \r\n");
sleep(TEST_TIME);
// dbg_printf ("... o?\r\n");
/* Delete contained entities... */
// printf (" -- deleting contained entities.\r\n");
retCode = DDS_DomainParticipant_delete_contained_entities(domainParticipant);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error deleting contained entities.\r\n");
return -1;
}
for (i = 0; i < nofConfigTopics; i++)
free (configTopicName[i]);
// printf (" -- contained entities deleted!\r\n");
// sleep(TEST_TIME);
/* Delete domain participants... */
retCode = DDS_DomainParticipantFactory_delete_participant(domainParticipant);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error deleting domain participant.\r\n");
return -1;
}
printf ("[%d] comp completed successfully.\r\n", mstime() - startTime);
return 0;
}
int main(int argc, char* argv[])
{
int startTime = mstime();
DDS_PoolConstraints reqs;
DDS_DomainParticipant domainParticipant;
DDS_Publisher publisher;
DDS_Subscriber subscriber;
DDS_ReturnCode_t retCode;
char *configTopicName[nofConfigTopics];
char *stateTopicName[nofStateTopics];
char *statisticTopicName[nofStatisticTopics];
DDS_Topic configTopic[nofConfigTopics];
DDS_Topic stateTopic[nofStateTopics];
DDS_Topic statisticTopic[nofStatisticTopics];
DDS_DataWriterQos dataWriterQos;
DDS_DataWriter configDataWriter[nofConfigTopics];
DDS_DataReaderQos dataReaderQos;
DDS_DataReader configDataReader[nofConfigTopics];
DDS_DataReader stateDataReader[nofStateTopics];
DDS_DataReader statisticDataReader[nofStatisticTopics];
#if 0
acceptance_high_end_Config config;
acceptance_high_end_State state;
acceptance_high_end_Statistic statistic;
DDS_InstanceHandle_t configInstance[nofConfigTopics][nofConfigInstances];
DDS_InstanceHandle_t stateInstance[nofStateTopics][nofStateInstances];
DDS_InstanceHandle_t statisticInstance[nofStatisticTopics][nofStatisticInstances];
#endif
int i = 0;
/*int j = 0;*/
int firstConfigTopic = (argc > 1) ? atoi(argv[1]) : 0;
int firstStateTopic = (argc > 2) ? atoi(argv[2]) : 0;
int firstStatisticTopic = (argc > 3) ? atoi(argv[3]) : 0;
int stagedLoading = (argc > 4) ? atoi(argv[4]) : 0;
printf("[0] Config topics start at %d...\r\n", firstConfigTopic);
printf("[0] State topics start at %d...\r\n", firstStateTopic);
printf("[0] Statistic topics start at %d...\r\n", firstStatisticTopic);
printf("[0] Staged loading %s.\r\n", stagedLoading ? "on" : "off");
DDS_program_name (&argc, argv);
DDS_entity_name ("Technicolor Limits Manager");
DDS_get_default_pool_constraints(&reqs, ~0, 100);
#ifdef MINIMAL_POOLS
configure_minimal_pool_constraints (&reqs);
#else
configure_optimal_pool_constraints (&reqs);
#endif
DDS_set_pool_constraints (&reqs);
#ifdef DDS_DEBUG
DDS_Debug_start ();
#endif
#ifdef TRACE_DATA
rtps_dtrace_set (DRTRC_TRACE_ALL);
#endif
/* Create domain participant... */
domainParticipant = DDS_DomainParticipantFactory_create_participant(0, NULL, NULL, 0);
if (!domainParticipant) {
fprintf(stderr, "Error creating domain participant.\r\n");
return -1;
}
printf("[%d] Created domain participant.\r\n", mstime() - startTime);
// sleep(1);
/* Create publisher... */
publisher = DDS_DomainParticipant_create_publisher(domainParticipant, NULL, NULL, 0);
if (!publisher) {
fprintf(stderr, "Error creating publisher.\r\n");
return -1;
}
printf("[%d] Created publisher.\r\n", mstime() - startTime);
// sleep(1);
/* Create subscriber... */
subscriber = DDS_DomainParticipant_create_subscriber(domainParticipant, NULL, NULL, 0);
if (!subscriber) {
fprintf(stderr, "Error creating subscriber.\r\n");
return -1;
}
printf("[%d] Created subscriber.\r\n", mstime() - startTime);
// sleep(1);
/* Register types... */
retCode = acceptance_high_end_ConfigTypeSupport_register_type(domainParticipant, NULL);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error registering type (%s).\r\n", DDS_error(retCode));
return -1;
}
printf("[%d] Registered config type.\r\n", mstime() - startTime);
retCode = acceptance_high_end_StateTypeSupport_register_type(domainParticipant, NULL);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error registering type (%s).\r\n", DDS_error(retCode));
return -1;
}
printf("[%d] Registered state type.\r\n", mstime() - startTime);
retCode = acceptance_high_end_StatisticTypeSupport_register_type(domainParticipant, NULL);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error registering type (%s).\r\n", DDS_error(retCode));
return -1;
}
printf("[%d] Registered statistic type.\r\n", mstime() - startTime);
// sleep(1);
/* Create topics... */
for (i = 0; i < nofConfigTopics; i++) {
char topicName[32];
sprintf(topicName, "ConfigTopic%d", firstConfigTopic + i);
configTopicName[i] = topicName;
configTopic[i] = DDS_DomainParticipant_create_topic(domainParticipant, topicName, acceptance_high_end_ConfigTypeSupport_get_type_name(), NULL, NULL, 0);
if (!configTopic[i]) {
fprintf(stderr, "Error creating topic.\r\n");
return -1;
}
}
printf("[%d] Created %d config topics.\r\n", mstime() - startTime, nofConfigTopics);
for (i = 0; i < nofStateTopics; i++) {
char topicName[32];
sprintf(topicName, "StateTopic%d", firstStateTopic + i);
stateTopicName[i] = topicName;
stateTopic[i] = DDS_DomainParticipant_create_topic(domainParticipant, topicName, acceptance_high_end_StateTypeSupport_get_type_name(), NULL, NULL, 0);
if (!stateTopic[i]) {
fprintf(stderr, "Error creating topic.\r\n");
return -1;
}
}
printf("[%d] Created %d state topics.\r\n", mstime() - startTime, nofStateTopics);
for (i = 0; i < nofStatisticTopics; i++) {
char topicName[32];
sprintf(topicName, "StatisticTopic%d", firstStatisticTopic + i);
statisticTopicName[i] = topicName;
statisticTopic[i] = DDS_DomainParticipant_create_topic(domainParticipant, topicName, acceptance_high_end_StatisticTypeSupport_get_type_name(), NULL, NULL, 0);
if (!statisticTopic[i]) {
fprintf(stderr, "Error creating topic.\r\n");
return -1;
}
}
printf("[%d] Created %d statistic topics.\r\n", mstime() - startTime, nofStatisticTopics);
// sleep(1);
/* Create data writers... */
DDS_Publisher_get_default_datawriter_qos(publisher, &dataWriterQos);
dataWriterQos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
dataWriterQos.durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
for (i = 0; i < nofConfigTopics; i++) {
configDataWriter[i] = DDS_Publisher_create_datawriter(publisher, configTopic[i], &dataWriterQos, NULL, 0);
if (!configDataWriter[i]) {
fprintf(stderr, "Error creating data writer.\r\n");
return -1;
}
}
printf("[%d] Created %d config data writers.\r\n", mstime() - startTime, nofConfigTopics);
// sleep(1);
/* Create data readers... */
DDS_Subscriber_get_default_datareader_qos(subscriber, &dataReaderQos);
dataReaderQos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
dataReaderQos.durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
for (i = 0; i < nofConfigTopics; i++) {
configDataReader[i] = DDS_Subscriber_create_datareader(subscriber, DDS_DomainParticipant_lookup_topicdescription(domainParticipant, configTopicName[i]), &dataReaderQos, &configDataReaderListener, DDS_DATA_AVAILABLE_STATUS);
if (!configDataReader[i]) {
fprintf(stderr, "Error creating data reader.\r\n");
return -1;
}
}
printf("[%d] Created %d config data readers.\r\n", mstime() - startTime, nofConfigTopics);
DDS_Subscriber_get_default_datareader_qos(subscriber, &dataReaderQos);
dataReaderQos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
dataReaderQos.durability.kind = DDS_TRANSIENT_LOCAL_DURABILITY_QOS;
for (i = 0; i < nofStateTopics; i++) {
stateDataReader[i] = DDS_Subscriber_create_datareader(subscriber, DDS_DomainParticipant_lookup_topicdescription(domainParticipant, stateTopicName[i]), &dataReaderQos, &stateDataReaderListener, DDS_DATA_AVAILABLE_STATUS);
if (!stateDataReader[i]) {
fprintf(stderr, "Error creating data reader.\r\n");
return -1;
}
}
printf("[%d] Created %d state data readers.\r\n", mstime() - startTime, nofStateTopics);
DDS_Subscriber_get_default_datareader_qos(subscriber, &dataReaderQos);
for (i = 0; i < nofStatisticTopics; i++) {
statisticDataReader[i] = DDS_Subscriber_create_datareader(subscriber, DDS_DomainParticipant_lookup_topicdescription(domainParticipant, statisticTopicName[i]), &dataReaderQos, &statisticDataReaderListener, DDS_DATA_AVAILABLE_STATUS);
if (!statisticDataReader[i]) {
fprintf(stderr, "Error creating data reader.\r\n");
return -1;
}
}
printf("[%d] Created %d statistic data readers.\r\n", mstime() - startTime, nofStatisticTopics);
// sleep(1);
/* Lookup instances... */
/*for (i = 0; i < nofConfigTopics; i++) {
for (j = 0; j < nofConfigInstances; j++) {
config.key = j;
configInstance[i][j] = acceptance_high_end_ConfigDataReader_lookup_instance(configDataReader[i], &config);
if (!configInstance[i][j]) {
fprintf(stderr, "Error looking up instance.\r\n");
break;
}
}
}
printf("[%d] Looked up %d config instances.\r\n", mstime() - startTime, nofConfigTopics * nofConfigInstances);
for (i = 0; i < nofStateTopics; i++) {
for (j = 0; j < nofStateInstances; j++) {
state.key = j;
stateInstance[i][j] = acceptance_high_end_StateDataReader_lookup_instance(stateDataReader[i], &state);
if (!stateInstance[i][j]) {
fprintf(stderr, "Error looking up instance.\r\n");
break;
}
}
}
printf("[%d] Looked up %d state instances.\r\n", mstime() - startTime, nofStatisticTopics * nofStatisticInstances);
for (i = 0; i < nofStatisticTopics; i++) {
for (j = 0; j < nofStatisticInstances; j++) {
statistic.key = j;
statisticInstance[i][j] = acceptance_high_end_StatisticDataReader_lookup_instance(statisticDataReader[i], &statistic);
if (!statisticInstance[i][j]) {
fprintf(stderr, "Error looking up instance.\r\n");
break;
}
}
}
printf("[%d] Looked up %d statistic instances.\r\n", mstime() - startTime, nofStatisticTopics * nofStatisticInstances);*/
// sleep(1);
if (stagedLoading) {
char file_name[24];
sprintf(file_name, "/tmp/acceptStageLoad0");
FILE *f = fopen(file_name, "w");
fclose(f);
}
int waitTime = mstime();
int _nofConfigSamples = 0;
int _nofStateSamples = 0;
int _nofStatisticSamples = 0;
while ((mstime() - waitTime) < (TEST_TIME * 1000)) {
if ((nofConfigSamples > _nofConfigSamples) || (nofStateSamples > _nofStateSamples) || (nofStatisticSamples > _nofStatisticSamples)) {
printf("[%d] Received %d config samples.\r\n", mstime() - startTime, nofConfigSamples);
printf("[%d] Received %d state samples.\r\n", mstime() - startTime, nofStateSamples);
printf("[%d] Received %d statistic samples.\r\n", mstime() - startTime, nofStatisticSamples);
_nofConfigSamples = nofConfigSamples;
_nofStateSamples = nofStateSamples;
_nofStatisticSamples = nofStatisticSamples;
}
sleep(1);
}
/* Delete contained entities... */
// printf (" -- Deleting contained entities.\r\n");
retCode = DDS_DomainParticipant_delete_contained_entities(domainParticipant);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error deleting contained entities.\r\n");
return -1;
}
// printf ("contained entities deleted!\r\n");
/* Delete domain participants... */
retCode = DDS_DomainParticipantFactory_delete_participant(domainParticipant);
if (retCode != DDS_RETCODE_OK) {
fprintf(stderr, "Error deleting domain participant.\r\n");
return -1;
}
printf ("[%d] mgmt completed successfully.\r\n", mstime() - startTime);
return 0;
}
