orte/examples/robot/publisher.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
  22 void send_dummy_cb(const ORTESendInfo *info, void *vinstance, void *sendCallBackParam)
  23 {
  24         struct motion_speed_type *m = (struct motion_speed_type *)vinstance;
  25
  26         switch(info->status) {
  27                 case NEED_DATA:
  28                         printf("Odeslana data: left: %d, right: %d\n", m->left, m->right);
  29                         break;
  30                 case CQL:
  31                         printf("Kriticka uroven fronty zprav na odeslani.\n");
  32                         break;
  33         }
  34 }
  35
  36 int robottype_roboorte_init(struct robottype_orte_data *data) {
  37         int rv = 0;
  38         ORTEDomainProp prop;
  39         ORTEInit();
  40
  41         if (data->strength < 0)
  42                 data->strength = 1;
  43
  44         ORTEVerbositySetOptions("ALL.0");
  45         ORTEDomainPropDefaultGet(&prop);
  46         NTPTIME_BUILD(prop.baseProp.refreshPeriod, 3);
  47         NTPTIME_BUILD(prop.baseProp.expirationTime, 10);
  48         data->orte_domain = ORTEDomainAppCreate(ORTE_DEFAULT_DOMAIN, &prop, NULL, ORTE_FALSE);
  49         if (!data->orte_domain) {
  50                 printf("ORTEDomainAppCreate failed!\n");
  51                 rv = -1;
  52         }
  53
  54         return rv;
  55 }
  56
  57 void motion_speed_serialize(CDR_Codec *cdrCodec, motion_speed *object) {
  58         CORBA_short_serialize(cdrCodec,&(object->left));
  59
  60         CORBA_short_serialize(cdrCodec,&(object->right));
  61
  62 }
  63
  64 void motion_speed_deserialize(CDR_Codec *cdrCodec, motion_speed *object) {
  65         CORBA_short_deserialize(cdrCodec,&(object->left));
  66
  67         CORBA_short_deserialize(cdrCodec,&(object->right));
  68
  69 }
  70
  71 int motion_speed_get_max_size(ORTEGetMaxSizeParam *gms, int num) {
  72         int loop_lim=2;
  73         int csize_save;
  74         while(num) {
  75                 if (!loop_lim--) {
  76                         gms->csize+=num*(gms->csize-csize_save);
  77                         return gms->csize;
  78                 }
  79                 num--;
  80                 csize_save=gms->csize;
  81                 CORBA_short_get_max_size(gms, 1);
  82                 CORBA_short_get_max_size(gms, 1);
  83         }
  84         return gms->csize;
  85 }
  86
  87 Boolean
  88 motion_speed_type_register(ORTEDomain *d) {
  89         Boolean ret;
  90
  91         ret=ORTETypeRegisterAdd(d,
  92                                 "motion_speed",
  93                                 (ORTETypeSerialize)motion_speed_serialize,
  94                                 (ORTETypeDeserialize)motion_speed_deserialize,
  95                                 motion_speed_get_max_size,
  96                                 0);
  97 }
  98
  99 void robottype_publisher_motion_speed_create(struct robottype_orte_data *data, ORTESendCallBack callback, void *arg) {
 100         NtpTime persistance, delay;
 101
 102         motion_speed_type_register(data->orte_domain);
 103         NtpTimeAssembFromMs(persistance, 3, 0);
 104         NtpTimeAssembFromMs(delay, 0, 100);
 105         printf("delay: s = %d, f = %d\n", delay.seconds, delay.fraction);
 106         data->publication_motion_speed = ORTEPublicationCreate(data->orte_domain, "motion_speed", "motion_speed", &data->motion_speed, &persistance, data->strength, callback, arg, &delay);
 107 }
 108
 109 void destroy(int sig) {
 110         ORTEPublicationDestroy(orte.publication_motion_speed);
 111         ORTETypeRegisterDestroyAll(orte.orte_domain);
 112         ORTEDomainAppDestroy(orte.orte_domain);
 113 }
 114
 115 int main(void) {
 116         orte.strength = 30;
 117         robottype_roboorte_init(&orte);
 118         robottype_publisher_motion_speed_create(&orte, send_dummy_cb, &orte);
 119
 120         orte.motion_speed.right = -19000;
 121         orte.motion_speed.left = 20000;
 122         ORTEPublicationSend(orte.publication_motion_speed);
 123
 124         signal(SIGINT, &destroy);
 125         signal(SIGTERM, &destroy);
 126         pause();
 127
 128         return 0;
 129 }