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 }
  85
  86 void robottype_subscriber_motion_speed_create(struct robottype_orte_data *data, ORTERecvCallBack callback, void *arg) {
  87         NtpTime deadline, minimumSeparation;
  88
  89         motion_speed_type_register(data->orte_domain);
  90
  91         NtpTimeAssembFromMs(deadline, 0, 300);
  92         NtpTimeAssembFromMs(minimumSeparation, 0, 0);
  93         s = ORTESubscriptionCreate(
  94                         data->orte_domain, IMMEDIATE, BEST_EFFORTS,
  95                         "motion_speed", "motion_speed",
  96                         &data->motion_speed, &deadline, &minimumSeparation,
  97                         callback, arg, IPADDRESS_INVALID);
  98 }
  99
 100 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 101                         void *recvCallBackParam)
 102 {
 103         struct motion_speed_type *m = (struct motion_speed_type *)vinstance;
 104
 105         switch (info->status) {
 106                 case NEW_DATA:
 107                         printf("Prijata data: left: %d, right: %d\n", m->left, m->right);
 108                         break;
 109                 case DEADLINE:
 110                         printf("ORTE deadline occurred - motion_speed receive\n");
 111                         break;
 112         }
 113 }
 114
 115 void destroy(int sig) {
 116         ORTESubscriptionDestroy(s);
 117         ORTETypeRegisterDestroyAll(orte.orte_domain);
 118         ORTEDomainAppDestroy(orte.orte_domain);
 119 }
 120
 121 int main(void) {
 122         orte.strength = 30;
 123         robottype_roboorte_init(&orte);
 124         robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
 125
 126         signal(SIGINT, &destroy);
 127         signal(SIGTERM, &destroy);
 128         pause();
 129
 130         return 0;
 131 }