Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [47] pkg/trunk
|
From: <mor...@us...> - 2008-03-27 22:43:55
|
Revision: 47
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=47&view=rev
Author: morgan_quigley
Date: 2008-03-27 15:43:59 -0700 (Thu, 27 Mar 2008)
Log Message:
-----------
SHARKS WITH LASER BEAMS ON THEIR HEADS is a triangulation-based laser scanner. to save typing i just call it 'sharks'
Added Paths:
-----------
pkg/trunk/sharks/
pkg/trunk/sharks/README
pkg/trunk/sharks/build.yaml
pkg/trunk/sharks/include/
pkg/trunk/sharks/include/sharks/
pkg/trunk/sharks/include/sharks/sharks.h
pkg/trunk/sharks/lib/
pkg/trunk/sharks/manifest.xml
pkg/trunk/sharks/nodes/
pkg/trunk/sharks/nodes/data_collection
pkg/trunk/sharks/rosbuild
pkg/trunk/sharks/src/
pkg/trunk/sharks/src/libsharks/
pkg/trunk/sharks/src/libsharks/Makefile
pkg/trunk/sharks/src/libsharks/kbhit.cpp
pkg/trunk/sharks/src/libsharks/kbhit.h
pkg/trunk/sharks/src/libsharks/sharks.cpp
pkg/trunk/sharks/src/loneshark/
pkg/trunk/sharks/src/loneshark/Makefile
pkg/trunk/sharks/src/loneshark/loneshark.cpp
pkg/trunk/sharks/src/postprocess/
pkg/trunk/sharks/src/postprocess/extract_laser/
pkg/trunk/sharks/src/postprocess/extract_laser/Makefile
pkg/trunk/sharks/src/postprocess/extract_laser/extract_laser.cpp
pkg/trunk/sharks/src/postprocess/extract_laser/launcher
pkg/trunk/sharks/src/postprocess/log_fusion/
pkg/trunk/sharks/src/postprocess/log_fusion/Makefile
pkg/trunk/sharks/src/postprocess/log_fusion/log_fusion.cpp
pkg/trunk/sharks/src/postprocess/log_fusion/test_position.cpp
pkg/trunk/sharks/src/postprocess/project_points/
pkg/trunk/sharks/src/postprocess/project_points/project.m
pkg/trunk/sharks/src/sharks/
pkg/trunk/sharks/src/sharks/Makefile
pkg/trunk/sharks/src/sharks/sharks.cpp
pkg/trunk/sharks/src/sharks2/
pkg/trunk/sharks/src/sharks2/Makefile
pkg/trunk/sharks/src/sharks2/sharks2.cpp
Added: pkg/trunk/sharks/README
===================================================================
--- pkg/trunk/sharks/README (rev 0)
+++ pkg/trunk/sharks/README 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,11 @@
+Lots in here is highly experimental and should not be counted upon to do
+anything productive. At first, I wanted to have the SHARKS scanner be an actual
+ROS flow graph, but I soon realized that for a tightly coupled system such as
+this, that will introduce unnecessary complication and overhead. So now I am
+writing "libsharks" which brings in the static library drivers for the Axis
+camera and the IPDCMOT motor controller. This means that SHARKS is now tied to
+a particular camera and motor controller, but the payoff in terms of simplicity
+seems worth it for now.
+
+libsharks is then used by LONESHARK which is a standalone program that grabs
+scans.
Added: pkg/trunk/sharks/build.yaml
===================================================================
--- pkg/trunk/sharks/build.yaml (rev 0)
+++ pkg/trunk/sharks/build.yaml 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,4 @@
+cpp:
+ make:
+ - src/libsharks
+ - src/loneshark
Added: pkg/trunk/sharks/include/sharks/sharks.h
===================================================================
--- pkg/trunk/sharks/include/sharks/sharks.h (rev 0)
+++ pkg/trunk/sharks/include/sharks/sharks.h 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,54 @@
+///////////////////////////////////////////////////////////////////////////////
+// The sharks package provides a triangulation-based laser scanner.
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef SHARKS_SHARKS_H
+#define SHARKS_SHARKS_H
+
+#include <string>
+#include "axis_cam/axis_cam.h"
+#include "ipdcmot/ipdcmot.h"
+using namespace std;
+
+class Sharks
+{
+public:
+ Sharks(string axis_ip, string ipdcmot_ip);
+ ~Sharks();
+ void loneshark();
+ inline bool ok() { return cam_ok && mot_ok; }
+
+private:
+ bool cam_ok, mot_ok;
+ string axis_ip, ipdcmot_ip;
+ AxisCam *cam;
+ IPDCMOT *mot;
+ bool get_and_save_image(string filename);
+};
+
+#endif
+
Added: pkg/trunk/sharks/manifest.xml
===================================================================
--- pkg/trunk/sharks/manifest.xml (rev 0)
+++ pkg/trunk/sharks/manifest.xml 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,24 @@
+<package>
+<description brief="A triangulation-based laser scanner">
+
+Sharks with laser beams on their heads. For all we know, they were using those
+laser beams to triangulate the distance to the nearest victim.
+
+This package uses a line laser mounted on a rotary stage (driven by the
+ipdcmot package) and the axis_cam camera package to create a
+triangulation-based laser range scanner.
+
+</description>
+<author>Morgan Quigley (email: mqu...@cs...)</author>
+<license>BSD</license>
+<url>http://stair.stanford.edu</url>
+<depend package="axis_cam"/>
+<depend package="sdl_image"/>
+<depend package="ipdcmot"/>
+<depend package="common_flows"/>
+<depend package="yamlgraph"/>
+<depend package="roscpp"/>
+<depend package="vacuum"/>
+<sys_depend lib="ncurses"/>
+</package>
+
Added: pkg/trunk/sharks/nodes/data_collection
===================================================================
--- pkg/trunk/sharks/nodes/data_collection (rev 0)
+++ pkg/trunk/sharks/nodes/data_collection 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,20 @@
+#!/usr/bin/env ruby
+(puts "aaaaaaaa no ROS_ROOT"; exit) if !ENV['ROS_ROOT']
+require "#{`#{ENV['ROS_ROOT']}/rospack latest yamlgraph`}/lib/yamlgraph/ygl.rb"
+g = YAMLGraph.new
+g.param 'servo.host', '192.168.1.38'
+g.param 'axis_cam_wget_polled.host', '192.168.1.90'
+g.param 'sharks.start', '-25.0'
+g.param 'sharks.end', '25.0'
+g.param 'sharks.step', '0.5'
+g.node 'ipdcmot/servo', {'launch'=>'xterm'}
+g.node 'sharks/sharks2', {'launch'=>'xterm'}
+g.node 'axis_cam/axis_cam_wget_polled', {'launch'=>'xterm'}
+g.flow 'sharks:getpos_request', 'servo:getpos_blocking'
+g.flow 'servo:getpos_result', 'sharks:getpos_result'
+g.flow 'sharks:setpos_request', 'servo:setpos_blocking'
+g.flow 'servo:setpos_result', 'sharks:setpos_result'
+g.flow 'sharks:shutter', 'axis_cam_wget_polled:shutter'
+g.flow 'axis_cam_wget_polled:image', 'sharks:image'
+YAMLGraphLauncher.new.launch g
+
Property changes on: pkg/trunk/sharks/nodes/data_collection
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/sharks/rosbuild
===================================================================
--- pkg/trunk/sharks/rosbuild (rev 0)
+++ pkg/trunk/sharks/rosbuild 2008-03-27 22:43:59 UTC (rev 47)
@@ -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/sharks/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/sharks/src/libsharks/Makefile
===================================================================
--- pkg/trunk/sharks/src/libsharks/Makefile (rev 0)
+++ pkg/trunk/sharks/src/libsharks/Makefile 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,4 @@
+SRC = sharks.cpp kbhit.cpp
+OUT = ../../lib/libsharks.a
+PKG = sharks
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/lib.mk
Added: pkg/trunk/sharks/src/libsharks/kbhit.cpp
===================================================================
--- pkg/trunk/sharks/src/libsharks/kbhit.cpp (rev 0)
+++ pkg/trunk/sharks/src/libsharks/kbhit.cpp 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,51 @@
+#include "kbhit.h"
+#include <termios.h>
+#include <unistd.h>
+
+static struct termios initial_settings, new_settings;
+static int peek_character = -1;
+
+void init_keyboard()
+{
+ tcgetattr(0, &initial_settings);
+ new_settings = initial_settings;
+ new_settings.c_lflag &= ~ICANON;
+ new_settings.c_lflag &= ~ECHO;
+// new_settings.c_lflag &= ~ISIG;
+ new_settings.c_cc[VMIN] = 1;
+ new_settings.c_cc[VTIME] = 0;
+ tcsetattr(0, TCSANOW, &new_settings);
+}
+
+void close_keyboard()
+{
+ tcsetattr(0, TCSANOW, &initial_settings);
+}
+
+int _kbhit()
+{
+ unsigned char ch;
+ int nread;
+ new_settings.c_cc[VMIN] = 0;
+ tcsetattr(0, TCSANOW, &new_settings);
+ nread = read(0, &ch, 1);
+ new_settings.c_cc[VMIN] = 1;
+ tcsetattr(0, TCSANOW, &new_settings);
+ if (nread == 1)
+ {
+ peek_character = ch;
+ return 1;
+ }
+ else
+ {
+ peek_character = 0;
+ return 0;
+ }
+}
+
+char _getch()
+{
+ char c = peek_character;
+ peek_character = 0;
+ return c;
+}
Added: pkg/trunk/sharks/src/libsharks/kbhit.h
===================================================================
--- pkg/trunk/sharks/src/libsharks/kbhit.h (rev 0)
+++ pkg/trunk/sharks/src/libsharks/kbhit.h 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,13 @@
+#ifndef KBHIT_H
+#define KBHIT_H
+
+// this code copied from linux-sys.org, where it was copied from "Beginning
+// Linux Programming" a book by Wrox Press
+
+void init_keyboard(void);
+void close_keyboard(void);
+int _kbhit(void);
+char _getch(void);
+
+#endif
+
Added: pkg/trunk/sharks/src/libsharks/sharks.cpp
===================================================================
--- pkg/trunk/sharks/src/libsharks/sharks.cpp (rev 0)
+++ pkg/trunk/sharks/src/libsharks/sharks.cpp 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,111 @@
+///////////////////////////////////////////////////////////////////////////////
+// The sharks package provides a triangulation-based laser scanner.
+//
+// Copyright (C) 2008, Morgan Quigley, Stanford Univerity
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names
+// of its contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <sstream>
+#include "sharks/sharks.h"
+#include "axis_cam/axis_cam.h"
+#include "ipdcmot/ipdcmot.h"
+#include "kbhit.h"
+
+Sharks::Sharks(string axis_ip, string ipdcmot_ip)
+ : axis_ip(axis_ip), ipdcmot_ip(ipdcmot_ip),
+ cam_ok(true), mot_ok(true)
+{
+ cam = new AxisCam(axis_ip);
+ // make sure we can get an image
+ uint8_t *jpeg_buf;
+ uint32_t jpeg_buf_size;
+ if (!cam->get_jpeg(&jpeg_buf, &jpeg_buf_size))
+ printf("woah! couldn't get an image from [%s]\n",
+ axis_ip.c_str());
+ else
+ {
+ printf("cam ok. grabbed a %d-byte image.\n",
+ jpeg_buf_size);
+ }
+ printf("entering ipdcmot construct\n");
+ mot = new IPDCMOT(ipdcmot_ip, 0, false);
+ printf("done with ipdcmot construct\n");
+}
+
+Sharks::~Sharks()
+{
+ printf("sharks destructor\n");
+ mot->stop();
+}
+
+bool Sharks::get_and_save_image(string filename)
+{
+ uint8_t *jpeg_buf;
+ uint32_t jpeg_buf_size;
+ if (!cam->get_jpeg(&jpeg_buf, &jpeg_buf_size))
+ {
+ printf("woah! no image\n");
+ return false;
+ }
+ //printf("got a %d-byte image\n", jpeg_buf_size);
+ FILE *f = fopen(filename.c_str(), "wb");
+ fwrite(jpeg_buf, 1, jpeg_buf_size, f);
+ fclose(f);
+ return true;
+}
+
+void Sharks::loneshark()
+{
+ // this function is a standalone data-collection routine.
+ const double left_bound = 90.0, right_bound = 130.0;
+ int image_count = 1;
+ init_keyboard();
+ mot->set_pos_deg_blocking(left_bound);
+ mot->set_patrol(left_bound, right_bound, 10, 1);
+ printf("press any key to stop scanning\n");
+ while (!_kbhit())
+ {
+ double pos;
+ if (!mot->get_pos_blocking(&pos, NULL, 1))
+ {
+ printf("woah! couldn't get position\n");
+ break;
+ }
+ printf(".");
+ fflush(stdout);
+ char fnamebuf[500];
+ sprintf(fnamebuf, "img_%06d_%012.6f.jpg", image_count, pos);
+ image_count++;
+ if (!get_and_save_image(fnamebuf))
+ {
+ printf("woah! couldn't get an image\n");
+ break;
+ }
+ }
+ mot->set_pos_deg_blocking(left_bound); // stop the patrol
+ char c = _getch();
+ printf("you pressed: [%c]\n", c);
+ close_keyboard();
+}
+
Added: pkg/trunk/sharks/src/loneshark/Makefile
===================================================================
--- pkg/trunk/sharks/src/loneshark/Makefile (rev 0)
+++ pkg/trunk/sharks/src/loneshark/Makefile 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,5 @@
+SRC = loneshark.cpp
+OUT = loneshark
+LFLAGS = -L../../lib -laxis_cam -lcurl -lipdcmot
+PKG = sharks
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/sharks/src/loneshark/loneshark.cpp
===================================================================
--- pkg/trunk/sharks/src/loneshark/loneshark.cpp (rev 0)
+++ pkg/trunk/sharks/src/loneshark/loneshark.cpp 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,45 @@
+///////////////////////////////////////////////////////////////////////////////
+// The sharks package provides a triangulation-based laser scanner.
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <cstdio>
+#include "sharks/sharks.h"
+
+int main(int argc, char **argv)
+{
+ printf("construct Sharks object...\n");
+ Sharks *sharks = new Sharks("192.168.1.90", "192.168.1.38");
+
+ printf("press enter to scan or type 'a' then enter to abort\n");
+ char c = fgetc(stdin);
+ if (c != 'a')
+ sharks->loneshark();
+
+ delete sharks;
+ return 0;
+}
+
Added: pkg/trunk/sharks/src/postprocess/extract_laser/Makefile
===================================================================
--- pkg/trunk/sharks/src/postprocess/extract_laser/Makefile (rev 0)
+++ pkg/trunk/sharks/src/postprocess/extract_laser/Makefile 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,6 @@
+SRC = extract_laser.cpp
+OUT = extract_laser
+PKG = stair__sharks
+CFLAGS = -I$(shell $(ROS_ROOT)/rospack find stair__sdl)/include -I$(shell $(ROS_ROOT)/rospack find stair__sdl_image)/include
+LFLAGS = -L$(shell $(ROS_ROOT)/rospack find stair__sdl)/lib -L$(shell $(ROS_ROOT)/rospack find stair__sdl_image)/lib -lSDL -lSDL_image
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/only_rules.mk
Added: pkg/trunk/sharks/src/postprocess/extract_laser/extract_laser.cpp
===================================================================
--- pkg/trunk/sharks/src/postprocess/extract_laser/extract_laser.cpp (rev 0)
+++ pkg/trunk/sharks/src/postprocess/extract_laser/extract_laser.cpp 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,152 @@
+#include "SDL/SDL.h"
+#include "SDL/SDL_image.h"
+#include <string>
+#include <vector>
+#include <math.h>
+using namespace std;
+
+void split(const string &s, vector<string> &t, const string &d)
+{
+ size_t start = 0, end;
+ while ((end = s.find_first_of(d, start)) != string::npos)
+ {
+ t.push_back(s.substr(start, end-start));
+ start = end + 1;
+ }
+ t.push_back(s.substr(start));
+}
+
+int main(int argc, char **argv)
+{
+ string bg, fg;
+ vector<string> fgs;
+ FILE *log = fopen("log.txt", "w");
+ if (argc < 3)
+ {
+ printf("give at least two filenames.\n");
+ //bg = string("test_bg.jpg");
+ //fg = string("test_fg.jpg");
+ bg = string("../../../stair__sharks_testdata/stapler_pen_apple/shark_00001_-024.99886_.jpg");
+ fgs.push_back(string("../../../stair__sharks_testdata/stapler_pen_apple/shark_00500_-000.05039_.jpg"));
+ fgs.push_back(string("../../../stair__sharks_testdata/stapler_pen_apple/shark_00501_-000.00508_.jpg"));
+ }
+ else
+ {
+ bg = string(argv[1]);
+ for (int i = 2; i < argc; i++)
+ fgs.push_back(string(argv[i]));
+ }
+
+ SDL_Surface *screen, *bg_image, *fg_image;
+ if (SDL_Init(SDL_INIT_VIDEO) < 0)
+ {
+ printf("SDL init error: %s\n", SDL_GetError());
+ return 1;
+ }
+ bg_image = IMG_Load(bg.c_str());
+ if (!bg_image)
+ {
+ printf("SDL image load error: %s\n", IMG_GetError());
+ SDL_Quit();
+ return 1;
+ }
+ screen = SDL_SetVideoMode(bg_image->w, bg_image->h, 32, SDL_ANYFORMAT);
+ if (!screen)
+ {
+ printf("SDL_SetVideoMode error: %s\n", SDL_GetError());
+ SDL_FreeSurface(bg_image);
+ SDL_FreeSurface(fg_image);
+ SDL_Quit();
+ return 1;
+ }
+
+ for (int f_idx = 0; f_idx < fgs.size(); f_idx++)
+ {
+ double lang = -1000;
+ fg = fgs[f_idx];
+ vector<string> tokens;
+ split(fg, tokens, "_");
+ if (tokens.size() < 2)
+ {
+ printf("woah! I expected at least two tokens separated by underscores in the filename\n");
+ return 5;
+ }
+// for (int i = 0; i < tokens.size(); i++)
+// printf("%d = [%s]\n", i, tokens[i].c_str());
+ string lang_str = tokens[tokens.size() - 2];
+// printf("lang str = [%s]\n", lang_str.c_str());
+ lang = atof(lang_str.c_str());
+ printf("lang = %f\n", lang);
+
+ fg_image = IMG_Load(fg.c_str());
+ if (!fg_image)
+ {
+ printf("SDL image load error: %s\n", IMG_GetError());
+ SDL_Quit();
+ return 1;
+ }
+ //printf("loaded %s: %dx%d %dbpp\n", argv[1], image->w, image->h, image->format->BitsPerPixel);
+ //printf("image bpp = %d\n", bg_image->format->BytesPerPixel);
+ for (int y = 0; y < bg_image->h; y++)
+ {
+ double centroid = 0, sum = 0;
+ for (int x = 0; x < bg_image->w; x++)
+ {
+ uint8_t *bg_p = (uint8_t *)bg_image->pixels + y*bg_image->pitch + x*3;
+ uint8_t *fg_p = (uint8_t *)fg_image->pixels + y*fg_image->pitch + x*3;
+ uint8_t bg_r = *(bg_p + bg_image->format->Rshift/8);
+ uint8_t fg_r = *(fg_p + fg_image->format->Rshift/8);
+ *(fg_p + fg_image->format->Gshift/8) = 0;
+ *(fg_p + bg_image->format->Bshift/8) = 0;
+ int diff = fg_r - bg_r;
+ if (diff > 50)
+ {
+ *(fg_p + fg_image->format->Rshift/8) = diff;
+ centroid += (diff) * x;
+ sum += diff;
+ }
+ else
+ *(fg_p + fg_image->format->Rshift/8) = 0;
+ }
+ centroid /= sum;
+ if (sum > 200)
+ {
+ //printf("centroid = %f sum = %f\n", centroid, sum);
+ int x = (int)floor(centroid);
+ if (x < 0) x = 0;
+ if (x >= fg_image->w) x = fg_image->w - 1;
+ uint8_t *fg_p = (uint8_t *)fg_image->pixels + y*fg_image->pitch + x*3;
+ *(fg_p + fg_image->format->Gshift/8) = 255;
+ fprintf(log, "%f %d %f %f\n", lang, y, centroid, sum);
+ }
+ }
+
+ SDL_BlitSurface(fg_image, 0, screen, 0);
+ SDL_Flip(screen);
+ SDL_FreeSurface(fg_image);
+ }
+ bool done = false;
+ SDL_Event event;
+ while (!done && SDL_WaitEvent(&event) != -1)
+ {
+ switch(event.type)
+ {
+ case SDL_KEYDOWN:
+ case SDL_QUIT:
+ done = true;
+ break;
+ case SDL_VIDEOEXPOSE:
+ SDL_BlitSurface(fg_image, 0, screen, 0);
+ SDL_Flip(screen);
+ break;
+ default:
+ break;
+ }
+ }
+ SDL_FreeSurface(bg_image);
+ SDL_Quit();
+ fclose(log);
+
+ return 0;
+}
+
Added: pkg/trunk/sharks/src/postprocess/extract_laser/launcher
===================================================================
--- pkg/trunk/sharks/src/postprocess/extract_laser/launcher (rev 0)
+++ pkg/trunk/sharks/src/postprocess/extract_laser/launcher 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,5 @@
+#!/usr/bin/env ruby
+rp = "#{ENV['ROS_ROOT']}/rospack"
+olp = ENV['LD_LIBRARY_PATH']
+ENV['LD_LIBRARY_PATH'] = (olp ? olp + ':' : '') + "#{`#{rp} find stair.sdl`}/lib:#{`#{rp} find stair__sdl_image`}/lib"
+exec('./extract_laser', *ARGV)
Property changes on: pkg/trunk/sharks/src/postprocess/extract_laser/launcher
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/sharks/src/postprocess/log_fusion/Makefile
===================================================================
--- pkg/trunk/sharks/src/postprocess/log_fusion/Makefile (rev 0)
+++ pkg/trunk/sharks/src/postprocess/log_fusion/Makefile 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,6 @@
+SRC = log_fusion.cpp
+OUT = log_fusion
+#CFLAGS = -I../../include
+#LFLAGS = -L../../lib -lipdcmot -lrt
+PKG = sharks
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/sharks/src/postprocess/log_fusion/log_fusion.cpp
===================================================================
--- pkg/trunk/sharks/src/postprocess/log_fusion/log_fusion.cpp (rev 0)
+++ pkg/trunk/sharks/src/postprocess/log_fusion/log_fusion.cpp 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,13 @@
+#include "vacuum/vacuparse.h"
+
+int main(int argc, char **argv)
+{
+ VacuParse vp("string.dump");
+ uint32_t atom_len, publish_count, secs, nsecs;
+ uint8_t *atom_ptr;
+ vp.get_next_atom(&atom_len, &atom_ptr, &publish_count, &secs, &nsecs);
+ vp.get_next_atom(&atom_len, &atom_ptr, &publish_count, &secs, &nsecs);
+ vp.get_next_atom(&atom_len, &atom_ptr, &publish_count, &secs, &nsecs);
+ vp.get_next_atom(&atom_len, &atom_ptr, &publish_count, &secs, &nsecs);
+ return 0;
+}
Added: pkg/trunk/sharks/src/postprocess/log_fusion/test_position.cpp
===================================================================
--- pkg/trunk/sharks/src/postprocess/log_fusion/test_position.cpp (rev 0)
+++ pkg/trunk/sharks/src/postprocess/log_fusion/test_position.cpp 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,19 @@
+#include <cstdio>
+#include "ipdcmot/ipdcmot.h"
+
+int main(int argc, char **argv)
+{
+ IPDCMOT *mot = new IPDCMOT("192.168.1.38", 75);
+ printf("sleeping...\n");
+ usleep(1000000);
+ printf("going to 20 degrees\n");
+ mot->set_pos_deg_blocking(20);
+ printf("done. going to 40 degrees\n");
+ mot->set_pos_deg_blocking(40);
+ printf("done.\n");
+
+
+ delete mot;
+ return 0;
+}
+
Added: pkg/trunk/sharks/src/postprocess/project_points/project.m
===================================================================
--- pkg/trunk/sharks/src/postprocess/project_points/project.m (rev 0)
+++ pkg/trunk/sharks/src/postprocess/project_points/project.m 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,40 @@
+if ~exist('d')
+ d = load('laser.txt');
+ fprintf('ok, loaded %d scanlines\n', size(d,1));
+end
+baseline = 0.49;
+hres = 704;
+vres = 480;
+hfov = 50;
+vfov = hfov * vres / hres;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+scene = [];
+r = 2000:length(d);
+lang = -d(r,1) * pi/180 + 40*pi/180;
+img_y = d(r,2);
+img_x = d(r,3);
+azi = ((hres/2-img_x) / hres * hfov + 90) * pi/180;
+ele = (vres/2-img_y) / vres * vfov * pi/180;
+ray = [cos(ele).*cos(azi) sin(ele) cos(ele).*sin(azi)];
+ln = [-cos(lang), zeros(length(lang),1), -sin(lang)];
+lorg = [baseline; 0; 0];
+numer = baseline * ln(:,1);
+denom = dot(ray',ln')';
+t = numer ./ denom;
+t = [t,t,t];
+proj = t .* ray;
+
+%plot_idx = 1:20:(length(proj));
+%plot3(proj(plot_idx,1),proj(plot_idx,3),proj(plot_idx,2),'.');
+%axis equal;
+
+%save -ascii 'test.txt' proj
+
+% in non-insane terms:
+%proj = t .* ray
+%for i=1:100:length(img_y)
+% t = lorg'*ln(i,:)' / (ray(i,:) * ln(i,:)');
+% proj = t * ray(i,:);
+% scene = [scene; proj];
+%end
+
Added: pkg/trunk/sharks/src/sharks/Makefile
===================================================================
--- pkg/trunk/sharks/src/sharks/Makefile (rev 0)
+++ pkg/trunk/sharks/src/sharks/Makefile 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,4 @@
+SRC = sharks.cpp
+OUT = ../../nodes/sharks
+PKG = sharks
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/sharks/src/sharks/sharks.cpp
===================================================================
--- pkg/trunk/sharks/src/sharks/sharks.cpp (rev 0)
+++ pkg/trunk/sharks/src/sharks/sharks.cpp 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,139 @@
+// You must have ncurses installed on your system.
+// on Ubuntu: sudo apt-get install libncurses-dev
+#include <ncurses.h>
+#include "ros/ros_slave.h"
+#include "ros/ros_flowpair.h"
+#include "common_flows/FlowInt32.h"
+#include "common_flows/FlowFloat64.h"
+#include "common_flows/FlowEmpty.h"
+#include "image_flows/FlowImage.h"
+#include "image_flows/image_flow_codec.h"
+
+class Sharks : public ROS_Slave
+{
+public:
+ FlowEmpty *shutter;
+ FlowFloat64 *setpos_request, *getpos_result;
+ FlowEmpty *getpos_request;
+ FlowInt32 *setpos_result;
+ FlowImage *image;
+ ImageFlowCodec<FlowImage> *codec;
+
+ double start_pos, end_pos, step;
+ ROS_FlowPair *setpos_pair, *getpos_pair, *cam_pair;
+ bool setpos_ok, image_ok;
+ double last_pos;
+ int image_count;
+
+ Sharks() : ROS_Slave(), image_count(0)
+ {
+ register_source(shutter = new FlowEmpty("shutter"));
+ register_source(setpos_request = new FlowFloat64("setpos_request"));
+ register_source(getpos_request = new FlowEmpty("getpos_request"));
+ register_sink(image = new FlowImage("image"), ROS_CALLBACK(Sharks, image_callback));
+ register_sink(setpos_result = new FlowInt32("setpos_result"), ROS_CALLBACK(Sharks, setpos_result_callback));
+ register_sink(getpos_result = new FlowFloat64("getpos_result"), ROS_CALLBACK(Sharks, getpos_result_callback));
+ setpos_pair = new ROS_FlowPair(setpos_request, setpos_result);
+ getpos_pair = new ROS_FlowPair(getpos_request, getpos_result);
+ cam_pair = new ROS_FlowPair(shutter, image);
+ register_with_master();
+ codec = new ImageFlowCodec<FlowImage>(image);
+ start_pos = -10;
+ end_pos = 10;
+ step = 1;
+ get_double_param(".start", &start_pos);
+ get_double_param(".end", &end_pos);
+ get_double_param(".step", &step);
+ }
+ virtual ~Sharks() { }
+
+ bool move_motor(double target, double *actual)
+ {
+ setpos_request->data = target;
+ setpos_pair->publish_and_block(30.0);
+ if (!setpos_ok)
+ {
+ printf("setpos not OK\n");
+ return false;
+ }
+ if (actual)
+ {
+ getpos_pair->publish_and_block(5.0);
+ *actual = last_pos;
+ }
+ return true;
+ }
+
+ void scan()
+ {
+ if (step == 0)
+ step = 1;
+ if (start_pos < end_pos && step < 0)
+ step *= -1;
+ else if (start_pos > end_pos && step > 0)
+ step *= -1;
+ for (double pos = start_pos; pos < end_pos; pos += step)
+ {
+ double actual;
+ if (!move_motor(pos, &actual))
+ {
+ printf("error moving to %f\n", pos);
+ break;
+ }
+ mvprintw(1,0,"commanded %f actual %f\n", pos, actual);
+ image_ok = false;
+ cam_pair->publish_and_block(5.0);
+ if (image_ok)
+ {
+ char fnamebuf[500];
+ image_count++;
+ snprintf(fnamebuf, sizeof(fnamebuf),
+ "shark_%05d_%010.5f_.jpg", image_count, actual);
+ codec->write_file(string(fnamebuf), 95);
+ }
+ }
+ }
+ void setpos_result_callback()
+ {
+ setpos_ok = (setpos_result->data == 1);
+ }
+ void getpos_result_callback()
+ {
+ last_pos = getpos_result->data;
+ }
+ void image_callback()
+ {
+ mvprintw(2,0,"received a %d by %d image\n", image->width, image->height);
+ image_ok = true;
+ }
+};
+
+void restore_terminal()
+{
+ echo();
+ noraw();
+ endwin();
+}
+
+int main(int argc, char **argv)
+{
+ Sharks s;
+ atexit(restore_terminal);
+ initscr();
+ noecho();
+ nodelay(stdscr, FALSE);
+ mvprintw(0, 0, "Press s to scan\n");
+ while (s.happy())
+ {
+ raw();
+ int c = getch();
+ noraw();
+ if (c == 0x03 || c == 0x04)
+ break;
+ else if (c == 's')
+ s.scan();
+ }
+ printf("sharks exiting\n");
+ return 0;
+}
+
Added: pkg/trunk/sharks/src/sharks2/Makefile
===================================================================
--- pkg/trunk/sharks/src/sharks2/Makefile (rev 0)
+++ pkg/trunk/sharks/src/sharks2/Makefile 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,4 @@
+SRC = sharks2.cpp
+OUT = ../../nodes/sharks2
+PKG = sharks
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/sharks/src/sharks2/sharks2.cpp
===================================================================
--- pkg/trunk/sharks/src/sharks2/sharks2.cpp (rev 0)
+++ pkg/trunk/sharks/src/sharks2/sharks2.cpp 2008-03-27 22:43:59 UTC (rev 47)
@@ -0,0 +1,140 @@
+// You must have ncurses installed on your system.
+// on Ubuntu: sudo apt-get install libncurses-dev
+#include <ncurses.h>
+#include "ros/ros_slave.h"
+#include "ros/ros_flowpair.h"
+#include "common_flows/FlowInt32.h"
+#include "common_flows/FlowFloat64.h"
+#include "common_flows/FlowEmpty.h"
+#include "image_flows/FlowImage.h"
+#include "image_flows/image_flow_codec.h"
+#include "ipdcmot/FlowPatrol.h"
+
+class Sharks : public ROS_Slave
+{
+public:
+ FlowEmpty *shutter;
+ FlowFloat64 *setpos_request, *getpos_result;
+ FlowEmpty *getpos_request;
+ FlowInt32 *setpos_result;
+ FlowImage *image;
+ ImageFlowCodec<FlowImage> *codec;
+
+ double start_pos, end_pos, step;
+ ROS_FlowPair *setpos_pair, *getpos_pair, *cam_pair;
+ bool setpos_ok, image_ok;
+ double last_pos;
+ int image_count;
+
+ Sharks() : ROS_Slave(), image_count(0)
+ {
+ register_source(shutter = new FlowEmpty("shutter"));
+ register_source(setpos_request = new FlowFloat64("setpos_request"));
+ register_source(getpos_request = new FlowEmpty("getpos_request"));
+ register_sink(image = new FlowImage("image"), ROS_CALLBACK(Sharks, image_callback));
+ register_sink(setpos_result = new FlowInt32("setpos_result"), ROS_CALLBACK(Sharks, setpos_result_callback));
+ register_sink(getpos_result = new FlowFloat64("getpos_result"), ROS_CALLBACK(Sharks, getpos_result_callback));
+ setpos_pair = new ROS_FlowPair(setpos_request, setpos_result);
+ getpos_pair = new ROS_FlowPair(getpos_request, getpos_result);
+ cam_pair = new ROS_FlowPair(shutter, image);
+ register_with_master();
+ codec = new ImageFlowCodec<FlowImage>(image);
+ start_pos = -10;
+ end_pos = 10;
+ step = 1;
+ get_double_param(".start", &start_pos);
+ get_double_param(".end", &end_pos);
+ get_double_param(".step", &step);
+ }
+ virtual ~Sharks() { }
+
+ bool move_motor(double target, double *actual)
+ {
+ setpos_request->data = target;
+ setpos_pair->publish_and_block(30.0);
+ if (!setpos_ok)
+ {
+ printf("setpos not OK\n");
+ return false;
+ }
+ if (actual)
+ {
+ getpos_pair->publish_and_block(5.0);
+ *actual = last_pos;
+ }
+ return true;
+ }
+
+ void scan()
+ {
+ if (step == 0)
+ step = 1;
+ if (start_pos < end_pos && step < 0)
+ step *= -1;
+ else if (start_pos > end_pos && step > 0)
+ step *= -1;
+ for (double pos = start_pos; pos < end_pos; pos += step)
+ {
+ double actual;
+ if (!move_motor(pos, &actual))
+ {
+ printf("error moving to %f\n", pos);
+ break;
+ }
+ mvprintw(1,0,"commanded %f actual %f\n", pos, actual);
+ image_ok = false;
+ cam_pair->publish_and_block(5.0);
+ if (image_ok)
+ {
+ char fnamebuf[500];
+ image_count++;
+ snprintf(fnamebuf, sizeof(fnamebuf),
+ "shark_%05d_%010.5f_.jpg", image_count, actual);
+ codec->write_file(string(fnamebuf), 95);
+ }
+ }
+ }
+ void setpos_result_callback()
+ {
+ setpos_ok = (setpos_result->data == 1);
+ }
+ void getpos_result_callback()
+ {
+ last_pos = getpos_result->data;
+ }
+ void image_callback()
+ {
+ mvprintw(2,0,"received a %d by %d image\n", image->width, image->height);
+ image_ok = true;
+ }
+};
+
+void restore_terminal()
+{
+ echo();
+ noraw();
+ endwin();
+}
+
+int main(int argc, char **argv)
+{
+ Sharks s;
+ atexit(restore_terminal);
+ initscr();
+ noecho();
+ nodelay(stdscr, FALSE);
+ mvprintw(0, 0, "Press s to scan\n");
+ while (s.happy())
+ {
+ raw();
+ int c = getch();
+ noraw();
+ if (c == 0x03 || c == 0x04)
+ break;
+ else if (c == 's')
+ s.scan();
+ }
+ printf("sharks exiting\n");
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
