Menu
▾
▴
personalrobots-commit
|
From: <mor...@us...> - 2008-03-27 00:28:04
|
Revision: 39
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=39&view=rev
Author: morgan_quigley
Date: 2008-03-26 17:28:10 -0700 (Wed, 26 Mar 2008)
Log Message:
-----------
image_flows is now split into part that landed in common_flows and part that landed in image_utils
Added Paths:
-----------
pkg/trunk/image_utils/
pkg/trunk/image_utils/build.yaml
pkg/trunk/image_utils/examples/
pkg/trunk/image_utils/manifest.xml
pkg/trunk/image_utils/nodes/
pkg/trunk/image_utils/rosbuild
pkg/trunk/image_utils/test/
Removed Paths:
-------------
pkg/trunk/image_flows/
pkg/trunk/image_utils/build.yaml
pkg/trunk/image_utils/examples/
pkg/trunk/image_utils/flows/
pkg/trunk/image_utils/include/
pkg/trunk/image_utils/lib/
pkg/trunk/image_utils/manifest.xml
pkg/trunk/image_utils/nodes/
pkg/trunk/image_utils/rosbuild
pkg/trunk/image_utils/test/
Copied: pkg/trunk/image_utils (from rev 33, pkg/trunk/image_flows)
Deleted: pkg/trunk/image_utils/build.yaml
===================================================================
--- pkg/trunk/image_flows/build.yaml 2008-03-26 02:28:34 UTC (rev 33)
+++ pkg/trunk/image_utils/build.yaml 2008-03-27 00:28:10 UTC (rev 39)
@@ -1,5 +0,0 @@
-cpp:
- make:
- - test/get_raster
- - test/read_write
- - examples/image_sender
Copied: pkg/trunk/image_utils/build.yaml (from rev 38, pkg/trunk/image_flows/build.yaml)
===================================================================
--- pkg/trunk/image_utils/build.yaml (rev 0)
+++ pkg/trunk/image_utils/build.yaml 2008-03-27 00:28:10 UTC (rev 39)
@@ -0,0 +1,5 @@
+cpp:
+ make:
+ - test/get_raster
+ - test/read_write
+ - examples/image_sender
Copied: pkg/trunk/image_utils/examples (from rev 38, pkg/trunk/image_flows/examples)
Deleted: pkg/trunk/image_utils/manifest.xml
===================================================================
--- pkg/trunk/image_flows/manifest.xml 2008-03-26 02:28:34 UTC (rev 33)
+++ pkg/trunk/image_utils/manifest.xml 2008-03-27 00:28:10 UTC (rev 39)
@@ -1,19 +0,0 @@
-<package>
- <description brief="Flows carrying images">
-
- This package contains serialization/deserialization for images, and handy
- libraries to compress/decompress them. The image flow itself is just a
- binary byte block. It is up to the sender and receiver to decide what goes
- in the byte block. This package supplies a library that makes it easy to do
- this. This library is just provided in C++ for now, but could be ported
- relatively easily to more modern languages, since many of them have image
- compression codecs built-in.
-
- </description>
- <author>Morgan Quigley (email: mqu...@cs...)</author>
- <license>BSD</license>
- <url>http://stair.stanford.edu</url>
- <depend package="roscpp"/>
- <depend package="ijg_libjpeg"/>
-</package>
-
Copied: pkg/trunk/image_utils/manifest.xml (from rev 38, pkg/trunk/image_flows/manifest.xml)
===================================================================
--- pkg/trunk/image_utils/manifest.xml (rev 0)
+++ pkg/trunk/image_utils/manifest.xml 2008-03-27 00:28:10 UTC (rev 39)
@@ -0,0 +1,15 @@
+<package>
+ <description brief="Image utilities">
+
+ This package provides a few sample nodes to show how to use the Image flow,
+ as well as a few test programs and utility programs (e.g. which send out
+ a static image at arbitrary intervals).
+
+ </description>
+ <author>Morgan Quigley (email: mqu...@cs...)</author>
+ <license>BSD</license>
+ <url>http://stair.stanford.edu</url>
+ <depend package="roscpp"/>
+ <depend package="ijg_libjpeg"/>
+</package>
+
Copied: pkg/trunk/image_utils/nodes (from rev 38, pkg/trunk/image_flows/nodes)
Deleted: pkg/trunk/image_utils/rosbuild
===================================================================
--- pkg/trunk/image_flows/rosbuild 2008-03-26 02:28:34 UTC (rev 33)
+++ pkg/trunk/image_utils/rosbuild 2008-03-27 00:28:10 UTC (rev 39)
@@ -1,2 +0,0 @@
-#!/usr/bin/env ruby
-exec("#{`#{ENV['ROS_ROOT']}/rospack find rostools`}/scripts/yamlbuild", 'build.yaml', *ARGV)
Copied: pkg/trunk/image_utils/rosbuild (from rev 38, pkg/trunk/image_flows/rosbuild)
===================================================================
--- pkg/trunk/image_utils/rosbuild (rev 0)
+++ pkg/trunk/image_utils/rosbuild 2008-03-27 00:28:10 UTC (rev 39)
@@ -0,0 +1,2 @@
+#!/usr/bin/env ruby
+exec("#{`#{ENV['ROS_ROOT']}/rospack find rostools`}/scripts/yamlbuild", 'build.yaml', *ARGV)
Copied: pkg/trunk/image_utils/test (from rev 38, pkg/trunk/image_flows/test)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
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_p...
[truncated message content] |
|
From: <mor...@us...> - 2008-03-27 23:49:29
|
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.
|
|
From: <mor...@us...> - 2008-03-27 23:52:27
|
Revision: 50
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=50&view=rev
Author: morgan_quigley
Date: 2008-03-27 16:52:34 -0700 (Thu, 27 Mar 2008)
Log Message:
-----------
range flows package. copying over from my personal svn. this will need to eventually get absorbed into the common_flows package and a range_utils package (for tests, visualization, etc.)
Added Paths:
-----------
pkg/trunk/range_flows/
pkg/trunk/range_flows/build.yaml
pkg/trunk/range_flows/flows/
pkg/trunk/range_flows/flows/RangeScan.flow
pkg/trunk/range_flows/manifest.xml
pkg/trunk/range_flows/nodes/
pkg/trunk/range_flows/nodes/test_send_recv
pkg/trunk/range_flows/rosbuild
pkg/trunk/range_flows/src/
pkg/trunk/range_flows/src/test/
pkg/trunk/range_flows/src/test/range_receiver/
pkg/trunk/range_flows/src/test/range_receiver/Makefile
pkg/trunk/range_flows/src/test/range_receiver/range_receiver.cpp
pkg/trunk/range_flows/src/test/range_sender/
pkg/trunk/range_flows/src/test/range_sender/Makefile
pkg/trunk/range_flows/src/test/range_sender/range_sender.cpp
Added: pkg/trunk/range_flows/build.yaml
===================================================================
--- pkg/trunk/range_flows/build.yaml (rev 0)
+++ pkg/trunk/range_flows/build.yaml 2008-03-27 23:52:34 UTC (rev 50)
@@ -0,0 +1,4 @@
+cpp:
+ make:
+ - src/test/range_sender
+ - src/test/range_receiver
Added: pkg/trunk/range_flows/flows/RangeScan.flow
===================================================================
--- pkg/trunk/range_flows/flows/RangeScan.flow (rev 0)
+++ pkg/trunk/range_flows/flows/RangeScan.flow 2008-03-27 23:52:34 UTC (rev 50)
@@ -0,0 +1,5 @@
+int32 frame_id
+float32 start_angle
+float32 angle_increment
+float32[] scan
+
Added: pkg/trunk/range_flows/manifest.xml
===================================================================
--- pkg/trunk/range_flows/manifest.xml (rev 0)
+++ pkg/trunk/range_flows/manifest.xml 2008-03-27 23:52:34 UTC (rev 50)
@@ -0,0 +1,10 @@
+<package>
+ <description brief="Flows carrying ranges">
+ This package contains serialization/deserialization for range scans.
+ </description>
+ <author>Morgan Quigley (email: mqu...@cs...)</author>
+ <license>BSD</license>
+ <url>http://stair.stanford.edu</url>
+ <depend package="roscpp"/>
+</package>
+
Added: pkg/trunk/range_flows/nodes/test_send_recv
===================================================================
--- pkg/trunk/range_flows/nodes/test_send_recv (rev 0)
+++ pkg/trunk/range_flows/nodes/test_send_recv 2008-03-27 23:52:34 UTC (rev 50)
@@ -0,0 +1,12 @@
+#!/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 'range_sender.freq', '1000.0'
+
+g.param 'ros_launch_args', ["/usr/bin/xterm", "-T", "%(node_name)s", "-e", "bash", "-c", "valgrind %(rospack)s run %(pkg)s %(node_type)s; bash"]
+
+g.node 'range_flows/range_sender'
+g.node 'range_flows/range_receiver'
+g.flow 'range_sender.scan', 'range_receiver.scan'
+YAMLGraphLauncher.new.launch g
Property changes on: pkg/trunk/range_flows/nodes/test_send_recv
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/range_flows/rosbuild
===================================================================
--- pkg/trunk/range_flows/rosbuild (rev 0)
+++ pkg/trunk/range_flows/rosbuild 2008-03-27 23:52:34 UTC (rev 50)
@@ -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/range_flows/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/range_flows/src/test/range_receiver/Makefile
===================================================================
--- pkg/trunk/range_flows/src/test/range_receiver/Makefile (rev 0)
+++ pkg/trunk/range_flows/src/test/range_receiver/Makefile 2008-03-27 23:52:34 UTC (rev 50)
@@ -0,0 +1,4 @@
+SRC = range_receiver.cpp
+OUT = ../../../nodes/range_receiver
+PKG = range_flows
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/range_flows/src/test/range_receiver/range_receiver.cpp
===================================================================
--- pkg/trunk/range_flows/src/test/range_receiver/range_receiver.cpp (rev 0)
+++ pkg/trunk/range_flows/src/test/range_receiver/range_receiver.cpp 2008-03-27 23:52:34 UTC (rev 50)
@@ -0,0 +1,37 @@
+#include <sstream>
+#include "ros/ros_slave.h"
+#include "range_flows/FlowRangeScan.h"
+
+class RangeReceiver : public ROS_Slave
+{
+public:
+ FlowRangeScan *my_scan;
+ double freq;
+
+ RangeReceiver() : ROS_Slave(), freq(1)
+ {
+ register_sink(my_scan = new FlowRangeScan("scan"), ROS_CALLBACK(RangeReceiver, range_cb));
+ printf("params:\n");
+ print_param_names();
+ printf("EO talker params\n");
+ bool b = get_double_param("freq", &freq);
+ printf("b = %d freq = %f\n", b, freq);
+ }
+ virtual ~RangeReceiver() { }
+ void range_cb()
+ {
+ printf("count = %d secs = %d nsecs = %d\n", my_scan->publish_count, my_scan->get_stamp_secs(), my_scan->get_stamp_nsecs());
+ /*
+ for (int i = 0; i < my_scan->get_scan_size(); i++)
+ printf("range %d = %f\n", i, my_scan->scan[i]);
+ */
+ }
+};
+
+int main(int argc, char **argv)
+{
+ RangeReceiver rr;
+ printf("entering receiver spin loop\n");
+ rr.spin();
+ return 0;
+}
Added: pkg/trunk/range_flows/src/test/range_sender/Makefile
===================================================================
--- pkg/trunk/range_flows/src/test/range_sender/Makefile (rev 0)
+++ pkg/trunk/range_flows/src/test/range_sender/Makefile 2008-03-27 23:52:34 UTC (rev 50)
@@ -0,0 +1,4 @@
+SRC = range_sender.cpp
+OUT = ../../../nodes/range_sender
+PKG = range_flows
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/range_flows/src/test/range_sender/range_sender.cpp
===================================================================
--- pkg/trunk/range_flows/src/test/range_sender/range_sender.cpp (rev 0)
+++ pkg/trunk/range_flows/src/test/range_sender/range_sender.cpp 2008-03-27 23:52:34 UTC (rev 50)
@@ -0,0 +1,43 @@
+#include <sstream>
+#include "ros/ros_slave.h"
+#include "range_flows/FlowRangeScan.h"
+
+class RangeSender : public ROS_Slave
+{
+public:
+ FlowRangeScan *my_scan;
+ double freq;
+
+ RangeSender() : ROS_Slave(), freq(1)
+ {
+ register_source(my_scan = new FlowRangeScan("scan"));
+ my_scan->set_scan_size(10);
+ printf("TALKER params:\n");
+ print_param_names();
+ printf("EO talker params\n");
+ bool b = get_double_param(".freq", &freq);
+ printf("b = %d freq = %f\n", b, freq);
+ }
+ virtual ~RangeSender() { }
+ void send_scan()
+ {
+ for (int i = 0; i < my_scan->get_scan_size(); i++)
+ my_scan->scan[i] = rand() % 100;
+ my_scan->publish();
+ }
+};
+
+int main(int argc, char **argv)
+{
+ RangeSender rs;
+ printf("entering talker spin loop\n");
+ int sleep_usecs = (int)(1000000.0 / rs.freq);
+ printf("sleep usecs = %d\n", sleep_usecs);
+ while (rs.happy())
+ {
+ usleep(sleep_usecs);
+ rs.send_scan();
+ }
+ printf("rangesender says GOODNIGHT\n");
+ return 0;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-03-27 23:56:08
|
Revision: 51
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=51&view=rev
Author: morgan_quigley
Date: 2008-03-27 16:56:14 -0700 (Thu, 27 Mar 2008)
Log Message:
-----------
copy my silly little serial port library from my personal svn
Added Paths:
-----------
pkg/trunk/serial_port/
pkg/trunk/serial_port/build.yaml
pkg/trunk/serial_port/include/
pkg/trunk/serial_port/include/serial_port/
pkg/trunk/serial_port/include/serial_port/lightweightserial.h
pkg/trunk/serial_port/lib/
pkg/trunk/serial_port/manifest.xml
pkg/trunk/serial_port/rosbuild
pkg/trunk/serial_port/src/
pkg/trunk/serial_port/src/libserial_port/
pkg/trunk/serial_port/src/libserial_port/Makefile
pkg/trunk/serial_port/src/libserial_port/lightweightserial.cpp
Added: pkg/trunk/serial_port/build.yaml
===================================================================
--- pkg/trunk/serial_port/build.yaml (rev 0)
+++ pkg/trunk/serial_port/build.yaml 2008-03-27 23:56:14 UTC (rev 51)
@@ -0,0 +1,3 @@
+cpp:
+ make:
+ - src/libserial_port
Added: pkg/trunk/serial_port/include/serial_port/lightweightserial.h
===================================================================
--- pkg/trunk/serial_port/include/serial_port/lightweightserial.h (rev 0)
+++ pkg/trunk/serial_port/include/serial_port/lightweightserial.h 2008-03-27 23:56:14 UTC (rev 51)
@@ -0,0 +1,62 @@
+///////////////////////////////////////////////////////////////////////////////
+// The serial_port package provides small, simple static libraries to access
+// serial devices
+//
+// 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 SERIALPORT_LIGHTWEIGHT_SERIAL_H
+#define SERIALPORT_LIGHTWEIGHT_SERIAL_H
+
+#include <stdint.h>
+
+class LightweightSerial
+{
+ public:
+ LightweightSerial(const char *port, int baud);
+ ~LightweightSerial();
+
+ bool read(uint8_t *b);
+ int read_block(uint8_t *block, uint32_t max_read_len);
+
+ bool write(const uint8_t b);
+ bool write_block(const uint8_t *block, uint32_t write_len);
+
+ inline bool is_ok() { return happy; }
+
+ // type-conversion wrappers so we can handle either signed or unsigned
+ inline bool read(char *c) { return read((uint8_t *)c); }
+ inline int read_block(char *block, uint32_t max_read_len) { return read_block(block, max_read_len); }
+ inline bool write(char c) { return write((uint8_t)c); }
+ inline bool write_block(const char *block, uint32_t write_len) { return write_block((uint8_t *)block, write_len); }
+
+ private:
+ int baud;
+ int fd;
+ bool happy;
+};
+
+#endif
+
Added: pkg/trunk/serial_port/manifest.xml
===================================================================
--- pkg/trunk/serial_port/manifest.xml (rev 0)
+++ pkg/trunk/serial_port/manifest.xml 2008-03-27 23:56:14 UTC (rev 51)
@@ -0,0 +1,9 @@
+<package>
+<description brief="A few simple serial port classes">
+These are used all over the place for talking to hardware. These implementations are certainly suboptimal, but they do seem to work.
+</description>
+<author>Morgan Quigley (email: mqu...@cs...)</author>
+<license>BSD</license>
+<url>http://stair.stanford.edu</url>
+</package>
+
Added: pkg/trunk/serial_port/rosbuild
===================================================================
--- pkg/trunk/serial_port/rosbuild (rev 0)
+++ pkg/trunk/serial_port/rosbuild 2008-03-27 23:56:14 UTC (rev 51)
@@ -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/serial_port/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/serial_port/src/libserial_port/Makefile
===================================================================
--- pkg/trunk/serial_port/src/libserial_port/Makefile (rev 0)
+++ pkg/trunk/serial_port/src/libserial_port/Makefile 2008-03-27 23:56:14 UTC (rev 51)
@@ -0,0 +1,5 @@
+SRC = lightweightserial.cpp
+OUT = ../../lib/libserial_port.a
+PKG = serial_port
+CFLAGS = -I../../include
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/only_rules_lib.mk
Added: pkg/trunk/serial_port/src/libserial_port/lightweightserial.cpp
===================================================================
--- pkg/trunk/serial_port/src/libserial_port/lightweightserial.cpp (rev 0)
+++ pkg/trunk/serial_port/src/libserial_port/lightweightserial.cpp 2008-03-27 23:56:14 UTC (rev 51)
@@ -0,0 +1,124 @@
+///////////////////////////////////////////////////////////////////////////////
+// The serial_port package provides small, simple static libraries to access
+// serial devices
+//
+// 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 "serial_port/lightweightserial.h"
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <string.h>
+
+LightweightSerial::LightweightSerial(const char *port, int baud) :
+ baud(baud), happy(false), fd(0)
+{
+ printf("about to try to open [%s]\n", port);
+ fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
+ if (fd < 0)
+ {
+ printf(" ahhhhhhhhh couldn't open port [%s]\n", port);
+ return;
+ }
+ else
+ printf("opened [%s] successfully\n", port);
+ // put the port in nonblocking mode
+ struct termios oldtio, newtio;
+ if (tcgetattr(fd, &oldtio) < 0)
+ {
+ printf("ahhhhhhh couldn't run tcgetattr()\n");
+ return;
+ }
+ bzero(&newtio, sizeof(newtio));
+ newtio.c_iflag = IGNPAR | INPCK;
+ newtio.c_oflag = 0;
+ newtio.c_cflag = CS8 | CLOCAL | CREAD;
+ newtio.c_lflag = 0;
+ newtio.c_cc[VTIME] = 0;
+ newtio.c_cc[VMIN] = 0; // poll
+ cfsetspeed(&newtio, baud);
+ tcflush(fd, TCIOFLUSH);
+ if (tcsetattr(fd, TCSANOW, &newtio) < 0)
+ {
+ printf(" ahhhhhhhhhhh tcsetattr failed\n");
+ return;
+ }
+
+ // flush the buffer of the serial device
+ uint8_t b;
+ while (this->read(&b) > 0);
+ happy = true;
+}
+
+LightweightSerial::~LightweightSerial()
+{
+ if (fd > 0)
+ close(fd);
+ fd = 0; // prevent future reads...
+}
+
+bool LightweightSerial::read(uint8_t *b)
+{
+ if (!happy)
+ return false;
+ unsigned long nread;
+ nread = ::read(fd,b,1);
+ if (nread < 0)
+ {
+ printf("ahhhhhh read returned <0\n");
+ happy = false;
+ return false;
+ }
+ return (nread == 1);
+}
+
+int LightweightSerial::read_block(uint8_t *block, uint32_t max_read_len)
+{
+ if (!happy)
+ return false;
+ unsigned long nread = ::read(fd,block,(size_t)max_read_len);
+ return (nread < 0 ? 0 : nread);
+}
+
+bool LightweightSerial::write(const uint8_t b)
+{
+ if (!happy)
+ return false;
+ if (fd >= 0 && ::write(fd, &b, 1) < 0)
+ return false;
+ else
+ return true;
+}
+
+bool LightweightSerial::write_block(const uint8_t *block, uint32_t block_len)
+{
+ if (fd >= 0 && ::write(fd, block, block_len) < 0)
+ return false;
+ tcflush(fd, TCOFLUSH);
+ return true;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-03-28 01:24:01
|
Revision: 53
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=53&view=rev
Author: gerkey
Date: 2008-03-27 18:24:08 -0700 (Thu, 27 Mar 2008)
Log Message:
-----------
added nodes that wraps player's urglaser driver, which can control a variety of Hokuyo lasers
Added Paths:
-----------
pkg/trunk/hokuyourg_player/
pkg/trunk/hokuyourg_player/Makefile
pkg/trunk/hokuyourg_player/hokuyourg_player.cc
pkg/trunk/hokuyourg_player/manifest.xml
pkg/trunk/hokuyourg_player/nodes/
pkg/trunk/hokuyourg_player/rosbuild
Added: pkg/trunk/hokuyourg_player/Makefile
===================================================================
--- pkg/trunk/hokuyourg_player/Makefile (rev 0)
+++ pkg/trunk/hokuyourg_player/Makefile 2008-03-28 01:24:08 UTC (rev 53)
@@ -0,0 +1,12 @@
+CPP = g++
+AR = ar
+CPPFLAGS = -g -Wall -Werror `pkg-config --cflags playercore playerxdr playerutils playerdrivers` `rospack cflags roscpp` `rospack cflags common_flows`
+LDFLAGS = `pkg-config --libs playercore playerxdr playerutils playerdrivers` `rospack lflags roscpp` `rospack lflags common_flows`
+
+all: nodes/hokuyourg_player
+
+nodes/hokuyourg_player: hokuyourg_player.cc
+ $(CPP) $(CPPFLAGS) -o $@ $< $(LDFLAGS)
+
+clean:
+ rm -f *.o nodes/hokuyourg_player
Added: pkg/trunk/hokuyourg_player/hokuyourg_player.cc
===================================================================
--- pkg/trunk/hokuyourg_player/hokuyourg_player.cc (rev 0)
+++ pkg/trunk/hokuyourg_player/hokuyourg_player.cc 2008-03-28 01:24:08 UTC (rev 53)
@@ -0,0 +1,163 @@
+#include <assert.h>
+
+// For core Player stuff (message queues, config file objects, etc.)
+#include <libplayercore/playercore.h>
+// TODO: remove XDR dependency
+#include <libplayerxdr/playerxdr.h>
+
+// roscpp
+#include <ros/ros_slave.h>
+// I'm using a LaserScan flow
+#include <common_flows/FlowLaserScan.h>
+
+#define PLAYER_QUEUE_LEN 32
+
+// Must prototype this function here. It's implemented inside
+// libplayerdrivers.
+Driver* URGLaserDriver_Init(ConfigFile* cf, int section);
+
+class HokuyoNode: public ROS_Slave
+{
+ public:
+ QueuePointer q;
+
+ HokuyoNode() : ROS_Slave()
+ {
+ // libplayercore boiler plate
+ player_globals_init();
+ itable_init();
+
+ // TODO: remove XDR dependency
+ playerxdr_ftable_init();
+
+ // The Player address that will be assigned to this device. The format
+ // is interface:index. The interface must match what the driver is
+ // expecting to provide. The value of the index doesn't really matter,
+ // but 0 is most common.
+ const char* player_addr = "laser:0";
+
+ // Create a ConfigFile object, into which we'll stuff parameters.
+ // Drivers assume that this object will persist throughout execution
+ // (e.g., they store pointers to data inside it). So it must NOT be
+ // deleted until after the driver is shut down.
+ this->cf = new ConfigFile();
+
+ // Insert (name,value) pairs into the ConfigFile object. These would
+ // presumably come from the param server
+ this->cf->InsertFieldValue(0,"provides",player_addr);
+ this->cf->InsertFieldValue(0,"port","/dev/ttyACM0");
+
+ // Create an instance of the driver, passing it the ConfigFile object.
+ // The -1 tells it to look into the "global" section of the ConfigFile,
+ // which is where ConfigFile::InsertFieldValue() put the parameters.
+ assert((this->driver = URGLaserDriver_Init(cf, -1)));
+
+ // Print out warnings about parameters that were set, but which the
+ // driver never looked at.
+ cf->WarnUnused();
+
+ // Grab from the global deviceTable a pointer to the Device that was
+ // created as part of the driver's initialization.
+ assert((this->device = deviceTable->GetDevice(player_addr,false)));
+
+ // Create a message queue
+ this->q = QueuePointer(false,PLAYER_QUEUE_LEN);
+ }
+
+ ~HokuyoNode()
+ {
+ delete driver;
+ delete cf;
+ player_globals_fini();
+ }
+
+ int Start()
+ {
+ // Subscribe to device, which causes it to startup
+ if(this->device->Subscribe(this->q) != 0)
+ {
+ puts("Failed to subscribe the driver");
+ return(-1);
+ }
+ else
+ return(0);
+ }
+
+ int Stop()
+ {
+ // Unsubscribe from the device, which causes it to shutdown
+ if(device->Unsubscribe(this->q) != 0)
+ {
+ puts("Failed to start the driver");
+ return(-1);
+ }
+ else
+ return(0);
+ }
+
+ private:
+ Driver* driver;
+ Device* device;
+ ConfigFile* cf;
+};
+
+int
+main(void)
+{
+ HokuyoNode hn;
+
+ // Start up the laser
+ if(hn.Start() != 0)
+ exit(-1);
+
+ // Declare a flow
+ FlowLaserScan fl(".laser");
+
+ /////////////////////////////////////////////////////////////////
+ // Main loop; grab messages off our queue and republish them via ROS
+ for(;;)
+ {
+ // Block until there's a message on the queue
+ hn.q->Wait();
+
+ // Pop off one message (we own the resulting memory)
+ Message* msg;
+ assert((msg = hn.q->Pop()));
+
+ // Is the message a laser scan?
+ player_msghdr_t* hdr = msg->GetHeader();
+ if((hdr->type == PLAYER_MSGTYPE_DATA) &&
+ (hdr->subtype == PLAYER_LASER_DATA_SCAN))
+ {
+ // Cast the message payload appropriately
+ player_laser_data_t* pdata = (player_laser_data_t*)msg->GetPayload();
+
+ // Translate from Player data to ROS data
+ fl.px = fl.py = fl.pyaw = 0.0;
+ fl.angle_min = pdata->min_angle;
+ fl.angle_max = pdata->max_angle;
+ fl.angle_increment = pdata->resolution;
+ fl.set_ranges_size(pdata->ranges_count);
+ for(unsigned int i=0;i<pdata->ranges_count;i++)
+ fl.ranges[i] = pdata->ranges[i];
+ fl.set_intensities_size(pdata->intensity_count);
+ for(unsigned int i=0;i<pdata->intensity_count;i++)
+ fl.intensities[i] = pdata->intensity[i];
+
+ // Publish the new data
+ fl.publish();
+
+ printf("Published a scan with %d ranges\n", pdata->ranges_count);
+ }
+
+ // We're done with the message now
+ delete msg;
+ }
+ /////////////////////////////////////////////////////////////////
+
+ // Stop the laser
+ hn.Stop();
+
+ // To quote Morgan, Hooray!
+ return(0);
+}
Added: pkg/trunk/hokuyourg_player/manifest.xml
===================================================================
--- pkg/trunk/hokuyourg_player/manifest.xml (rev 0)
+++ pkg/trunk/hokuyourg_player/manifest.xml 2008-03-28 01:24:08 UTC (rev 53)
@@ -0,0 +1,7 @@
+<package>
+ <description>A ROS node that wraps up the Player urglaser driver, which provides access to a wide range of Hokuyo URG lasers.</description>
+ <author>Brian P. Gerkey</author>
+ <license>BSD</license>
+ <depend package="roscpp" />
+ <depend package="common_flows" />
+</package>
Added: pkg/trunk/hokuyourg_player/rosbuild
===================================================================
--- pkg/trunk/hokuyourg_player/rosbuild (rev 0)
+++ pkg/trunk/hokuyourg_player/rosbuild 2008-03-28 01:24:08 UTC (rev 53)
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+make $*
Property changes on: pkg/trunk/hokuyourg_player/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-03-28 01:36:36
|
Revision: 54
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=54&view=rev
Author: morgan_quigley
Date: 2008-03-27 18:36:42 -0700 (Thu, 27 Mar 2008)
Log Message:
-----------
a silly little string-wrapping library so i don't have to write it in every stupid package
Added Paths:
-----------
pkg/trunk/string_utils/
pkg/trunk/string_utils/build.yaml
pkg/trunk/string_utils/include/
pkg/trunk/string_utils/include/string_utils/
pkg/trunk/string_utils/include/string_utils/string_utils.h
pkg/trunk/string_utils/lib/
pkg/trunk/string_utils/manifest.xml
pkg/trunk/string_utils/rosbuild
pkg/trunk/string_utils/src/
pkg/trunk/string_utils/src/libstring_utils/
pkg/trunk/string_utils/src/libstring_utils/Makefile
pkg/trunk/string_utils/src/libstring_utils/string_utils.cpp
Added: pkg/trunk/string_utils/build.yaml
===================================================================
--- pkg/trunk/string_utils/build.yaml (rev 0)
+++ pkg/trunk/string_utils/build.yaml 2008-03-28 01:36:42 UTC (rev 54)
@@ -0,0 +1,3 @@
+cpp:
+ make:
+ - src/libstring_utils
Added: pkg/trunk/string_utils/include/string_utils/string_utils.h
===================================================================
--- pkg/trunk/string_utils/include/string_utils/string_utils.h (rev 0)
+++ pkg/trunk/string_utils/include/string_utils/string_utils.h 2008-03-28 01:36:42 UTC (rev 54)
@@ -0,0 +1,15 @@
+#ifndef STRING_UTILS_STRING_UTILS_H
+#define STRING_UTILS_STRING_UTILS_H
+
+#include <string>
+#include <vector>
+
+namespace string_utils
+{
+void split(const std::string &str,
+ std::vector<std::string> &token_vec,
+ const std::string &delim);
+}
+
+#endif
+
Added: pkg/trunk/string_utils/manifest.xml
===================================================================
--- pkg/trunk/string_utils/manifest.xml (rev 0)
+++ pkg/trunk/string_utils/manifest.xml 2008-03-28 01:36:42 UTC (rev 54)
@@ -0,0 +1,15 @@
+<package>
+ <description brief="String Utilities">
+
+ This package provides a simple static library which makes dealing
+ with strings in C++ a bit less of a headache. This includes tokenizer
+ wrappers, etc. I suppose we could add utilities for other languages
+ in here, but most modern languages seem to have nice string stuff
+ provided already.
+
+ </description>
+ <author>Morgan Quigley (email: mqu...@cs...)</author>
+ <license>BSD</license>
+ <url>http://stair.stanford.edu</url>
+</package>
+
Added: pkg/trunk/string_utils/rosbuild
===================================================================
--- pkg/trunk/string_utils/rosbuild (rev 0)
+++ pkg/trunk/string_utils/rosbuild 2008-03-28 01:36:42 UTC (rev 54)
@@ -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/string_utils/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/string_utils/src/libstring_utils/Makefile
===================================================================
--- pkg/trunk/string_utils/src/libstring_utils/Makefile (rev 0)
+++ pkg/trunk/string_utils/src/libstring_utils/Makefile 2008-03-28 01:36:42 UTC (rev 54)
@@ -0,0 +1,4 @@
+SRC = string_utils.cpp
+OUT = ../../lib/libstring_utils.a
+PKG = string_utils
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/lib.mk
Added: pkg/trunk/string_utils/src/libstring_utils/string_utils.cpp
===================================================================
--- pkg/trunk/string_utils/src/libstring_utils/string_utils.cpp (rev 0)
+++ pkg/trunk/string_utils/src/libstring_utils/string_utils.cpp 2008-03-28 01:36:42 UTC (rev 54)
@@ -0,0 +1,19 @@
+#include "string_utils/string_utils.h"
+using namespace std;
+
+namespace string_utils
+{
+
+void split(const string &s, vector<string> &t, const string &d)
+{
+ t.clear();
+ 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));
+}
+
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-03-28 01:56:59
|
Revision: 56
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=56&view=rev
Author: gerkey
Date: 2008-03-27 18:57:04 -0700 (Thu, 27 Mar 2008)
Log Message:
-----------
added wrapped Player erratic driver
Added Paths:
-----------
pkg/trunk/erratic_player/
pkg/trunk/erratic_player/Makefile
pkg/trunk/erratic_player/erratic_player.cc
pkg/trunk/erratic_player/manifest.xml
pkg/trunk/erratic_player/nodes/
Added: pkg/trunk/erratic_player/Makefile
===================================================================
--- pkg/trunk/erratic_player/Makefile (rev 0)
+++ pkg/trunk/erratic_player/Makefile 2008-03-28 01:57:04 UTC (rev 56)
@@ -0,0 +1,12 @@
+CPP = g++
+AR = ar
+CPPFLAGS = -g -Wall -Werror `pkg-config --cflags playercore playerxdr playerutils playerdrivers` `rospack cflags roscpp` `rospack cflags common_flows`
+LDFLAGS = `pkg-config --libs playercore playerxdr playerutils playerdrivers` `rospack lflags roscpp` `rospack lflags common_flows`
+
+all: nodes/erratic_player
+
+nodes/erratic_player: erratic_player.cc
+ $(CPP) $(CPPFLAGS) -o $@ $< $(LDFLAGS)
+
+clean:
+ rm -f *.o nodes/erratic_player
Added: pkg/trunk/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/erratic_player/erratic_player.cc (rev 0)
+++ pkg/trunk/erratic_player/erratic_player.cc 2008-03-28 01:57:04 UTC (rev 56)
@@ -0,0 +1,185 @@
+#include <assert.h>
+
+// For core Player stuff (message queues, config file objects, etc.)
+#include <libplayercore/playercore.h>
+// TODO: remove XDR dependency
+#include <libplayerxdr/playerxdr.h>
+
+// roscpp
+#include <ros/ros_slave.h>
+// I'm using a RobotBase2D flow
+#include <common_flows/FlowRobotBase2D.h>
+
+#define PLAYER_QUEUE_LEN 32
+
+// Must prototype this function here. It's implemented inside
+// libplayerdrivers.
+Driver* Erratic_Init(ConfigFile* cf, int section);
+
+class ErraticNode: public ROS_Slave
+{
+ public:
+ QueuePointer q;
+
+ ErraticNode() : ROS_Slave()
+ {
+ // libplayercore boiler plate
+ player_globals_init();
+ itable_init();
+
+ // TODO: remove XDR dependency
+ playerxdr_ftable_init();
+
+ // The Player address that will be assigned to this device. The format
+ // is interface:index. The interface must match what the driver is
+ // expecting to provide. The value of the index doesn't really matter,
+ // but 0 is most common.
+ const char* player_addr = "position2d:0";
+
+ // Create a ConfigFile object, into which we'll stuff parameters.
+ // Drivers assume that this object will persist throughout execution
+ // (e.g., they store pointers to data inside it). So it must NOT be
+ // deleted until after the driver is shut down.
+ this->cf = new ConfigFile();
+
+ // Insert (name,value) pairs into the ConfigFile object. These would
+ // presumably come from the param server
+ this->cf->InsertFieldValue(0,"provides",player_addr);
+ this->cf->InsertFieldValue(0,"port","/dev/ttyUSB0");
+
+ // Create an instance of the driver, passing it the ConfigFile object.
+ // The -1 tells it to look into the "global" section of the ConfigFile,
+ // which is where ConfigFile::InsertFieldValue() put the parameters.
+ assert((this->driver = Erratic_Init(cf, -1)));
+
+ // Print out warnings about parameters that were set, but which the
+ // driver never looked at.
+ cf->WarnUnused();
+
+ // Grab from the global deviceTable a pointer to the Device that was
+ // created as part of the driver's initialization.
+ assert((this->device = deviceTable->GetDevice(player_addr,false)));
+
+ // Create a message queue
+ this->q = QueuePointer(false,PLAYER_QUEUE_LEN);
+ }
+
+ ~ErraticNode()
+ {
+ delete driver;
+ delete cf;
+ player_globals_fini();
+ }
+
+ int Start()
+ {
+ // Subscribe to device, which causes it to startup
+ if(this->device->Subscribe(this->q) != 0)
+ {
+ puts("Failed to subscribe the driver");
+ return(-1);
+ }
+ else
+ return(0);
+ }
+
+ int Stop()
+ {
+ // Unsubscribe from the device, which causes it to shutdown
+ if(device->Unsubscribe(this->q) != 0)
+ {
+ puts("Failed to start the driver");
+ return(-1);
+ }
+ else
+ return(0);
+ }
+
+ private:
+ Driver* driver;
+ Device* device;
+ ConfigFile* cf;
+};
+
+int
+main(void)
+{
+ ErraticNode en;
+ Message* msg;
+
+ // Start up the robot
+ if(en.Start() != 0)
+ exit(-1);
+
+ // Declare a flow
+ FlowRobotBase2D fl(".odometry");
+
+#if 0
+ // Enable the motors
+ player_position2d_power_config_t motorconfig;
+ motorconfig.state = 1;
+ if(!(msg = this->position->Request(this->InQueue,
+ PLAYER_MSGTYPE_REQ,
+ PLAYER_POSITION2D_REQ_MOTOR_POWER,
+ (void*)&motorconfig,
+ sizeof(motorconfig), NULL, false)))
+ {
+ PLAYER_WARN("failed to enable motors");
+ }
+ else
+ delete msg;
+#endif
+
+ /////////////////////////////////////////////////////////////////
+ // Main loop; grab messages off our queue and republish them via ROS
+ for(;;)
+ {
+ // Block until there's a message on the queue
+ en.q->Wait();
+
+ // Pop off one message (we own the resulting memory)
+ assert((msg = en.q->Pop()));
+
+ // Is the message one we care about?
+ player_msghdr_t* hdr = msg->GetHeader();
+ if((hdr->type == PLAYER_MSGTYPE_DATA) &&
+ (hdr->subtype == PLAYER_POSITION2D_DATA_STATE))
+ {
+ // Cast the message payload appropriately
+ player_position2d_data_t* pdata = (player_position2d_data_t*)msg->GetPayload();
+
+ // Translate from Player data to ROS data
+ fl.px = pdata->pos.px;
+ fl.py = pdata->pos.py;
+ fl.pyaw = pdata->pos.pa;
+ fl.vx = pdata->vel.px;
+ fl.vy = pdata->vel.py;
+ fl.vyaw = pdata->vel.pa;
+ fl.stall = pdata->stall;
+
+ // Publish the new data
+ fl.publish();
+
+ printf("Published new odom: (%.3f,%.3f,%.3f)\n", fl.px,fl.py,fl.pyaw);
+ }
+ else
+ {
+ printf("%d:%d:%d:%d\n",
+ hdr->type,
+ hdr->subtype,
+ hdr->addr.interf,
+ hdr->addr.index);
+
+ }
+
+ // We're done with the message now
+ delete msg;
+ }
+ /////////////////////////////////////////////////////////////////
+
+ // Stop the robot
+ en.Stop();
+
+ // To quote Morgan, Hooray!
+ return(0);
+}
Added: pkg/trunk/erratic_player/manifest.xml
===================================================================
--- pkg/trunk/erratic_player/manifest.xml (rev 0)
+++ pkg/trunk/erratic_player/manifest.xml 2008-03-28 01:57:04 UTC (rev 56)
@@ -0,0 +1,7 @@
+<package>
+ <description>A ROS node that wraps up the Player erratic driver, which provides access to the Erratic mobile robot.</description>
+ <author>Brian P. Gerkey</author>
+ <license>BSD</license>
+ <depend package="roscpp" />
+ <depend package="common_flows" />
+</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-03-28 18:22:30
|
Revision: 59
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=59&view=rev
Author: morgan_quigley
Date: 2008-03-28 11:22:34 -0700 (Fri, 28 Mar 2008)
Log Message:
-----------
the cv_cam package uses opencv to grab whatever camera it can find. it won't work until i get the image flows running, however, which requires flowgen_cpp to be modernized... TIME2CODE
Modified Paths:
--------------
pkg/trunk/image_viewer/manifest.xml
pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp
Added Paths:
-----------
pkg/trunk/cv_cam/
pkg/trunk/cv_cam/build.yaml
pkg/trunk/cv_cam/manifest.xml
pkg/trunk/cv_cam/nodes/
pkg/trunk/cv_cam/nodes/test_cv_cam
pkg/trunk/cv_cam/rosbuild
pkg/trunk/cv_cam/src/
pkg/trunk/cv_cam/src/cv_cam/
pkg/trunk/cv_cam/src/cv_cam/Makefile
pkg/trunk/cv_cam/src/cv_cam/cv_cam.cpp
Added: pkg/trunk/cv_cam/build.yaml
===================================================================
--- pkg/trunk/cv_cam/build.yaml (rev 0)
+++ pkg/trunk/cv_cam/build.yaml 2008-03-28 18:22:34 UTC (rev 59)
@@ -0,0 +1,3 @@
+cpp:
+ make:
+ - src/cv_cam
Added: pkg/trunk/cv_cam/manifest.xml
===================================================================
--- pkg/trunk/cv_cam/manifest.xml (rev 0)
+++ pkg/trunk/cv_cam/manifest.xml 2008-03-28 18:22:34 UTC (rev 59)
@@ -0,0 +1,13 @@
+<package>
+<description brief="Simple OpenCV-based camera grabber">
+
+This is a simple program which grabs frames from a V4L device using OpenCV and publishes them.
+
+</description>
+<author>Morgan Quigley (email: mqu...@cs...)</author>
+<license>BSD</license>
+<url>http://stair.stanford.edu</url>
+<depend package="common_flows"/>
+<depend package="opencv"/>
+</package>
+
Added: pkg/trunk/cv_cam/nodes/test_cv_cam
===================================================================
--- pkg/trunk/cv_cam/nodes/test_cv_cam (rev 0)
+++ pkg/trunk/cv_cam/nodes/test_cv_cam 2008-03-28 18:22:34 UTC (rev 59)
@@ -0,0 +1,9 @@
+#!/usr/bin/env ruby
+require "#{`#{ENV['ROS_ROOT']}/rospack latest yamlgraph`}/lib/yamlgraph/ygl.rb"
+g = YAMLGraph.new
+g.param 'cv_cam.show_image',0
+g.node 'cv_cam/cv_cam', {'launch' => 'xterm'}
+g.node 'image_viewer/image_viewer', {'launch' => 'xterm'}
+g.flow 'cv_cam:image','image_viewer:image'
+YAMLGraphLauncher.new.launch g
+
Property changes on: pkg/trunk/cv_cam/nodes/test_cv_cam
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/cv_cam/rosbuild
===================================================================
--- pkg/trunk/cv_cam/rosbuild (rev 0)
+++ pkg/trunk/cv_cam/rosbuild 2008-03-28 18:22:34 UTC (rev 59)
@@ -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/cv_cam/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/cv_cam/src/cv_cam/Makefile
===================================================================
--- pkg/trunk/cv_cam/src/cv_cam/Makefile (rev 0)
+++ pkg/trunk/cv_cam/src/cv_cam/Makefile 2008-03-28 18:22:34 UTC (rev 59)
@@ -0,0 +1,5 @@
+SRC = cv_cam.cpp
+OUT = ../../nodes/cv_cam
+PKG = cv_cam
+LFLAGS = -lhighgui
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/cv_cam/src/cv_cam/cv_cam.cpp
===================================================================
--- pkg/trunk/cv_cam/src/cv_cam/cv_cam.cpp (rev 0)
+++ pkg/trunk/cv_cam/src/cv_cam/cv_cam.cpp 2008-03-28 18:22:34 UTC (rev 59)
@@ -0,0 +1,96 @@
+///////////////////////////////////////////////////////////////////////////////
+// The cv_cam package provides a simple node which grabs images from a camera
+// using OpenCV and publishes them.
+//
+// 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 "ros/ros_slave.h"
+#include "common_flows/FlowImage.h"
+#include "common_flows/ImageCvBridge.h"
+#include "opencv/highgui.h"
+
+class CVCam : public ROS_Slave
+{
+public:
+ FlowImage *image_f;
+ ImageCvBridge<FlowImage> *cv_bridge;
+ CvCapture *capture;
+ IplImage *cam_image;
+ double last_image_time;
+ int show_image;
+
+ CVCam() : ROS_Slave(), last_image_time(0), show_image(0)
+ {
+ register_source(image_f = new FlowImage("image"));
+ image_f->frame_id = 0; // frame is zero unless specified
+ get_int_param(".frame_id", &image_f->frame_id);
+ get_int_param(".show_image", &show_image);
+ cv_bridge = new ImageCvBridge<FlowImage>(image_f);
+ capture = cvCaptureFromCAM(CV_CAP_ANY);
+ if (!capture)
+ log(ROS::ERROR,"woah! couldn't open a camera. is one connected?\n");
+ }
+ virtual ~CVCam()
+ {
+ if (capture)
+ cvReleaseCapture(&capture);
+ }
+ bool take_and_send_picture()
+ {
+ if (!capture)
+ return false;
+ cam_image = cvQueryFrame(capture);
+ double t = runtime();
+ double dt = t - last_image_time;
+ printf("dt = %f\t(%f fps)\n", dt, 1.0 / dt);
+ last_image_time = t;
+ if (show_image)
+ cvShowImage("CV Cam", cam_image);
+ image_f->compression = "raw";
+ cv_bridge->from_cv(cam_image);
+ cv_bridge->set_flow_data();
+ image_f->publish();
+ return true;
+ }
+};
+
+int main(int argc, char **argv)
+{
+ CVCam a;
+ if (a.show_image)
+ cvNamedWindow("CV Cam", CV_WINDOW_AUTOSIZE);
+ while (a.happy())
+ {
+ if (a.show_image)
+ cvWaitKey(5);
+ if (!a.take_and_send_picture())
+ usleep(100000);
+ }
+ if (a.show_image)
+ cvDestroyWindow("CV Cam");
+ return 0;
+}
+
Modified: pkg/trunk/image_viewer/manifest.xml
===================================================================
--- pkg/trunk/image_viewer/manifest.xml 2008-03-28 18:08:35 UTC (rev 58)
+++ pkg/trunk/image_viewer/manifest.xml 2008-03-28 18:22:34 UTC (rev 59)
@@ -6,7 +6,7 @@
<license>BSD</license>
<url>http://stair.stanford.edu</url>
<depend package="roscpp"/>
-<depend package="image_flows"/>
+<depend package="common_flows"/>
<depend package="sdl"/>
</package>
Modified: pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp
===================================================================
--- pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp 2008-03-28 18:08:35 UTC (rev 58)
+++ pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp 2008-03-28 18:22:34 UTC (rev 59)
@@ -40,7 +40,8 @@
ImageViewer() : ROS_Slave(), blit_prep(NULL)
{
- register_sink(image = new FlowImage("image"), ROS_CALLBACK(ImageViewer, image_received));
+ register_sink(image = new FlowImage("image"),
+ ROS_CALLBACK(ImageViewer, image_cb));
codec = new ImageFlowCodec<FlowImage>(image);
register_with_master();
}
@@ -50,12 +51,13 @@
screen = SDL_SetVideoMode(320, 240, 24, 0);
return (screen ? true : false);
}
- void image_received()
+ void image_cb()
{
if (!screen)
return; // paranoia. shouldn't happen. we should have bailed by now.
if (screen->h != image->height || screen->w != image->width)
{
+ printf("resizing for a %d by %d image\n", image->width, image->height);
screen = SDL_SetVideoMode(image->width, image->height, 24, 0);
if (!screen)
{
@@ -64,7 +66,9 @@
exit(1);
}
}
- if (!blit_prep || blit_prep->w != image->width || blit_prep->h != image->width)
+ if (!blit_prep ||
+ blit_prep->w != image->width ||
+ blit_prep->h != image->width)
{
if (blit_prep)
SDL_FreeSurface(blit_prep);
@@ -120,9 +124,7 @@
exit(1);
}
while (v.happy())
- {
usleep(1000000);
- }
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-03-29 00:50:59
|
Revision: 68
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=68&view=rev
Author: gerkey
Date: 2008-03-28 17:51:03 -0700 (Fri, 28 Mar 2008)
Log Message:
-----------
added wrapper for player linuxjoystick driver
Modified Paths:
--------------
pkg/trunk/axis_cam/manifest.xml
pkg/trunk/axis_cam/src/axis_cam_polled/axis_cam_polled.cpp
pkg/trunk/erratic_player/erratic_player.cc
pkg/trunk/hokuyourg_player/hokuyourg_player.cc
Added Paths:
-----------
pkg/trunk/linuxjoystick_player/
pkg/trunk/linuxjoystick_player/Makefile
pkg/trunk/linuxjoystick_player/linuxjoystick_player.cc
pkg/trunk/linuxjoystick_player/manifest.xml
pkg/trunk/linuxjoystick_player/nodes/
pkg/trunk/linuxjoystick_player/rosbuild
Modified: pkg/trunk/axis_cam/manifest.xml
===================================================================
--- pkg/trunk/axis_cam/manifest.xml 2008-03-28 22:41:35 UTC (rev 67)
+++ pkg/trunk/axis_cam/manifest.xml 2008-03-29 00:51:03 UTC (rev 68)
@@ -12,7 +12,8 @@
<license>BSD</license>
<url>http://stair.stanford.edu</url>
<depend package="common_flows"/>
-<depend package="image_viewer"/>
+<depend package="roscpp"/>
+<!-- <depend package="image_viewer"/> -->
<depend package="string_utils"/>
</package>
Modified: pkg/trunk/axis_cam/src/axis_cam_polled/axis_cam_polled.cpp
===================================================================
--- pkg/trunk/axis_cam/src/axis_cam_polled/axis_cam_polled.cpp 2008-03-28 22:41:35 UTC (rev 67)
+++ pkg/trunk/axis_cam/src/axis_cam_polled/axis_cam_polled.cpp 2008-03-29 00:51:03 UTC (rev 68)
@@ -28,8 +28,7 @@
// POSSIBILITY OF SUCH DAMAGE.
#include "ros/ros_slave.h"
-#include "SDL/SDL.h"
-#include "image_flows/FlowImage.h"
+#include "common_flows/FlowImage.h"
#include "common_flows/FlowEmpty.h"
#include "axis_cam/axis_cam.h"
Modified: pkg/trunk/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/erratic_player/erratic_player.cc 2008-03-28 22:41:35 UTC (rev 67)
+++ pkg/trunk/erratic_player/erratic_player.cc 2008-03-29 00:51:03 UTC (rev 68)
@@ -67,8 +67,8 @@
// Create a message queue
this->q = QueuePointer(false,PLAYER_QUEUE_LEN);
- this->register_source(this->odom = new FlowRobotBase2DOdom(".odometry"));
- this->register_sink(this->cmdvel = new FlowRobotBase2DCmdVel(".cmdvel"),
+ this->register_source(this->odom = new FlowRobotBase2DOdom("odometry"));
+ this->register_sink(this->cmdvel = new FlowRobotBase2DCmdVel("cmdvel"),
ROS_CALLBACK(ErraticNode, cmdvelReceived));
}
Modified: pkg/trunk/hokuyourg_player/hokuyourg_player.cc
===================================================================
--- pkg/trunk/hokuyourg_player/hokuyourg_player.cc 2008-03-28 22:41:35 UTC (rev 67)
+++ pkg/trunk/hokuyourg_player/hokuyourg_player.cc 2008-03-29 00:51:03 UTC (rev 68)
@@ -21,6 +21,8 @@
public:
QueuePointer q;
+ FlowLaserScan* fl;
+
HokuyoNode() : ROS_Slave()
{
// libplayercore boiler plate
@@ -62,6 +64,8 @@
// Create a message queue
this->q = QueuePointer(false,PLAYER_QUEUE_LEN);
+
+ this->register_source(this->fl = new FlowLaserScan(".scans"));
}
~HokuyoNode()
@@ -71,7 +75,7 @@
player_globals_fini();
}
- int Start()
+ int start()
{
// Subscribe to device, which causes it to startup
if(this->device->Subscribe(this->q) != 0)
@@ -83,7 +87,7 @@
return(0);
}
- int Stop()
+ int stop()
{
// Unsubscribe from the device, which causes it to shutdown
if(device->Unsubscribe(this->q) != 0)
@@ -107,12 +111,9 @@
HokuyoNode hn;
// Start up the laser
- if(hn.Start() != 0)
+ if(hn.start() != 0)
exit(-1);
- // Declare a flow
- FlowLaserScan fl(".laser");
-
/////////////////////////////////////////////////////////////////
// Main loop; grab messages off our queue and republish them via ROS
for(;;)
@@ -133,19 +134,19 @@
player_laser_data_t* pdata = (player_laser_data_t*)msg->GetPayload();
// Translate from Player data to ROS data
- fl.px = fl.py = fl.pyaw = 0.0;
- fl.angle_min = pdata->min_angle;
- fl.angle_max = pdata->max_angle;
- fl.angle_increment = pdata->resolution;
- fl.set_ranges_size(pdata->ranges_count);
+ hn.fl->px = hn.fl->py = hn.fl->pyaw = 0.0;
+ hn.fl->angle_min = pdata->min_angle;
+ hn.fl->angle_max = pdata->max_angle;
+ hn.fl->angle_increment = pdata->resolution;
+ hn.fl->set_ranges_size(pdata->ranges_count);
for(unsigned int i=0;i<pdata->ranges_count;i++)
- fl.ranges[i] = pdata->ranges[i];
- fl.set_intensities_size(pdata->intensity_count);
+ hn.fl->ranges[i] = pdata->ranges[i];
+ hn.fl->set_intensities_size(pdata->intensity_count);
for(unsigned int i=0;i<pdata->intensity_count;i++)
- fl.intensities[i] = pdata->intensity[i];
+ hn.fl->intensities[i] = pdata->intensity[i];
// Publish the new data
- fl.publish();
+ hn.fl->publish();
printf("Published a scan with %d ranges\n", pdata->ranges_count);
}
@@ -156,7 +157,7 @@
/////////////////////////////////////////////////////////////////
// Stop the laser
- hn.Stop();
+ hn.stop();
// To quote Morgan, Hooray!
return(0);
Added: pkg/trunk/linuxjoystick_player/Makefile
===================================================================
--- pkg/trunk/linuxjoystick_player/Makefile (rev 0)
+++ pkg/trunk/linuxjoystick_player/Makefile 2008-03-29 00:51:03 UTC (rev 68)
@@ -0,0 +1,12 @@
+CPP = g++
+AR = ar
+CPPFLAGS = -g -Wall -Werror `pkg-config --cflags playercore playerxdr playerutils playerdrivers` `rospack cflags roscpp` `rospack cflags common_flows`
+LDFLAGS = `pkg-config --libs playercore playerxdr playerutils playerdrivers` `rospack lflags roscpp` `rospack lflags common_flows`
+
+all: nodes/linuxjoystick_player
+
+nodes/linuxjoystick_player: linuxjoystick_player.cc
+ $(CPP) $(CPPFLAGS) -o $@ $< $(LDFLAGS)
+
+clean:
+ rm -f *.o nodes/linuxjoystick_player
Added: pkg/trunk/linuxjoystick_player/linuxjoystick_player.cc
===================================================================
--- pkg/trunk/linuxjoystick_player/linuxjoystick_player.cc (rev 0)
+++ pkg/trunk/linuxjoystick_player/linuxjoystick_player.cc 2008-03-29 00:51:03 UTC (rev 68)
@@ -0,0 +1,228 @@
+#include <assert.h>
+
+// For core Player stuff (message queues, config file objects, etc.)
+#include <libplayercore/playercore.h>
+// TODO: remove XDR dependency
+#include <libplayerxdr/playerxdr.h>
+
+// roscpp
+#include <ros/ros_slave.h>
+// Flows that I need
+#include <common_flows/FlowRobotBase2DCmdVel.h>
+
+#define PLAYER_QUEUE_LEN 32
+
+// Must prototype this function here. It's implemented inside
+// libplayerdrivers.
+Driver* LinuxJoystick_Init(ConfigFile* cf, int section);
+
+class LinuxJoystickNode: public ROS_Slave
+{
+ public:
+ QueuePointer q;
+
+ FlowRobotBase2DCmdVel* cmdvel;
+
+ LinuxJoystickNode() : ROS_Slave()
+ {
+ // libplayercore boiler plate
+ player_globals_init();
+ itable_init();
+
+ // get more debug info from underlying driver
+ ErrorInit(9);
+
+ // TODO: remove XDR dependency
+ playerxdr_ftable_init();
+
+ // The Player address that will be assigned to this device. The format
+ // is interface:index. The interface must match what the driver is
+ // expecting to provide. The value of the index doesn't really matter,
+ // but 0 is most common.
+ const char* player_addr = "joystick:0";
+
+ // Create a ConfigFile object, into which we'll stuff parameters.
+ // Drivers assume that this object will persist throughout execution
+ // (e.g., they store pointers to data inside it). So it must NOT be
+ // deleted until after the driver is shut down.
+ this->cf = new ConfigFile();
+
+ // Insert (name,value) pairs into the ConfigFile object. These would
+ // presumably come from the param server
+ this->cf->InsertFieldValue(0,"provides",player_addr);
+ this->cf->InsertFieldValue(0,"port","/dev/input/js0");
+
+ // Create an instance of the driver, passing it the ConfigFile object.
+ // The -1 tells it to look into the "global" section of the ConfigFile,
+ // which is where ConfigFile::InsertFieldValue() put the parameters.
+ assert((this->driver = LinuxJoystick_Init(cf, -1)));
+
+ // Print out warnings about parameters that were set, but which the
+ // driver never looked at.
+ cf->WarnUnused();
+
+ // Grab from the global deviceTable a pointer to the Device that was
+ // created as part of the driver's initialization.
+ assert((this->device = deviceTable->GetDevice(player_addr,false)));
+
+ // Create a message queue
+ this->q = QueuePointer(false,PLAYER_QUEUE_LEN);
+
+ this->register_source(this->cmdvel = new FlowRobotBase2DCmdVel("cmdvel"));
+ }
+
+ ~LinuxJoystickNode()
+ {
+ delete driver;
+ delete cf;
+ player_globals_fini();
+ }
+
+ int start()
+ {
+ // Subscribe to device, which causes it to startup
+ if(this->device->Subscribe(this->q) != 0)
+ {
+ puts("Failed to subscribe the driver");
+ return(-1);
+ }
+ else
+ return(0);
+ }
+
+ int stop()
+ {
+ // Unsubscribe from the device, which causes it to shutdown
+ if(device->Unsubscribe(this->q) != 0)
+ {
+ puts("Failed to start the driver");
+ return(-1);
+ }
+ else
+ return(0);
+ }
+
+ // TODO: move the hardcoded stuff from here to param server, e.g.:
+ // - axis assignment
+ // - axes max / min
+ void putPositionCommand(player_joystick_data_t* pdata)
+ {
+ double scaled[3];
+ double speed[3];
+ //struct timeval curr;
+ //double diff;
+ int axes_min[3] = {5000, 5000, 5000};
+ int axes_max[3] = {32767, 32767, 32767};
+ double max_speed[3] = {0.5, 0.0, DTOR(30.0)};
+
+ for(int i=0;i<3;i++)
+ {
+ if(abs(pdata->pos[i]) < axes_min[i])
+ scaled[i] = 0;
+ else
+ scaled[i] = pdata->pos[i] / (double) axes_max[i];
+ }
+
+ // sanity check
+ if((scaled[0] > 1.0) || (scaled[0] < -1.0))
+ {
+ PLAYER_ERROR2("X position (%d) outside of axis max (+-%d); ignoring",
+ pdata->pos[0], axes_max[0]);
+ return;
+ }
+ if((scaled[1] > 1.0) || (scaled[1] < -1.0))
+ {
+ PLAYER_ERROR2("Y position (%d) outside of axis max (+-%d); ignoring",
+ pdata->pos[1], axes_max[1]);
+ return;
+
+ }
+ if((scaled[2] > 1.0) || (scaled[2] < -1.0))
+ {
+ PLAYER_ERROR2("Yaw position (%d) outside of axis max (+-%d); ignoring",
+ pdata->pos[2], axes_max[2]);
+ return;
+ }
+
+ // As joysticks axes are backwards with respect to intuitive driving
+ // controls, we invert the values here.
+ speed[0] = -scaled[0] * max_speed[0];
+ speed[1] = -scaled[1] * max_speed[1];
+ speed[2] = -scaled[2] * max_speed[2];
+
+#if 0
+ // Make sure we've gotten a joystick fairly recently.
+ GlobalTime->GetTime(&curr);
+ diff = (curr.tv_sec - curr.tv_usec/1e6) -
+ (this->lastread.tv_sec - this->lastread.tv_usec/1e6);
+ if(this->timeout && (diff > this->timeout) && (speed[0] || speed[1] || speed[2]))
+ {
+ PLAYER_WARN("Timeout on joystick; stopping robot");
+ speed[0] = speed[1] = speed[2] = 0.0;
+ }
+#endif
+
+ PLAYER_MSG3(2,"sending speeds: (%f,%f,%f)", speed[0], speed[1], speed[2]);
+
+ this->cmdvel->vx = speed[0];
+ this->cmdvel->vy = speed[1];
+ this->cmdvel->vyaw = speed[2];
+ this->cmdvel->publish();
+ }
+
+ private:
+ Driver* driver;
+ Device* device;
+ ConfigFile* cf;
+};
+
+int
+main(void)
+{
+ LinuxJoystickNode jn;
+ Message* msg;
+
+ // Start up the robot
+ if(jn.start() != 0)
+ exit(-1);
+
+ /////////////////////////////////////////////////////////////////
+ // Main loop; grab messages off our queue and republish them via ROS
+ for(;;)
+ {
+ // Block until there's a message on the queue
+ jn.q->Wait();
+
+ // Pop off one message (we own the resulting memory)
+ assert((msg = jn.q->Pop()));
+
+ // Is the message one we care about?
+ player_msghdr_t* hdr = msg->GetHeader();
+ if((hdr->type == PLAYER_MSGTYPE_DATA) &&
+ (hdr->subtype == PLAYER_JOYSTICK_DATA_STATE))
+ {
+ // Cast the message payload appropriately
+ player_joystick_data_t* pdata = (player_joystick_data_t*)msg->GetPayload();
+ jn.putPositionCommand(pdata);
+ }
+ else
+ {
+ printf("%d:%d:%d:%d\n",
+ hdr->type,
+ hdr->subtype,
+ hdr->addr.interf,
+ hdr->addr.index);
+
+ }
+
+ // We're done with the message now
+ delete msg;
+ }
+ /////////////////////////////////////////////////////////////////
+
+ // Stop the robot
+ jn.stop();
+
+ // To quote Morgan, Hooray!
+ return(0);
+}
Added: pkg/trunk/linuxjoystick_player/manifest.xml
===================================================================
--- pkg/trunk/linuxjoystick_player/manifest.xml (rev 0)
+++ pkg/trunk/linuxjoystick_player/manifest.xml 2008-03-29 00:51:03 UTC (rev 68)
@@ -0,0 +1,7 @@
+<package>
+ <description>A ROS node that wraps up the Player linuxjoystick driver, which provides access to Linux joystick.</description>
+ <author>Brian P. Gerkey</author>
+ <license>BSD</license>
+ <depend package="roscpp" />
+ <depend package="common_flows" />
+</package>
Added: pkg/trunk/linuxjoystick_player/rosbuild
===================================================================
--- pkg/trunk/linuxjoystick_player/rosbuild (rev 0)
+++ pkg/trunk/linuxjoystick_player/rosbuild 2008-03-29 00:51:03 UTC (rev 68)
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+make $*
Property changes on: pkg/trunk/linuxjoystick_player/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-04-02 20:48:51
|
Revision: 74
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=74&view=rev
Author: morgan_quigley
Date: 2008-04-02 13:48:56 -0700 (Wed, 02 Apr 2008)
Log Message:
-----------
ipdcmot driver
Added Paths:
-----------
pkg/trunk/ipdcmot/
pkg/trunk/ipdcmot/build.yaml
pkg/trunk/ipdcmot/flows/
pkg/trunk/ipdcmot/flows/Patrol.flow
pkg/trunk/ipdcmot/include/
pkg/trunk/ipdcmot/include/ipdcmot/
pkg/trunk/ipdcmot/include/ipdcmot/ipdcmot.h
pkg/trunk/ipdcmot/lib/
pkg/trunk/ipdcmot/manifest.xml
pkg/trunk/ipdcmot/nodes/
pkg/trunk/ipdcmot/nodes/servo_stepper_test
pkg/trunk/ipdcmot/rosbuild
pkg/trunk/ipdcmot/src/
pkg/trunk/ipdcmot/src/libipdcmot/
pkg/trunk/ipdcmot/src/libipdcmot/Makefile
pkg/trunk/ipdcmot/src/libipdcmot/ipdcmot.cpp
pkg/trunk/ipdcmot/src/servo/
pkg/trunk/ipdcmot/src/servo/Makefile
pkg/trunk/ipdcmot/src/servo/servo.cpp
pkg/trunk/ipdcmot/src/servo_stepper/
pkg/trunk/ipdcmot/src/servo_stepper/Makefile
pkg/trunk/ipdcmot/src/servo_stepper/servo_stepper.cpp
pkg/trunk/ipdcmot/test/
pkg/trunk/ipdcmot/test/test_patrol/
pkg/trunk/ipdcmot/test/test_patrol/Makefile
pkg/trunk/ipdcmot/test/test_patrol/test_patrol.cpp
pkg/trunk/ipdcmot/test/test_position/
pkg/trunk/ipdcmot/test/test_position/Makefile
pkg/trunk/ipdcmot/test/test_position/test_position.cpp
Added: pkg/trunk/ipdcmot/build.yaml
===================================================================
--- pkg/trunk/ipdcmot/build.yaml (rev 0)
+++ pkg/trunk/ipdcmot/build.yaml 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,7 @@
+cpp:
+ make:
+ - src/libipdcmot
+ - src/servo
+ - src/servo_stepper
+ - test/test_position
+ - test/test_patrol
Added: pkg/trunk/ipdcmot/flows/Patrol.flow
===================================================================
--- pkg/trunk/ipdcmot/flows/Patrol.flow (rev 0)
+++ pkg/trunk/ipdcmot/flows/Patrol.flow 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,4 @@
+float64 stop1
+float64 stop2
+float64 speed
+int32 init_dir
Added: pkg/trunk/ipdcmot/include/ipdcmot/ipdcmot.h
===================================================================
--- pkg/trunk/ipdcmot/include/ipdcmot/ipdcmot.h (rev 0)
+++ pkg/trunk/ipdcmot/include/ipdcmot/ipdcmot.h 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,107 @@
+///////////////////////////////////////////////////////////////////////////////
+// The ipdcmot package provides a library that talks to the FMOD IP-based
+// motor controller. I just have their single-channel 1.5A box, but perhaps
+// some of this code will be useful on their other boxes as well.
+//
+// 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 IPDCMOT_IPDCMOT_H
+#define IPDCMOT_IPDCMOT_H
+
+#include <string>
+#include <arpa/inet.h>
+#include <time.h>
+#include <pthread.h>
+using namespace std;
+
+class IPDCMOT
+{
+public:
+ IPDCMOT(string host, double mount_bias_deg = 0, bool home_myself = true);
+ ~IPDCMOT();
+
+ void home();
+ void stop();
+ bool get_pos_blocking(double *position, int *position_enc, int max_wait_secs = 20);
+ bool set_pos_deg_blocking(double deg, int tick_tol = 1, int max_wait_secs = 20);
+ void set_pos_deg_nonblocking(double deg);
+ void set_patrol(double stop1, double stop2, double speed, int32_t init_dir);
+
+private:
+ double max_ang_vel, mount_bias_deg;
+ bool ok, homing_in_progress, awaiting_response, awaiting_position;
+ enum patrol_dir_t { INCREASING = 0, DECREASING };
+ struct
+ {
+ double stop1, stop2, speed;
+ patrol_dir_t dir;
+ } patrol;
+ enum
+ {
+ IDLE = 0,
+ PATROL,
+ } servo_mode;
+ string host;
+ enum reg_mode_t
+ {
+ UNKNOWN = 0x00,
+ VELOCITY = 0x04,
+ POSITION = 0x05
+ } reg_mode;
+ double last_pos_deg;
+ int last_pos_enc;
+ static int calc_checksum(uint8_t *pkt, unsigned len);
+ static void place_checksum(uint8_t *pkt, unsigned len);
+ void set_regulation_mode(reg_mode_t mode);
+ void set_input(int input);
+
+ void set_pos_enc(int pan_request);
+ void set_vel(int vel_request);
+
+ void request_pos();
+ //pthread_cond_t pos_cond;
+ //pthread_mutex_t pos_mutex;
+
+ const static double DEG_TO_ENC, RAD_TO_ENC;
+ const static double ENC_TO_DEG, ENC_TO_RAD;
+ const static double RAD_TO_DEG, DEG_TO_RAD;
+
+ int sock;
+ sockaddr_in server;
+
+ static void *s_recv_thread(void *parent);
+ void recv_thread();
+ void send_packet(uint8_t *pkt, unsigned len);
+
+ static void *s_patrol_thread(void *parent);
+ void patrol_thread();
+
+ struct timespec init_time;
+ double get_runtime();
+};
+
+#endif
+
Added: pkg/trunk/ipdcmot/manifest.xml
===================================================================
--- pkg/trunk/ipdcmot/manifest.xml (rev 0)
+++ pkg/trunk/ipdcmot/manifest.xml 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,11 @@
+<package>
+<description brief="Driver for the IPDCMOT motor controller">
+This is an IP-based motor controller.
+</description>
+<author>Morgan Quigley (email: mqu...@cs...)</author>
+<license>BSD</license>
+<url>http://stair.stanford.edu</url>
+<depend package="common_flows"/>
+<depend package="roscpp"/>
+</package>
+
Added: pkg/trunk/ipdcmot/nodes/servo_stepper_test
===================================================================
--- pkg/trunk/ipdcmot/nodes/servo_stepper_test (rev 0)
+++ pkg/trunk/ipdcmot/nodes/servo_stepper_test 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,13 @@
+#!/usr/bin/env ruby
+(puts "aaaaaaaa no ROS_ROOT"; exit) if !ENV['ROS_ROOT']
+require "#{`#{ENV['ROS_ROOT']}/rospack latest yamlgraph`}/lib/yamlgraph/yamlgraph_launcher.rb"
+g = ROS_Graph.new
+g.param 'servo.host', '192.168.1.38'
+g.node 'ipdcmot/servo'
+g.node 'ipdcmot/servo_stepper'
+g.flow 'servo_stepper.getpos_request', 'servo.getpos_blocking'
+g.flow 'servo.getpos_result', 'servo_stepper.getpos_result'
+g.flow 'servo_stepper.setpos_request', 'servo.setpos_blocking'
+g.flow 'servo.setpos_result', 'servo_stepper.setpos_result'
+ROS_GraphLauncher.new.launch g
+
Property changes on: pkg/trunk/ipdcmot/nodes/servo_stepper_test
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/ipdcmot/rosbuild
===================================================================
--- pkg/trunk/ipdcmot/rosbuild (rev 0)
+++ pkg/trunk/ipdcmot/rosbuild 2008-04-02 20:48:56 UTC (rev 74)
@@ -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/ipdcmot/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/ipdcmot/src/libipdcmot/Makefile
===================================================================
--- pkg/trunk/ipdcmot/src/libipdcmot/Makefile (rev 0)
+++ pkg/trunk/ipdcmot/src/libipdcmot/Makefile 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,4 @@
+SRC = ipdcmot.cpp
+OUT = ../../lib/libipdcmot.a
+PKG = ipdcmot
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/lib.mk
Added: pkg/trunk/ipdcmot/src/libipdcmot/ipdcmot.cpp
===================================================================
--- pkg/trunk/ipdcmot/src/libipdcmot/ipdcmot.cpp (rev 0)
+++ pkg/trunk/ipdcmot/src/libipdcmot/ipdcmot.cpp 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,511 @@
+///////////////////////////////////////////////////////////////////////////////
+// The ipdcmot package provides a library that talks to the FMOD IP-based
+// motor controller. I just have their single-channel 1.5A box, but perhaps
+// some of this code will be useful on their other boxes as well.
+//
+// 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 <arpa/inet.h>
+#include <netdb.h>
+#include <sys/socket.h>
+#include <ipdcmot/ipdcmot.h>
+#include <cstdio>
+#include <pthread.h>
+#include <errno.h>
+#include <math.h>
+
+const double IPDCMOT::ENC_TO_DEG = 0.00453032;
+const double IPDCMOT::ENC_TO_RAD = ENC_TO_DEG * M_PI / 180;
+const double IPDCMOT::DEG_TO_ENC = 1.0 / ENC_TO_DEG;
+const double IPDCMOT::RAD_TO_ENC = 1.0 / ENC_TO_RAD;
+const double IPDCMOT::RAD_TO_DEG = 180.0 / M_PI;
+const double IPDCMOT::DEG_TO_RAD = M_PI / 180.0;
+
+IPDCMOT::IPDCMOT(string host, double mount_bias_deg, bool home_myself) :
+ host(host), max_ang_vel(500), ok(true), mount_bias_deg(mount_bias_deg),
+ homing_in_progress(false), awaiting_response(false),
+ reg_mode(UNKNOWN), last_pos_deg(0), last_pos_enc(0), sock(0)
+{
+ if (clock_gettime(CLOCK_REALTIME, &init_time) == -1)
+ {
+ printf("woah! couldn't get the system time. BYE.\n");
+ exit(100);
+ }
+
+ printf("opening connection to host [%s]...\n", host.c_str());
+ sock = socket(AF_INET, SOCK_DGRAM, 0);
+ if (sock < 0)
+ {
+ printf("couldn't create socket. ur done.\n");
+ return;
+ }
+ hostent *hp;
+ server.sin_family = AF_INET;
+ server.sin_port = htons(7010);
+ if (inet_addr(host.c_str()) == INADDR_NONE)
+ {
+ hp = gethostbyname(host.c_str());
+ if (!hp)
+ {
+ printf("couldn't resolve host name: [%s]\n", host.c_str());
+ close(sock);
+ return;
+ }
+ server.sin_addr.s_addr = *((unsigned long *)hp->h_addr);
+ printf("ip address for %s is %s\n", host.c_str(), inet_ntoa(server.sin_addr));
+ }
+ else
+ server.sin_addr.s_addr = inet_addr(host.c_str());
+/*
+ if (pthread_cond_init(&pos_cond, NULL))
+ {
+ printf("woah! couldn't create the position pthread condition variable\n");
+ exit(123);
+ }
+ if (pthread_mutex_init(&pos_mutex, NULL))
+ {
+ printf("woah! couldn't create the position mutex\n");
+ exit(124);
+ }
+*/
+
+ // do something non-insane
+ patrol.stop1 = 1000;
+ patrol.stop2 = 3000;
+ patrol.speed = 100;
+ patrol.dir = INCREASING;
+
+ printf("spinning out receive thread\n");
+ pthread_t recv_thread_handle;
+ pthread_create(&recv_thread_handle, NULL, s_recv_thread, this);
+
+ printf("spinning out patrol thread\n");
+ pthread_t patrol_thread_handle;
+ pthread_create(&patrol_thread_handle, NULL, s_patrol_thread, this);
+
+ if (home_myself)
+ {
+ home(); // no need, because the hardware homes itself at boot
+ set_pos_deg_blocking(1, 5);
+ }
+}
+
+IPDCMOT::~IPDCMOT()
+{
+ ok = false;
+ usleep(200000);
+ stop();
+ //pthread_cond_destroy(&pos_cond);
+}
+
+void IPDCMOT::home()
+{
+ servo_mode = IDLE;
+ unsigned char pkt[10];
+ pkt[0] = 0x00;
+ pkt[1] = 0x22;
+ pkt[2] = 0x12;
+ pkt[3] = 0x34;
+ pkt[4] = 0x00;
+ pkt[5] = 0x01;
+ pkt[6] = 0x49;
+ place_checksum(pkt, 7);
+ homing_in_progress = true;
+ send_packet(pkt, 9);
+ printf("sent home packet\n");
+ double start_time = get_runtime(); //clock.runtime();
+ while (homing_in_progress && get_runtime() < start_time + 15.0)
+ {
+ usleep(100000);
+ // query the warning register to see if we've arrived at home yet
+ uint8_t pkt[10];
+ pkt[0] = 0x00;
+ pkt[1] = 0x21;
+ pkt[2] = 0x12;
+ pkt[3] = 0x34;
+ pkt[4] = 0x00;
+ pkt[5] = 0x01;
+ pkt[6] = 0x08; // warning register
+ place_checksum(pkt, 7);
+ send_packet(pkt, 9);
+ }
+ if (homing_in_progress)
+ {
+ printf("never got to home. I should abort...\n");
+ // TODO: send the HOMINGSTOP command
+ }
+ else
+ {
+ printf("homed OK\n");
+ }
+}
+
+bool IPDCMOT::get_pos_blocking(double *position, int *position_enc, int max_wait_secs)
+{
+ struct timespec wait_ts;
+ int wait_result;
+
+ if ((!position && !position_enc) || max_wait_secs <= 0)
+ return false;
+
+ request_pos();
+/*
+ if (clock_gettime(CLOCK_REALTIME, &wait_ts) == -1)
+ {
+ printf("woah! couldn't get the system time. BYE.\n");
+ exit(100);
+ }
+ wait_ts.tv_sec += max_wait_secs;
+ */
+ //pthread_mutex_lock(&pos_mutex);
+ awaiting_position = true;
+ //printf("going into wait loop\n");
+ for (int req_sent = 0; req_sent < 5 && awaiting_position; req_sent++) // (awaiting_position)
+ {
+// wait_result = pthread_cond_timedwait(&pos_cond, &pos_mutex, &wait_ts);
+ for (int i = 0; i < 500 && awaiting_position; i++)
+ usleep(1000);
+ if (awaiting_position)
+ request_pos();
+ }
+ volatile double local_pos = last_pos_deg;
+ volatile int local_pos_enc = last_pos_enc;
+ //pthread_mutex_unlock(&pos_mutex);
+ //if (wait_result == ETIMEDOUT)
+ // return false;
+
+ if (awaiting_position)
+ {
+ printf("still awaiting position. BAILING\n");
+ return false;
+ }
+
+ if (position)
+ *position = local_pos;
+ if (position_enc)
+ *position_enc = local_pos_enc;
+ return true;
+}
+
+bool IPDCMOT::set_pos_deg_blocking(double deg, int tick_tol, int max_wait_secs)
+{
+ servo_mode = IDLE;
+ int encoder_reading, encoder_target = (int)((deg + mount_bias_deg) * DEG_TO_ENC);
+ //printf("blocking position call: target = %d\n", encoder_target);
+ set_pos_enc(encoder_target);
+ int start_time = (int)time(NULL), cur_time;
+ while ((cur_time = (int)time(NULL)) < start_time + max_wait_secs)
+ {
+ //printf("getting position...\n");
+ if (!get_pos_blocking(NULL, &encoder_reading))
+ {
+ printf("error: couldn't get an encoder reading in IPDCMOT::set_pos_deg_blocking()\n");
+ return false;
+ }
+ //printf("encoder = %d\ttarget = %d\n", encoder_reading, encoder_target);
+ if (abs(encoder_reading - encoder_target) <= tick_tol)
+ {
+ //printf("GOT THERE\n");
+ break;
+ }
+ }
+ return (cur_time < start_time + max_wait_secs);
+}
+
+void IPDCMOT::request_pos()
+{
+ uint8_t pkt[10];
+ pkt[0] = 0x00;
+ pkt[1] = 0x21;
+ pkt[2] = 0x12;
+ pkt[3] = 0x34;
+ pkt[4] = 0x00;
+ pkt[5] = 0x01;
+ pkt[6] = 0x26; // position register
+ place_checksum(pkt, 7);
+ send_packet(pkt, 9);
+}
+
+void IPDCMOT::set_pos_deg_nonblocking(double deg)
+{
+ servo_mode = IDLE;
+ set_pos_enc((int)((deg + mount_bias_deg) * DEG_TO_ENC));
+}
+
+int IPDCMOT::calc_checksum(uint8_t *pkt, unsigned len)
+{
+ unsigned sum = 0;
+ bool add_high_byte = true;
+ for (int i = 0; i < len; i++)
+ {
+ sum += (pkt[i] << (add_high_byte ? 8 : 0)) ^ (add_high_byte ? 0xff00 : 0x00ff);
+ add_high_byte = !add_high_byte;
+ }
+ if (!add_high_byte)
+ sum += 0xff;
+ unsigned checksum = ((sum >> 16) & 0xffff) + (sum & 0xffff);
+ checksum = ((checksum >> 16) & 0xffff) + (checksum & 0xffff);
+ return checksum;
+}
+
+void IPDCMOT::place_checksum(uint8_t *pkt, unsigned len)
+{
+ int csum = calc_checksum(pkt, len);
+ pkt[len] = (uint8_t)((csum >> 8) & 0xff);
+ pkt[len+1] = (uint8_t)(csum & 0xff);
+}
+
+void IPDCMOT::set_regulation_mode(reg_mode_t mode)
+{
+ uint8_t pkt[20];
+ pkt[0] = 0x00;
+ pkt[1] = 0x22;
+ pkt[2] = 0x12;
+ pkt[3] = 0x34;
+ pkt[4] = 0x00;
+ pkt[5] = 0x02;
+ pkt[6] = 0x20;
+ pkt[7] = mode;
+ place_checksum(pkt, 8);
+ send_packet(pkt, 10);
+ double send_time = get_runtime();
+ awaiting_response = true;
+ while (awaiting_response && get_runtime() < send_time + 1.0)
+ usleep(10000);
+ if (awaiting_response)
+ printf("no acknowledgement of new regulation mode.\n");
+ else
+ {
+ printf("regulation mode set to %d\n", mode);
+ reg_mode = mode;
+ }
+}
+
+void IPDCMOT::set_input(int input)
+{
+ uint8_t pkt[20];
+ pkt[0] = 0x00;
+ pkt[1] = 0x22;
+ pkt[2] = 0x13;
+ pkt[3] = 0x57;
+ pkt[4] = 0x00;
+ pkt[5] = 0x05;
+ pkt[6] = 0x21;
+ pkt[7] = ((unsigned)input >> 24) & 0xff;
+ pkt[8] = ((unsigned)input >> 16) & 0xff;
+ pkt[9] = ((unsigned)input >> 8) & 0xff;
+ pkt[10] = ((unsigned)input >> 0) & 0xff;
+ place_checksum(pkt, 11);
+ send_packet(pkt, 13);
+}
+
+void IPDCMOT::set_pos_enc(int pos_request)
+{
+ servo_mode = IDLE;
+ if (pos_request < 100)
+ pos_request = 100; // NEVER let it go below zero! otherwise, it may
+ // power up past the limit and have to spin around...
+ if (pos_request > 60000)
+ pos_request = 60000; // ditto
+ if (reg_mode != POSITION)
+ set_regulation_mode(POSITION);
+ if (reg_mode == POSITION)
+ {
+ set_input(pos_request);
+ printf("set position input to %d\n", pos_request);
+ }
+ else
+ printf("woah! regulation mode is not position\n");
+}
+
+void IPDCMOT::set_vel(int vel_request)
+{
+ if (vel_request > 1000)
+ vel_request = 1000;
+ else if (vel_request < -1000)
+ vel_request = -1000;
+
+ if (reg_mode != VELOCITY)
+ set_regulation_mode(VELOCITY);
+ if (reg_mode == VELOCITY)
+ set_input(vel_request);
+}
+
+void *IPDCMOT::s_recv_thread(void *parent)
+{
+ ((IPDCMOT *)parent)->recv_thread();
+ return NULL;
+}
+
+void IPDCMOT::recv_thread()
+{
+ printf("in receive thread\n");
+ char buf[100];
+ while(sock)
+ {
+ int n = recv(sock, buf, 100, 0);
+ //printf("received %d bytes\n", n);
+ if (n < 5)
+ continue; // this shouldn't happen...
+ // this only handles single-register responses:
+ if (buf[1] == 0x23)
+ {
+ switch(buf[6])
+ {
+ case 0x08:
+ if (!(buf[9] & 0x04))
+ homing_in_progress = false;
+ break;
+ case 0x26:
+ {
+ int pos_enc = 0;
+ // ahhhhhhh big endian.
+ unsigned pos_enc_bigendian = *((unsigned *)(buf+7));
+ pos_enc = htonl(pos_enc_bigendian);
+ //printf("little endian = %x = %d\n", pos_enc, pos_enc);
+ //printf("position = %d\n", pos_enc);
+ //pthread_mutex_lock(&pos_mutex);
+ last_pos_enc = pos_enc;
+ last_pos_deg = (double)pos_enc * ENC_TO_DEG - mount_bias_deg;
+ awaiting_position = false;
+ /*
+ if (awaiting_position)
+ {
+ printf("releasing mutex\n");
+ awaiting_position = false;
+ pthread_cond_signal(&pos_cond);
+ }
+ pthread_mutex_unlock(&pos_mutex);
+ */
+ //printf("pos = %f\n", pos);
+ break;
+ }
+ }
+ }
+ else if (buf[1] == 0x24)
+ awaiting_response = false;
+ }
+ printf("leaving receive thread\n");
+}
+
+void IPDCMOT::send_packet(uint8_t *pkt, unsigned len)
+{
+ if (!sendto(sock, pkt, len, 0, (struct sockaddr *)&server, sizeof(server)))
+ {
+ printf("couldn't send data\n");
+ close(sock);
+ sock = 0;
+ }
+}
+
+double IPDCMOT::get_runtime()
+{
+ struct timespec curtime;
+ clock_gettime(CLOCK_REALTIME, &curtime);
+
+ long sec_diff = curtime.tv_sec - init_time.tv_sec;
+ long nsec_diff = curtime.tv_nsec - init_time.tv_nsec;
+ if (nsec_diff < 0)
+ {
+ sec_diff--;
+ nsec_diff += 1000000000;
+ }
+ return (double)(sec_diff) + 1.0e-9 * (double)(nsec_diff);
+}
+
+void *IPDCMOT::s_patrol_thread(void *parent)
+{
+ ((IPDCMOT *)parent)->patrol_thread();
+ return NULL;
+}
+
+void IPDCMOT::patrol_thread()
+{
+ printf("entering patrol thread\n");
+ while (ok)
+ {
+ usleep(100000);
+ if (servo_mode != PATROL)
+ continue;
+ double pos;
+ if (!get_pos_blocking(&pos, NULL, 1))
+ {
+ // set speed to zero
+ set_vel(0);
+ break;
+ }
+ if (patrol.dir == INCREASING && pos > patrol.stop2)
+ {
+ patrol.dir = DECREASING;
+ set_vel(-(int32_t)(patrol.speed / ENC_TO_DEG));
+ }
+ else if (patrol.dir == DECREASING && pos < patrol.stop1)
+ {
+ patrol.dir = INCREASING;
+ set_vel((int32_t)(patrol.speed / ENC_TO_DEG));
+ }
+ }
+ set_vel(0);
+ printf("exiting patrol thread\n");
+}
+
+void IPDCMOT::set_patrol(double stop1, double stop2,
+ double speed, int32_t init_dir)
+{
+ if (stop1 < 10) stop1 = 10; // always be well above 0 so homing works OK
+ if (stop2 < 10) stop2 = 10; // you don't want to go around an extra time
+ // and get all the wires tangled
+ if (stop1 > 350) stop1 = 350; // don't wrap around for same reason
+ if (stop2 > 350) stop2 = 350;
+ if (stop1 > stop2)
+ {
+ double swapme = stop1;
+ stop1 = stop2;
+ stop2 = swapme;
+ }
+ if (speed < 0) speed = 0;
+ // upper speed limit is clamped in the set_vel() function
+ patrol.stop1 = stop1;
+ patrol.stop2 = stop2;
+ patrol.speed = speed;
+ if (init_dir > 0)
+ {
+ patrol.dir = INCREASING;
+ set_vel((int32_t)(patrol.speed * DEG_TO_ENC));
+ }
+ else
+ {
+ patrol.dir = DECREASING;
+ set_vel(-(int32_t)(patrol.speed * DEG_TO_ENC));
+ }
+ servo_mode = PATROL;
+}
+
+void IPDCMOT::stop()
+{
+ set_vel(0);
+}
+
Added: pkg/trunk/ipdcmot/src/servo/Makefile
===================================================================
--- pkg/trunk/ipdcmot/src/servo/Makefile (rev 0)
+++ pkg/trunk/ipdcmot/src/servo/Makefile 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,4 @@
+SRC = servo.cpp
+OUT = ../../nodes/servo
+PKG = ipdcmot
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/ipdcmot/src/servo/servo.cpp
===================================================================
--- pkg/trunk/ipdcmot/src/servo/servo.cpp (rev 0)
+++ pkg/trunk/ipdcmot/src/servo/servo.cpp 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,138 @@
+///////////////////////////////////////////////////////////////////////////////
+// The ipdcmot package provides a library that talks to the FMOD IP-based
+// motor controller. I just have their single-channel 1.5A box, but perhaps
+// some of this code will be useful on their other boxes as well.
+//
+// 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 <csignal>
+#include "ros/ros_slave.h"
+#include "ipdcmot/ipdcmot.h"
+#include "common_flows/FlowInt32.h"
+#include "common_flows/FlowFloat64.h"
+#include "common_flows/FlowEmpty.h"
+#include "common_flows/FlowString.h"
+#include "ipdcmot/FlowPatrol.h"
+
+class Servo : public ROS_Slave
+{
+public:
+ FlowFloat64 *setpos_blocking, *getpos_result;
+ FlowEmpty *getpos_blocking;
+ FlowInt32 *setpos_result;
+ FlowPatrol *patrol;
+ FlowFloat64 *pos_f;
+ FlowString *status_f;
+ string ipdcmot_host;
+ IPDCMOT *mot;
+ double pos_send_freq;
+
+ Servo() : ROS_Slave(), mot(NULL), ipdcmot_host("192.168.1.123"),
+ pos_send_freq(1.0)
+ {
+ register_sink(setpos_blocking = new FlowFloat64("setpos_blocking"), ROS_CALLBACK(Servo, setpos_blocking_cb));
+ register_source(setpos_result = new FlowInt32("setpos_result"));
+
+ register_sink(getpos_blocking = new FlowEmpty("getpos_blocking"), ROS_CALLBACK(Servo, getpos_blocking_cb));
+ register_source(getpos_result = new FlowFloat64("getpos_result"));
+
+ register_sink(patrol = new FlowPatrol("patrol"), ROS_CALLBACK(Servo, patrol_cb));
+ register_source(pos_f = new FlowFloat64("pos"));
+ register_source(status_f = new FlowString("status"));
+
+ register_with_master();
+ if (!get_string_param(".host", ipdcmot_host))
+ printf("ipdcmot_host parameter not specified... defaulting to [%s]\n", ipdcmot_host.c_str());
+ printf("ipdcmot host set to [%s]\n", ipdcmot_host.c_str());
+ get_double_param(".pos_send_freq", &pos_send_freq);
+ printf("position send frequency set to %f\n", pos_send_freq);
+ mot = new IPDCMOT(ipdcmot_host, 75.0);
+ status_f->data = "ready";
+ status_f->publish();
+ }
+ virtual ~Servo()
+ {
+ if (mot)
+ delete mot;
+ mot = NULL;
+ }
+ void patrol_cb()
+ {
+ printf("set patrol: (%f, %f, %f, %d)\n",
+ patrol->stop1, patrol->stop2, patrol->speed, patrol->init_dir);
+ mot->set_patrol(patrol->stop1, patrol->stop2, patrol->speed, patrol->init_dir);
+ }
+ void setpos_blocking_cb()
+ {
+ //printf("received new position target: %f\n", setpos_blocking->data);
+ if (mot->set_pos_deg_blocking(setpos_blocking->data))
+ setpos_result->data = 1;
+ else
+ setpos_result->data = 0;
+ setpos_result->publish();
+ }
+ void getpos_blocking_cb()
+ {
+ if (mot->get_pos_blocking(&getpos_result->data, NULL))
+ getpos_result->publish();
+ else
+ printf("woah! couldn't get the motor position.\n");
+ }
+ void send_pos()
+ {
+ mot->get_pos_blocking(&pos_f->data, NULL, 1);
+ pos_f->data *= 3.1415926 / 180.0; // motor library is in radians
+ pos_f->publish();
+ }
+};
+
+Servo *g_s = NULL;
+
+void safe_term(int)
+{
+ if (g_s)
+ delete g_s;
+ g_s = NULL;
+}
+
+int main(int argc, char **argv)
+{
+ signal(SIGTERM, safe_term);
+ signal(SIGINT, safe_term);
+ signal(SIGQUIT, safe_term);
+ signal(SIGHUP, safe_term);
+ Servo s;
+ g_s = &s;
+ int sleep_usecs = (int)(1000000.0 / s.pos_send_freq);
+ while (s.happy())
+ {
+ usleep(sleep_usecs);
+ s.send_pos();
+ }
+ g_s = NULL;
+ return 0;
+}
+
Added: pkg/trunk/ipdcmot/src/servo_stepper/Makefile
===================================================================
--- pkg/trunk/ipdcmot/src/servo_stepper/Makefile (rev 0)
+++ pkg/trunk/ipdcmot/src/servo_stepper/Makefile 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,4 @@
+SRC = servo_stepper.cpp
+OUT = ../../nodes/servo_stepper
+PKG = ipdcmot
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/ipdcmot/src/servo_stepper/servo_stepper.cpp
===================================================================
--- pkg/trunk/ipdcmot/src/servo_stepper/servo_stepper.cpp (rev 0)
+++ pkg/trunk/ipdcmot/src/servo_stepper/servo_stepper.cpp 2008-04-02 20:48:56 UTC (rev 74)
@@ -0,0 +1,125 @@
+///////////////////////////////////////////////////////////////////////////////
+// The ipdcmot package provides a library that talks to the FMOD IP-based
+// motor controller. I just have their single-channel 1.5A box, but perhaps
+// some of this code will be useful on their other boxes as well.
+//
+// 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.
+// * Redistribut...
[truncated message content] |
|
From: <mor...@us...> - 2008-04-03 04:44:05
|
Revision: 82
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=82&view=rev
Author: morgan_quigley
Date: 2008-04-02 21:44:12 -0700 (Wed, 02 Apr 2008)
Log Message:
-----------
thread_utils package. For now, it just has a mutex class in there, but i intend to move all the cross-platform mutex/semaphore stuff in there from roscpp so that stuff just lives in one place (and isn't unnecessarily tied to the roscpp package)
Added Paths:
-----------
pkg/trunk/thread_utils/
pkg/trunk/thread_utils/build.yaml
pkg/trunk/thread_utils/include/
pkg/trunk/thread_utils/include/thread_utils/
pkg/trunk/thread_utils/include/thread_utils/mutex.h
pkg/trunk/thread_utils/lib/
pkg/trunk/thread_utils/manifest.xml
pkg/trunk/thread_utils/rosbuild
pkg/trunk/thread_utils/src/
pkg/trunk/thread_utils/src/libthread_utils/
pkg/trunk/thread_utils/src/libthread_utils/Makefile
pkg/trunk/thread_utils/src/libthread_utils/mutex.cpp
Added: pkg/trunk/thread_utils/build.yaml
===================================================================
--- pkg/trunk/thread_utils/build.yaml (rev 0)
+++ pkg/trunk/thread_utils/build.yaml 2008-04-03 04:44:12 UTC (rev 82)
@@ -0,0 +1,3 @@
+cpp:
+ make:
+ - src/libthread_utils
Added: pkg/trunk/thread_utils/include/thread_utils/mutex.h
===================================================================
--- pkg/trunk/thread_utils/include/thread_utils/mutex.h (rev 0)
+++ pkg/trunk/thread_utils/include/thread_utils/mutex.h 2008-04-03 04:44:12 UTC (rev 82)
@@ -0,0 +1,56 @@
+///////////////////////////////////////////////////////////////////////////////
+// The thread_utils package provides a few handy cross-platform c++ classes
+// for handling thread issues.
+//
+// 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 THREAD_UTILS_MUTEX_H
+#define THREAD_UTILS_MUTEX_H
+
+#include "pthread.h"
+
+namespace ThreadUtils
+{
+
+class Mutex
+{
+ public:
+ Mutex();
+ ~Mutex();
+
+ void lock();
+ bool trylock();
+ void unlock();
+
+ pthread_mutex_t mutex;
+ // todo: #ifdef in the Windows / OSX equivalents from Switchyard
+};
+
+}
+
+#endif
+
Added: pkg/trunk/thread_utils/manifest.xml
===================================================================
--- pkg/trunk/thread_utils/manifest.xml (rev 0)
+++ pkg/trunk/thread_utils/manifest.xml 2008-04-03 04:44:12 UTC (rev 82)
@@ -0,0 +1,15 @@
+<package>
+ <description brief="Thread Utilities">
+
+ This package provides a simple static library which makes dealing with
+ threads in C++ a bit less of a headache. This includes cross-platform
+ wrappers for mutexes, etc. I suppose we could add utilities for other
+ languages in here, but most modern languages seem to have nice
+ cross-platform thread stuff already.
+
+ </description>
+ <author>Morgan Quigley (email: mqu...@cs...)</author>
+ <license>BSD</license>
+ <url>http://stair.stanford.edu</url>
+</package>
+
Added: pkg/trunk/thread_utils/rosbuild
===================================================================
--- pkg/trunk/thread_utils/rosbuild (rev 0)
+++ pkg/trunk/thread_utils/rosbuild 2008-04-03 04:44:12 UTC (rev 82)
@@ -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/thread_utils/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/thread_utils/src/libthread_utils/Makefile
===================================================================
--- pkg/trunk/thread_utils/src/libthread_utils/Makefile (rev 0)
+++ pkg/trunk/thread_utils/src/libthread_utils/Makefile 2008-04-03 04:44:12 UTC (rev 82)
@@ -0,0 +1,4 @@
+SRC = mutex.cpp
+OUT = ../../lib/libthread_utils.a
+PKG = thread_utils
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/lib.mk
Added: pkg/trunk/thread_utils/src/libthread_utils/mutex.cpp
===================================================================
--- pkg/trunk/thread_utils/src/libthread_utils/mutex.cpp (rev 0)
+++ pkg/trunk/thread_utils/src/libthread_utils/mutex.cpp 2008-04-03 04:44:12 UTC (rev 82)
@@ -0,0 +1,57 @@
+///////////////////////////////////////////////////////////////////////////////
+// The thread_utils package provides a few handy cross-platform c++ classes
+// for threading.
+//
+// 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 "thread_utils/mutex.h"
+
+ThreadUtils::Mutex::Mutex()
+{
+ pthread_mutex_init(&mutex, NULL);
+}
+
+ThreadUtils::Mutex::~Mutex()
+{
+ pthread_mutex_destroy(&mutex);
+}
+
+void ThreadUtils::Mutex::lock()
+{
+ pthread_mutex_lock(&mutex);
+}
+
+void ThreadUtils::Mutex::unlock()
+{
+ pthread_mutex_unlock(&mutex);
+}
+
+bool ThreadUtils::Mutex::trylock()
+{
+ return (pthread_mutex_trylock(&mutex) ? false : true);
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-04-09 23:06:59
|
Revision: 92
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=92&view=rev
Author: jleibs
Date: 2008-04-09 16:07:00 -0700 (Wed, 09 Apr 2008)
Log Message:
-----------
Initial check-in of etherdrive library.
Modified Paths:
--------------
pkg/trunk/sdl_image/rosbuild
Added Paths:
-----------
pkg/trunk/etherdrive/
pkg/trunk/etherdrive/bin/
pkg/trunk/etherdrive/build.yaml
pkg/trunk/etherdrive/include/
pkg/trunk/etherdrive/include/etherdrive.h
pkg/trunk/etherdrive/lib/
pkg/trunk/etherdrive/manifest.xml
pkg/trunk/etherdrive/rosbuild
pkg/trunk/etherdrive/src/
pkg/trunk/etherdrive/src/libetherdrive/
pkg/trunk/etherdrive/src/libetherdrive/Makefile
pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp
pkg/trunk/etherdrive/test/
pkg/trunk/etherdrive/test/Makefile
pkg/trunk/etherdrive/test/test_etherdrive.cpp
Added: pkg/trunk/etherdrive/build.yaml
===================================================================
--- pkg/trunk/etherdrive/build.yaml (rev 0)
+++ pkg/trunk/etherdrive/build.yaml 2008-04-09 23:07:00 UTC (rev 92)
@@ -0,0 +1,5 @@
+cpp:
+ make:
+ - src/libetherdrive
+ - test
+
Added: pkg/trunk/etherdrive/include/etherdrive.h
===================================================================
--- pkg/trunk/etherdrive/include/etherdrive.h (rev 0)
+++ pkg/trunk/etherdrive/include/etherdrive.h 2008-04-09 23:07:00 UTC (rev 92)
@@ -0,0 +1,84 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* 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 the Willow Garage 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 ETHERDRIVE_H
+#define ETHERDRIVE_H
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+
+#include <string>
+
+using namespace std;
+
+class EtherDrive
+{
+public:
+ EtherDrive();
+ ~EtherDrive();
+
+ // Initialize
+ bool init(string ip);
+
+ void shutdown();
+
+ // Manually send EtherDrive command.
+ int send_cmd(char* cmd, size_t cmd_len, char* buf, size_t buf_len);
+
+ // Enable motors
+ bool motors_on();
+
+ // Disable motors
+ bool motors_off();
+
+ // Send an array of motor commands up to 6 in length.
+ bool drive(size_t num, int32_t* drv);
+
+ // Send most recent motor commands, and retrieve update (this must be run at sufficient rate).
+ bool tick(size_t num = 0, int32_t* enc = 0, int32_t* curr = 0, int32_t* pwm = 0);
+private:
+ bool ready;
+
+ int32_t last_drv[6];
+
+ int mot_sock;
+ int cmd_sock;
+
+ struct sockaddr_in mot_addr_out;
+ struct sockaddr_in cmd_addr_out;
+};
+
+#endif
+
Added: pkg/trunk/etherdrive/manifest.xml
===================================================================
--- pkg/trunk/etherdrive/manifest.xml (rev 0)
+++ pkg/trunk/etherdrive/manifest.xml 2008-04-09 23:07:00 UTC (rev 92)
@@ -0,0 +1,15 @@
+<package>
+<description brief='Package for commanding the EtherDrive motor controller/driver'>
+
+This package interfaces to the EtherDrive motor controller/driver. The package
+communicates with the EtherDrive module via UDP. For more information see:
+
+http://hubbard.engr.scu.edu/embedded/motorcontrol/etherdrive/v20/
+</description>
+<author>Jeremy Leibs (email: le...@wi...)</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<depend package='roscpp'/>
+<depend package='common_flows'/>
+<repository>http://pr.willowgarage.com/repos</repository>
+</package>
Added: pkg/trunk/etherdrive/rosbuild
===================================================================
--- pkg/trunk/etherdrive/rosbuild (rev 0)
+++ pkg/trunk/etherdrive/rosbuild 2008-04-09 23:07:00 UTC (rev 92)
@@ -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/etherdrive/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/etherdrive/src/libetherdrive/Makefile
===================================================================
--- pkg/trunk/etherdrive/src/libetherdrive/Makefile (rev 0)
+++ pkg/trunk/etherdrive/src/libetherdrive/Makefile 2008-04-09 23:07:00 UTC (rev 92)
@@ -0,0 +1,4 @@
+SRC = etherdrive.cpp
+OUT = ../../lib/libetherdrive.a
+PKG = etherdrive
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/lib.mk
Added: pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp
===================================================================
--- pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp (rev 0)
+++ pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp 2008-04-09 23:07:00 UTC (rev 92)
@@ -0,0 +1,246 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* 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 the Willow Garage 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 "etherdrive.h"
+
+EtherDrive::EtherDrive()
+{
+
+ ready = false;
+
+ for (int i = 0; i < 6; i++) {
+ last_drv[i] = 0;
+ }
+
+ cmd_sock = socket(AF_INET, SOCK_DGRAM, 0);
+ mot_sock = socket(AF_INET, SOCK_DGRAM, 0);
+
+}
+
+bool EtherDrive::init(string ip) {
+
+ if (ready) {
+ shutdown();
+ }
+
+ struct sockaddr_in mot_addr;
+ struct sockaddr_in cmd_addr;
+
+
+
+ cmd_addr.sin_family = AF_INET;
+ cmd_addr.sin_port = htons(4950);
+ cmd_addr.sin_addr.s_addr = INADDR_ANY;
+
+ if (bind(cmd_sock, (struct sockaddr *)&cmd_addr, sizeof(cmd_addr)) < 0) {
+ printf("ERROR on binding to command port: 4950\n");
+ return false;
+ }
+
+
+ mot_addr.sin_family = AF_INET;
+ mot_addr.sin_port = htons(4900);
+ mot_addr.sin_addr.s_addr = INADDR_ANY;
+
+ if (bind(mot_sock, (struct sockaddr *)&mot_addr, sizeof(mot_addr)) < 0) {
+ printf("ERROR on binding to motor port: 4900\n");
+
+ close(cmd_sock);
+
+ return false;
+ }
+
+ mot_addr_out.sin_family = AF_INET;
+ mot_addr_out.sin_port = htons(4900);
+ mot_addr_out.sin_addr.s_addr = inet_addr(ip.c_str());
+
+ cmd_addr_out.sin_family = AF_INET;
+ cmd_addr_out.sin_port = htons(4950);
+ cmd_addr_out.sin_addr.s_addr = inet_addr(ip.c_str());
+
+ ready = true;
+ return true;
+}
+
+
+EtherDrive::~EtherDrive()
+{
+ shutdown();
+}
+
+
+void EtherDrive::shutdown() {
+
+ if (ready = true) {
+ close(cmd_sock);
+ close(mot_sock);
+ }
+
+ ready = false;
+
+}
+
+
+bool EtherDrive::motors_on() {
+
+ if (ready) {
+ int32_t cmd[3];
+ cmd[0] = 'm';
+ cmd[1] = 1;
+ cmd[2] = 0;
+
+ int32_t res[3];
+
+ int res_len = send_cmd((char*)cmd, 3*sizeof(int32_t), (char*)res, 100*sizeof(int32_t));
+
+ if (res_len == 3) {
+ if (res[1] == 1) {
+ return true;
+ }
+ }
+ }
+
+ return false;
+
+}
+
+bool EtherDrive::motors_off() {
+
+ if (ready) {
+ int32_t cmd[3];
+ cmd[0] = 'm';
+ cmd[1] = 0;
+ cmd[2] = 0;
+
+ int32_t res[3];
+
+ int res_len = send_cmd((char*)cmd, 3*sizeof(int32_t), (char*)res, 100*sizeof(int32_t));
+
+ if (res_len == 3) {
+ if (res[1] == 0) {
+ return true;
+ }
+ }
+ }
+
+ return false;
+
+}
+
+
+int EtherDrive::send_cmd(char* cmd, size_t cmd_len, char* buf, size_t buf_len) {
+
+ int n_sent;
+ int n_recv;
+
+ struct sockaddr_in from;
+ socklen_t fromlen = sizeof(struct sockaddr_in);
+
+ n_sent = sendto(cmd_sock, cmd, cmd_len, 0, (struct sockaddr *)&cmd_addr_out, sizeof(mot_addr_out));
+
+ if (n_sent != cmd_len) {
+ return -1;
+ }
+
+ n_recv = recvfrom(cmd_sock, buf, buf_len, 0, (struct sockaddr *)&from, &fromlen);
+
+ return n_recv;
+
+}
+
+
+bool EtherDrive::drive(size_t num, int32_t* drv)
+{
+ if (num > 6) {
+ return false;
+ }
+
+ for (int i = 0; i < num; i++) {
+ last_drv[i] = drv[i];
+ }
+
+ for (int i = num; i < 6; i++) {
+ last_drv[i] = 0;
+ }
+
+ return true;
+}
+
+bool EtherDrive::tick(size_t num, int32_t* enc, int32_t* curr, int32_t* pwm)
+{
+
+ if (num > 6) {
+ return false;
+ }
+
+ int n_sent;
+ int n_recv;
+
+ struct sockaddr_in from;
+ socklen_t fromlen = sizeof(struct sockaddr_in);
+
+ n_sent = sendto(mot_sock, (char*)last_drv, 6*sizeof(int32_t), 0, (struct sockaddr *)&mot_addr_out, sizeof(mot_addr_out));
+
+ if (n_sent != 6*sizeof(int32_t)) {
+ return false;
+ }
+
+ int32_t buf[72];
+
+ n_recv = recvfrom(mot_sock, (char*)buf, 72, 0, (struct sockaddr *)&from, &fromlen);
+
+ if (n_recv != 72) {
+ return false;
+ }
+
+ if (enc > 0) {
+ for (int i = 0; i < num; i++) {
+ enc[i] = buf[i];
+ }
+ }
+
+ if (curr > 0) {
+ for (int i = 0; i < num; i++) {
+ curr[i] = buf[6+i];
+ }
+ }
+
+ if (pwm > 0) {
+ for (int i = 0; i < num; i++) {
+ pwm[i] = buf[12+i];
+ }
+ }
+
+ return n_recv;
+}
Added: pkg/trunk/etherdrive/test/Makefile
===================================================================
--- pkg/trunk/etherdrive/test/Makefile (rev 0)
+++ pkg/trunk/etherdrive/test/Makefile 2008-04-09 23:07:00 UTC (rev 92)
@@ -0,0 +1,4 @@
+SRC = test_etherdrive.cpp
+OUT = ../bin/test_etherdrive
+PKG = etherdrive
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/etherdrive/test/test_etherdrive.cpp
===================================================================
--- pkg/trunk/etherdrive/test/test_etherdrive.cpp (rev 0)
+++ pkg/trunk/etherdrive/test/test_etherdrive.cpp 2008-04-09 23:07:00 UTC (rev 92)
@@ -0,0 +1,67 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* 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 the Willow Garage 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 "etherdrive.h"
+#include <iostream>
+
+using namespace std;
+
+int main() {
+
+ EtherDrive e;
+
+ if (!e.init("10.0.0.151")) {
+ cout << "Could not initialize etherdrive." << endl;
+ return -1;
+ }
+ e.motors_on();
+
+ int drv = 100000;
+ int enc;
+
+ while (1) {
+ e.drive(1,&drv);
+ e.tick(1,&enc);
+
+ printf("Encoder: %d\n", enc);
+
+ if (enc == 100000) {
+ drv = -100000;
+ } else if (enc == -100000) {
+ drv = 100000;
+ }
+ }
+
+ e.shutdown();
+}
Modified: pkg/trunk/sdl_image/rosbuild
===================================================================
--- pkg/trunk/sdl_image/rosbuild 2008-04-04 16:45:35 UTC (rev 91)
+++ pkg/trunk/sdl_image/rosbuild 2008-04-09 23:07:00 UTC (rev 92)
@@ -6,19 +6,19 @@
exit
end
-libjpeg_path = `#{ENV['ROS_ROOT']}/rospack find stair__ijg_libjpeg`
+libjpeg_path = `#{ENV['ROS_ROOT']}/rospack find ijg_libjpeg`
puts "libjpeg path = #{libjpeg_path}"
if libjpeg_path.length <= 1
puts "couldn't find libjpeg. try running this from your ROS_ROOT directory:"
- puts " ./rospack install stair__ijg_libjpeg"
+ puts " ./rospack install ijg_libjpeg"
exit
end
-sdl_path = `#{ENV['ROS_ROOT']}/rospack find stair__sdl`
+sdl_path = `#{ENV['ROS_ROOT']}/rospack find sdl`
puts "sdl path = #{sdl_path}"
if sdl_path.length <= 1
puts "couldn't find SDL. try running this from your ROS_ROOT directory:"
- puts " ./rospack install stair__sdl"
+ puts " ./rospack install sdl"
exit
end
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-04-10 01:01:45
|
Revision: 93
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=93&view=rev
Author: jleibs
Date: 2008-04-09 18:01:50 -0700 (Wed, 09 Apr 2008)
Log Message:
-----------
Accomodating move of image flows into common flows.
Modified Paths:
--------------
pkg/trunk/camera_calibration/manifest.xml
pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp
pkg/trunk/driver_axis213/manifest.xml
pkg/trunk/driver_axis213/src/axis213_cam/axis213_cam.cpp
pkg/trunk/driver_axis213/src/axis213_ptz/axis213_ptz.cpp
pkg/trunk/simple_sdl_gui/manifest.xml
pkg/trunk/simple_sdl_gui/src/simple_sdl_gui/simple_sdl_gui.cpp
Modified: pkg/trunk/camera_calibration/manifest.xml
===================================================================
--- pkg/trunk/camera_calibration/manifest.xml 2008-04-09 23:07:00 UTC (rev 92)
+++ pkg/trunk/camera_calibration/manifest.xml 2008-04-10 01:01:50 UTC (rev 93)
@@ -5,7 +5,6 @@
<author>Jeremy Leibs (email: le...@wi...)</author>
<license>BSD</license>
<depend package='roscpp'/>
-<depend package='image_flows'/>
<depend package='driver_axis213'/>
<depend package='common_flows'/>
<depend package='yamlgraph'/>
Modified: pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp
===================================================================
--- pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp 2008-04-09 23:07:00 UTC (rev 92)
+++ pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp 2008-04-10 01:01:50 UTC (rev 93)
@@ -45,8 +45,8 @@
#include "SDL/SDL.h"
#include "ros/ros_slave.h"
-#include "image_flows/FlowImage.h"
-#include "image_flows/image_flow_codec.h"
+#include "common_flows/FlowImage.h"
+#include "common_flows/ImageCodec.h"
#include "driver_axis213/FlowPTZPosition.h"
#include "simple_sdl_gui/FlowSDLKeyEvent.h"
@@ -65,10 +65,10 @@
{
public:
FlowImage *image_in;
- ImageFlowCodec<FlowImage> *codec_in;
+ ImageCodec<FlowImage> *codec_in;
FlowImage *image_out;
- ImageFlowCodec<FlowImage> *codec_out;
+ ImageCodec<FlowImage> *codec_out;
FlowPTZPosition *control;
FlowPTZPosition *observe;
@@ -94,10 +94,10 @@
Calibrator() : ROS_Slave()
{
register_sink(image_in = new FlowImage("image_in"), ROS_CALLBACK(Calibrator, image_received));
- codec_in = new ImageFlowCodec<FlowImage>(image_in);
+ codec_in = new ImageCodec<FlowImage>(image_in);
register_source(image_out = new FlowImage("image_out"));
- codec_out = new ImageFlowCodec<FlowImage>(image_out);
+ codec_out = new ImageCodec<FlowImage>(image_out);
register_sink(observe = new FlowPTZPosition("observe"), ROS_CALLBACK(Calibrator, ptz_received));
register_source(control = new FlowPTZPosition("control"));
@@ -323,7 +323,7 @@
image_out->compression = "raw";
image_out->colorspace = "rgb24";
- codec_out->realloc_raster_if_needed();
+ // codec_out->realloc_raster_if_needed();
cvSetData(cvimage_out, codec_out->get_raster(), 3*image_out->width);
cvConvertImage(cvimage_undistort, cvimage_out, CV_CVTIMG_SWAP_RB);
Modified: pkg/trunk/driver_axis213/manifest.xml
===================================================================
--- pkg/trunk/driver_axis213/manifest.xml 2008-04-09 23:07:00 UTC (rev 92)
+++ pkg/trunk/driver_axis213/manifest.xml 2008-04-10 01:01:50 UTC (rev 93)
@@ -8,7 +8,7 @@
<author>Jeremy Leibs (email: le...@wi...)</author>
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
-<depend package='image_flows'/>
+<depend package='roscpp'/>
<depend package='common_flows'/>
<repository>http://pr.willowgarage.com/repos</repository>
</package>
Modified: pkg/trunk/driver_axis213/src/axis213_cam/axis213_cam.cpp
===================================================================
--- pkg/trunk/driver_axis213/src/axis213_cam/axis213_cam.cpp 2008-04-09 23:07:00 UTC (rev 92)
+++ pkg/trunk/driver_axis213/src/axis213_cam/axis213_cam.cpp 2008-04-10 01:01:50 UTC (rev 93)
@@ -36,11 +36,8 @@
#include "ros/ros_slave.h"
#include "common_flows/FlowEmpty.h"
-#include "image_flows/FlowImage.h"
-#include "image_flows/image_flow_codec.h"
+#include "common_flows/FlowImage.h"
-
-
class Axis213_cam : public ROS_Slave
{
public:
Modified: pkg/trunk/driver_axis213/src/axis213_ptz/axis213_ptz.cpp
===================================================================
--- pkg/trunk/driver_axis213/src/axis213_ptz/axis213_ptz.cpp 2008-04-09 23:07:00 UTC (rev 92)
+++ pkg/trunk/driver_axis213/src/axis213_ptz/axis213_ptz.cpp 2008-04-10 01:01:50 UTC (rev 93)
@@ -33,7 +33,6 @@
*********************************************************************/
#include "ros/ros_slave.h"
-#include "image_flows/FlowImage.h"
#include "common_flows/FlowEmpty.h"
#include "axis213/axis213.h"
#include "driver_axis213/FlowPTZPosition.h"
Modified: pkg/trunk/simple_sdl_gui/manifest.xml
===================================================================
--- pkg/trunk/simple_sdl_gui/manifest.xml 2008-04-09 23:07:00 UTC (rev 92)
+++ pkg/trunk/simple_sdl_gui/manifest.xml 2008-04-10 01:01:50 UTC (rev 93)
@@ -7,8 +7,9 @@
<author>Jeremy Leibs (email: le...@wi...)</author>
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
+<depend package="common_flows"/>
+<depend package="image_utils"/>
<depend package="roscpp"/>
-<depend package="image_flows"/>
<depend package="sdl"/>
</package>
Modified: pkg/trunk/simple_sdl_gui/src/simple_sdl_gui/simple_sdl_gui.cpp
===================================================================
--- pkg/trunk/simple_sdl_gui/src/simple_sdl_gui/simple_sdl_gui.cpp 2008-04-09 23:07:00 UTC (rev 92)
+++ pkg/trunk/simple_sdl_gui/src/simple_sdl_gui/simple_sdl_gui.cpp 2008-04-10 01:01:50 UTC (rev 93)
@@ -35,15 +35,15 @@
#include "ros/ros_slave.h"
#include "SDL/SDL.h"
-#include "image_flows/FlowImage.h"
-#include "image_flows/image_flow_codec.h"
+#include "common_flows/FlowImage.h"
+#include "common_flows/ImageCodec.h"
#include "simple_sdl_gui/FlowSDLKeyEvent.h"
class Simple_SDL_GUI : public ROS_Slave
{
public:
FlowImage *image;
- ImageFlowCodec<FlowImage> *codec;
+ ImageCodec<FlowImage> *codec;
FlowSDLKeyEvent *key;
@@ -55,7 +55,7 @@
Simple_SDL_GUI() : ROS_Slave(), blit_prep(NULL)
{
register_sink(image = new FlowImage("image"), ROS_CALLBACK(Simple_SDL_GUI, image_received));
- codec = new ImageFlowCodec<FlowImage>(image);
+ codec = new ImageCodec<FlowImage>(image);
register_source(key = new FlowSDLKeyEvent("key"));
@@ -122,7 +122,7 @@
}
// NOTE: get_raster internally locks the image mutex
- uint8_t *raster = codec->get_raster(); // decompress if required
+ uint8_t *raster = codec->get_raster();
int row_offset = 0;
for (int row = 0; row < image->height; row++, row_offset += screen->pitch)
memcpy((char *)blit_prep->pixels + row_offset, raster + (row * image->width * 3), image->width * 3);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-04-10 01:17:18
|
Revision: 94
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=94&view=rev
Author: jleibs
Date: 2008-04-09 18:17:24 -0700 (Wed, 09 Apr 2008)
Log Message:
-----------
Changing image_viewer to work with moved image_flow.
Modified Paths:
--------------
pkg/trunk/image_viewer/manifest.xml
pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp
pkg/trunk/simple_sdl_gui/nodes/test_gui
Modified: pkg/trunk/image_viewer/manifest.xml
===================================================================
--- pkg/trunk/image_viewer/manifest.xml 2008-04-10 01:01:50 UTC (rev 93)
+++ pkg/trunk/image_viewer/manifest.xml 2008-04-10 01:17:24 UTC (rev 94)
@@ -8,5 +8,6 @@
<depend package="roscpp"/>
<depend package="common_flows"/>
<depend package="sdl"/>
+<depend package="ijg_libjpeg"/>
</package>
Modified: pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp
===================================================================
--- pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp 2008-04-10 01:01:50 UTC (rev 93)
+++ pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp 2008-04-10 01:17:24 UTC (rev 94)
@@ -28,21 +28,21 @@
#include "ros/ros_slave.h"
#include "SDL/SDL.h"
-#include "image_flows/FlowImage.h"
-#include "image_flows/image_flow_codec.h"
+#include "common_flows/FlowImage.h"
+#include "common_flows/ImageCodec.h"
class ImageViewer : public ROS_Slave
{
public:
FlowImage *image;
- ImageFlowCodec<FlowImage> *codec;
+ ImageCodec<FlowImage> *codec;
SDL_Surface *screen, *blit_prep;
ImageViewer() : ROS_Slave(), blit_prep(NULL)
{
register_sink(image = new FlowImage("image"),
ROS_CALLBACK(ImageViewer, image_cb));
- codec = new ImageFlowCodec<FlowImage>(image);
+ codec = new ImageCodec<FlowImage>(image);
register_with_master();
}
virtual ~ImageViewer() { if (blit_prep) SDL_FreeSurface(blit_prep); }
Modified: pkg/trunk/simple_sdl_gui/nodes/test_gui
===================================================================
--- pkg/trunk/simple_sdl_gui/nodes/test_gui 2008-04-10 01:01:50 UTC (rev 93)
+++ pkg/trunk/simple_sdl_gui/nodes/test_gui 2008-04-10 01:17:24 UTC (rev 94)
@@ -1,12 +1,14 @@
#!/usr/bin/env ruby
require "#{`#{ENV['ROS_ROOT']}/rospack latest yamlgraph`}/lib/yamlgraph/ygl.rb"
g = YAMLGraph.new
-g.param 'image_sender.freq', 0.5
-g.param 'image_sender.image_file', 'examples/image_sender/test.jpg'
-g.node 'image_flows/image_sender', {'launch'=>'xterm'}
+#g.node 'driver_axis213/axis213_cam', {'launch'=>'xterm', }
+g.node 'axis_cam/axis_cam', {'launch'=>'xterm', }
+#g.param 'axis213_cam.host', '10.0.0.150'
+g.param 'axis_cam.host', '10.0.0.150'
g.node 'simple_sdl_gui/gui', {'launch'=>'xterm'}
g.node 'simple_sdl_gui/keymon', {'launch'=>'xterm'}
-g.flow 'image_sender:image', 'gui:image'
+#g.flow 'axis213_cam:image_all', 'gui:image'
+g.flow 'axis_cam:image', 'gui:image'
g.flow 'gui:key', 'keymon:key'
YAMLGraphLauncher.new.launch g
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-04-10 17:08:28
|
Revision: 95
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=95&view=rev
Author: jleibs
Date: 2008-04-10 10:08:27 -0700 (Thu, 10 Apr 2008)
Log Message:
-----------
More files fixed to deal with reshuffling of image_flows.
Modified Paths:
--------------
pkg/trunk/axis_cam/src/axis_cam_polled/axis_cam_polled.cpp
pkg/trunk/axis_cam/test/test_get/Makefile
pkg/trunk/axis_cam/test/test_ptz/Makefile
pkg/trunk/axis_cam/test/test_ptz/test_ptz.cpp
pkg/trunk/image_utils/examples/image_sender/Makefile
pkg/trunk/image_utils/examples/image_sender/image_sender.cpp
pkg/trunk/image_utils/manifest.xml
pkg/trunk/image_utils/test/get_raster/Makefile
pkg/trunk/image_utils/test/get_raster/get_raster.cpp
pkg/trunk/image_utils/test/read_write/Makefile
pkg/trunk/image_utils/test/read_write/read_write.cpp
pkg/trunk/image_viewer/nodes/test_image_viewer
Modified: pkg/trunk/axis_cam/src/axis_cam_polled/axis_cam_polled.cpp
===================================================================
--- pkg/trunk/axis_cam/src/axis_cam_polled/axis_cam_polled.cpp 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/axis_cam/src/axis_cam_polled/axis_cam_polled.cpp 2008-04-10 17:08:27 UTC (rev 95)
@@ -29,7 +29,7 @@
#include "ros/ros_slave.h"
#include "SDL/SDL.h"
-#include "image_flows/FlowImage.h"
+#include "common_flows/FlowImage.h"
#include "common_flows/FlowEmpty.h"
#include "axis_cam/axis_cam.h"
Modified: pkg/trunk/axis_cam/test/test_get/Makefile
===================================================================
--- pkg/trunk/axis_cam/test/test_get/Makefile 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/axis_cam/test/test_get/Makefile 2008-04-10 17:08:27 UTC (rev 95)
@@ -2,4 +2,4 @@
OUT = test_get
LFLAGS = -L../../lib -laxis_cam -lcurl
PKG = axis_cam
-include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node_just_libros.mk
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Modified: pkg/trunk/axis_cam/test/test_ptz/Makefile
===================================================================
--- pkg/trunk/axis_cam/test/test_ptz/Makefile 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/axis_cam/test/test_ptz/Makefile 2008-04-10 17:08:27 UTC (rev 95)
@@ -2,4 +2,4 @@
OUT = test_ptz
LFLAGS = -L../../lib -laxis_cam -lcurl
PKG = axis_cam
-include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node_just_libros.mk
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Modified: pkg/trunk/axis_cam/test/test_ptz/test_ptz.cpp
===================================================================
--- pkg/trunk/axis_cam/test/test_ptz/test_ptz.cpp 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/axis_cam/test/test_ptz/test_ptz.cpp 2008-04-10 17:08:27 UTC (rev 95)
@@ -33,10 +33,10 @@
int main(int argc, char **argv)
{
AxisCam *axis = new AxisCam("192.168.1.90");
- axis->ptz(0,0,0);
+ axis->set_ptz(0,0,0);
for (int i = 0; i < 5; i++)
- axis->ptz(i*10, i*10, 0);
- axis->ptz(0,0,0);
+ axis->set_ptz(i*10, i*10, 0);
+ axis->set_ptz(0,0,0);
delete axis;
return 0;
}
Modified: pkg/trunk/image_utils/examples/image_sender/Makefile
===================================================================
--- pkg/trunk/image_utils/examples/image_sender/Makefile 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/image_utils/examples/image_sender/Makefile 2008-04-10 17:08:27 UTC (rev 95)
@@ -1,4 +1,4 @@
SRC = image_sender.cpp
OUT = ../../nodes/image_sender
-PKG = image_flows
+PKG = image_utils
include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Modified: pkg/trunk/image_utils/examples/image_sender/image_sender.cpp
===================================================================
--- pkg/trunk/image_utils/examples/image_sender/image_sender.cpp 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/image_utils/examples/image_sender/image_sender.cpp 2008-04-10 17:08:27 UTC (rev 95)
@@ -28,8 +28,8 @@
// POSSIBILITY OF SUCH DAMAGE.
#include <cstdio>
-#include "image_flows/FlowImage.h"
-#include "image_flows/image_flow_codec.h"
+#include "common_flows/FlowImage.h"
+#include "common_flows/ImageCodec.h"
#include "ros/ros_slave.h"
class ImageSender : public ROS_Slave
@@ -38,12 +38,12 @@
double freq;
string image_file;
FlowImage *image;
- ImageFlowCodec<FlowImage> *codec;
+ ImageCodec<FlowImage> *codec;
ImageSender() : ROS_Slave(), freq(1), image_file("test.jpg")
{
register_source(image = new FlowImage("image"));
- codec = new ImageFlowCodec<FlowImage>(image);
+ codec = new ImageCodec<FlowImage>(image);
register_with_master();
get_double_param(".freq", &freq);
get_string_param(".image_file", image_file);
Modified: pkg/trunk/image_utils/manifest.xml
===================================================================
--- pkg/trunk/image_utils/manifest.xml 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/image_utils/manifest.xml 2008-04-10 17:08:27 UTC (rev 95)
@@ -10,6 +10,7 @@
<license>BSD</license>
<url>http://stair.stanford.edu</url>
<depend package="roscpp"/>
+ <depend package="common_flows"/>
<depend package="ijg_libjpeg"/>
</package>
Modified: pkg/trunk/image_utils/test/get_raster/Makefile
===================================================================
--- pkg/trunk/image_utils/test/get_raster/Makefile 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/image_utils/test/get_raster/Makefile 2008-04-10 17:08:27 UTC (rev 95)
@@ -1,4 +1,4 @@
SRC = get_raster.cpp
OUT = get_raster
-PKG = image_flows
+PKG = image_utils
include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Modified: pkg/trunk/image_utils/test/get_raster/get_raster.cpp
===================================================================
--- pkg/trunk/image_utils/test/get_raster/get_raster.cpp 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/image_utils/test/get_raster/get_raster.cpp 2008-04-10 17:08:27 UTC (rev 95)
@@ -33,8 +33,8 @@
#include <unistd.h>
#include <errno.h>
#include <string.h>
-#include "image_flows/FlowImage.h"
-#include "image_flows/image_flow_codec.h"
+#include "common_flows/FlowImage.h"
+#include "common_flows/ImageCodec.h"
void toast(char *msg)
{
@@ -63,7 +63,7 @@
image.colorspace = "rgb24";
image.width = 200;
image.height = 153;
- ImageFlowCodec<FlowImage> codec(&image);
+ ImageCodec<FlowImage> codec(&image);
uint8_t *raster;
if (!(raster = codec.get_raster()))
printf("couldn't get raster\n");
Modified: pkg/trunk/image_utils/test/read_write/Makefile
===================================================================
--- pkg/trunk/image_utils/test/read_write/Makefile 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/image_utils/test/read_write/Makefile 2008-04-10 17:08:27 UTC (rev 95)
@@ -1,4 +1,4 @@
SRC = read_write.cpp
OUT = read_write
-PKG = image_flows
+PKG = image_utils
include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Modified: pkg/trunk/image_utils/test/read_write/read_write.cpp
===================================================================
--- pkg/trunk/image_utils/test/read_write/read_write.cpp 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/image_utils/test/read_write/read_write.cpp 2008-04-10 17:08:27 UTC (rev 95)
@@ -29,14 +29,14 @@
#include <cstdio>
-#include "image_flows/FlowImage.h"
-#include "image_flows/image_flow_codec.h"
+#include "common_flows/FlowImage.h"
+#include "common_flows/ImageCodec.h"
int main(int argc, char **argv)
{
struct stat sbuf;
FlowImage image("image");
- ImageFlowCodec<FlowImage> codec(&image);
+ ImageCodec<FlowImage> codec(&image);
codec.read_file("test.jpg");
codec.write_file("out.ppm");
codec.write_file("out.jpg", 5, false);
Modified: pkg/trunk/image_viewer/nodes/test_image_viewer
===================================================================
--- pkg/trunk/image_viewer/nodes/test_image_viewer 2008-04-10 01:17:24 UTC (rev 94)
+++ pkg/trunk/image_viewer/nodes/test_image_viewer 2008-04-10 17:08:27 UTC (rev 95)
@@ -3,7 +3,7 @@
g = YAMLGraph.new
g.param 'image_sender.freq', 0.5
g.param 'image_sender.image_file', 'examples/image_sender/test.jpg'
-g.node 'image_flows/image_sender', {'launch'=>'xterm'}
+g.node 'image_utils/image_sender', {'launch'=>'xterm'}
g.node 'image_viewer/image_viewer', {'launch'=>'xterm'}
#g.node 'vacuum/vacuum', {'launch' => 'xterm'}
g.flow 'image_sender:image', 'image_viewer:image'
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-04-15 15:56:17
|
Revision: 103
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=103&view=rev
Author: morgan_quigley
Date: 2008-04-15 08:56:17 -0700 (Tue, 15 Apr 2008)
Log Message:
-----------
a small library and standalone application for viewing point clouds in SDL using the keyboard and mouse to fly through them.
Added Paths:
-----------
pkg/trunk/cloud_viewer/
pkg/trunk/cloud_viewer/build.yaml
pkg/trunk/cloud_viewer/include/
pkg/trunk/cloud_viewer/include/cloud_viewer/
pkg/trunk/cloud_viewer/include/cloud_viewer/cloud_viewer.h
pkg/trunk/cloud_viewer/lib/
pkg/trunk/cloud_viewer/manifest.xml
pkg/trunk/cloud_viewer/rosbuild
pkg/trunk/cloud_viewer/src/
pkg/trunk/cloud_viewer/src/libcloud_viewer/
pkg/trunk/cloud_viewer/src/libcloud_viewer/Makefile
pkg/trunk/cloud_viewer/src/libcloud_viewer/cloud_viewer.cpp
pkg/trunk/cloud_viewer/src/standalone/
pkg/trunk/cloud_viewer/src/standalone/Makefile
pkg/trunk/cloud_viewer/src/standalone/cloud.cpp
Added: pkg/trunk/cloud_viewer/build.yaml
===================================================================
--- pkg/trunk/cloud_viewer/build.yaml (rev 0)
+++ pkg/trunk/cloud_viewer/build.yaml 2008-04-15 15:56:17 UTC (rev 103)
@@ -0,0 +1,4 @@
+cpp:
+ make:
+ - src/libcloud_viewer
+ - src/standalone
Added: pkg/trunk/cloud_viewer/include/cloud_viewer/cloud_viewer.h
===================================================================
--- pkg/trunk/cloud_viewer/include/cloud_viewer/cloud_viewer.h (rev 0)
+++ pkg/trunk/cloud_viewer/include/cloud_viewer/cloud_viewer.h 2008-04-15 15:56:17 UTC (rev 103)
@@ -0,0 +1,37 @@
+#ifndef CLOUD_VIEWER_H
+#define CLOUD_VIEWER_H
+
+#include <vector>
+
+class CloudViewerPoint
+{
+public:
+ float x, y, z;
+ uint8_t r, g, b;
+ CloudViewerPoint(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b) :
+ x(x), y(y), z(z), r(r), g(g), b(b) { }
+};
+
+class CloudViewer
+{
+public:
+ CloudViewer();
+ ~CloudViewer();
+
+ void clear_cloud();
+ void add_point(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b);
+ void set_opengl_params(unsigned width, unsigned height);
+ void render();
+ void mouse_button(int x, int y, int button, bool is_down);
+ void mouse_motion(int x, int y, int dx, int dy);
+ void keypress(char c);
+
+private:
+ std::vector<CloudViewerPoint> points;
+ float cam_x, cam_y, cam_z, cam_rho, cam_ele, cam_azi;
+ float look_tgt_x, look_tgt_y, look_tgt_z;
+ bool left_button_down, right_button_down;
+ bool hide_axes;
+};
+
+#endif
Added: pkg/trunk/cloud_viewer/manifest.xml
===================================================================
--- pkg/trunk/cloud_viewer/manifest.xml (rev 0)
+++ pkg/trunk/cloud_viewer/manifest.xml 2008-04-15 15:56:17 UTC (rev 103)
@@ -0,0 +1,10 @@
+<package>
+<description brief="SDL-based point cloud">
+A simple point cloud viewer written using SDL.
+</description>
+<author>Morgan Quigley mqu...@cs...</author>
+<license>BSD</license>
+<url>http://stair.stanford.edu</url>
+<depend package="sdl"/>
+</package>
+
Added: pkg/trunk/cloud_viewer/rosbuild
===================================================================
--- pkg/trunk/cloud_viewer/rosbuild (rev 0)
+++ pkg/trunk/cloud_viewer/rosbuild 2008-04-15 15:56:17 UTC (rev 103)
@@ -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/cloud_viewer/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/cloud_viewer/src/libcloud_viewer/Makefile
===================================================================
--- pkg/trunk/cloud_viewer/src/libcloud_viewer/Makefile (rev 0)
+++ pkg/trunk/cloud_viewer/src/libcloud_viewer/Makefile 2008-04-15 15:56:17 UTC (rev 103)
@@ -0,0 +1,5 @@
+SRC = cloud_viewer.cpp
+OUT = ../../lib/libcloud_viewer.a
+PKG = cloud_viewer
+CFLAGS = -I../../include
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/only_rules_lib.mk
Added: pkg/trunk/cloud_viewer/src/libcloud_viewer/cloud_viewer.cpp
===================================================================
--- pkg/trunk/cloud_viewer/src/libcloud_viewer/cloud_viewer.cpp (rev 0)
+++ pkg/trunk/cloud_viewer/src/libcloud_viewer/cloud_viewer.cpp 2008-04-15 15:56:17 UTC (rev 103)
@@ -0,0 +1,129 @@
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <GL/gl.h>
+#include <GL/glu.h>
+#include "cloud_viewer/cloud_viewer.h"
+
+
+CloudViewer::CloudViewer() :
+ cam_x(0), cam_y(0), cam_z(0),
+ cam_azi(M_PI), cam_ele(0), cam_rho(3),
+ look_tgt_x(0), look_tgt_y(0), look_tgt_z(0),
+ left_button_down(false), right_button_down(false),
+ hide_axes(false)
+{
+}
+
+CloudViewer::~CloudViewer()
+{
+}
+
+void CloudViewer::clear_cloud()
+{
+ points.clear();
+}
+
+void CloudViewer::add_point(float x, float y, float z, uint8_t r, uint8_t g, uint8_t b)
+{
+ points.push_back(CloudViewerPoint(x,y,z,r,g,b));
+}
+
+void CloudViewer::render()
+{
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glScalef(1.0f, 1.0f, 1.0f);
+ // convert camera from spherical coordinates to cartesian...
+ cam_x = cam_rho * sinf((float)(M_PI/2) - cam_ele) * cosf(cam_azi) + look_tgt_x;
+ cam_y = cam_rho * cosf((float)(M_PI/2) - cam_ele) + look_tgt_y;
+ cam_z = cam_rho * sinf((float)(M_PI/2) - cam_ele) * sinf(cam_azi) + look_tgt_z;
+ gluLookAt(cam_x, cam_y, cam_z, look_tgt_x, look_tgt_y, look_tgt_z, 0, 1, 0);
+
+ if (!hide_axes)
+ {
+ glPushMatrix();
+ glTranslatef(look_tgt_x, look_tgt_y, look_tgt_z);
+ const float axes_length = 0.1f;
+ glLineWidth(2);
+ glBegin(GL_LINES);
+ glColor3f(1,0.5,0.5);
+ glVertex3f(0,0,0);
+ glVertex3f(axes_length,0,0);
+ glColor3f(0.5,1,0.5);
+ glVertex3f(0,0,0);
+ glVertex3f(0,axes_length,0);
+ glColor3f(0.5,0.5,1);
+ glVertex3f(0,0,0);
+ glVertex3f(0,0,axes_length);
+ glEnd();
+ glLineWidth(1);
+ glPopMatrix();
+
+ // draw a vector from the origin so we don't get lost
+ glBegin(GL_LINES);
+ glColor3f(1,1,1);
+ glVertex3f(0,0,0);
+ glVertex3f(look_tgt_x, look_tgt_y, look_tgt_z);
+ glEnd();
+ }
+
+
+ glBegin(GL_POINTS);
+ for (size_t i = 0; i < points.size(); i++)
+ {
+ glColor3ub(points[i].r, points[i].g, points[i].b);
+ glVertex3f(points[i].x, points[i].y, points[i].z);
+ }
+ glEnd();
+}
+
+void CloudViewer::set_opengl_params(unsigned width, unsigned height)
+{
+ glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
+ glViewport(0, 0, (GLint)width, (GLint)height);
+ glEnable(GL_DEPTH_TEST);
+ double aspect_ratio = (double)width / height;
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ gluPerspective(60.0, aspect_ratio, 0.01, 50.0);
+}
+
+void CloudViewer::mouse_button(int x, int y, int button, bool is_down)
+{
+ if (button == 0)
+ left_button_down = is_down;
+ else if (button == 2)
+ right_button_down = is_down;
+}
+
+void CloudViewer::mouse_motion(int x, int y, int dx, int dy)
+{
+ if (left_button_down)
+ {
+ cam_azi += 0.01f * dx;
+ cam_ele += 0.01f * dy;
+ // saturate cam_ele to prevent blowing through the singularity at cam_ele = +/- PI/2
+ if (cam_ele > 1.5f)
+ cam_ele = 1.5f;
+ else if (cam_ele < -1.5f)
+ cam_ele = -1.5f;
+ }
+ else if (right_button_down)
+ cam_rho *= (1.0f + 0.01f * dy);
+}
+
+void CloudViewer::keypress(char c)
+{
+ switch(c)
+ {
+ case 'w': look_tgt_z += 0.05; break;
+ case 'x': look_tgt_z -= 0.05; break;
+ case 'a': look_tgt_x -= 0.05; break;
+ case 'd': look_tgt_x += 0.05; break;
+ case 'i': look_tgt_y += 0.05; break;
+ case 'k': look_tgt_y -= 0.05; break;
+ case 'h': hide_axes = !hide_axes; break;
+ }
+}
Added: pkg/trunk/cloud_viewer/src/standalone/Makefile
===================================================================
--- pkg/trunk/cloud_viewer/src/standalone/Makefile (rev 0)
+++ pkg/trunk/cloud_viewer/src/standalone/Makefile 2008-04-15 15:56:17 UTC (rev 103)
@@ -0,0 +1,6 @@
+SRC = cloud.cpp
+OUT = cloud
+PKG = cloud_viewer
+CFLAGS = -I$(shell $(ROS_ROOT)/rospack find sdl)/include -I../../include
+LFLAGS = -L$(shell $(ROS_ROOT)/rospack find sdl)/lib -lSDL -L../../lib -lcloud_viewer -lGL -lGLU
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/only_rules.mk
Added: pkg/trunk/cloud_viewer/src/standalone/cloud.cpp
===================================================================
--- pkg/trunk/cloud_viewer/src/standalone/cloud.cpp (rev 0)
+++ pkg/trunk/cloud_viewer/src/standalone/cloud.cpp 2008-04-15 15:56:17 UTC (rev 103)
@@ -0,0 +1,111 @@
+#include <SDL/SDL.h>
+#include <cstdio>
+#include <unistd.h>
+#include <string.h>
+#include "cloud_viewer/cloud_viewer.h"
+
+int main(int argc, char **argv)
+{
+ if (argc < 2)
+ {
+ printf("please give the cloudfile as the first parameter\n");
+ return 0;
+ }
+
+ printf("opening [%s]\n", argv[1]);
+ FILE *f = fopen(argv[1], "r");
+ if (!f)
+ {
+ printf("couldn't open [%s]\n", argv[1]);
+ return -1;
+ }
+ CloudViewer cloud_viewer;
+ int line_num = 1;
+ while (!feof(f))
+ {
+ double x, y, z;
+ if (3 != fscanf(f, "%lf %lf %lf\n", &x, &y, &z))
+ {
+ printf("bad syntax on line %d\n", line_num);
+ break;
+ }
+ y *= -1; // convert from right-hand coordinate system
+ if (line_num == 1)
+ {
+ printf("%f %f %f\n", x, y, z);
+ }
+ cloud_viewer.add_point(x, y, z, 255,255,255);
+ line_num++;
+ }
+ printf("read %d points\n", line_num);
+
+ if (SDL_Init(SDL_INIT_VIDEO) < 0)
+ {
+ fprintf(stderr, "video init failed: %s\n", SDL_GetError());
+ return 1;
+ }
+ SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 24);
+ SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
+ const int w = 640, h = 480;
+ if (SDL_SetVideoMode(w, h, 32, SDL_OPENGL | SDL_HWSURFACE) == 0)
+ {
+ fprintf(stderr, "setvideomode failed: %s\n", SDL_GetError());
+ return 2;
+ }
+
+ cloud_viewer.set_opengl_params(w,h);
+ cloud_viewer.render();
+ SDL_GL_SwapBuffers();
+/*
+ for (int i = 0; i < 10000; i++)
+ {
+ cloud_viewer.add_point(rand() % 10, rand() % 10, rand() % 10, 255, 255, 255);
+ }
+*/
+
+ bool done = false;
+ while(!done)
+ {
+ usleep(1000);
+ SDL_Event event;
+ while(SDL_PollEvent(&event))
+ {
+ switch(event.type)
+ {
+ case SDL_MOUSEMOTION:
+ cloud_viewer.mouse_motion(event.motion.x, event.motion.y, event.motion.xrel, event.motion.yrel);
+ cloud_viewer.render();
+ SDL_GL_SwapBuffers();
+ break;
+ case SDL_MOUSEBUTTONDOWN:
+ case SDL_MOUSEBUTTONUP:
+ {
+ int button = -1;
+ switch(event.button.button)
+ {
+ case SDL_BUTTON_LEFT: button = 0; break;
+ case SDL_BUTTON_MIDDLE: button = 1; break;
+ case SDL_BUTTON_RIGHT: button = 2; break;
+ }
+ cloud_viewer.mouse_button(event.button.x, event.button.y,
+ button, (event.type == SDL_MOUSEBUTTONDOWN ? true : false));
+ break;
+ }
+ case SDL_KEYDOWN:
+ if (event.key.keysym.sym == SDLK_ESCAPE)
+ done = true;
+ else
+ cloud_viewer.keypress(event.key.keysym.sym);
+ cloud_viewer.render();
+ SDL_GL_SwapBuffers();
+ break;
+ case SDL_QUIT:
+ done = true;
+ break;
+ }
+ }
+ }
+ SDL_Quit();
+
+ return 0;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-04-21 21:50:02
|
Revision: 150
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=150&view=rev
Author: jleibs
Date: 2008-04-21 14:49:53 -0700 (Mon, 21 Apr 2008)
Log Message:
-----------
New node to control and view the output of a tilting laser.
Added Paths:
-----------
pkg/trunk/tilting_laser/
pkg/trunk/tilting_laser/build.yaml
pkg/trunk/tilting_laser/manifest.xml
pkg/trunk/tilting_laser/nodes/
pkg/trunk/tilting_laser/nodes/launch_tl
pkg/trunk/tilting_laser/rosbuild
pkg/trunk/tilting_laser/src/
pkg/trunk/tilting_laser/src/tilting_laser/
pkg/trunk/tilting_laser/src/tilting_laser/Makefile
pkg/trunk/tilting_laser/src/tilting_laser/tilting_laser.cpp
Added: pkg/trunk/tilting_laser/build.yaml
===================================================================
--- pkg/trunk/tilting_laser/build.yaml (rev 0)
+++ pkg/trunk/tilting_laser/build.yaml 2008-04-21 21:49:53 UTC (rev 150)
@@ -0,0 +1,3 @@
+cpp:
+ make:
+ - src/tilting_laser
\ No newline at end of file
Added: pkg/trunk/tilting_laser/manifest.xml
===================================================================
--- pkg/trunk/tilting_laser/manifest.xml (rev 0)
+++ pkg/trunk/tilting_laser/manifest.xml 2008-04-21 21:49:53 UTC (rev 150)
@@ -0,0 +1,16 @@
+<package>
+<description brief="A package for a laser mounted on a tilting stage.">
+
+This is a package to deal with a laser range finder mounted on a
+tilting stage, such as the one found in the head of PR2.
+
+</description>
+<author>Jeremy Leibs</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<depend package="roscpp"/>
+<depend package="common_flows"/>
+<depend package="unstable_flows"/>
+<depend package="hokuyourg_player"/>
+</package>
+
Added: pkg/trunk/tilting_laser/nodes/launch_tl
===================================================================
--- pkg/trunk/tilting_laser/nodes/launch_tl (rev 0)
+++ pkg/trunk/tilting_laser/nodes/launch_tl 2008-04-21 21:49:53 UTC (rev 150)
@@ -0,0 +1,17 @@
+#!/usr/bin/env ruby
+require "#{`#{ENV['ROS_ROOT']}/rospack latest yamlgraph`}/lib/yamlgraph/ygl.rb"
+g = YAMLGraph.new
+g.node 'tilting_laser/tilting_laser', {'launch'=>'xterm'}
+g.param 'hokuyourg.port', '/dev/ttyACM0'
+g.node 'hokuyourg_player/hokuyourg', {'launch'=>'xterm'}
+g.node 'etherdrive/etherdrive', {'launch'=>'xterm'}
+g.param 'etherdrive.pulse_per_rad1', '9549.29659'
+g.node 'cloud_viewer/cloud_node', {'launch'=>'xterm'}
+g.flow 'etherdrive:mot1', 'tilting_laser:encoder'
+g.flow 'tilting_laser:cmd', 'etherdrive:mot1_cmd'
+g.param 'tilting_laser.period', '10.0'
+g.flow 'hokuyourg:scans', 'tilting_laser:scans'
+g.flow 'tilting_laser:cloud', 'cloud_node:cloud'
+g.flow 'tilting_laser:shutter', 'cloud_node:shutter'
+YAMLGraphLauncher.new.launch g
+
Property changes on: pkg/trunk/tilting_laser/nodes/launch_tl
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/tilting_laser/rosbuild
===================================================================
--- pkg/trunk/tilting_laser/rosbuild (rev 0)
+++ pkg/trunk/tilting_laser/rosbuild 2008-04-21 21:49:53 UTC (rev 150)
@@ -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/tilting_laser/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/tilting_laser/src/tilting_laser/Makefile
===================================================================
--- pkg/trunk/tilting_laser/src/tilting_laser/Makefile (rev 0)
+++ pkg/trunk/tilting_laser/src/tilting_laser/Makefile 2008-04-21 21:49:53 UTC (rev 150)
@@ -0,0 +1,4 @@
+SRC = tilting_laser.cpp
+OUT = ../../nodes/tilting_laser
+PKG = tilting_laser
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/tilting_laser/src/tilting_laser/tilting_laser.cpp
===================================================================
--- pkg/trunk/tilting_laser/src/tilting_laser/tilting_laser.cpp (rev 0)
+++ pkg/trunk/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-04-21 21:49:53 UTC (rev 150)
@@ -0,0 +1,134 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* 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 the Willow Garage 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 "ros/ros_slave.h"
+#include "common_flows/FlowEmpty.h"
+#include "common_flows/FlowLaserScan.h"
+#include "common_flows/FlowPointCloudFloat32.h"
+#include "unstable_flows/FlowActuator.h"
+#include "math.h"
+
+class Tilting_Laser : public ROS_Slave
+{
+public:
+ FlowLaserScan *scans;
+ FlowActuator *encoder;
+ FlowActuator *cmd;
+ FlowPointCloudFloat32 *cloud;
+ FlowEmpty *shutter;
+
+ struct timeval starttime;
+
+ double next_shutter;
+ double period;
+
+ Tilting_Laser() : ROS_Slave(), next_shutter(0.0)
+ {
+ register_source(cloud = new FlowPointCloudFloat32("cloud"));
+ register_source(shutter = new FlowEmpty("shutter"));
+ register_sink(scans = new FlowLaserScan("scans"), ROS_CALLBACK(Tilting_Laser, scans_callback));
+ register_sink(encoder = new FlowActuator("encoder"), ROS_CALLBACK(Tilting_Laser, encoder_callback));
+ register_source(cmd = new FlowActuator("cmd"));
+ register_with_master();
+ printf("package path is [%s]\n", get_my_package_path().c_str());
+
+ if (!get_double_param(".period", &period))
+ period = 5.0;
+
+ gettimeofday(&starttime,NULL);
+ }
+
+ virtual ~Tilting_Laser()
+ {
+
+ }
+
+ void scans_callback() {
+ encoder->lock_atom();
+ cloud->set_x_size(scans->get_ranges_size());
+ cloud->set_y_size(scans->get_ranges_size());
+ cloud->set_z_size(scans->get_ranges_size());
+
+ for (int i = 0; i < scans->get_ranges_size(); i++) {
+ cloud->x[i] = cos(encoder->val)*cos(scans->angle_min + i*scans->angle_increment) * scans->ranges[i];
+ cloud->y[i] = sin(encoder->val)*cos(scans->angle_min + i*scans->angle_increment) * scans->ranges[i];
+ cloud->z[i] = sin(scans->angle_min + i*scans->angle_increment) * scans->ranges[i];
+ }
+ encoder->unlock_atom();
+ cloud->publish();
+ }
+
+ void encoder_callback() {
+ motor_control(); // Control on encoder reads sounds reasonable
+ // printf("I got some encoder values: %g!\n", encoder->val);
+ }
+
+ double timeval_diff (struct timeval x,
+ struct timeval y) {
+ double dsec = x.tv_sec - y.tv_sec;
+ double dusec = x.tv_usec - y.tv_usec;
+ return dsec + dusec/1000000.0;
+ }
+
+ void motor_control() {
+
+ struct timeval nowtime;
+ gettimeofday(&nowtime,NULL);
+
+ double elapsed_cycles = timeval_diff(nowtime, starttime) / (period);
+ double index = fabs(fmod(elapsed_cycles, 1) * 2.0 - 1.0);
+
+ cmd->val = index * (0.6 + 1.3) - 1.3;
+ cmd->rel = false;
+ cmd->valid = true;
+
+ if (elapsed_cycles > next_shutter) {
+ next_shutter += 0.5;
+ shutter->publish();
+ }
+
+ cmd->publish();
+ }
+
+};
+
+int main(int argc, char **argv)
+{
+ Tilting_Laser tl;
+ while (tl.happy()) {
+ usleep(10000);
+ }
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-05-01 01:37:19
|
Revision: 271
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=271&view=rev
Author: tfoote
Date: 2008-04-30 18:36:52 -0700 (Wed, 30 Apr 2008)
Log Message:
-----------
acutally moving controllers
Added Paths:
-----------
pkg/trunk/controllers/
Removed Paths:
-------------
pkg/trunk/controllers/
pkg/trunk/controllers/Makefile
pkg/trunk/util/control/controllers/
Copied: pkg/trunk/controllers (from rev 268, pkg/trunk/util/control/controllers)
Deleted: pkg/trunk/controllers/Makefile
===================================================================
--- pkg/trunk/util/control/controllers/Makefile 2008-04-30 23:44:41 UTC (rev 268)
+++ pkg/trunk/controllers/Makefile 2008-05-01 01:36:52 UTC (rev 271)
@@ -1,3 +0,0 @@
-SUBDIRS = src
-include $(shell rospack find mk)/recurse_subdirs.mk
-
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-05-12 00:13:33
|
Revision: 390
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=390&view=rev
Author: gerkey
Date: 2008-05-11 17:13:41 -0700 (Sun, 11 May 2008)
Log Message:
-----------
added ros wrapper for stage
Added Paths:
-----------
pkg/trunk/simulators/
pkg/trunk/simulators/rosstage/
pkg/trunk/simulators/rosstage/.player
pkg/trunk/simulators/rosstage/Makefile
pkg/trunk/simulators/rosstage/manifest.xml
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/simulators/rosstage/willow-erratic.world
pkg/trunk/simulators/rosstage/willow-full.png
Added: pkg/trunk/simulators/rosstage/.player
===================================================================
--- pkg/trunk/simulators/rosstage/.player (rev 0)
+++ pkg/trunk/simulators/rosstage/.player 2008-05-12 00:13:41 UTC (rev 390)
@@ -0,0 +1,21 @@
+plugins.cc:160 trying to load /Users/gerkey/code/personalrobots/simulators/rosstage/../../3rdparty/stage/stage-svn/worlds/libstageplugin...
+plugins.cc:164 failed (dlopen(/Users/gerkey/code/personalrobots/simulators/rosstage/../../3rdparty/stage/stage-svn/worlds/libstageplugin.so, 9): image not found)
+
+plugins.cc:175 trying to load /Users/gerkey/ps-3.0/lib/libstageplugin...
+plugins.cc:177 success
+plugins.cc:217 invoking player_driver_init()...
+plugins.cc:233 success
+plugins.cc:160 trying to load /Users/gerkey/code/personalrobots/simulators/rosstage/../../3rdparty/stage/stage-svn/worlds/libstageplugin...
+plugins.cc:164 failed (dlopen(/Users/gerkey/code/personalrobots/simulators/rosstage/../../3rdparty/stage/stage-svn/worlds/libstageplugin.so, 9): image not found)
+
+plugins.cc:175 trying to load /Users/gerkey/ps-2.1/lib/libstageplugin...
+plugins.cc:177 success
+plugins.cc:217 invoking player_driver_init()...
+plugins.cc:233 success
+plugins.cc:160 trying to load /Users/gerkey/ps-2.1/share/stage/worlds/libstageplugin...
+plugins.cc:164 failed (dlopen(/Users/gerkey/ps-2.1/share/stage/worlds/libstageplugin.so, 9): image not found)
+
+plugins.cc:175 trying to load /Users/gerkey/ps-2.1/lib/libstageplugin...
+plugins.cc:177 success
+plugins.cc:217 invoking player_driver_init()...
+plugins.cc:233 success
Added: pkg/trunk/simulators/rosstage/Makefile
===================================================================
--- pkg/trunk/simulators/rosstage/Makefile (rev 0)
+++ pkg/trunk/simulators/rosstage/Makefile 2008-05-12 00:13:41 UTC (rev 390)
@@ -0,0 +1,12 @@
+PKG = rosstage
+CXX = g++
+all: $(PKG)
+
+CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
+LFLAGS = $(shell rospack export/cpp/lflags $(PKG))
+
+rosstage: rosstage.cc
+ $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
+
+clean:
+ rm -rf *.o $(PKG) $(PKG).dSYM
Added: pkg/trunk/simulators/rosstage/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosstage/manifest.xml (rev 0)
+++ pkg/trunk/simulators/rosstage/manifest.xml 2008-05-12 00:13:41 UTC (rev 390)
@@ -0,0 +1,8 @@
+<package>
+ <description>A ROS node that wraps up the Stage mobile robot simulator.</description>
+ <author>Brian P. Gerkey</author>
+ <license>GPL</license>
+ <depend package="roscpp" />
+ <depend package="stage" />
+ <depend package="std_msgs" />
+</package>
Added: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc (rev 0)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-12 00:13:41 UTC (rev 390)
@@ -0,0 +1,172 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+// libstage
+#include <stage.hh>
+
+// roscpp
+#include <ros/node.h>
+#include <std_msgs/MsgLaserScan.h>
+#include <std_msgs/MsgRobotBase2DOdom.h>
+#include <std_msgs/MsgBaseVel.h>
+
+#define USAGE "rosstage <worldfile>"
+
+class StageNode : public ros::node
+{
+ private:
+ MsgBaseVel velMsg;
+ MsgLaserScan laserMsg;
+ MsgRobotBase2DOdom odomMsg;
+
+ public:
+ StageNode(int argc, char** argv, const char* fname);
+ ~StageNode();
+ int SubscribeModels();
+ void cmdvelReceived();
+ void Update();
+
+ static void ghfunc(gpointer key, Stg::StgModel* mod, StageNode* node);
+
+ Stg::StgWorldGui* world;
+ Stg::StgModelLaser* lasermodel;
+ Stg::StgModelPosition* positionmodel;
+};
+
+void
+StageNode::ghfunc(gpointer key,
+ Stg::StgModel* mod,
+ StageNode* node)
+{
+ if(!(node->lasermodel) &&
+ (node->lasermodel = dynamic_cast<Stg::StgModelLaser*>(mod)))
+ {
+ puts("found laser");
+ }
+ if(!(node->positionmodel) &&
+ (node->positionmodel = dynamic_cast<Stg::StgModelPosition*>(mod)))
+ {
+ puts("found position");
+ }
+}
+
+void
+StageNode::cmdvelReceived()
+{
+ printf("received cmd: %.3f %.3f\n",
+ this->velMsg.vx, this->velMsg.vw);
+}
+
+StageNode::StageNode(int argc, char** argv, const char* fname) :
+ ros::node("rosstage")
+{
+ this->lasermodel = NULL;
+ this->positionmodel = NULL;
+
+ // initialize libstage
+ Stg::Init( &argc, &argv );
+ //StgWorld world;
+ this->world = new Stg::StgWorldGui(800, 700, "Stage (ROS)");
+
+ this->world->Load(fname);
+
+ this->world->ForEachModel((GHFunc)ghfunc, this);
+}
+
+int
+StageNode::SubscribeModels()
+{
+ if(this->lasermodel)
+ this->lasermodel->Subscribe();
+ else
+ {
+ puts("no laser");
+ return(-1);
+ }
+ if(this->positionmodel)
+ this->positionmodel->Subscribe();
+ else
+ {
+ puts("no position");
+ return(-1);
+ }
+
+ advertise<MsgLaserScan>("scan");
+ advertise<MsgRobotBase2DOdom>("odom");
+ subscribe("cmd_vel", velMsg, &StageNode::cmdvelReceived);
+ return(0);
+}
+
+StageNode::~StageNode()
+{
+}
+
+void
+StageNode::Update()
+{
+ if(this->world->RealTimeUpdate())
+ {
+ Stg::stg_laser_sample_t* samples = this->lasermodel->GetSamples();
+ if(samples)
+ {
+ Stg::stg_laser_cfg_t cfg = this->lasermodel->GetConfig();
+ this->laserMsg.angle_min = -cfg.fov/2.0;
+ this->laserMsg.angle_max = +cfg.fov/2.0;
+ this->laserMsg.angle_increment = cfg.fov / (double)(cfg.sample_count-1);
+ this->laserMsg.range_max = cfg.range_bounds.max;
+ this->laserMsg.set_ranges_size(cfg.sample_count);
+ this->laserMsg.set_intensities_size(cfg.sample_count);
+ for(unsigned int i=0;i<cfg.sample_count;i++)
+ {
+ this->laserMsg.ranges[i] = samples[i].range;
+ this->laserMsg.intensities[i] = (uint8_t)samples[i].reflectance;
+ }
+
+ publish("scan",this->laserMsg);
+ }
+
+ this->odomMsg.pos.x = this->positionmodel->est_pose.x;
+ this->odomMsg.pos.y = this->positionmodel->est_pose.y;
+ this->odomMsg.pos.th = this->positionmodel->est_pose.a;
+
+ Stg::stg_velocity_t v = this->positionmodel->GetVelocity();
+
+ this->odomMsg.vel.x = v.x;
+ this->odomMsg.vel.y = v.y;
+ this->odomMsg.vel.th = v.a;
+
+ this->odomMsg.stall = this->positionmodel->Stall();
+
+ publish("odom",this->odomMsg);
+
+ }
+}
+
+int
+main(int argc, char** argv)
+{
+ if( argc < 2 )
+ {
+ puts(USAGE);
+ exit(-1);
+ }
+
+ ros::init(argc,argv);
+
+ StageNode sn(argc,argv,argv[1]);
+
+ if(sn.SubscribeModels() != 0)
+ exit(-1);
+
+ while(sn.ok() && !sn.world->TestQuit())
+ {
+ sn.Update();
+ }
+
+ // have to call this explicitly for some reason. probably interference
+ // from signal handling in Stage?
+ ros::msg_destruct();
+
+ exit(0);
+}
Added: pkg/trunk/simulators/rosstage/willow-erratic.world
===================================================================
--- pkg/trunk/simulators/rosstage/willow-erratic.world (rev 0)
+++ pkg/trunk/simulators/rosstage/willow-erratic.world 2008-05-12 00:13:41 UTC (rev 390)
@@ -0,0 +1,64 @@
+define topurg laser
+(
+ range_min 0.0
+ range_max 30.0
+ fov 282.0
+ samples 1128
+
+ # generic model properties
+ color "black"
+ size [ 0.05 0.05 0.1 ]
+)
+
+define erratic position
+(
+ size3 [0.415 0.392 0.25]
+ origin3 [-0.05 0 0 0]
+ gui_nose 1
+ drive "diff"
+ topurg(pose [0.050 0.000 0.000])
+ color "blue"
+)
+
+define floorplan model
+(
+ # sombre, sensible, artistic
+ color "gray30"
+
+ # most maps will need a bounding box
+ boundary 1
+
+ gui_nose 0
+ gui_grid 0
+ gui_movemask 0
+ gui_outline 0
+ gripper_return 0
+ fiducial_return 0
+ laser_return 1
+)
+
+# set the resolution of the underlying raytrace model in meters
+resolution 0.02
+
+interval_sim 100 # simulation timestep in milliseconds
+interval_real 100 # real-time interval between simulation updates in milliseconds
+
+window
+(
+ size [ 745.000 448.000 ]
+ center [-7.010 5.960]
+ rotate [ 0.000 -1.560 ]
+ scale 28.806
+)
+
+# load an environment bitmap
+floorplan
+(
+ name "willow"
+ bitmap "willow-full.png"
+ size3 [54.0 58.7 0.5]
+ pose [0.000 0.000 0.000]
+)
+
+# throw in a robot
+erratic(pose [0.000 0.000 0.000] name "era")
Added: pkg/trunk/simulators/rosstage/willow-full.png
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/simulators/rosstage/willow-full.png
___________________________________________________________________
Name: svn:mime-type
+ application/octet-stream
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-05-14 01:42:35
|
Revision: 411
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=411&view=rev
Author: morgan_quigley
Date: 2008-05-13 18:42:37 -0700 (Tue, 13 May 2008)
Log Message:
-----------
tweak 3rdparty makefiles to provide a 'download' target
Modified Paths:
--------------
pkg/trunk/3rdparty/ffmpeg/Makefile
pkg/trunk/3rdparty/newmat10/Makefile
pkg/trunk/3rdparty/opencv/Makefile
pkg/trunk/3rdparty/sdl/Makefile
pkg/trunk/3rdparty/sdl_image/Makefile
pkg/trunk/unported/vision/sharks/src/postprocess/projector/borg.cpp
pkg/trunk/unported/vision/sharks/src/postprocess/projector/mqmat.h
Removed Paths:
-------------
pkg/trunk/util/image_utils/nodes/
Modified: pkg/trunk/3rdparty/ffmpeg/Makefile
===================================================================
--- pkg/trunk/3rdparty/ffmpeg/Makefile 2008-05-14 00:45:49 UTC (rev 410)
+++ pkg/trunk/3rdparty/ffmpeg/Makefile 2008-05-14 01:42:37 UTC (rev 411)
@@ -3,6 +3,8 @@
ffmpeg-latest:
svn export svn://svn.mplayerhq.hu/ffmpeg/trunk ffmpeg-latest
+download: ffmpeg-latest
+
ffmpeg: ffmpeg-latest
cd ffmpeg-latest && ./configure --prefix=$(PWD)/ffmpeg --enable-shared
cd ffmpeg-latest && make && make install
Modified: pkg/trunk/3rdparty/newmat10/Makefile
===================================================================
--- pkg/trunk/3rdparty/newmat10/Makefile 2008-05-14 00:45:49 UTC (rev 410)
+++ pkg/trunk/3rdparty/newmat10/Makefile 2008-05-14 01:42:37 UTC (rev 411)
@@ -1,8 +1,12 @@
-all: newmat
+all: newmat/newmat10
-newmat:
+newmat/newmat_1.10.4/newmat10.tar.gz:
mkdir -p newmat/newmat_1.10.4
cd newmat/newmat_1.10.4 && wget http://www.robertnz.net/ftp/newmat10.tar.gz
+
+download: newmat/newmat_1.10.4/newmat10.tar.gz
+
+newmat/newmat10: newmat/newmat_1.10.4/newmat10.tar.gz
cd newmat/newmat_1.10.4 && tar xzf newmat10.tar.gz
cd newmat/newmat_1.10.4 && patch -p1 <../../newmat_1.10.4-2build1.diff
cd newmat/newmat_1.10.4 && touch config.h
Modified: pkg/trunk/3rdparty/opencv/Makefile
===================================================================
--- pkg/trunk/3rdparty/opencv/Makefile 2008-05-14 00:45:49 UTC (rev 410)
+++ pkg/trunk/3rdparty/opencv/Makefile 2008-05-14 01:42:37 UTC (rev 411)
@@ -1,9 +1,13 @@
all: opencv
-opencv-1.0.0:
+opencv-1.0.0.tar.gz:
wget http://internap.dl.sourceforge.net/sourceforge/opencvlibrary/opencv-1.0.0.tar.gz
+
+opencv-1.0.0: opencv-1.0.0.tar.gz
tar xzf opencv-1.0.0.tar.gz
+download: opencv-1.0.0.tar.gz
+
FFMPEG := $(shell rospack find ffmpeg)
opencv: opencv-1.0.0
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${FFMPEG}/ffmpeg/lib && cd opencv-1.0.0 && ./configure --without-quicktime --with-ffmpeg --prefix=$(PWD)/opencv CFLAGS="-I${FFMPEG}/ffmpeg/include -I${FFMPEG}/ffmpeg/include/libavutils -I${FFMPEG}/include/libavformat" CPPFLAGS="-I${FFMPEG}/ffmpeg/include -I${FFMPEG}/ffmpeg/include/libavutils -I${FFMPEG}/ffmpeg/include/libavformat" LDFLAGS=-L${FFMPEG}/ffmpeg/lib --with-gnu-ld --with-x
@@ -14,4 +18,4 @@
clean:
-rm -rf opencv opencv-1.0.0 opencv-1.0.0.tar.gz
-.PHONY : clean
+.PHONY : clean download
Modified: pkg/trunk/3rdparty/sdl/Makefile
===================================================================
--- pkg/trunk/3rdparty/sdl/Makefile 2008-05-14 00:45:49 UTC (rev 410)
+++ pkg/trunk/3rdparty/sdl/Makefile 2008-05-14 01:42:37 UTC (rev 411)
@@ -1,7 +1,11 @@
all: SDL
-SDL-1.2.13:
+SDL-1.2.13.tar.gz:
wget http://libsdl.org/release/SDL-1.2.13.tar.gz
+
+download: SDL-1.2.13.tar.gz
+
+SDL-1.2.13: SDL-1.2.13.tar.gz
tar xzf SDL-1.2.13.tar.gz
SDL: SDL-1.2.13
Modified: pkg/trunk/3rdparty/sdl_image/Makefile
===================================================================
--- pkg/trunk/3rdparty/sdl_image/Makefile 2008-05-14 00:45:49 UTC (rev 410)
+++ pkg/trunk/3rdparty/sdl_image/Makefile 2008-05-14 01:42:37 UTC (rev 411)
@@ -1,7 +1,11 @@
all: SDL_image
-SDL_image-1.2.6:
+SDL_image-1.2.6.tar.gz:
wget http://www.libsdl.org/projects/SDL_image/release/SDL_image-1.2.6.tar.gz
+
+download: SDL_image-1.2.6.tar.gz
+
+SDL_image-1.2.6: SDL_image-1.2.6.tar.gz
tar xzf SDL_image-1.2.6.tar.gz
SDL_image: SDL_image-1.2.6
Modified: pkg/trunk/unported/vision/sharks/src/postprocess/projector/borg.cpp
===================================================================
--- pkg/trunk/unported/vision/sharks/src/postprocess/projector/borg.cpp 2008-05-14 00:45:49 UTC (rev 410)
+++ pkg/trunk/unported/vision/sharks/src/postprocess/projector/borg.cpp 2008-05-14 01:42:37 UTC (rev 411)
@@ -9,21 +9,72 @@
using namespace mqmath;
double ray[3], laser_norm[3], laser_origin[3], cam_point[3], world_point[3];
-const double cam_tilt = 30.0 * units::DEGREES();
+const double cam_tilt = 0.0 * units::DEGREES(); //30
-void intrinsics(double u, double v)
+
+
+//void intrinsics(double u, double v)
+//{
+//// focal length: (393.756 393.798)
+////principal point: (315.432 247.361)
+//
+// //double fx = 949.415, fy = 870.535;
+// //double u0 = 340.7 , v0 = 278.8;
+// double fx = 393.756, fy = 393.798;
+// double u0 = 315.432 , v0 = 247.361;
+// // express the pixel ray in the base kinematic frame
+// ray[0] = 1;
+// ray[1] = -(v - v0) / fy;
+// ray[2] = (u - u0) / fx;
+//
+// double ray_norm = sqrt(ray[0]*ray[0] + ray[1]*ray[1] + ray[2]*ray[2]);
+// ray[0] /= ray_norm;
+// ray[1] /= ray_norm;
+// ray[2] /= ray_norm;
+//}
+
+void intrinsics(double xp, double yp)
{
- double fx = 949.415, fy = 870.535;
- double u0 = 340.7 , v0 = 278.8;
- // express the pixel ray in the base kinematic frame
- ray[0] = 1; //(u - u0) / fx;
- ray[1] = -(v - v0) / fy;
- ray[2] = (u - u0) / fx;
- double ray_norm = sqrt(ray[0]*ray[0] + ray[1]*ray[1] + ray[2]*ray[2]);
- ray[0] /= ray_norm;
- ray[1] /= ray_norm;
- ray[2] /= ray_norm;
+ //394.493 0 318.619
+ //0 394.327 247.207
+ //0 0 1
+ //first two radial distortion coefficients:
+ //-0.344023 0.127415
+ //and the first two tangential distortion coefficients:
+ //-0.000815 -0.000350
+ double fx = 394.493, fy = 394.327;
+ double x0 = 318.619 , y0 = 247.207;
+ double k1 = -0.344023 , k2 = 0.127415 , k3 = -0.000815 , k4 = -0.000350 , k5 = 0;
+
+ // express the pixel ray in the base kinematic frame
+ //ray[0] = 1;
+ //ray[1] = -(y - y0) / fy;
+ //ray[2] = (x - x0) / fx;
+ double zd = 1;
+ double yd = (yp - y0) / fy;
+ double xd = (xp - x0) / fx;
+
+
+ double r2 = pow(xd,2) + pow(yd,2);
+
+ double radialDistortion = 1 + k1 * r2 + k2 * pow(r2,2) + k5 * pow(r2,2) * r2;
+
+ double tangentialX = 2 * k3 * xd * yd + k4 * ( r2 + 2 * pow(xd,2));
+ double tangentialY = k3 * (r2 + 2 * pow(yd,2)) + 2 * k4 * xd * yd;
+
+ double x = (xd - tangentialX) / radialDistortion;
+ double y = (yd - tangentialY) / radialDistortion;
+
+ ray[0] = 1;
+ ray[1] = -y;
+ ray[2] = x;
+
+
+ double ray_norm = sqrt(ray[0]*ray[0] + ray[1]*ray[1] + ray[2]*ray[2]);
+ ray[0] /= ray_norm;
+ ray[1] /= ray_norm;
+ ray[2] /= ray_norm;
}
void extrinsics(const mqmat<4,4> &T)
@@ -73,32 +124,45 @@
printf("LOWER YOUR SHIELDS\n\n");
const double laser_encoder_offset = 150 * units::DEGREES();
- const double baseline = 9.25 * units::INCHES();
- const double stage_tilt = 15 * units::DEGREES();
+ const double baseline = 6.25 * units::INCHES(); //9.25
+ const double stage_tilt = 5 * units::DEGREES();//15
const double stage_back = 0.5 * units::INCHES();
- const double stage_up = 7.5 * units::INCHES();
+ const double stage_up = 8 * units::INCHES();
const double a2 = stage_up * sin(-stage_tilt) + stage_back * cos(-stage_tilt);
const double d2 = stage_up * cos(-stage_tilt) - stage_back * sin(-stage_tilt);
+ // const double laser_encoder_offset = 150 * units::DEGREES();
+ //const double baseline = 6.25 * units::INCHES();
+ //const double stage_tilt = 7 * units::DEGREES();
+ //const double stage_back = 0.5 * units::INCHES();
+ //const double stage_up = 8 * units::INCHES();
+ //const double a2 = stage_up * sin(-stage_tilt) + stage_back * cos(-stage_tilt);
+ //const double d2 = stage_up * cos(-stage_tilt) - stage_back * sin(-stage_tilt);
+
// mqdh T0(0, 0, 0, cam_tilt);
mqdh T0(0, 0, 0, 0);
mqdh T1(0, 0, baseline, stage_tilt);
- mqdh T3(M_PI/2, 0, 0, 0 * units::DEGREES());
+ mqdh T3(M_PI/2, 0, 0, 0 * units::DEGREES()); //M_PI in linux
- const char *laserfile = "laserData.txt";
+ //const char *laserfile = "laserData.txt";
+ char *laserfile = argv[1];
FILE *f = fopen(laserfile, "r");
if (!f)
{
printf("couldn't open [%s]\n", laserfile);
return 1;
}
- FILE *out = fopen("points.txt", "w");
+ //FILE *out = fopen("points.txt", "w");
+ char *pointcloud = argv[2];
+ FILE *out = fopen(pointcloud, "w");
int line = 0;
while (!feof(f))
{
line++;
double laser_ang, row, col;
- if (3 != fscanf(f, "%lf %lf %lf\n", &laser_ang, &row, &col))
+ double r,g,b;
+ //if (3 != fscanf(f, "%lf %lf %lf\n", &laser_ang, &row, &col))
+ if (6 != fscanf(f, "%lf %lf %lf %lf %lf %lf\n", &laser_ang, &row, &col,&r,&g,&b))
{
printf("error in parse\n");
break;
@@ -108,7 +172,7 @@
break;
#endif
double stage = laser_encoder_offset - laser_ang * units::DEGREES();
- mqdh T2(-M_PI/2, a2, d2, stage);
+ mqdh T2(-M_PI/2, a2, d2, stage); //M_PI in linux
mqmat<4,4> T(T0 * T1 * T2 * T3);
extrinsics(T);
intrinsics(col, row);
@@ -125,9 +189,10 @@
printf("\n");
#endif
- fprintf(out, "%f %f %f\n", world_point[0], world_point[1], world_point[2]);
+ //fprintf(out, "%f %f %f\n", world_point[0], world_point[1], world_point[2]);
+ fprintf(out, "%f %f %f %d %d %d %f %f %f %d\n", world_point[0], world_point[1], world_point[2],0,0,0,r,g,b,0);
- }
+ }//while laser points are being read
fclose(f);
fclose(out);
Modified: pkg/trunk/unported/vision/sharks/src/postprocess/projector/mqmat.h
===================================================================
--- pkg/trunk/unported/vision/sharks/src/postprocess/projector/mqmat.h 2008-05-14 00:45:49 UTC (rev 410)
+++ pkg/trunk/unported/vision/sharks/src/postprocess/projector/mqmat.h 2008-05-14 01:42:37 UTC (rev 411)
@@ -16,7 +16,7 @@
template<size_t rows, size_t cols, typename T>
std::ostream &operator<<(std::ostream &, const mqmat<rows, cols, T> &);
-template<size_t rows, size_t cols, typename T = double>
+template<size_t rows, size_t cols = 1, typename T = double>
class mqmat
{
friend std::ostream &operator<< <rows, cols, T>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-05-16 07:01:59
|
Revision: 447
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=447&view=rev
Author: gerkey
Date: 2008-05-16 00:02:04 -0700 (Fri, 16 May 2008)
Log Message:
-----------
finally tracked down that pesky bug; naturally it was pointer-related
Modified Paths:
--------------
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/nav_view/nav_view.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-16 03:15:49 UTC (rev 446)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-16 07:02:04 UTC (rev 447)
@@ -310,10 +310,6 @@
(long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) *
1000000000ULL));
*/
- printf("pose: (%.3f %.3f %.3f)\n",
- pdata->pos.px,
- pdata->pos.py,
- RTOD(pdata->pos.pa));
localizedOdomMsg.pos.x = pdata->pos.px;
localizedOdomMsg.pos.y = pdata->pos.py;
localizedOdomMsg.pos.th = pdata->pos.pa;
@@ -569,7 +565,7 @@
PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
(void*)&pdata,0,
- ×tamp,true);
+ ×tamp);
}
int
Modified: pkg/trunk/nav/nav_view/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view/nav_view.cpp 2008-05-16 03:15:49 UTC (rev 446)
+++ pkg/trunk/nav/nav_view/nav_view.cpp 2008-05-16 07:02:04 UTC (rev 447)
@@ -1,3 +1,39 @@
+/*
+ * nav_view
+ * Copyright (c) 2008, Morgan Quigley
+ * All rights reserved.
+ *
+ * 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 the <ORGANIZATION> 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.
+ */
+
+/*
+ * @mainpage
+ *
+ * foo
+ */
+
#include <unistd.h>
#include <assert.h>
#include <math.h>
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-16 03:15:49 UTC (rev 446)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-16 07:02:04 UTC (rev 447)
@@ -117,6 +117,8 @@
double* laser_hitpts;
size_t laser_hitpts_len, laser_hitpts_size;
+ double laser_pose[3];
+
// Controller paramters
double lookahead_maxdist;
double lookahead_distweight;
@@ -191,7 +193,7 @@
rotate_dir(0),
printed_warning(false),
stopped(false),
- robot_radius(0.2),
+ robot_radius(0.20),
safety_dist(0.1),
max_radius(1.0),
dist_penalty(1.0),
@@ -200,7 +202,7 @@
ang_eps(DTOR(10.0)),
cycletime(0.1),
laser_maxrange(4.0),
- laser_buffer_time(0.5),
+ laser_buffer_time(2.0),
lookahead_maxdist(2.0),
lookahead_distweight(10.0),
tvmin(0.1),
@@ -231,17 +233,7 @@
for(int j=0;j<sy;j++)
{
for(int i=0;i<sx;i++)
- {
this->plan->cells[i+j*sx].occ_state = mapdata[i+j*sx];
- /*
- if(mapdata[i+j*sx] < 0.1*255)
- this->plan->cells[i+j*sx].occ_state = -1;
- else if(mapdata[i+j*sx] > 0.5*255)
- this->plan->cells[i+j*sx].occ_state = 1;
- else
- this->plan->cells[i+j*sx].occ_state = 0;
- */
- }
}
free(mapdata);
@@ -264,6 +256,11 @@
this->firstodom = true;
+ // TODO: get this info via ROS somehow
+ laser_pose[0] = 0.0;
+ laser_pose[1] = 0.0;
+ laser_pose[2] = 0.0;
+
advertise<MsgPlanner2DState>("state");
advertise<MsgPolyline2D>("gui_path");
advertise<MsgPolyline2D>("gui_laser");
@@ -342,10 +339,12 @@
this->pose[0] = odomMsg.pos.x;
this->pose[1] = odomMsg.pos.y;
this->pose[2] = odomMsg.pos.th;
+ /*
printf("gpose: %.3f %.3f %.3f\n",
this->pose[0],
this->pose[1],
RTOD(this->pose[2]));
+ */
if(this->firstodom)
{
@@ -394,20 +393,31 @@
p->y = prevOdom.pos.y + (dy / dt) * dtl;
p->th = ANG_NORM(prevOdom.pos.th + (da / dt) * dtl);
+ /*
printf("0: %.3f %.3f %.3f\t%.6f\n",
- odomMsg.pos.x,
- odomMsg.pos.y,
- odomMsg.pos.th,
+ prevOdom.pos.x,
+ prevOdom.pos.y,
+ prevOdom.pos.th,
t0);
printf("I: %.3f %.3f %.3f\t%.6f\n",
p->x,p->y,p->th, tl);
printf("1: %.3f %.3f %.3f\t%.6f\n",
- prevOdom.pos.x,
- prevOdom.pos.y,
- prevOdom.pos.th,
+ odomMsg.pos.x,
+ odomMsg.pos.y,
+ odomMsg.pos.th,
t1);
+ */
- MsgLaserScan* scan = new MsgLaserScan(this->laserMsg);
+ //MsgLaserScan* scan = new MsgLaserScan(**it);
+ MsgLaserScan* scan = new MsgLaserScan();
+ scan->angle_min = (*it)->angle_min;
+ scan->angle_increment = (*it)->angle_increment;
+ scan->range_max = (*it)->range_max;
+ scan->set_ranges_size((*it)->get_ranges_size());
+ scan->header.stamp_secs = (*it)->header.stamp_secs;
+ scan->header.stamp_nsecs = (*it)->header.stamp_nsecs;
+ memcpy(scan->ranges,(*it)->ranges,
+ sizeof(float)*(*it)->get_ranges_size());
std::pair<MsgPose2DFloat32*,MsgLaserScan*> item(p,scan);
@@ -416,10 +426,12 @@
it--;
}
+ //printf("%lu scans\n", laser_scans.size());
+
// Remove anything that's too old
// Also count how many points we have
- double currtime = this->laserMsg.header.stamp_secs +
- this->laserMsg.header.stamp_nsecs / 1e9;
+ double currtime = this->odomMsg.header.stamp_secs +
+ this->odomMsg.header.stamp_nsecs / 1e9;
unsigned int hitpt_cnt=0;
for(std::list<std::pair<MsgPose2DFloat32*,MsgLaserScan*> >::iterator it = this->laser_scans.begin();
it != this->laser_scans.end();
@@ -429,6 +441,7 @@
if((currtime - msgtime) > this->laser_buffer_time)
{
delete it->first;
+ delete it->second->ranges;
delete it->second;
it = this->laser_scans.erase(it);
it--;
@@ -454,6 +467,12 @@
it != this->laser_scans.end();
it++)
{
+ /*
+ printf("%.3f %.3f %.3f\n",
+ it->first->x,
+ it->first->y,
+ it->first->th);
+ */
float b=it->second->angle_min;
float* r=it->second->ranges;
for(unsigned int j=0;
@@ -463,17 +482,29 @@
if(((*r) >= this->laser_maxrange) || ((*r) >= it->second->range_max))
continue;
+ // Project into laser frame
+ double lx, ly;
+ lx = (*r)*cos(b);
+ ly = (*r)*sin(b);
+
+ // Convert into robot frame
+ double rx,ry;
double cs,sn;
- cs = cos(it->first->th+b);
- sn = sin(it->first->th+b);
+ cs = cos(laser_pose[2]);
+ sn = sin(laser_pose[2]);
+ rx = laser_pose[0] + lx*cs - ly*sn;
+ ry = laser_pose[1] + lx*sn + ly*cs;
- double lx,ly;
- lx = it->first->x + (*r)*cs;
- ly = it->first->y + (*r)*sn;
+ // Convert to world frame
+ double wx,wy;
+ cs = cos(it->first->th);
+ sn = sin(it->first->th);
+ wx = it->first->x + cs*rx - sn*ry;
+ wy = it->first->y + sn*rx + cs*ry;
assert(this->laser_hitpts_len*2 < this->laser_hitpts_size);
- *(pts++) = lx;
- *(pts++) = ly;
+ *(pts++) = wx;
+ *(pts++) = wy;
this->laser_hitpts_len++;
}
}
@@ -498,7 +529,6 @@
prevOdom = odomMsg;
}
-
this->lock.unlock();
}
@@ -557,7 +587,7 @@
this->stopRobot();
break;
case REACHED_GOAL:
- puts("still done");
+ //puts("still done");
this->stopRobot();
break;
case NEW_GOAL:
@@ -647,7 +677,7 @@
}
publish("gui_path", polylineMsg);
- printf("computed velocities: %.3f %.3f\n", vx, RTOD(va));
+ //printf("computed velocities: %.3f %.3f\n", vx, RTOD(va));
this->sendVelCmd(vx, 0.0, va);
break;
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-16 03:15:49 UTC (rev 446)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-16 07:02:04 UTC (rev 447)
@@ -96,9 +96,6 @@
{
this->lock.lock();
- printf("received cmd: %.3f %.3f\n",
- this->velMsg.vx, this->velMsg.vw);
-
this->positionmodel->SetSpeed(this->velMsg.vx, 0.0, this->velMsg.vw);
this->lock.unlock();
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-03 15:52:59
|
Revision: 604
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=604&view=rev
Author: hsujohnhsu
Date: 2008-06-03 08:53:04 -0700 (Tue, 03 Jun 2008)
Log Message:
-----------
updates to gazebo makefile for 3rd party software
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/Makefile
pkg/trunk/vision/cam_viewer/cam_viewer.cpp
Modified: pkg/trunk/3rdparty/gazebo/Makefile
===================================================================
--- pkg/trunk/3rdparty/gazebo/Makefile 2008-06-03 06:15:37 UTC (rev 603)
+++ pkg/trunk/3rdparty/gazebo/Makefile 2008-06-03 15:53:04 UTC (rev 604)
@@ -4,9 +4,7 @@
@if [ ! -d gazebo ]; then git clone /u/pr/simpr/pr2sim.git gazebo-git; fi
gazebo: gazebo-git
- cd gazebo-git && /bin/bash -c "source setup.bash; cd packages; make"
- cd gazebo-git && /bin/bash -c "source setup.bash; cd plugin; scons; scons install"
- cd gazebo-git && /bin/bash -c "source setup.bash; cd examples/pr2API; scons; scons install"
+ (cd gazebo-git && /bin/bash -c "source setup.bash; make")
clean:
-cd gazebo-git && make clean
Modified: pkg/trunk/vision/cam_viewer/cam_viewer.cpp
===================================================================
--- pkg/trunk/vision/cam_viewer/cam_viewer.cpp 2008-06-03 06:15:37 UTC (rev 603)
+++ pkg/trunk/vision/cam_viewer/cam_viewer.cpp 2008-06-03 15:53:04 UTC (rev 604)
@@ -23,7 +23,7 @@
IplImage *cv_image = cvCreateImage( cvSize( image_msg.width, image_msg.height), IPL_DEPTH_8U, 3);
memcpy(cv_image->imageData, image_msg.data, cv_image->imageSize);
cvShowImage("cam_viewer", cv_image);
- cvWaitKey(3);
+ cvWaitKey(10);
cvReleaseImage(&cv_image);
}
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-22 08:02:27
|
Revision: 890
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=890&view=rev
Author: hsujohnhsu
Date: 2008-06-21 00:14:35 -0700 (Sat, 21 Jun 2008)
Log Message:
-----------
splitting PR2 API into lowlevel libpr2HW and high level libpr2API.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/Makefile
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/src/test/test_pr2HW.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/Makefile
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-21 06:23:05 UTC (rev 889)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-21 07:14:35 UTC (rev 890)
@@ -6,11 +6,13 @@
CDFLAGS = $(shell rospack export/cpp/cflags gazebo) \
$(shell rospack export/cpp/cflags pr2Core) \
$(shell rospack export/cpp/cflags libKinematics) \
+ $(shell rospack export/cpp/cflags libpr2HW) \
$(shell rospack export/cpp/cflags newmat10)
LDFLAGS = $(shell rospack export/cpp/lflags gazebo) \
$(shell rospack export/cpp/lflags pr2Core) \
$(shell rospack export/cpp/lflags libKinematics) \
+ $(shell rospack export/cpp/lflags libpr2HW) \
$(shell rospack export/cpp/lflags newmat10)
LFLAGS = $(shell rospack export/cpp/lflags $(PKG))
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-21 06:23:05 UTC (rev 889)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-21 07:14:35 UTC (rev 890)
@@ -39,6 +39,8 @@
#include <stdint.h>
#include <string>
+#include <libpr2HW/pr2HW.h>
+
#ifdef KDL_KINEMATICS
#include <libKDL/kdl_kinematics.h> // for kinematics using KDL -- util/kinematics/libKDL
#endif
@@ -51,6 +53,7 @@
class PR2Robot
{
+ public: PR2::PR2HW hw;
/*! \fn
\brief Constructor
@@ -244,13 +247,6 @@
/*! \fn
- \brief Enable the model (i.e. enable all actuators corresponding to a particular part of the robot)
- \param id - modelID, see pr2Core.h for a list of model IDs
- */
- public: PR2_ERROR_CODE EnableModel(PR2_MODEL_ID id);
-
-
- /*! \fn
\brief Enable the head (i.e. enable all actuators corresponding to the head)
*/
public: PR2_ERROR_CODE EnableHead();
@@ -291,15 +287,7 @@
*/
public: PR2_ERROR_CODE EnableSpine();
-
/*! \fn
- \brief Disable the model (i.e. disable all actuators corresponding to a particular part of the robot)
- \param id - model ID, see pr2Core.h for a list of model IDs
- */
- public: PR2_ERROR_CODE DisableModel(PR2_MODEL_ID id);
-
-
- /*! \fn
\brief Disable the head (i.e. enable all actuators corresponding to the head)
*/
public: PR2_ERROR_CODE DisableHead();
@@ -342,14 +330,6 @@
/*! \fn
- \brief Check whether all actuators in a particular part of the robot have been enabled
- \param id - model ID, see pr2Core.h for a list of model IDs
- \param enabled - pointer to return value - 0 if any actuator in that part of the robot is disabled, 1 if all actuators in that part of the robot are enabled
- */
- public: PR2_ERROR_CODE IsEnabledModel(PR2_MODEL_ID id, int *enabled);
-
-
- /*! \fn
\brief Command the arm to go to a particular position in joint space
\param id - model ID, see pr2Core.h for a list of model IDs
\param jointPosition - array of desired positions of all the joints
@@ -371,25 +351,6 @@
#endif
/*! \fn
- \brief Get the actual wrist pose
- \param id - model ID, see pr2Core.h for a list of model IDs
- \param *x pointer to return value of x position of the wrist
- \param *y pointer to return value of y position of the wrist
- \param *z pointer to return value of z position of the wrist
- \param *roll pointer to return value of roll of the wrist
- \param *pitch pointer to return value of pitch of the wrist
- \param *yaw pointer to return value of yaw of the wrist
- */
- public: PR2_ERROR_CODE GetWristPoseGroundTruth(PR2_MODEL_ID id, double *x, double *y, double *z, double *roll, double *pitch, double *yaw);
-
-
- /*! \fn
- \brief Get the actual wrist pose
- \param id - model ID, see pr2Core.h for a list of model IDs
- */
- public: NEWMAT::Matrix GetWristPoseGroundTruth(PR2_MODEL_ID id);
-
- /*! \fn
\brief Get the actual joint values for an arm
\param id - model ID, see pr2Core.h for a list of model IDs
\param jointPosition - array of desired positions of all the joints
@@ -603,42 +564,6 @@
*/
public: PR2_ERROR_CODE StopRobot();
- /*! \fn
- \brief - Get laser range data
- */
- public: PR2_ERROR_CODE GetLaserRanges(PR2_SENSOR_ID id,
- float* angle_min, float* angle_max, float* angle_increment,
- float* range_max,uint32_t* ranges_size ,uint32_t* ranges_alloc_size,
- uint32_t* intensities_size,uint32_t* intensities_alloc_size,
- float* ranges ,uint8_t* intensities);
-
- /*! \fn
- \brief - Open gripper
- */
- public: PR2_ERROR_CODE OpenGripper(PR2_MODEL_ID id,double gap, double force);
-
- /*! \fn
- \brief - Close gripper
- */
- public: PR2_ERROR_CODE CloseGripper(PR2_MODEL_ID id,double gap, double force);
-
- /*! \fn
- \brief - Set gripper p,i,d gains
- */
- public: PR2_ERROR_CODE SetGripperGains(PR2_MODEL_ID id,double p,double i, double d);
- /*!
- \brief - control mode for the arms, possible values are joint space control or cartesian space control
- */
-
- /*! \fn
- \brief - Get camera data
- */
- public: PR2_ERROR_CODE GetCameraImage(PR2_SENSOR_ID id ,
- uint32_t* width ,uint32_t* height ,
- uint32_t* depth ,
- std::string* compression ,std::string* colorspace ,
- uint32_t* data_size ,void* data );
-
protected: PR2_CONTROL_MODE armControlMode[2];
protected: PR2_CONTROL_MODE baseControlMode;
protected: PR2Arm myArm;
@@ -646,18 +571,7 @@
#ifdef KDL_KINEMATICS
public: PR2_kinematics pr2_kin; // for kinematics using KDL.
#endif
- /*! \fn
- \brief - Oscillate the Hokuyo, return point cloud
- */
- //public: PR2_ERROR_CODE GetPointCloud();
- /*! \fn
- \brief - returns simulation time
- */
- public: PR2_ERROR_CODE GetSimTime(double *sim_time);
-
-
-
};
}
#endif
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-06-21 06:23:05 UTC (rev 889)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-06-21 07:14:35 UTC (rev 890)
@@ -10,6 +10,7 @@
<depend package="newmat10"/>
<depend package="gazebo"/>
<depend package="pr2Core"/>
+<depend package="libpr2HW"/>
<depend package="libKinematics"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lpr2API"/>
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-21 06:23:05 UTC (rev 889)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-21 07:14:35 UTC (rev 890)
@@ -1,354 +1,19 @@
+
#include <libpr2API/pr2API.h>
+#include <pr2Core/pr2Misc.h>
#include <math.h>
-#include <gazebo/gazebo.h>
-#include <gazebo/GazeboError.hh>
-
-using namespace gazebo;
using namespace PR2;
-static gazebo::Client *client;
-static gazebo::SimulationIface *simIface;
-
-static gazebo::PR2ArrayIface *pr2Iface;
-static gazebo::PR2ArrayIface *pr2HeadIface;
-static gazebo::PR2GripperIface *pr2GripperLeftIface;
-static gazebo::PR2GripperIface *pr2GripperRightIface;
-static gazebo::LaserIface *pr2LaserIface;
-static gazebo::LaserIface *pr2BaseLaserIface;
-static gazebo::CameraIface *pr2CameraIface;
-static gazebo::CameraIface *pr2CameraGlobalIface;
-static gazebo::CameraIface *pr2CameraHeadLeftIface;
-static gazebo::CameraIface *pr2CameraHeadRightIface;
-
-static gazebo::PositionIface *pr2LeftWristIface;
-static gazebo::PositionIface *pr2RightWristIface;
-static gazebo::PositionIface *pr2BaseIface;
-
-
-point Rot2D(point p,double theta)
-{
- point q;
- q.x = cos(theta)*p.x - sin(theta)*p.y;
- q.y = sin(theta)*p.y + cos(theta)*p.x;
- return q;
-}
-
-bool IsHead(PR2_MODEL_ID id)
-{
- if(id == HEAD)
- return true;
- return false;
-}
-bool IsGripperLeft(PR2_MODEL_ID id)
-{
- if(id == PR2_LEFT_GRIPPER)
- return true;
- return false;
-}
-bool IsGripperRight(PR2_MODEL_ID id)
-{
- if(id == PR2_RIGHT_GRIPPER)
- return true;
- return false;
-}
-
-bool IsHead(PR2_JOINT_ID id)
-{
- if(id >= JointStart[HEAD] && id <= JointEnd[HEAD])
- return true;
- return false;
-}
-
-bool IsGripperLeft(PR2_JOINT_ID id)
-{
- if (id >= JointStart[ARM_L_GRIPPER] && id <= JointEnd[ARM_L_GRIPPER])
- return true;
- return false;
-}
-bool IsGripperRight(PR2_JOINT_ID id)
-{
- if (id >= JointStart[ARM_R_GRIPPER] && id <= JointEnd[ARM_R_GRIPPER])
- return true;
- return false;
-}
-
-double GetMagnitude(double xl[], int num)
-{
- int ii;
- double mag=0;
- for(ii=0; ii < num; ii++)
- mag += (xl[ii]*xl[ii]);
- return sqrt(mag);
-}
-
-double GetMagnitude(double x, double y)
-{
- return sqrt(x*x+y*y);
-}
-
-
-/* --------------- utility functions ---------------- */
-/* --------------- TODO ---------------- */
-/* --------------- move to a library ---------------- */
-
-#define FROM_DEGREES(degrees) (((double)(degrees))/180*M_PI)
-/*
- * normalize_angle_positive
- *
- * Normalizes the angle to be 0 circle to 1 circle
- * It takes and returns native units.
- */
-
-double normalize_angle_positive(double angle)
-{
- return fmod(fmod(angle, FROM_DEGREES(360))+FROM_DEGREES(360), FROM_DEGREES(360));
-}
-/*
- * normalize
- *
- * Normalizes the angle to be -1/2 circle to +1/2 circle
- * It takes and returns native units.
- *
- */
-
-double normalize_angle(double angle)
-{
- double a=normalize_angle_positive(angle);
- if (a>FROM_DEGREES(180))
- a-=FROM_DEGREES(360);
- return(a);
-}
-/*
- * shortest_angular_distance
- *
- * Given 2 angles, this returns the shortest angular
- * difference. The inputs and ouputs are of course native
- * units.
- *
- * As an example, if native units are degrees, the result
- * would always be -180 <= result <= 180. Adding the result
- * to "from" will always get you an equivelent angle to "to".
- */
-
-double shortest_angular_distance(double from, double to)
-{
- double result;
- result=normalize_angle_positive(
- normalize_angle_positive(to) - normalize_angle_positive(from));
-
- if ( result > FROM_DEGREES(180) ) { // If the result > 180,
- // It's shorter the other way.
- result=-(FROM_DEGREES(360)-result);
- }
- return normalize_angle(result);
-}
-
-
-
-
-
-
PR2Robot::PR2Robot()
{
+
};
PR2Robot::~PR2Robot(){};
-PR2_ERROR_CODE PR2Robot::GetSimTime(double *sim_time)
-{
- *sim_time = simIface->data->simTime;
- return PR2_ALL_OK;
-};
-
PR2_ERROR_CODE PR2Robot::InitializeRobot()
{
- client = new gazebo::Client();
- simIface = new gazebo::SimulationIface();
- pr2Iface = new gazebo::PR2ArrayIface();
- pr2HeadIface = new gazebo::PR2ArrayIface();
- pr2GripperLeftIface = new gazebo::PR2GripperIface();
- pr2GripperRightIface = new gazebo::PR2GripperIface();
- pr2LaserIface = new gazebo::LaserIface();
- pr2BaseLaserIface = new gazebo::LaserIface();
- pr2CameraGlobalIface = new gazebo::CameraIface();
- pr2CameraHeadLeftIface = new gazebo::CameraIface();
- pr2CameraHeadRightIface = new gazebo::CameraIface();
-
- pr2LeftWristIface = new gazebo::PositionIface();
- pr2RightWristIface = new gazebo::PositionIface();
- pr2BaseIface = new gazebo::PositionIface();
-
- int serverId = 0;
-
- /// Connect to the libgazebo server
- try
- {
- client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
- }
- catch (gazebo::GazeboError e)
- {
- std::cout << "Gazebo error: Unable to connect\n" << e << "\n";
- return PR2_ERROR;
- }
-
- /// Open the Simulation Interface
- try
- {
- simIface->Open(client, "default");
- }
- catch (gazebo::GazeboError e)
- {
- std::cout << "Gazebo error: Unable to connect to the sim interface\n" << e << "\n";
- return PR2_ERROR;
- }
-
- /// Open the Position interface
- try
- {
- pr2Iface->Open(client, "pr2_iface");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 interface\n"
- << e << "\n";
- return PR2_ERROR;
- }
-
- /// Open the Position interface
- try
- {
- pr2HeadIface->Open(client, "pr2_head_iface");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 head interface\n"
- << e << "\n";
- return PR2_ERROR;
- }
-
- /// Open the Position interface for gripper left
- try
- {
- pr2GripperLeftIface->Open(client, "pr2_gripper_left_iface");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 gripper left interface\n"
- << e << "\n";
- return PR2_ERROR;
- }
-
- /// Open the Position interface for gripper right
- try
- {
- pr2GripperRightIface->Open(client, "pr2_gripper_right_iface");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 gripper right interface\n"
- << e << "\n";
- return PR2_ERROR;
- }
-
- /// Open the laser interface for hokuyo
- try
- {
- pr2LaserIface->Open(client, "laser_iface_1");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 laser interface\n"
- << e << "\n";
- pr2LaserIface = NULL;
- //return PR2_ERROR;
- }
-
-
- /// Open the base laser interface for hokuyo
- try
- {
- pr2BaseLaserIface->Open(client, "base_laser_iface_1");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 base laser interface\n"
- << e << "\n";
- pr2BaseLaserIface = NULL;
- //return PR2_ERROR;
- }
-
-
- /// Open the camera interface for hokuyo
- try
- {
- pr2CameraGlobalIface->Open(client, "cam1_iface_0");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 camera interface\n"
- << e << "\n";
- pr2CameraGlobalIface = NULL;
- //return PR2_ERROR;
- }
-
- try
- {
- pr2CameraHeadLeftIface->Open(client, "head_cam_left_iface_0");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 camera interface\n"
- << e << "\n";
- pr2CameraHeadLeftIface = NULL;
- //return PR2_ERROR;
- }
-
- try
- {
- pr2CameraHeadRightIface->Open(client, "head_cam_right_iface_0");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 camera interface\n"
- << e << "\n";
- pr2CameraHeadRightIface = NULL;
- //return PR2_ERROR;
- }
-
- try
- {
- pr2LeftWristIface->Open(client, "p3d_left_wrist_position");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the left wrist interface\n"
- << e << "\n";
- pr2LeftWristIface = NULL;
- }
-
- try
- {
- pr2RightWristIface->Open(client, "p3d_right_wrist_position");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the right wrist interface\n"
- << e << "\n";
- pr2RightWristIface = NULL;
- }
-
- try
- {
- pr2BaseIface->Open(client, "p3d_base_position");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the base position interface\n"
- << e << "\n";
- pr2BaseIface = NULL;
- }
-
return PR2_ALL_OK;
}
@@ -357,135 +22,16 @@
return PR2_ALL_OK;
};
-PR2_ERROR_CODE PR2Robot::EnableJoint(PR2_JOINT_ID id)
-{
- if (IsHead(id))
- {
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor = 1;
- pr2HeadIface->Unlock();
- }
- else if (IsGripperLeft(id))
- {
- pr2GripperLeftIface ->Lock(1);
- pr2GripperLeftIface ->data->cmdEnableMotor = 1;
- pr2GripperLeftIface ->Unlock();
- }
- else if (IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = 1;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].cmdEnableMotor = 1;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
-};
-PR2_ERROR_CODE PR2Robot::DisableJoint(PR2_JOINT_ID id)
-{
- if (IsHead(id))
- {
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor = 0;
- pr2HeadIface->Unlock();
- }
- else if (IsGripperLeft(id))
- {
- pr2GripperLeftIface ->Lock(1);
- pr2GripperLeftIface ->data->cmdEnableMotor = 0;
- pr2GripperLeftIface ->Unlock();
- }
- else if (IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = 0;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].cmdEnableMotor = 0;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
-};
-
-PR2_ERROR_CODE PR2Robot::IsEnabledJoint(PR2_JOINT_ID id, int *enabled)
-{
- if (IsHead(id))
- {
- pr2HeadIface->Lock(1);
- *enabled = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor;
- pr2HeadIface->Unlock();
- }
- else if (IsGripperLeft(id))
- {
- pr2GripperLeftIface->Lock(1);
- *enabled = pr2GripperLeftIface->data->cmdEnableMotor;
- pr2GripperLeftIface->Unlock();
- }
- else if (IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- *enabled = pr2GripperRightIface->data->cmdEnableMotor;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- *enabled = pr2Iface->data->actuators[id].cmdEnableMotor;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
-};
-
-PR2_ERROR_CODE PR2Robot::EnableModel(PR2_MODEL_ID id)
-{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- // subtrace JointStart[HEAD] since HEAD controllers is a new actarray.
- pr2HeadIface->data->actuators[ii-JointStart[HEAD]].cmdEnableMotor = 1;
- pr2Iface->Unlock();
- }
- else if(IsGripperLeft(id))
- {
- // there is only one... for now
- pr2GripperLeftIface->Lock(1);
- pr2GripperLeftIface->data->cmdEnableMotor = 1;
- pr2GripperLeftIface->Unlock();
- }
- else if(IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = 1;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- pr2Iface->data->actuators[ii].cmdEnableMotor = 1;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
-};
-
PR2_ERROR_CODE PR2Robot::EnableArm(PR2_MODEL_ID id)
{
switch(id)
{
case PR2_LEFT_ARM:
- EnableModel(PR2_LEFT_ARM);
+ this->hw.EnableModel(PR2_LEFT_ARM);
break;
case PR2_RIGHT_ARM:
- EnableModel(PR2_RIGHT_ARM);
+ hw.EnableModel(PR2_RIGHT_ARM);
break;
default:
break;
@@ -498,10 +44,10 @@
switch(id)
{
case PR2_LEFT_GRIPPER:
- pr2GripperLeftIface->data->cmdEnableMotor = 1;
+ EnableGripperLeft();
break;
case PR2_RIGHT_GRIPPER:
- pr2GripperRightIface->data->cmdEnableMotor = 1;
+ EnableGripperRight();
break;
default:
break;
@@ -511,74 +57,43 @@
PR2_ERROR_CODE PR2Robot::EnableHead()
{
- EnableModel(HEAD);
+ hw.EnableModel(HEAD);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::EnableGripperLeft()
{
- EnableModel(PR2_LEFT_GRIPPER);
+ hw.EnableModel(PR2_LEFT_GRIPPER);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::EnableGripperRight()
{
- EnableModel(PR2_RIGHT_GRIPPER);
+ hw.EnableModel(PR2_RIGHT_GRIPPER);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::EnableBase()
{
- EnableModel(PR2_BASE);
+ hw.EnableModel(PR2_BASE);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::EnableSpine()
{
- EnableModel(PR2_SPINE);
+ hw.EnableModel(PR2_SPINE);
return PR2_ALL_OK;
}
-PR2_ERROR_CODE PR2Robot::DisableModel(PR2_MODEL_ID id)
-{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- pr2HeadIface->data->actuators[ii-JointStart[HEAD]].cmdEnableMotor = 0;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft(id))
- {
- pr2GripperLeftIface->Lock(1);
- pr2GripperLeftIface->data->cmdEnableMotor = 0;
- pr2GripperLeftIface->Unlock();
- }
- else if(IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = 0;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- pr2Iface->data->actuators[ii].cmdEnableMotor = 0;
- pr2Iface->Unlock();
- }
- return PR2_ALL_OK;
-};
-
PR2_ERROR_CODE PR2Robot::DisableArm(PR2_MODEL_ID id)
{
switch(id)
{
case PR2_LEFT_ARM:
- DisableModel(PR2_LEFT_ARM);
+ hw.DisableModel(PR2_LEFT_ARM);
break;
case PR2_RIGHT_ARM:
- DisableModel(PR2_RIGHT_ARM);
+ hw.DisableModel(PR2_RIGHT_ARM);
break;
default:
break;
@@ -591,10 +106,10 @@
switch(id)
{
case PR2_LEFT_GRIPPER:
- pr2GripperLeftIface->data->cmdEnableMotor = 0;
+ DisableGripperLeft();
break;
case PR2_RIGHT_GRIPPER:
- pr2GripperRightIface->data->cmdEnableMotor = 0;
+ DisableGripperRight();
break;
default:
break;
@@ -604,68 +119,35 @@
PR2_ERROR_CODE PR2Robot::DisableHead()
{
- DisableModel(HEAD);
+ hw.DisableModel(HEAD);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::DisableGripperLeft()
{
- DisableModel(PR2_LEFT_GRIPPER);
+ hw.DisableModel(PR2_LEFT_GRIPPER);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::DisableGripperRight()
{
- DisableModel(PR2_RIGHT_GRIPPER);
+ hw.DisableModel(PR2_RIGHT_GRIPPER);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::DisableBase()
{
- DisableModel(PR2_BASE);
+ hw.DisableModel(PR2_BASE);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::DisableSpine()
{
- DisableModel(PR2_SPINE);
+ hw.DisableModel(PR2_SPINE);
return PR2_ALL_OK;
}
-PR2_ERROR_CODE PR2Robot::IsEnabledModel(PR2_MODEL_ID id, int *enabled)
-{
- int isEnabled = 1;
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- isEnabled = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor && isEnabled;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft(id))
- {
- pr2GripperLeftIface->Lock(1);
- isEnabled = pr2GripperLeftIface->data->cmdEnableMotor && isEnabled;
- pr2GripperLeftIface->Unlock();
- }
- else if(IsGripperRight(id))
- {
- pr2GripperRightIface->Lock(1);
- isEnabled = pr2GripperRightIface->data->cmdEnableMotor && isEnabled;
- pr2GripperRightIface->Unlock();
- }
- else
- {
- pr2Iface->Lock(1);
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
- isEnabled = pr2Iface->data->actuators[id].cmdEnableMotor && isEnabled;
- pr2Iface->Unlock();
- }
- *enabled = isEnabled;
- return PR2_ALL_OK;
-};
-
#ifdef KDL_KINEMATICS
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f, const KDL::JntArray &q_init, KDL::JntArray &q_out)
@@ -690,7 +172,7 @@
cout<<"Could not compute Fwd Kin. (After IK)"<<endl;
for(int ii = 0; ii < 7; ii++)
- this->SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),q_out(ii),0);
+ hw.SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),q_out(ii),0);
}
#endif
@@ -731,7 +213,7 @@
{
angles[ii] = theta(ii+1,validSolution);
speeds[ii] = 0;
- this->SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),angles[ii],0);
+ hw.SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),angles[ii],0);
}
}
return PR2_ALL_OK;
@@ -740,8 +222,8 @@
NEWMAT::Matrix PR2Robot::ComputeArmInverseKinematics(PR2_MODEL_ID id, NEWMAT::Matrix g)
{
NEWMAT::Matrix theta(8,8);
- double angles[7], speeds[7];
- int validSolution;
+ //double angles[7], speeds[7];
+ //int validSolution;
if (id == PR2_RIGHT_ARM)
{
@@ -790,19 +272,19 @@
baseControlMode = mode;
if (mode == PR2_CARTESIAN_CONTROL)
{
- SetJointControlMode(CASTER_FL_DRIVE_L,SPEED_CONTROL);
- SetJointControlMode(CASTER_FL_DRIVE_R,SPEED_CONTROL);
- SetJointControlMode(CASTER_FR_DRIVE_L,SPEED_CONTROL);
- SetJointControlMode(CASTER_FR_DRIVE_R,SPEED_CONTROL);
- SetJointControlMode(CASTER_RL_DRIVE_L,SPEED_CONTROL);
- SetJointControlMode(CASTER_RL_DRIVE_R,SPEED_CONTROL);
- SetJointControlMode(CASTER_RR_DRIVE_L,SPEED_CONTROL);
- SetJointControlMode(CASTER_RR_DRIVE_R,SPEED_CONTROL);
+ hw.SetJointControlMode(CASTER_FL_DRIVE_L,SPEED_CONTROL);
+ hw.SetJointControlMode(CASTER_FL_DRIVE_R,SPEED_CONTROL);
+ hw.SetJointControlMode(CASTER_FR_DRIVE_L,SPEED_CONTROL);
+ hw.SetJointControlMode(CASTER_FR_DRIVE_R,SPEED_CONTROL);
+ hw.SetJointControlMode(CASTER_RL_DRIVE_L,SPEED_CONTROL);
+ hw.SetJointControlMode(CASTER_RL_DRIVE_R,SPEED_CONTROL);
+ hw.SetJointControlMode(CASTER_RR_DRIVE_L,SPEED_CONTROL);
+ hw.SetJointControlMode(CASTER_RR_DRIVE_R,SPEED_CONTROL);
- SetJointControlMode(CASTER_FL_STEER,PD_CONTROL);
- SetJointControlMode(CASTER_FR_STEER,PD_CONTROL);
- SetJointControlMode(CASTER_RL_STEER,PD_CONTROL);
- SetJointControlMode(CASTER_RR_STEER,PD_CONTROL);
+ hw.SetJointControlMode(CASTER_FL_STEER,PD_CONTROL);
+ hw.SetJointControlMode(CASTER_FR_STEER,PD_CONTROL);
+ hw.SetJointControlMode(CASTER_RL_STEER,PD_CONTROL);
+ hw.SetJointControlMode(CASTER_RR_STEER,PD_CONTROL);
}
return PR2_ALL_OK;
};
@@ -820,23 +302,23 @@
{
case PR2_RIGHT_ARM:
armControlMode[0] = mode;
- SetJointControlMode(ARM_L_PAN , PD_CONTROL);
- SetJointControlMode(ARM_L_SHOULDER_PITCH , PD_CONTROL);
- SetJointControlMode(ARM_L_SHOULDER_ROLL , PD_CONTROL);
- SetJointControlMode(ARM_L_ELBOW_PITCH , PD_CONTROL);
- SetJointControlMode(ARM_L_ELBOW_ROLL , PD_CONTROL);
- SetJointControlMode(ARM_L_WRIST_PITCH , PD_CONTROL);
- SetJointControlMode(ARM_L_WRIST_ROLL , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_PAN , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_SHOULDER_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_SHOULDER_ROLL , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_ELBOW_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_ELBOW_ROLL , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_WRIST_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_L_WRIST_ROLL , PD_CONTROL);
break;
case PR2_LEFT_ARM:
armControlMode[1] = mode;
- SetJointControlMode(ARM_R_PAN , PD_CONTROL);
- SetJointControlMode(ARM_R_SHOULDER_PITCH , PD_CONTROL);
- SetJointControlMode(ARM_R_SHOULDER_ROLL , PD_CONTROL);
- SetJointControlMode(ARM_R_ELBOW_PITCH , PD_CONTROL);
- SetJointControlMode(ARM_R_ELBOW_ROLL , PD_CONTROL);
- SetJointControlMode(ARM_R_WRIST_PITCH , PD_CONTROL);
- SetJointControlMode(ARM_R_WRIST_ROLL , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_PAN , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_SHOULDER_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_SHOULDER_ROLL , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_ELBOW_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_ELBOW_ROLL , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_WRIST_PITCH , PD_CONTROL);
+ hw.SetJointControlMode(ARM_R_WRIST_ROLL , PD_CONTROL);
break;
default:
break;
@@ -860,200 +342,19 @@
return PR2_ALL_OK;
};
-PR2_ERROR_CODE PR2Robot::SetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE mode)
-{
- if(IsHead(id))
- {
- pr2HeadIface->Lock(1);
- pr2HeadIfac...
[truncated message content] |
|
From: <is...@us...> - 2008-06-24 16:44:44
|
Revision: 921
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=921&view=rev
Author: isucan
Date: 2008-06-24 09:44:53 -0700 (Tue, 24 Jun 2008)
Log Message:
-----------
update documentation for my packages
Modified Paths:
--------------
pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/test_kinematic_planning/manifest.xml
Modified: pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp 2008-06-24 16:31:04 UTC (rev 920)
+++ pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp 2008-06-24 16:44:53 UTC (rev 921)
@@ -60,7 +60,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- full_cloud/PointCloudFloat32 : point cloud with data from a complete laser scan (top to bottom)
+- @b full_cloud/PointCloudFloat32 : point cloud with data from a complete laser scan (top to bottom)
Publishes to (name/type):
- @b "world_3d_map"/PointCloudFloat32 : point cloud describing the 3D environment
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-06-24 16:31:04 UTC (rev 920)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-06-24 16:44:53 UTC (rev 921)
@@ -1,13 +1,10 @@
<package>
<description>3D Map Construction</description>
-<author>Willow Garage</author>
+<author>Ioan A. Sucan</author>
<license>BSD</license>
<depend package="roscpp" />
<depend package="std_msgs" />
<depend package="std_srvs" />
<depend package="xmlparam" />
<depend package="MPK" />
-<export>
- <cpp cflags="-I${prefix}/msg/cpp"/>
-</export>
</package>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-06-24 16:31:04 UTC (rev 920)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-06-24 16:44:53 UTC (rev 921)
@@ -59,7 +59,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- world_3d_map/PointCloudFloat32 : point cloud with data describing the 3D environment
+- @b world_3d_map/PointCloudFloat32 : point cloud with data describing the 3D environment
Publishes to (name/type):
- None
Modified: pkg/trunk/motion_planning/test_kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/test_kinematic_planning/manifest.xml 2008-06-24 16:31:04 UTC (rev 920)
+++ pkg/trunk/motion_planning/test_kinematic_planning/manifest.xml 2008-06-24 16:44:53 UTC (rev 921)
@@ -1,12 +1,9 @@
<package>
<description>3D Map Construction</description>
-<author>Willow Garage</author>
+<author>Ioan A. Sucan</author>
<license>BSD</license>
<depend package="roscpp" />
<depend package="std_msgs" />
<depend package="std_srvs" />
<depend package="xmlparam" />
-<export>
- <cpp cflags="-I${prefix}/msg/cpp"/>
-</export>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X