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);
}
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)
#endif
{
/* Generic DDS entities */
DDS_DomainParticipant participant;
DDS_Topic chatMessageTopic;
DDS_Topic nameServiceTopic;
DDS_Subscriber chatSubscriber;
DDS_QueryCondition singleUser;
DDS_ReadCondition newUser;
DDS_StatusCondition leftUser;
DDS_GuardCondition guard;
DDS_WaitSet userLoadWS;
DDS_LivelinessChangedStatus livChangStatus;
/* QosPolicy holders */
DDS_TopicQos *setting_topic_qos;
DDS_TopicQos *reliable_topic_qos;
DDS_SubscriberQos *sub_qos;
DDS_DataReaderQos *message_qos;
/* DDS Identifiers */
DDS_DomainId_t domain = NULL;
DDS_ReturnCode_t status;
DDS_ConditionSeq *guardList = NULL;
DDS_Duration_t timeout = DDS_DURATION_INFINITE;
/* Type-specific DDS entities */
Chat_ChatMessageTypeSupport chatMessageTS;
Chat_NameServiceTypeSupport nameServiceTS;
Chat_NameServiceDataReader nameServer;
Chat_ChatMessageDataReader loadAdmin;
DDS_sequence_Chat_ChatMessage msgList = { 0, 0, DDS_OBJECT_NIL, FALSE };
DDS_sequence_Chat_NameService nsList = { 0, 0, DDS_OBJECT_NIL, FALSE };
DDS_SampleInfoSeq infoSeq = { 0, 0, DDS_OBJECT_NIL, FALSE };
DDS_SampleInfoSeq infoSeq2 = { 0, 0, DDS_OBJECT_NIL, FALSE };
/* Others */
DDS_StringSeq args;
int closed = 0;
DDS_unsigned_long i, j;
DDS_long prevCount = 0;
char *partitionName;
char *chatMessageTypeName = NULL;
char *nameServiceTypeName = NULL;
pthread_t tid;
pthread_attr_t tattr;
printf("Starting UserLoad example.\n");
fflush(stdout);
/* Create a DomainParticipant (using the 'TheParticipantFactory' convenience macro). */
participant = DDS_DomainParticipantFactory_create_participant (
DDS_TheParticipantFactory,
domain,
DDS_PARTICIPANT_QOS_DEFAULT,
NULL,
DDS_STATUS_MASK_NONE);
checkHandle(participant, "DDS_DomainParticipantFactory_create_participant");
/* Register the required datatype for ChatMessage. */
chatMessageTS = Chat_ChatMessageTypeSupport__alloc();
checkHandle(chatMessageTS, "Chat_ChatMessageTypeSupport__alloc");
chatMessageTypeName = Chat_ChatMessageTypeSupport_get_type_name(chatMessageTS);
status = Chat_ChatMessageTypeSupport_register_type(
chatMessageTS,
participant,
chatMessageTypeName);
checkStatus(status, "Chat_ChatMessageTypeSupport_register_type");
/* Register the required datatype for NameService. */
nameServiceTS = Chat_NameServiceTypeSupport__alloc();
checkHandle(nameServiceTS, "Chat_NameServiceTypeSupport__alloc");
nameServiceTypeName = Chat_NameServiceTypeSupport_get_type_name(nameServiceTS);
status = Chat_NameServiceTypeSupport_register_type(
nameServiceTS,
participant,
nameServiceTypeName);
checkStatus(status, "Chat_NameServiceTypeSupport_register_type");
/* Set the ReliabilityQosPolicy to RELIABLE. */
reliable_topic_qos = DDS_TopicQos__alloc();
checkHandle(reliable_topic_qos, "DDS_TopicQos__alloc");
status = DDS_DomainParticipant_get_default_topic_qos(participant, reliable_topic_qos);
checkStatus(status, "DDS_DomainParticipant_get_default_topic_qos");
reliable_topic_qos->reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
/* Make the tailored QoS the new default. */
status = DDS_DomainParticipant_set_default_topic_qos(participant, reliable_topic_qos);
checkStatus(status, "DDS_DomainParticipant_set_default_topic_qos");
/* Use the changed policy when defining the ChatMessage topic */
chatMessageTopic = DDS_DomainParticipant_create_topic(
participant,
"Chat_ChatMessage",
chatMessageTypeName,
reliable_topic_qos,
NULL,
DDS_STATUS_MASK_NONE);
checkHandle(chatMessageTopic, "DDS_DomainParticipant_create_topic (ChatMessage)");
/* Set the DurabilityQosPolicy to TRANSIENT. */
setting_topic_qos = DDS_TopicQos__alloc();
checkHandle(setting_topic_qos, "DDS_TopicQos__alloc");
status = DDS_DomainParticipant_get_default_topic_qos(participant, setting_topic_qos);
checkStatus(status, "DDS_DomainParticipant_get_default_topic_qos");
setting_topic_qos->durability.kind = DDS_TRANSIENT_DURABILITY_QOS;
/* Create the NameService Topic. */
nameServiceTopic = DDS_DomainParticipant_create_topic(
participant,
"Chat_NameService",
nameServiceTypeName,
setting_topic_qos,
NULL,
DDS_STATUS_MASK_NONE);
checkHandle(nameServiceTopic, "DDS_DomainParticipant_create_topic");
/* Adapt the default SubscriberQos to read from the "ChatRoom" Partition. */
partitionName = "ChatRoom";
sub_qos = DDS_SubscriberQos__alloc();
checkHandle(sub_qos, "DDS_SubscriberQos__alloc");
status = DDS_DomainParticipant_get_default_subscriber_qos (participant, sub_qos);
checkStatus(status, "DDS_DomainParticipant_get_default_subscriber_qos");
sub_qos->partition.name._length = 1;
sub_qos->partition.name._maximum = 1;
sub_qos->partition.name._buffer = DDS_StringSeq_allocbuf (1);
checkHandle(sub_qos->partition.name._buffer, "DDS_StringSeq_allocbuf");
sub_qos->partition.name._buffer[0] = DDS_string_alloc (strlen(partitionName) + 1);
checkHandle(sub_qos->partition.name._buffer[0], "DDS_string_alloc");
strcpy (sub_qos->partition.name._buffer[0], partitionName);
/* Create a Subscriber for the UserLoad application. */
chatSubscriber = DDS_DomainParticipant_create_subscriber(participant, sub_qos, NULL, DDS_STATUS_MASK_NONE);
checkHandle(chatSubscriber, "DDS_DomainParticipant_create_subscriber");
/* Create a DataReader for the NameService Topic (using the appropriate QoS). */
nameServer = DDS_Subscriber_create_datareader(
chatSubscriber,
nameServiceTopic,
DDS_DATAREADER_QOS_USE_TOPIC_QOS,
NULL,
DDS_STATUS_MASK_NONE);
checkHandle(nameServer, "DDS_Subscriber_create_datareader (NameService)");
/* Adapt the DataReaderQos for the ChatMessageDataReader to keep track of all messages. */
message_qos = DDS_DataReaderQos__alloc();
checkHandle(message_qos, "DDS_DataReaderQos__alloc");
status = DDS_Subscriber_get_default_datareader_qos(chatSubscriber, message_qos);
checkStatus(status, "DDS_Subscriber_get_default_datareader_qos");
status = DDS_Subscriber_copy_from_topic_qos(chatSubscriber, message_qos, reliable_topic_qos);
checkStatus(status, "DDS_Subscriber_copy_from_topic_qos");
message_qos->history.kind = DDS_KEEP_ALL_HISTORY_QOS;
/* Create a DataReader for the ChatMessage Topic (using the appropriate QoS). */
loadAdmin = DDS_Subscriber_create_datareader(
chatSubscriber,
chatMessageTopic,
message_qos,
NULL,
DDS_STATUS_MASK_NONE);
checkHandle(loadAdmin, "DDS_Subscriber_create_datareader (ChatMessage)");
/* Initialize the Query Arguments. */
args._length = 1;
args._maximum = 1;
args._buffer = DDS_StringSeq_allocbuf(1);
checkHandle(args._buffer, "DDS_StringSeq_allocbuf");
args._buffer[0] = DDS_string_alloc (12); /* Enough for maximum size numbers. */
checkHandle(args._buffer[0], "DDS_string_alloc");
sprintf(args._buffer[0], "%d", 0);
/* Create a QueryCondition that will contain all messages with userID=ownID */
singleUser = DDS_DataReader_create_querycondition(
loadAdmin,
DDS_ANY_SAMPLE_STATE,
DDS_ANY_VIEW_STATE,
DDS_ANY_INSTANCE_STATE,
"userID=%0",
&args);
checkHandle(singleUser, "DDS_DataReader_create_querycondition (singleUser Query)");
/* Create a ReadCondition that will contain new users only */
newUser = DDS_DataReader_create_readcondition(
nameServer,
DDS_NOT_READ_SAMPLE_STATE,
DDS_NEW_VIEW_STATE,
DDS_ALIVE_INSTANCE_STATE);
checkHandle(newUser, "DDS_DataReader_create_readcondition (newUser)");
/* Obtain a StatusCondition that triggers only when a Writer changes Liveliness */
leftUser = DDS_DataReader_get_statuscondition(loadAdmin);
checkHandle(leftUser, "DDS_DataReader_get_statuscondition");
status = DDS_StatusCondition_set_enabled_statuses(leftUser, DDS_LIVELINESS_CHANGED_STATUS);
checkStatus(status, "DDS_StatusCondition_set_enabled_statuses");
/* Create a bare guard which will be used to close the room */
escape = DDS_GuardCondition__alloc();
checkHandle(escape, "DDS_GuardCondition__alloc");
/* Create a waitset and add the ReadConditions */
userLoadWS = DDS_WaitSet__alloc();
checkHandle(userLoadWS, "DDS_WaitSet__alloc");
status = DDS_WaitSet_attach_condition(userLoadWS, newUser);
checkStatus(status, "DDS_WaitSet_attach_condition (newUser)");
status = DDS_WaitSet_attach_condition(userLoadWS, leftUser);
checkStatus(status, "DDS_WaitSet_attach_condition (leftUser)");
status = DDS_WaitSet_attach_condition(userLoadWS, escape);
checkStatus(status, "DDS_WaitSet_attach_condition (escape)");
/* Initialize and pre-allocate the GuardList used to obtain the triggered Conditions. */
guardList = DDS_ConditionSeq__alloc();
checkHandle(guardList, "DDS_ConditionSeq__alloc");
guardList->_maximum = 3;
guardList->_length = 0;
guardList->_buffer = DDS_ConditionSeq_allocbuf(3);
checkHandle(guardList->_buffer, "DDS_ConditionSeq_allocbuf");
/* Remove all known Users that are not currently active. */
status = Chat_NameServiceDataReader_take(
nameServer,
&nsList,
&infoSeq,
DDS_LENGTH_UNLIMITED,
DDS_ANY_SAMPLE_STATE,
DDS_ANY_VIEW_STATE,
DDS_NOT_ALIVE_INSTANCE_STATE);
checkStatus(status, "Chat_NameServiceDataReader_take");
status = Chat_NameServiceDataReader_return_loan(nameServer, &nsList, &infoSeq);
checkStatus(status, "Chat_NameServiceDataReader_return_loan");
/* Start the sleeper thread. */
pthread_attr_init(&tattr);
pthread_attr_setdetachstate(&tattr, PTHREAD_CREATE_DETACHED);
pthread_create (&tid, &tattr, delayedEscape, NULL);
pthread_attr_destroy(&tattr);
while (!closed) {
/* Wait until at least one of the Conditions in the waitset triggers. */
status = DDS_WaitSet_wait(userLoadWS, guardList, &timeout);
checkStatus(status, "DDS_WaitSet_wait");
/* Walk over all guards to display information */
for (i = 0; i < guardList->_length; i++) {
guard = guardList->_buffer[i];
if (guard == newUser) {
/* The newUser ReadCondition contains data */
status = Chat_NameServiceDataReader_read_w_condition(
nameServer,
&nsList,
&infoSeq,
DDS_LENGTH_UNLIMITED,
newUser);
checkStatus(status, "Chat_NameServiceDataReader_read_w_condition");
for (j = 0; j < nsList._length; j++) {
printf ("New user: %s\n", nsList._buffer[j].name);
}
status = Chat_NameServiceDataReader_return_loan(nameServer, &nsList, &infoSeq);
checkStatus(status, "Chat_NameServiceDataReader_return_loan");
} else if (guard == leftUser) {
/* Some liveliness has changed (either a DataWriter joined or a DataWriter left) */
status = DDS_DataReader_get_liveliness_changed_status(loadAdmin, &livChangStatus);
checkStatus(status, "DDS_DataReader_get_liveliness_changed_status");
if (livChangStatus.alive_count < prevCount) {
/* A user has left the ChatRoom, since a DataWriter lost its liveliness */
/* Take the effected users so tey will not appear in the list later on. */
status = Chat_NameServiceDataReader_take(
nameServer,
&nsList,
&infoSeq,
DDS_LENGTH_UNLIMITED,
DDS_ANY_SAMPLE_STATE,
DDS_ANY_VIEW_STATE,
DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE);
checkStatus(status, "Chat_NameServiceDataReader_take");
for (j = 0; j < nsList._length; j++) {
/* re-apply query arguments */
sprintf(args._buffer[0], "%d", nsList._buffer[j].userID);
status = DDS_QueryCondition_set_query_parameters(singleUser, &args);
checkStatus(status, "DDS_QueryCondition_set_query_parameters");
/* Read this users history */
status = Chat_ChatMessageDataReader_take_w_condition(
loadAdmin,
&msgList,
&infoSeq2,
DDS_LENGTH_UNLIMITED,
singleUser);
checkStatus(status, "Chat_ChatMessageDataReader_take_w_condition");
/* Display the user and his history */
printf (
"Departed user %s has sent %d messages\n",
nsList._buffer[j].name,
msgList._length);
status = Chat_ChatMessageDataReader_return_loan(loadAdmin, &msgList, &infoSeq2);
checkStatus(status, "Chat_ChatMessageDataReader_return_loan");
}
status = Chat_NameServiceDataReader_return_loan(nameServer, &nsList, &infoSeq);
checkStatus(status, "Chat_NameServiceDataReader_return_loan");
}
prevCount = livChangStatus.alive_count;
} else if (guard == escape) {
printf ("UserLoad has terminated.\n");
closed = 1;
}
else
{
assert(0);
};
} /* for */
} /* while (!closed) */
/* Remove all Conditions from the WaitSet. */
status = DDS_WaitSet_detach_condition(userLoadWS, escape);
checkStatus(status, "DDS_WaitSet_detach_condition (escape)");
status = DDS_WaitSet_detach_condition(userLoadWS, leftUser);
checkStatus(status, "DDS_WaitSet_detach_condition (leftUser)");
status = DDS_WaitSet_detach_condition(userLoadWS, newUser);
checkStatus(status, "DDS_WaitSet_detach_condition (newUser)");
/* Free all resources */
DDS_free(guardList);
DDS_free(args._buffer);
DDS_free(userLoadWS);
DDS_free(escape);
DDS_free(setting_topic_qos);
DDS_free(reliable_topic_qos);
DDS_free(nameServiceTypeName);
DDS_free(chatMessageTypeName);
DDS_free(nameServiceTS);
DDS_free(chatMessageTS);
status = DDS_DomainParticipant_delete_contained_entities(participant);
checkStatus(status, "DDS_DomainParticipant_delete_contained_entities");
status = DDS_DomainParticipantFactory_delete_participant(
DDS_TheParticipantFactory,
participant);
checkStatus(status, "DDS_DomainParticipantFactory_delete_participant");
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 OSPL_MAIN (int argc, char *argv[])
{
DDS_sequence_WaitSetData_Msg* message_seq = DDS_sequence_WaitSetData_Msg__alloc();
DDS_SampleInfoSeq* message_infoSeq = DDS_SampleInfoSeq__alloc();
DDS_Subscriber message_Subscriber;
DDS_DataReader message_DataReader;
DDS_char* query_parameter = "Hello again";
DDS_char *query_string = "message=%0";
DDS_ReadCondition readCondition;
DDS_QueryCondition queryCondition;
DDS_StatusCondition statusCondition;
DDS_WaitSet waitSet;
DDS_GuardCondition escape;
DDS_LivelinessChangedStatus livelinessChangedStatus;
DDS_ConditionSeq *guardList = NULL;
DDS_Duration_t timeout = { 20, 0 };
int count = 0;
DDS_long previousLivelinessCount;
c_bool isClosed = FALSE;
c_bool isEscaped = FALSE;
c_bool writerLeft = FALSE;
unsigned long i, j;
// Create DDS DomainParticipant
createParticipant("WaitSet example");
// Register the Topic's type in the DDS Domain.
g_MessageTypeSupport = WaitSetData_MsgTypeSupport__alloc();
checkHandle(g_MessageTypeSupport, "WaitSetData_MsgTypeSupport__alloc");
registerMessageType(g_MessageTypeSupport);
// Create the Topic's in the DDS Domain.
g_MessageTypeName = WaitSetData_MsgTypeSupport_get_type_name(g_MessageTypeSupport);
g_MessageTopic = createTopic("WaitSetData_Msg", g_MessageTypeName);
DDS_free(g_MessageTypeName);
DDS_free(g_MessageTypeSupport);
// Create the Subscriber's in the DDS Domain.
message_Subscriber = createSubscriber();
// Request a Reader from the the Subscriber.
message_DataReader = createDataReader(message_Subscriber, g_MessageTopic);
// 1- Create a ReadCondition that will contain new Msg only.
readCondition = DDS_DataReader_create_readcondition(message_DataReader, DDS_NOT_READ_SAMPLE_STATE, DDS_NEW_VIEW_STATE, DDS_ALIVE_INSTANCE_STATE);
checkHandle(readCondition, "DDS_DataReader_create_readcondition (newUser)");
// 2- Create QueryCondition.
queryCondition = createQueryCondition(message_DataReader, query_string, (DDS_char*) query_parameter);
printf("=== [WaitSetDataSubscriber] Query : message = \"%s\"", query_parameter);
// 3- Obtain a StatusCondition associated to a Writer
// and that triggers only when the Writer changes Liveliness.
statusCondition = DDS_DataReader_get_statuscondition(message_DataReader);
checkHandle(statusCondition, "DDS_DataReader_get_statuscondition");
g_status = DDS_StatusCondition_set_enabled_statuses(statusCondition, DDS_LIVELINESS_CHANGED_STATUS);
checkStatus(g_status, "DDS_StatusCondition_set_enabled_statuses");
// 4- Create a GuardCondition which will be used to close the subscriber.
escape = DDS_GuardCondition__alloc();
checkHandle(escape, "DDS_GuardCondition__alloc");
// Create a waitset and add the 4 Conditions created above :
// ReadCondition, QueryCondition, StatusCondition, GuardCondition.
waitSet = DDS_WaitSet__alloc();
checkHandle(waitSet, "DDS_WaitSet__alloc");
g_status = DDS_WaitSet_attach_condition(waitSet, readCondition);
checkStatus(g_status, "DDS_WaitSet_attach_condition (readCondition)");
g_status = DDS_WaitSet_attach_condition(waitSet, queryCondition);
checkStatus(g_status, "DDS_WaitSet_attach_condition (queryCondition)");
g_status = DDS_WaitSet_attach_condition(waitSet, statusCondition);
checkStatus(g_status, "DDS_WaitSet_attach_condition (statusCondition)");
g_status = DDS_WaitSet_attach_condition(waitSet, escape);
checkStatus(g_status, "DDS_WaitSet_attach_condition (escape)");
// Initialize and pre-allocate the GuardList used to obtain the triggered Conditions.
guardList = DDS_ConditionSeq__alloc();
checkHandle(guardList, "DDS_ConditionSeq__alloc");
guardList->_maximum = 4;
guardList->_length = 0;
guardList->_buffer = DDS_ConditionSeq_allocbuf(4);
checkHandle(guardList->_buffer, "DDS_ConditionSeq_allocbuf");
printf("\n=== [WaitSetDataSubscriber] Ready ...");
// Used to store the livelinessChangedStatus.alive_count value (StatusCondition).
previousLivelinessCount = 0;
do
{
// Wait until at least one of the Conditions in the waitset triggers.
g_status = DDS_WaitSet_wait(waitSet, guardList, &timeout);
if (g_status == DDS_RETCODE_OK) {
i = 0;
do
{
if( guardList->_buffer[i] == statusCondition )
{
// StatusCondition triggered.
// Some liveliness has changed (either a DataWriter joined or a DataWriter left).
// Check whether a new Writer appeared.
g_status = DDS_DataReader_get_liveliness_changed_status(message_DataReader, &livelinessChangedStatus);
checkStatus(g_status, "DDS_DataReader_get_liveliness_changed_status");
if( livelinessChangedStatus.alive_count > previousLivelinessCount )
{
// a DataWriter joined.
printf("\n\n!!! a MsgWriter joined");
g_status = DDS_StatusCondition_set_enabled_statuses(statusCondition, DDS_LIVELINESS_CHANGED_STATUS);
checkStatus(g_status, "DDS_StatusCondition_set_enabled_statuses");
}
else if( livelinessChangedStatus.alive_count < previousLivelinessCount )
{
// A MsgWriter lost its liveliness.
printf("\n\n!!! a MsgWriter lost its liveliness");
g_status = DDS_StatusCondition_set_enabled_statuses(statusCondition, DDS_LIVELINESS_CHANGED_STATUS);
checkStatus(g_status, "DDS_StatusCondition_set_enabled_statuses");
writerLeft = TRUE;
printf("\n === Triggering escape condition");
g_status = DDS_GuardCondition_set_trigger_value(escape, TRUE);
checkStatus(g_status, "DDS_GuardCondition_set_trigger_value");
}
else if ( livelinessChangedStatus.alive_count == previousLivelinessCount )
{
// The status has changed, though alive_count is still zero,
// this implies that both events have occurred,
// and that the value is back to zero already.
printf("\n\n!!! a MsgWriter joined\n");
printf("\n\n!!! a MsgWriter lost its liveliness\n");
writerLeft = TRUE;
printf("\n === Triggering escape condition");
g_status = DDS_GuardCondition_set_trigger_value(escape, TRUE);
checkStatus(g_status, "DDS_GuardCondition_set_trigger_value");
// NB: we do not need to activate this Condition anymore;
// both events occurred.
}
// NB: Here we record the count a in order to compare it next time this condition triggers (?).
previousLivelinessCount = livelinessChangedStatus.alive_count;
}
else if( guardList->_buffer[i] == readCondition )
{
/* The newMsg ReadCondition contains data */
g_status = WaitSetData_MsgDataReader_read_w_condition(message_DataReader, message_seq, message_infoSeq, DDS_LENGTH_UNLIMITED, readCondition);
checkStatus(g_status, "WaitSetData_MsgDataReader_read_w_condition");
for( j = 0; j < message_seq->_length ; j++ )
{
printf("\n --- New message received ---");
if( message_infoSeq->_buffer[j].valid_data == TRUE )
{
printf("\n userID : %d", message_seq->_buffer[j].userID);
printf("\n Message : \"%s\"", message_seq->_buffer[j].message);
}
else
{
printf("\n Data is invalid!");
}
}
fflush(stdout);
g_status = WaitSetData_MsgDataReader_return_loan(message_DataReader, message_seq, message_infoSeq);
checkStatus(g_status, "WaitSetData_MsgDataReader_return_loan");
}
else if( guardList->_buffer[i] == queryCondition )
{
/* The queryCond QueryCondition contains data */
g_status = WaitSetData_MsgDataReader_take_w_condition(message_DataReader, message_seq, message_infoSeq, DDS_LENGTH_UNLIMITED, queryCondition);
checkStatus(g_status, "WaitSetData_MsgDataReader_take_w_condition");
for( j = 0; j < message_seq->_length ; j++ )
{
printf("\n\n --- message received (with QueryCOndition on message field) ---");
if( message_infoSeq->_buffer[j].valid_data == TRUE )
{
printf("\n userID : %d", message_seq->_buffer[j].userID);
printf("\n Message : \"%s\"", message_seq->_buffer[j].message);
}
else
{
printf("\n Data is invalid!");
}
}
fflush(stdout);
g_status = WaitSetData_MsgDataReader_return_loan(message_DataReader, message_seq, message_infoSeq);
checkStatus(g_status, "WaitSetData_MsgDataReader_return_loan");
}
// This checks whether the Timeout Thread has resumed waiting.
else if( guardList->_buffer[i] == escape )
{
printf("\n escape condition triggered! - count = %d\n ", count);
isEscaped = TRUE;
g_status = DDS_GuardCondition_set_trigger_value(escape, FALSE);
checkStatus(g_status, "DDS_GuardCondition_set_trigger_value");
}
}
while( ++i < guardList->_length );
}
else if (g_status != DDS_RETCODE_TIMEOUT) {
// DDS_RETCODE_TIMEOUT is considered as an error
// only after it has occurred count times
checkStatus(g_status, "DDS_WaitSet_wait");
} else {
printf("!!! [INFO] WaitSet timedout - count = %d\n ", count);
}
++count;
isClosed = isEscaped && writerLeft;
} while( isClosed == FALSE && count < 20);
if (count >= 20) {
printf( "*** Error : Timed out - count = %d ***\n", count);
}
/* Remove all Conditions from the WaitSet. */
g_status = DDS_WaitSet_detach_condition(waitSet, escape);
checkStatus(g_status, "DDS_WaitSet_detach_condition (escape)");
g_status = DDS_WaitSet_detach_condition(waitSet, statusCondition);
checkStatus(g_status, "DDS_WaitSet_detach_condition (statusCondition)");
g_status = DDS_WaitSet_detach_condition(waitSet, queryCondition);
checkStatus(g_status, "DDS_WaitSet_detach_condition (queryCondition)");
g_status = DDS_WaitSet_detach_condition(waitSet, readCondition);
checkStatus(g_status, "DDS_WaitSet_detach_condition (readCondition)");
// Cleanup DDS from the created Entities.
deleteQueryCondition(message_DataReader, readCondition);
deleteQueryCondition(message_DataReader, queryCondition);
deleteDataReader(message_Subscriber, message_DataReader);
deleteSubscriber(message_Subscriber);
deleteTopic(g_MessageTopic);
deleteParticipant();
// Cleanup C allocations
// Recursively free the instances sequence using the OpenSplice API.
DDS_free(guardList);
DDS_free(message_seq);
DDS_free(message_infoSeq);
DDS_free(waitSet);
DDS_free(escape);
printf("\n");
return 0;
}