Hokuyo: updated for new device using urg_ctrl.h library

diff --git a/src/hokuyo/hokuyo.c b/src/hokuyo/hokuyo.c
index 2e3a69cf48ff5cb994e1c68e61c3a56183309082..1a409f78953d531858f362e03c2502181ef25617 100644 (file)
--- a/src/hokuyo/hokuyo.c
+++ b/src/hokuyo/hokuyo.c
@@ -13,200 +13,16 @@
 #include<sys/termios.h>
 #include <string.h>
 #include "hokuyo.h"
+#include <urg_ctrl.h>
 #include <robottype.h>
 #include <roboorte_robottype.h>
 #include <unistd.h>
 
-static FILE *dev;
-u_int16_t *scan;
-struct robottype_orte_data orte;
-
-// SERIAL PORT RELATED FUNCTIONS
-
-//! SPEED SETTINGS
-/*!
-  @param aBR [in] Declared for communication speed (9600-500000)
-  @return cfset[i|o] Returned value for speed function
-  Returns B0 if failed 
- */
-static speed_t bitrate(int aBR)
-{
-  switch(aBR)
-    {
-    case 9600:
-      return B9600;
-
-    case 19200:
-      return B19200;
-
-    case 38400:
-      return B38400;
-
-    case 57600:
-      return B57600;
-
-    case 115200:
-      return B115200;
-
-    case 230400:
-      return B230400;
-
-    case 460800:
-      return B460800;
-
-    case 500000:
-      return B500000;
-
-    default: // invalid bitrate
-      return B0;
-    }
-}
-
-/* Parameter settings for serial port communication
-  @param aFd [in] File descriptor to point serial port
-  @param aSpeed [in] Communication speed (bps)
-  @retval 0 Failed
-  @retval 1 Success
- */
-static int setupSerial(int aFd, int aSpeed)
-{
-  int speed;
-  struct termios tio;
-
-  speed=bitrate(aSpeed);
-  if(speed==0)
-    return 0;  // invalid bitrate
-
-
-
-  tcgetattr(aFd,&tio);
-
-  cfsetispeed(&tio,speed); // bitrate
-  cfsetospeed(&tio,speed); // bitrate
-
-
-
-
-
-  tio.c_cflag=(tio.c_cflag & ~CSIZE) | CS8; // data bits = 8bit
-
-  tio.c_iflag&= ~( BRKINT | ICRNL | ISTRIP );
-  tio.c_iflag&= ~ IXON;    // no XON/XOFF
-  tio.c_cflag&= ~PARENB;   // no parity
-  tio.c_cflag&= ~CRTSCTS;  // no CTS/RTS
-  tio.c_cflag&= ~CSTOPB;   // stop bit = 1bit
-
-  // Other
-  tio.c_lflag &= ~( ISIG | ICANON | ECHO );
-
-  // Commit
-  if(tcsetattr(aFd,TCSANOW,&tio)==0)
-    return 1;
-  return 0;
-}
-
-//! Open and set up specified serial port
-/*!
-  @param aDevNme [in] Serial port device file name
-  @param aSpeed [in] Communication speed(bps)
-  @return Serial port file handle
-  Returns NULL when failed to open.
- */
-static FILE* openSerial(char *aDevName, int aSpeed)
-{
-  FILE* dev;
-
-  dev=fopen(aDevName,"w+");
-  if(dev==NULL)
-    return NULL;  // invalid device file
-
-  // setup parameters
-  if(setupSerial(fileno(dev),aSpeed))
-    return dev;
+static urg_t urg;
+static long *scan;
+static int scan_max;
 
-  // fail
-  perror("setting term parameters");
-  fclose(dev);
-  return NULL;
-}
-
-//
-
-//! Obtain scanned data
-/*!
-  @param aPort [in] Serial port file handle where URG is connected
-  @param aStart [in] Starting scan step from (0-768)
-  @param aEnd [in] Ending scan step at (0-768)
-  @param aSkip [in] Number of steps to be merged when multiple points are combined(1-99)
-  @return Obtained distance data
-  memory pointer is malloc. Should be freed after use.
-  Returns NULL when failed 
- */
-static u_int16_t*  urgGetScan(FILE* aPort, int aStart, int aEnd, int aSkip)
-{
-  char req[16];
-  u_int16_t *pt;
-  char line[80];
-  int i;
-
-  sprintf(req,"G%03d%03d%02d\n",aStart,aEnd,aSkip);
-  i=fwrite(req,sizeof(char),strlen(req),aPort);
-  if(i==0) {
-       printf("chyba 1\n");
-      return NULL;
-  }
-  // echo-back
-  if(fgets(line,sizeof(line),aPort)==NULL) {
-         printf("chyba 2\n");
-      return NULL;
-  }
-  if(strcmp(req,line)!=0)
-    {
-      // invalid
-           printf("chyba 3\n");
-      return NULL;
-    }
-
-  // status
-  if(fgets(line,sizeof(line),aPort)==NULL) {
-         printf("chyba 4\n");
-      return NULL;
-  }
-  if(strcmp("0\n",line)!=0)
-    {
-      //invalid
-           printf("chyba 5\n");
-      return NULL;
-    }
-  // result
-  scan = malloc(sizeof(u_int16_t)*((aEnd-aStart)/aSkip+1));
-  if(scan == NULL) {
-         printf("chyba malloc\n");
-         return NULL;
-  }
-         
-  pt=scan;
-  while(1)
-    {
-      if(fgets(line,sizeof(line),aPort)==NULL)
-        {
-          free(scan);
-          return NULL;
-        }
-      if(!strcmp("\n",line))
-        {
-          //end of result
-          break;
-        }
-      for(i=0;i<strlen(line)-1;i+=2,++pt)
-        {
-          // Note: It is assumed that received data is complete 
-          //     (Problem will occour if data is incomplete)
-          *pt=((line[i] - 0x30)<<6) + line[i+1] - 0x30;
-        }
-    }
-  return scan;
-}
+struct robottype_orte_data orte;
 
 void scan_publisher_callback(const ORTESendInfo *info, 
                void *vinstance, void *arg)
