src/hokuyo/shape-detect/online.cc

   1 /**
   2  * @file
   3  * @author Martin Synek
   4  * @author Michal Sojka
   5  * @date 11/02/25
   6  *
   7  * @brief Debug file for testing detection shape.
   8  *
   9  * More information about using introduced methods
  10  * can be found in dokumentation of class Shape_detect (shape_detect.h)
  11  *
  12  * @ingroup shapedet
  13  */
  14
  15 #include <shape_detect.h>
  16 #include <roboorte_robottype.h>
  17
  18 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
  19                         void *recvCallBackParam)
  20 {
  21         Shape_detect sd;
  22
  23         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
  24         static int count = 0;
  25
  26         switch (info->status) {
  27                 case NEW_DATA: {
  28                         printf("Scan\n");
  29                         if (++count >= 2) {
  30                                 printf("Detect\n");
  31                                 count = 0;
  32
  33                                 std::vector<Shape_detect::Line> output;
  34                                 sd.prepare(instance->data);
  35                                 sd.line_detect(output);
  36                         }
  37                         break;
  38                 }
  39                 case DEADLINE:
  40                         printf("Deadline\n");
  41                         break;
  42         }
  43 }
  44
  45 struct robottype_orte_data orte;
  46
  47 int robot_init_orte()
  48 {
  49         int rv = 0;
  50
  51         rv = robottype_roboorte_init(&orte);
  52         if (rv) return rv;
  53
  54         robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
  55         return rv;
  56 }
  57
  58 int main()
  59 {
  60         robot_init_orte();
  61         return 0;
  62 }