orte/examples/robot/subscriber.c

   1 #include <stdlib.h>
   2 #include <stdio.h>
   3 #include <signal.h>
   4 #include "orte.h"
   5
   6 typedef struct motion_speed_type motion_speed;
   7 struct motion_speed_type {
   8         CORBA_short left;
   9         CORBA_short right;
  10 };
  11
  12 struct robottype_orte_data {
  13         ORTEDomain *orte_domain;
  14         int strength;
  15
  16         struct motion_speed_type motion_speed;
  17         ORTEPublication *publication_motion_speed;
  18 };
  19
  20 struct robottype_orte_data orte;
  21 ORTESubscription *s;
  22
  23 int robottype_roboorte_init(struct robottype_orte_data *data) {
  24         int rv = 0;
  25         ORTEDomainProp prop;
  26         ORTEInit();
  27
  28         if (data->strength < 0)
  29                 data->strength = 1;
  30
  31         ORTEVerbositySetOptions("ALL.0");
  32         ORTEDomainPropDefaultGet(&prop);
  33         NTPTIME_BUILD(prop.baseProp.refreshPeriod, 3);
  34         NTPTIME_BUILD(prop.baseProp.expirationTime, 10);
  35         data->orte_domain = ORTEDomainAppCreate(ORTE_DEFAULT_DOMAIN, &prop, NULL, ORTE_FALSE);
  36         if (!data->orte_domain) {
  37                 printf("ORTEDomainAppCreate failed!\n");
  38                 rv = -1;
  39         }
  40
  41         return rv;
  42 }
  43
  44 void motion_speed_serialize(CDR_Codec *cdrCodec, motion_speed *object) {
  45         CORBA_short_serialize(cdrCodec,&(object->left));
  46
  47         CORBA_short_serialize(cdrCodec,&(object->right));
  48
  49 }
  50
  51 void motion_speed_deserialize(CDR_Codec *cdrCodec, motion_speed *object) {
  52         CORBA_short_deserialize(cdrCodec,&(object->left));
  53
  54         CORBA_short_deserialize(cdrCodec,&(object->right));
  55
  56 }
  57
  58 int motion_speed_get_max_size(ORTEGetMaxSizeParam *gms, int num) {
  59         int loop_lim=2;
  60         int csize_save;
  61         while(num) {
  62                 if (!loop_lim--) {
  63                         gms->csize+=num*(gms->csize-csize_save);
  64                         return gms->csize;
  65                 }
  66                 num--;
  67                 csize_save=gms->csize;
  68                 CORBA_short_get_max_size(gms, 1);
  69                 CORBA_short_get_max_size(gms, 1);
  70         }
  71         return gms->csize;
  72 }
  73
  74 Boolean
  75 motion_speed_type_register(ORTEDomain *d) {
  76         Boolean ret;
  77
  78         ret=ORTETypeRegisterAdd(d,
  79                                 "motion_speed",
  80                                 (ORTETypeSerialize)motion_speed_serialize,
  81                                 (ORTETypeDeserialize)motion_speed_deserialize,
  82                                 motion_speed_get_max_size,
  83                                 0,
  84                                 NULL,
  85                                 NULL);
  86 }
  87
  88 void robottype_subscriber_motion_speed_create(struct robottype_orte_data *data, ORTERecvCallBack callback, void *arg) {
  89         NtpTime deadline, minimumSeparation;
  90
  91         motion_speed_type_register(data->orte_domain);
  92
  93         NtpTimeAssembFromMs(deadline, 0, 300);
  94         NtpTimeAssembFromMs(minimumSeparation, 0, 0);
  95         s = ORTESubscriptionCreate(
  96                         data->orte_domain, IMMEDIATE, BEST_EFFORTS,
  97                         "motion_speed", "motion_speed",
  98                         &data->motion_speed, &deadline, &minimumSeparation,
  99                         callback, arg, IPADDRESS_INVALID);
 100 }
 101
 102 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 103                         void *recvCallBackParam)
 104 {
 105         struct motion_speed_type *m = (struct motion_speed_type *)vinstance;
 106
 107         switch (info->status) {
 108                 case NEW_DATA:
 109                         printf("Prijata data: left: %d, right: %d\n", m->left, m->right);
 110                         break;
 111                 case DEADLINE:
 112                         printf("ORTE deadline occurred - motion_speed receive\n");
 113                         break;
 114         }
 115 }
 116
 117 void destroy(int sig) {
 118         ORTESubscriptionDestroy(s);
 119         ORTETypeRegisterDestroyAll(orte.orte_domain);
 120         ORTEDomainAppDestroy(orte.orte_domain);
 121 }
 122
 123 int main(void) {
 124         orte.strength = 30;
 125         robottype_roboorte_init(&orte);
 126         robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
 127
 128         signal(SIGINT, &destroy);
 129         signal(SIGTERM, &destroy);
 130         pause();
 131
 132         return 0;
 133 }