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