[Personalrobots-commit] SF.net SVN: personalrobots: [49] pkg/trunk

[<mor...@us...>]

Revision: 49
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=49&view=rev
Author:   morgan_quigley
Date:     2008-03-27 16:49:32 -0700 (Thu, 27 Mar 2008)

Log Message:
-----------
hokuyo driver copy from my personal svn

Added Paths:
-----------
    pkg/trunk/hokuyo_top_urg/
    pkg/trunk/hokuyo_top_urg/build.yaml
    pkg/trunk/hokuyo_top_urg/include/
    pkg/trunk/hokuyo_top_urg/include/hokuyo_top_urg/
    pkg/trunk/hokuyo_top_urg/include/hokuyo_top_urg/hokuyo_top_urg.h
    pkg/trunk/hokuyo_top_urg/lib/
    pkg/trunk/hokuyo_top_urg/manifest.xml
    pkg/trunk/hokuyo_top_urg/nodes/
    pkg/trunk/hokuyo_top_urg/nodes/test
    pkg/trunk/hokuyo_top_urg/rosbuild
    pkg/trunk/hokuyo_top_urg/src/
    pkg/trunk/hokuyo_top_urg/src/libhokuyo_top_urg/
    pkg/trunk/hokuyo_top_urg/src/libhokuyo_top_urg/Makefile
    pkg/trunk/hokuyo_top_urg/src/libhokuyo_top_urg/hokuyo_top_urg.cpp
    pkg/trunk/hokuyo_top_urg/src/test_standalone/
    pkg/trunk/hokuyo_top_urg/src/test_standalone/Makefile
    pkg/trunk/hokuyo_top_urg/src/test_standalone/print_scans.cpp
    pkg/trunk/hokuyo_top_urg/src/top_urg_node/
    pkg/trunk/hokuyo_top_urg/src/top_urg_node/Makefile
    pkg/trunk/hokuyo_top_urg/src/top_urg_node/top_urg_node.cpp

Added: pkg/trunk/hokuyo_top_urg/build.yaml
===================================================================
--- pkg/trunk/hokuyo_top_urg/build.yaml	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/build.yaml	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,5 @@
+cpp:
+  make:
+    - src/libhokuyo_top_urg
+    - src/test_standalone
+    - src/top_urg_node

Added: pkg/trunk/hokuyo_top_urg/include/hokuyo_top_urg/hokuyo_top_urg.h
===================================================================
--- pkg/trunk/hokuyo_top_urg/include/hokuyo_top_urg/hokuyo_top_urg.h	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/include/hokuyo_top_urg/hokuyo_top_urg.h	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,37 @@
+#ifndef STAIR_HOKUYO_TOP_URG_TOP_URG_H
+#define STAIR_HOKUYO_TOP_URG_TOP_URG_H
+
+#include <string>
+using namespace std;
+
+class LightweightSerial;
+
+class HokuyoTopUrg
+{
+public:
+  HokuyoTopUrg(string serial_str);
+  ~HokuyoTopUrg();
+  bool poll();
+  inline bool is_ok() { return happy; }
+  static const int num_ranges = 1128;
+  double latest_scan[num_ranges];
+  static const double SCAN_FOV;
+
+private:
+  bool happy;
+  uint16_t incoming_scan[num_ranges], incoming_idx;
+  void process_data_line(char *line, int line_num);
+  void process_response(char *d, unsigned n);
+  LightweightSerial *serial;
+  string serial_str;
+  enum decode_state_t
+  {
+    DS_BYTE_1,
+    DS_BYTE_2,
+    DS_BYTE_3,
+  } decode_state;
+  uint32_t range_idx, cur_range;
+};
+
+#endif
+

Added: pkg/trunk/hokuyo_top_urg/manifest.xml
===================================================================
--- pkg/trunk/hokuyo_top_urg/manifest.xml	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/manifest.xml	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,11 @@
+<package>
+<description brief="Driver for the Hokuyo TOP-URG scanner">
+This package just grabs scans from the Hokuyp TOP-URG laser scanner and publishes them.
+</description>
+<author>Morgan Quigley (email: mqu...@cs...)</author>
+<license>BSD</license>
+<url>http://stair.stanford.edu</url>
+<depend package="serial_port"/>
+<depend package="range_flows"/>
+</package>
+

Added: pkg/trunk/hokuyo_top_urg/nodes/test
===================================================================
--- pkg/trunk/hokuyo_top_urg/nodes/test	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/nodes/test	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,8 @@
+#!/usr/bin/env ruby
+(puts "aaaaaa no ROS_ROOT"; exit) if !ENV['ROS_ROOT']
+require "#{`#{ENV['ROS_ROOT']}/rospack latest yamlgraph`}/lib/yamlgraph/ygl.rb"
+g = YAMLGraph.new
+#g.param 'ros_launch_args', '["/usr/bin/xterm", "-T", launch_node_name, "-e", launch_rospack, "run", launch_pkg, launch_node_exe]'
+g.param 'top_urg_node.device', '/dev/ttyACM0'
+g.node 'hokuyo_top_urg/top_urg_node'
+YAMLGraphLauncher.new.launch g


