static int context__poll(struct sensors_poll_device_t *dev, sensors_event_t *data, int count)
{
struct sensor_context *ctx = (struct sensor_context *)dev;
int ret;
while (1) {
poll_data(dev, data);
ret = submit_sensor(dev, data);
if (ret)
return ret;
nanosleep(&ctx->delay, &ctx->delay);
}
return 0;
}
static int subscriber_main(int domainId, int sample_count) {
DDS_DomainParticipant *participant = NULL;
DDS_Subscriber *subscriber[MAX_SUBSCRIBERS] = { 0 };
DDS_Topic *topic = NULL;
DDS_DataReader *reader[MAX_SUBSCRIBERS] = { 0 };
DDS_ReturnCode_t retcode;
const char *type_name = NULL;
int count = 0;
struct DDS_Duration_t poll_period = { 4, 0 };
orderedDataReader *ordered_reader[MAX_SUBSCRIBERS] = { 0 };
int i;
struct DDS_SubscriberQos subscriber_qos = DDS_SubscriberQos_INITIALIZER;
struct DDS_DataReaderQos datareader_qos = DDS_DataReaderQos_INITIALIZER;
char* profile_name[] = { "ordered_Profile_subscriber_instance",
"ordered_Profile_subscriber_topic"
};
/* To customize participant QoS, use
the configuration file USER_QOS_PROFILES.xml */
participant = DDS_DomainParticipantFactory_create_participant(
DDS_TheParticipantFactory, domainId, &DDS_PARTICIPANT_QOS_DEFAULT,
NULL /* listener */, DDS_STATUS_MASK_NONE);
if (participant == NULL) {
printf("create_participant error\n");
subscriber_shutdown(participant, &subscriber_qos, &datareader_qos);
return -1;
}
/* Register the type before creating the topic */
type_name = orderedTypeSupport_get_type_name();
retcode = orderedTypeSupport_register_type(participant, type_name);
if (retcode != DDS_RETCODE_OK) {
printf("register_type error %d\n", retcode);
subscriber_shutdown(participant, &subscriber_qos, &datareader_qos);
return -1;
}
/* To customize topic QoS, use
the configuration file USER_QOS_PROFILES.xml */
topic = DDS_DomainParticipant_create_topic(participant, "Example ordered",
type_name, &DDS_TOPIC_QOS_DEFAULT, NULL /* listener */,
DDS_STATUS_MASK_NONE);
if (topic == NULL) {
printf("create_topic error\n");
subscriber_shutdown(participant, &subscriber_qos, &datareader_qos);
return -1;
}
/* Start changes for ordered_presentation */
/* Create two subscriber to illustrate different presentation QoS
* This is a publisher/subscriber level QoS, so we have to do it
* here instead of just making two datareaders
*/
/* Subscriber[0], reader[0] and ordered_reader[0] is getting
* the profile "ordered_Profile_subscriber_instance"
*/
for (i = 0; i < MAX_SUBSCRIBERS; ++i) {
/* Subscriber[1], reader[1] and ordered_reader[1] is getting
* the profile "ordered_Profile_subscriber_topic"
*/
printf("Subscriber %d using %s\n", i, profile_name[i]);
subscriber[i] = DDS_DomainParticipant_create_subscriber_with_profile(
participant, "ordered_Library", profile_name[i],
NULL /* Listener */, DDS_STATUS_MASK_NONE);
if (subscriber[i] == NULL) {
printf("create_subscriber%d error\n", i);
subscriber_shutdown(participant, &subscriber_qos, &datareader_qos);
return -1;
}
reader[i] = DDS_Subscriber_create_datareader_with_profile(subscriber[i],
DDS_Topic_as_topicdescription(topic), "ordered_Library",
profile_name[i], NULL, DDS_STATUS_MASK_NONE);
if (reader[i] == NULL) {
printf("create_datareader%d error\n", i);
subscriber_shutdown(participant, &subscriber_qos, &datareader_qos);
return -1;
}
ordered_reader[i] = orderedDataReader_narrow(reader[i]);
if (ordered_reader[i] == NULL) {
printf("DataReader%d narrow error\n", i);
return -1;
}
}
/* If you want to change the Publisher's QoS programmatically rather than
* using the XML file, you will need to add the following lines to your
* code and comment out the above 'for' loop.
*/
/*
for (i = 0; i < MAX_SUBSCRIBERS; ++i) {
*/ /* Get default subscriber QoS to customize */
/* retcode = DDS_DomainParticipant_get_default_subscriber_qos(participant,
&subscriber_qos);
if (retcode != DDS_RETCODE_OK) {
printf("get_default_subscriber_qos error\n");
return -1;
}
*/ /* Set ordered_access = TRUE for both subscribers */
/* subscriber_qos.presentation.ordered_access = DDS_BOOLEAN_TRUE;
*/ /* We are assuming there are only 2 subscribers. Otherwise you
* will need to modify the following conditions.
*/
/* if (i == 0) {
printf("Subscriber 0 using Instance access scope\n");
subscriber_qos.presentation.access_scope =
DDS_INSTANCE_PRESENTATION_QOS;
} else {
printf("Subscriber 1 using Topic access scope\n");
subscriber_qos.presentation.access_scope =
DDS_TOPIC_PRESENTATION_QOS;
}
*/ /* To create subscriber with default QoS, use DDS_SUBSCRIBER_QOS_DEFAULT
instead of subscriber_qos */
/* subscriber[i] = DDS_DomainParticipant_create_subscriber(participant,
&subscriber_qos, NULL, DDS_STATUS_MASK_NONE);
if (subscriber == NULL) {
printf("create_subscriber error\n");
subscriber_shutdown(participant, &subscriber_qos, &datareader_qos);
return -1;
}
*/ /* No listener needed, but we do need to increase history depth */
/* Get default datareader QoS to customize */
/* retcode = DDS_Subscriber_get_default_datareader_qos(subscriber[i],
&datareader_qos);
if (retcode != DDS_RETCODE_OK) {
printf("get_default_datareader_qos error\n");
return -1;
}
datareader_qos.history.depth = 10;
*/ /* To create datareader with default QoS, use DDS_DATAREADER_QOS_DEFAULT
instead of datareader_qos */
/* reader[i] = DDS_Subscriber_create_datareader(subscriber[i],
DDS_Topic_as_topicdescription(topic), &datareader_qos, NULL,
DDS_STATUS_MASK_NONE);
if (reader == NULL) {
printf("create_datareader error\n");
subscriber_shutdown(participant, &subscriber_qos, &datareader_qos);
return -1;
}
ordered_reader[i] = orderedDataReader_narrow(reader[i]);
if (ordered_reader[i] == NULL) {
printf("DataReader narrow error\n");
return -1;
}
}
*/
/* Main loop */
for (count = 0; (sample_count == 0) || (count < sample_count); ++count) {
printf("ordered subscriber sleeping for %d sec...\n", poll_period.sec);
NDDS_Utility_sleep(&poll_period);
poll_data(ordered_reader, MAX_SUBSCRIBERS);
}
/* Cleanup and delete all entities */
return subscriber_shutdown(participant, &subscriber_qos, &datareader_qos);
}
int main(int argc, char *argv[])
{
int pid;
pthread_t tcp_thread;
char *err_msg;
/* parse cmdline arguments */
parse_cmdline(argc, argv);
/* initialize device communication */
err_msg = NULL;
if (aquaero_init(&err_msg) != 0) {
log_msg(LOG_ERR, err_msg);
exit(EXIT_FAILURE);
}
/* daemonize */
if (opts.fork) {
pid = fork();
switch (pid) {
case -1:
/* fork failure */
log_msg(LOG_ERR, "failed to fork into background: %s",
strerror(errno));
exit(EXIT_FAILURE);
break;
case 0:
/* child */
setsid();
log_msg(LOG_INFO, "started by user %d", getuid());
break;
default:
/* parent */
exit(EXIT_SUCCESS);
break;
}
}
/* write pid-file */
if (opts.pidfile)
if (write_pidfile(pid) != 0)
log_msg(LOG_WARNING, "failed to write %s: %s", PID_FILE,
strerror(errno));
/* register signals */
signal(SIGINT, signal_handler);
signal(SIGHUP, signal_handler);
signal(SIGTERM, signal_handler);
signal(SIGQUIT, signal_handler);
/* setup data sync mutex */
if (pthread_mutex_init(&data_lock, NULL) != 0)
err_die("failed to setup mutex, terminating");
/* initial poll */
if (poll_data() != 0)
die();
/* start tcp server, spawn handler thread */
if (tcp_start_server() != 0)
err_die("error opening tcp server socket, terminating");
if (pthread_create(&tcp_thread, NULL, tcp_handler, NULL) != 0)
err_die("error spawning listen-thread, terminating");
log_msg(LOG_INFO, "listening on port %d", opts.port);
/* start infinite polling-loop */
while (1) {
if (poll_data() != 0)
die();
sleep(opts.interval);
}
return 0;
}