@@ -216,6 +32,7 @@ void scan_publisher_callback(const ORTESendInfo *info,
 
        switch(info->status) {
                case NEED_DATA:
+                       printf("hokuyo: scan request!\n");
                        hokuyo_get_scan();
                        for(i=0; i <= HOKUYO_CLUSTER_CNT; i++)
                                instance->data[i] = scan[i];
@@ -228,27 +45,50 @@ void scan_publisher_callback(const ORTESendInfo *info,
 
 int hokuyo_init(char *device)
 {
-       
-       dev=openSerial(device,19200);
-       if(dev==NULL)
-       {
-               printf("hokuyo: Could not open serial port\n");
-               return -1;
+       int ret;
+       ret = urg_connect(&urg, device, 115200);
+       if (ret < 0) {
+         printf("hokuyo: hokuyo_init failed!\n");
+         urg_disconnect(&urg);
        }
-
+       
        robottype_roboorte_init(&orte);
        robottype_publisher_hokuyo_scan_create(&orte, scan_publisher_callback, NULL);
-
+       
        return 0;
-
-
 }
 
 int hokuyo_get_scan()
-{
-       scan=urgGetScan(dev,44,725,HOKUYO_BEAMS_IN_CLUSTER);
-       if(scan==NULL) {
-               printf("hokuyo: chyba get_scan\n");
+{      
+       int ret;
+       int n,i;
+       
+       /* Reserve for reception data */
+       scan_max = urg_dataMax(&urg);
+       scan = (long*)malloc(sizeof(long) * scan_max);
+       if (scan == NULL) {
+               printf("hokuyo: get_scan - malloc failed!\n");
+               return -1;
+       }
+       
+       /* Request for GD data */
+       ret = urg_requestData(&urg, URG_MD, URG_FIRST, URG_LAST);
+       if (ret < 0) {
+               printf("hokuyo: get_scan - request data failed!\n");
+               return -1;
+       }
+       
+       /* Reception */
+       n = urg_receiveData(&urg, scan, scan_max);
+       
+//     printf("# n = %d\n", n);
+//     for (i = 0; i < n; ++i) {
+//             printf("%d %ld\n", i, scan[i]);
+//     }
+//     printf("\n");
+       
+       if (n < 0) {
+               printf("hokuyo: get_scan recive data failed!\n");
                return -1;
        }
        return 0;
@@ -290,7 +130,11 @@ int main(int argc, char **argv)
 //        }
 //    }
 //    printf("Complete!\n");
-  for(;;) sleep(1);
+  int i = 0;
+  for(;;){
+    printf("Hokuyo: scan in progress: %d\n",i++);
+    sleep(1);
+  }
 
     return 0;
 }