Property changes on: pkg/trunk/hokuyo_top_urg/nodes/test
___________________________________________________________________
Name: svn:executable
   + *

Added: pkg/trunk/hokuyo_top_urg/rosbuild
===================================================================
--- pkg/trunk/hokuyo_top_urg/rosbuild	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/rosbuild	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,2 @@
+#!/usr/bin/env ruby
+exec("#{`#{ENV['ROS_ROOT']}/rospack find rostools`}/scripts/yamlbuild", 'build.yaml', *ARGV)


Property changes on: pkg/trunk/hokuyo_top_urg/rosbuild
___________________________________________________________________
Name: svn:executable
   + *

Added: pkg/trunk/hokuyo_top_urg/src/libhokuyo_top_urg/Makefile
===================================================================
--- pkg/trunk/hokuyo_top_urg/src/libhokuyo_top_urg/Makefile	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/src/libhokuyo_top_urg/Makefile	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,5 @@
+SRC = hokuyo_top_urg.cpp
+OUT = ../../lib/libstair__hokuyo_top_urg.a
+PKG = stair__hokuyo_top_urg
+CFLAGS = -I../../include -I$(shell $(ROS_ROOT)/rospack find stair__serial_port)/include
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/only_rules_lib.mk

Added: pkg/trunk/hokuyo_top_urg/src/libhokuyo_top_urg/hokuyo_top_urg.cpp
===================================================================
--- pkg/trunk/hokuyo_top_urg/src/libhokuyo_top_urg/hokuyo_top_urg.cpp	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/src/libhokuyo_top_urg/hokuyo_top_urg.cpp	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,109 @@
+#include <math.h>
+#include "stair__serial_port/lightweightserial.h"
+#include "stair__hokuyo_top_urg/hokuyo_top_urg.h"
+
+const double HokuyoTopUrg::SCAN_FOV = 270.0 * M_PI / 180.0;
+
+HokuyoTopUrg::HokuyoTopUrg(string serial_str) : 
+  serial(NULL)
+{
+  serial = new LightweightSerial(serial_str.c_str(), 115200);
+  happy = serial->is_ok();
+}
+
+HokuyoTopUrg::~HokuyoTopUrg()
+{
+  if (serial)
+    delete serial;
+  serial = NULL;
+}
+
+void HokuyoTopUrg::process_data_line(char *line, int line_num)
+{
+  int linelen = strlen(line);
+  for (int i = 0; i < linelen-1; i++)
+  {
+    switch(decode_state)
+    {
+      case DS_BYTE_1:
+        incoming_scan[incoming_idx] = ((unsigned short)line[i] - 0x0030) << 12;
+        decode_state = DS_BYTE_2;
+        break;
+      case DS_BYTE_2:
+        incoming_scan[incoming_idx] += ((unsigned short)line[i] - 0x0030) << 6;
+        decode_state = DS_BYTE_3;
+        break;
+      case DS_BYTE_3:
+        incoming_scan[incoming_idx] += ((unsigned short)line[i] - 0x0030) << 0;
+        decode_state = DS_BYTE_1;
+        incoming_idx++;
+        break;
+    }
+  }
+}
+
+bool HokuyoTopUrg::poll()
+{
+  if (!happy)
+    return false;
+  
+  unsigned char serbuf[4096];
+  serial->write_block("GD0000112700\n", 13); // get a whole scan
+  bool scan_complete = false;
+  int attempts = 0;
+  incoming_idx = 0;
+  decode_state = DS_BYTE_1;
+  char count = 0;
+  const int LINEBUF_LEN = 80;
+  char linebuf[LINEBUF_LEN];
+  char linepos = 0;
+  int line_num = 0;
+  enum { L_ECHO, L_STATUS, L_TIMESTAMP, L_DATA } line_type = L_ECHO;
+  while (!scan_complete)
+  {
+    char c;
+    count++;
+    if (!serial->read(&c))
+    {
+      attempts++;
+      if (++attempts > 1000)
+      {
+        printf("Hokuyo TOP-URG: no response (%d recv)\n", count-1);
+        count = 0;
+        happy = false;
+        break;
+      }
+      usleep(1000);
+      continue;
+    }
+    if (linepos >= LINEBUF_LEN-1)
+    {
+      printf("ahhh! tried to run through linebuf. DON'T THINK SO.\n");
+      linepos = 0;
+      continue;
+    }
+    if (c == '\n')
+    {
+      linebuf[linepos] = 0;
+      if (linepos == 0)
+      {
+        //printf("end of tx range_idx = %d\n", range_idx);
+        break;
+      }
+      switch(line_type)
+      {
+        case L_ECHO:       line_type = L_STATUS; line_num = 0; break;
+        case L_STATUS:     line_type = L_TIMESTAMP; break;
+        case L_TIMESTAMP:  line_type = L_DATA; break;
+        case L_DATA:       process_data_line(linebuf, line_num++); break;
+      }
+      linepos = 0;
+    }
+    else
+      linebuf[linepos++] = c;
+  }
+  for (int i = 0; i < num_ranges; i++)
+    latest_scan[i] = incoming_scan[i] * 0.001;
+  return true;
+}
+

