New robot type for laser data (only period and a measure). CAN daemon

has been updated to use this new data structure.

diff --git a/src/cand/eb2008/cand_eb2008.cc b/src/cand/eb2008/cand_eb2008.cc
index dcc6f12a45a33c946af4474546c6d8736b5b9fe0..02e49d2c6072ccb4bb574ed2dc77721440c2347c 100644 (file)
--- a/src/cand/eb2008/cand_eb2008.cc
+++ b/src/cand/eb2008/cand_eb2008.cc
@@ -202,6 +202,7 @@ int cand_parse_frame(struct eb2008_orte_data *orte_eb2008,
        int ir_cnt = 0;
        unsigned short *las_meas;
        int las_mi, las_di, las_bcnt;
+       static unsigned short prev_meas;
        int i;
 
        switch(frame.can_id) {
@@ -328,6 +329,15 @@ int cand_parse_frame(struct eb2008_orte_data *orte_eb2008,
                                ORTEPublicationSend(orte_eb2008->publication_laser_meas);
                        }
                        break;
+
+               case CAN_LAS_DATA:
+                       orte_eb2008->laser_data.period = (frame.data[0]<<8)|(frame.data[1]);
+                       orte_eb2008->laser_data.measure = (frame.data[2]<<8)|(frame.data[3]);
+                       if (prev_meas != orte_eb2008->laser_data.measure)
+                               ORTEPublicationSend(orte_eb2008->publication_laser_data);
+                       prev_meas = orte_eb2008->laser_data.measure;
+                       break;
+
                default:
 //                     printf("received CAN msg with unknown id: %x\n",frame.can_id);
                        break;
@@ -465,6 +475,7 @@ int main(int argc, char *argv[])
        generic_publisher_motion_irc_create(&orte_generic, NULL, NULL);
        generic_publisher_robot_cmd_create(&orte_generic, NULL, NULL);
        eb2008_publisher_laser_meas_create(&orte_eb2008, NULL, NULL);
+       eb2008_publisher_laser_data_create(&orte_eb2008, NULL, NULL);
 
        /* creating subscribers */
        generic_subscriber_pwr_ctrl_create(&orte_generic, 

diff --git a/src/types/robottype_eb2008.c b/src/types/robottype_eb2008.c
index 25e5ba853e4473a1c73c2cb5a6cd96349fcbb2f5..e7dddb11b78fc41f71e51a7504c8856f13a24818 100644 (file)
--- a/src/types/robottype_eb2008.c
+++ b/src/types/robottype_eb2008.c
@@ -124,6 +124,41 @@ laser_cmd_type_register(ORTEDomain *d) {
   return ret;
 }
 
+/****************************************************************/
+/* struct - laser_data                                          */
+/****************************************************************/
+
+void laser_data_serialize(CDR_Codec *cdrCodec,laser_data *object) {
+  CORBA_unsigned_short_serialize(cdrCodec,&(object->period));
+  CORBA_unsigned_short_serialize(cdrCodec,&(object->measure));
+}
+
+void
+laser_data_deserialize(CDR_Codec *cdrCodec,laser_data *object) {
+  CORBA_unsigned_short_deserialize(cdrCodec,&(object->period));
+  CORBA_unsigned_short_deserialize(cdrCodec,&(object->measure));
+}
+
+int
+laser_data_get_max_size(ORTEGetMaxSizeParam *gms) {
+  CORBA_unsigned_short_get_max_size(gms);
+  CORBA_unsigned_short_get_max_size(gms);
+  return gms->csize;
+}
+
+Boolean
+laser_data_type_register(ORTEDomain *d) {
+  Boolean ret;
+
+  ret=ORTETypeRegisterAdd(d,
+                          "laser_data",
+                          (ORTETypeSerialize)laser_data_serialize,
+                          (ORTETypeDeserialize)laser_data_deserialize,
+                          laser_data_get_max_size,
+                          0);
+  return ret;
+}
+
 /****************************************************************/
 /* struct - laser_meas                                          */
 /****************************************************************/

diff --git a/src/types/robottype_eb2008.h b/src/types/robottype_eb2008.h
index 6927be5672b6f2f9a7c5bd8c55e85c12275aac9f..4ba7de44bf17106cb36fa92c259cae76bf928c7e 100644 (file)
--- a/src/types/robottype_eb2008.h
+++ b/src/types/robottype_eb2008.h
@@ -45,6 +45,15 @@ struct laser_cmd_type {
 CORBA_octet engine;
 };
 
+#endif
+#if !defined(_laser_data_defined)
+#define _laser_data_defined 1
+typedef struct laser_data_type laser_data;
+struct laser_data_type {
+CORBA_unsigned_short period;
+CORBA_unsigned_short measure;
+};
+
 #endif
 #if !defined(_laser_meas_defined)
 #define _laser_meas_defined 1
@@ -95,6 +104,11 @@ void laser_cmd_deserialize(CDR_Codec *cdrCodec,laser_cmd *object);
 int laser_cmd_get_max_size(ORTEGetMaxSizeParam *gms);
 Boolean laser_cmd_type_register(ORTEDomain *d);
 
+void laser_data_serialize(CDR_Codec *cdrCodec,laser_data *object);
+void laser_data_deserialize(CDR_Codec *cdrCodec,laser_data *object);
+int laser_data_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean laser_data_type_register(ORTEDomain *d);
+
 void laser_meas_serialize(CDR_Codec *cdrCodec,laser_meas *object);
 void laser_meas_deserialize(CDR_Codec *cdrCodec,laser_meas *object);
 int laser_meas_get_max_size(ORTEGetMaxSizeParam *gms);

diff --git a/src/types/robottype_eb2008.idl b/src/types/robottype_eb2008.idl
index 73d430657083ed44bfb80f1c5ddd3846444457f4..61d5e915085df4ddbddf4500ac05cd7971858a3e 100644 (file)
--- a/src/types/robottype_eb2008.idl
+++ b/src/types/robottype_eb2008.idl
@@ -18,6 +18,11 @@ struct laser_cmd {
        octet engine;
 };
 
+struct laser_data {
+       unsigned short period;
+       unsigned short measure;
+};
+
 struct laser_meas {
        unsigned short cnt;
        unsigned short period;