Added: pkg/trunk/hokuyo_top_urg/src/test_standalone/Makefile
===================================================================
--- pkg/trunk/hokuyo_top_urg/src/test_standalone/Makefile	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/src/test_standalone/Makefile	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,6 @@
+SRC = print_scans.cpp
+OUT = print_scans
+PKG = hokuyo_top_urg
+CFLAGS = -I$(shell $(ROS_ROOT)/rospack find serial_port)/include -I../../include
+LFLAGS = -L$(shell $(ROS_ROOT)/rospack find serial_port)/lib -L../../lib -lhokuyo_top_urg -lserial_port
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/only_rules.mk

Added: pkg/trunk/hokuyo_top_urg/src/test_standalone/print_scans.cpp
===================================================================
--- pkg/trunk/hokuyo_top_urg/src/test_standalone/print_scans.cpp	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/src/test_standalone/print_scans.cpp	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,20 @@
+#include <cstdio>
+#include "hokuyo_top_urg/hokuyo_top_urg.h"
+
+int main(int argc, char **argv)
+{
+  HokuyoTopUrg h("/dev/ttyACM0");
+  while(h.is_ok())
+  {
+    char line[80] = {0};
+    fgets(line, 80, stdin);
+    printf("press enter to poll a scan. type 'q<enter>' to quit...\n");
+    if (line[0] == 'q')
+      break;
+    h.poll();
+    for (int i = 0; i < h.num_ranges; i += 200)
+      printf("range %d = %f meters\n", i, h.latest_scan[i]);
+  }
+  return 0;
+}
+

Added: pkg/trunk/hokuyo_top_urg/src/top_urg_node/Makefile
===================================================================
--- pkg/trunk/hokuyo_top_urg/src/top_urg_node/Makefile	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/src/top_urg_node/Makefile	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,4 @@
+SRC = top_urg_node.cpp
+OUT = ../../nodes/top_urg_node
+PKG = hokuyo_top_urg
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk

Added: pkg/trunk/hokuyo_top_urg/src/top_urg_node/top_urg_node.cpp
===================================================================
--- pkg/trunk/hokuyo_top_urg/src/top_urg_node/top_urg_node.cpp	                        (rev 0)
+++ pkg/trunk/hokuyo_top_urg/src/top_urg_node/top_urg_node.cpp	2008-03-27 23:49:32 UTC (rev 49)
@@ -0,0 +1,52 @@
+#include <math.h>
+#include "ros/ros_slave.h"
+#include "range_flows/FlowRangeScan.h"
+#include "hokuyo_top_urg/hokuyo_top_urg.h"
+
+class TopUrgNode : public ROS_Slave
+{
+public:
+  FlowRangeScan *scan;
+  string urg_dev;
+  HokuyoTopUrg *h;
+
+  TopUrgNode() : ROS_Slave()
+  {
+    register_source(scan = new FlowRangeScan("scan"));
+    scan->start_angle = -HokuyoTopUrg::SCAN_FOV / 2.0;
+    scan->angle_increment = HokuyoTopUrg::SCAN_FOV / (double)HokuyoTopUrg::num_ranges;
+    if (!get_string_param(".device", urg_dev))
+    {
+      printf("woah! param 'device' not specified in the graph. defaulting to /dev/null\n");
+      urg_dev = "/dev/null";
+    }
+    printf("URG device set to [%s]\n", urg_dev.c_str());
+    h = new HokuyoTopUrg(urg_dev);
+    scan->set_scan_size(h->num_ranges);
+  }
+  bool send_scan()
+  {
+    if (!h->poll())
+    {
+      printf("couldn't poll the laser\n");
+      return false;
+    }
+    for (int i = 0; i < h->num_ranges; i++)
+      scan->scan[i] = h->latest_scan[i];
+    scan->publish();
+    return true;
+  }
+};
+
+int main(int argc, char **argv)
+{
+  TopUrgNode t;
+  while (t.happy())
+    if (!t.send_scan())
+    {
+      printf("couldn't get/send scan\n");
+      return 1;
+    }
+  printf("unhappy\n");
+  return 0;
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.