Menu
▾
▴
personalrobots-commit — Subversion commit messages
You can subscribe to this list here.
| 2008 |
Jan
|
Feb
|
Mar
(43) |
Apr
(196) |
May
(316) |
Jun
(518) |
Jul
(1293) |
Aug
(1446) |
Sep
(930) |
Oct
(1271) |
Nov
(1275) |
Dec
(1385) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2009 |
Jan
(1622) |
Feb
(1546) |
Mar
(1236) |
Apr
(1512) |
May
(1782) |
Jun
(1551) |
Jul
(2300) |
Aug
(3088) |
Sep
(452) |
Oct
|
Nov
|
Dec
|
|
From: <ge...@us...> - 2008-04-17 19:49:34
|
Revision: 116
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=116&view=rev
Author: gerkey
Date: 2008-04-17 12:49:41 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
copy of gmapping from openslam.org
Added Paths:
-----------
pkg/trunk/gmapping/
pkg/trunk/gmapping/Makefile
pkg/trunk/gmapping/TODO.txt
pkg/trunk/gmapping/bin/
pkg/trunk/gmapping/build_tools/
pkg/trunk/gmapping/build_tools/Makefile.app
pkg/trunk/gmapping/build_tools/Makefile.generic-shared-object
pkg/trunk/gmapping/build_tools/Makefile.subdirs
pkg/trunk/gmapping/build_tools/generate_shared_object
pkg/trunk/gmapping/build_tools/message
pkg/trunk/gmapping/build_tools/pretty_compiler
pkg/trunk/gmapping/build_tools/testlib
pkg/trunk/gmapping/carmenwrapper/
pkg/trunk/gmapping/carmenwrapper/Makefile
pkg/trunk/gmapping/carmenwrapper/carmenwrapper.cpp
pkg/trunk/gmapping/carmenwrapper/carmenwrapper.h
pkg/trunk/gmapping/configfile/
pkg/trunk/gmapping/configfile/Makefile
pkg/trunk/gmapping/configfile/configfile.cpp
pkg/trunk/gmapping/configfile/configfile.h
pkg/trunk/gmapping/configfile/configfile_test.cpp
pkg/trunk/gmapping/configfile/demo.cfg
pkg/trunk/gmapping/configfile/test.ini
pkg/trunk/gmapping/configure
pkg/trunk/gmapping/docs/
pkg/trunk/gmapping/docs/Instructions.txt
pkg/trunk/gmapping/docs/scanmatcher.tex
pkg/trunk/gmapping/docs/userver.txt
pkg/trunk/gmapping/gfs-carmen/
pkg/trunk/gmapping/gfs-carmen/Makefile
pkg/trunk/gmapping/gfs-carmen/gfs-carmen.cpp
pkg/trunk/gmapping/grid/
pkg/trunk/gmapping/grid/Makefile
pkg/trunk/gmapping/grid/accessstate.h
pkg/trunk/gmapping/grid/array2d.h
pkg/trunk/gmapping/grid/graphmap.cpp
pkg/trunk/gmapping/grid/harray2d.h
pkg/trunk/gmapping/grid/map.h
pkg/trunk/gmapping/grid/map_test.cpp
pkg/trunk/gmapping/gridfastslam/
pkg/trunk/gmapping/gridfastslam/Makefile
pkg/trunk/gmapping/gridfastslam/gfs2log.cpp
pkg/trunk/gmapping/gridfastslam/gfs2neff.cpp
pkg/trunk/gmapping/gridfastslam/gfs2rec.cpp
pkg/trunk/gmapping/gridfastslam/gfs2stat.cpp
pkg/trunk/gmapping/gridfastslam/gfs2stream.cpp
pkg/trunk/gmapping/gridfastslam/gfsreader.cpp
pkg/trunk/gmapping/gridfastslam/gfsreader.h
pkg/trunk/gmapping/gridfastslam/gridslamprocessor.cpp
pkg/trunk/gmapping/gridfastslam/gridslamprocessor.h
pkg/trunk/gmapping/gridfastslam/gridslamprocessor.hxx
pkg/trunk/gmapping/gridfastslam/gridslamprocessor_tree.cpp
pkg/trunk/gmapping/gridfastslam/motionmodel.cpp
pkg/trunk/gmapping/gridfastslam/motionmodel.h
pkg/trunk/gmapping/gui/
pkg/trunk/gmapping/gui/Makefile
pkg/trunk/gmapping/gui/gfs2img.cpp
pkg/trunk/gmapping/gui/gfs_logplayer.cpp
pkg/trunk/gmapping/gui/gfs_nogui.cpp
pkg/trunk/gmapping/gui/gfs_simplegui.cpp
pkg/trunk/gmapping/gui/gsp_thread.cpp
pkg/trunk/gmapping/gui/gsp_thread.h
pkg/trunk/gmapping/gui/qgraphpainter.cpp
pkg/trunk/gmapping/gui/qgraphpainter.h
pkg/trunk/gmapping/gui/qmappainter.cpp
pkg/trunk/gmapping/gui/qmappainter.h
pkg/trunk/gmapping/gui/qnavigatorwidget.cpp
pkg/trunk/gmapping/gui/qnavigatorwidget.h
pkg/trunk/gmapping/gui/qparticleviewer.cpp
pkg/trunk/gmapping/gui/qparticleviewer.h
pkg/trunk/gmapping/gui/qpixmapdumper.cpp
pkg/trunk/gmapping/gui/qpixmapdumper.h
pkg/trunk/gmapping/gui/qslamandnavwidget.cpp
pkg/trunk/gmapping/gui/qslamandnavwidget.h
pkg/trunk/gmapping/ini/
pkg/trunk/gmapping/ini/gfs-LMS-10cm.ini
pkg/trunk/gmapping/ini/gfs-LMS-20cm.ini
pkg/trunk/gmapping/ini/gfs-LMS-5cm.ini
pkg/trunk/gmapping/ini/gfs-PLS-10cm.ini
pkg/trunk/gmapping/ini/gfs-PLS-5cm.ini
pkg/trunk/gmapping/lib/
pkg/trunk/gmapping/log/
pkg/trunk/gmapping/log/Makefile
pkg/trunk/gmapping/log/carmenconfiguration.cpp
pkg/trunk/gmapping/log/carmenconfiguration.h
pkg/trunk/gmapping/log/configuration.cpp
pkg/trunk/gmapping/log/configuration.h
pkg/trunk/gmapping/log/log_plot.cpp
pkg/trunk/gmapping/log/log_test.cpp
pkg/trunk/gmapping/log/rdk2carmen.cpp
pkg/trunk/gmapping/log/scanstudio2carmen.cpp
pkg/trunk/gmapping/log/sensorlog.cpp
pkg/trunk/gmapping/log/sensorlog.h
pkg/trunk/gmapping/log/sensorstream.cpp
pkg/trunk/gmapping/log/sensorstream.h
pkg/trunk/gmapping/manual.mk
pkg/trunk/gmapping/manual.mk-template
pkg/trunk/gmapping/particlefilter/
pkg/trunk/gmapping/particlefilter/Makefile
pkg/trunk/gmapping/particlefilter/particlefilter.cpp
pkg/trunk/gmapping/particlefilter/particlefilter.h
pkg/trunk/gmapping/particlefilter/particlefilter_test.cpp
pkg/trunk/gmapping/particlefilter/pf.h
pkg/trunk/gmapping/particlefilter/range_bearing.cpp
pkg/trunk/gmapping/scanmatcher/
pkg/trunk/gmapping/scanmatcher/Makefile
pkg/trunk/gmapping/scanmatcher/gridlinetraversal.h
pkg/trunk/gmapping/scanmatcher/icp.h
pkg/trunk/gmapping/scanmatcher/icptest.cpp
pkg/trunk/gmapping/scanmatcher/lumiles.h
pkg/trunk/gmapping/scanmatcher/scanmatch_test.cpp
pkg/trunk/gmapping/scanmatcher/scanmatcher.cpp
pkg/trunk/gmapping/scanmatcher/scanmatcher.h
pkg/trunk/gmapping/scanmatcher/scanmatcher.new.cpp
pkg/trunk/gmapping/scanmatcher/scanmatcherprocessor.cpp
pkg/trunk/gmapping/scanmatcher/scanmatcherprocessor.h
pkg/trunk/gmapping/scanmatcher/smmap.cpp
pkg/trunk/gmapping/scanmatcher/smmap.h
pkg/trunk/gmapping/sensor/
pkg/trunk/gmapping/sensor/Makefile
pkg/trunk/gmapping/sensor/sensor_base/
pkg/trunk/gmapping/sensor/sensor_base/Makefile
pkg/trunk/gmapping/sensor/sensor_base/sensor.cpp
pkg/trunk/gmapping/sensor/sensor_base/sensor.h
pkg/trunk/gmapping/sensor/sensor_base/sensoreading.h
pkg/trunk/gmapping/sensor/sensor_base/sensorreading.cpp
pkg/trunk/gmapping/sensor/sensor_base/sensorreading.h
pkg/trunk/gmapping/sensor/sensor_odometry/
pkg/trunk/gmapping/sensor/sensor_odometry/Makefile
pkg/trunk/gmapping/sensor/sensor_odometry/odometryreading.cpp
pkg/trunk/gmapping/sensor/sensor_odometry/odometryreading.h
pkg/trunk/gmapping/sensor/sensor_odometry/odometrysensor.cpp
pkg/trunk/gmapping/sensor/sensor_odometry/odometrysensor.h
pkg/trunk/gmapping/sensor/sensor_range/
pkg/trunk/gmapping/sensor/sensor_range/Makefile
pkg/trunk/gmapping/sensor/sensor_range/rangereading.cpp
pkg/trunk/gmapping/sensor/sensor_range/rangereading.h
pkg/trunk/gmapping/sensor/sensor_range/rangesensor.cpp
pkg/trunk/gmapping/sensor/sensor_range/rangesensor.h
pkg/trunk/gmapping/setlibpath
pkg/trunk/gmapping/utils/
pkg/trunk/gmapping/utils/Makefile
pkg/trunk/gmapping/utils/autoptr.h
pkg/trunk/gmapping/utils/autoptr_test.cpp
pkg/trunk/gmapping/utils/commandline.h
pkg/trunk/gmapping/utils/datasmoother.h
pkg/trunk/gmapping/utils/dmatrix.h
pkg/trunk/gmapping/utils/gvalues.h
pkg/trunk/gmapping/utils/macro_params.h
pkg/trunk/gmapping/utils/movement.cpp
pkg/trunk/gmapping/utils/movement.h
pkg/trunk/gmapping/utils/optimizer.h
pkg/trunk/gmapping/utils/orientedboundingbox.h
pkg/trunk/gmapping/utils/orientedboundingbox.hxx
pkg/trunk/gmapping/utils/point.h
pkg/trunk/gmapping/utils/printmemusage.cpp
pkg/trunk/gmapping/utils/printmemusage.h
pkg/trunk/gmapping/utils/printpgm.h
pkg/trunk/gmapping/utils/stat.cpp
pkg/trunk/gmapping/utils/stat.h
pkg/trunk/gmapping/utils/stat_test.cpp
Added: pkg/trunk/gmapping/Makefile
===================================================================
--- pkg/trunk/gmapping/Makefile (rev 0)
+++ pkg/trunk/gmapping/Makefile 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,17 @@
+-include ./global.mk
+
+ifeq ($(CARMENSUPPORT),1)
+SUBDIRS=utils sensor log configfile scanmatcher carmenwrapper gridfastslam gui gfs-carmen
+else
+ifeq ($(MACOSX),1)
+SUBDIRS=utils sensor log configfile scanmatcher gridfastslam
+else
+SUBDIRS=utils sensor log configfile scanmatcher gridfastslam gui
+endif
+endif
+
+LDFLAGS+=
+CPPFLAGS+= -I../sensor
+
+-include ./build_tools/Makefile.subdirs
+
Added: pkg/trunk/gmapping/TODO.txt
===================================================================
--- pkg/trunk/gmapping/TODO.txt (rev 0)
+++ pkg/trunk/gmapping/TODO.txt 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,21 @@
+TODO-List for gmapping (and partly explore)
+--------------------------------------------
+
+open:
+-----
+
+1. implement a working(!) ancestry tree
+
+2. compute trajectory uncertainty based on the
+ ancestry tree formula
+
+3. possibility to choose the way the pose uncertainty
+ for a particle set is computed (grid vs set of
+ gaussians)
+
+4. Fix the NAN Problem with the pose uncertainty with gaussians
+
+done:
+-----
+
+(move the done stuff down here)
\ No newline at end of file
Added: pkg/trunk/gmapping/build_tools/Makefile.app
===================================================================
--- pkg/trunk/gmapping/build_tools/Makefile.app (rev 0)
+++ pkg/trunk/gmapping/build_tools/Makefile.app 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,80 @@
+# Makefile generico per applicazione
+#
+# Variabili:
+# APPS lista delle applicazioni
+# OBJS lista degli oggetti
+# QOBJS lista degli oggetti QT
+# LIBS librerie
+#
+# Ogni applicazione viene linkata con tutti gli oggetti
+
+export VERBOSE
+
+ifeq ($(LINUX),1)
+CPPFLAGS+=-DLINUX
+endif
+
+
+APPLICATIONS= $(foreach a, $(APPS),$(BINDIR)/$(a))
+all: $(APPLICATIONS)
+
+PACKAGE=$(notdir $(shell pwd))
+
+.SECONDARY: $(OBJS) $(QOBJS)
+.PHONY: all clean copy doc
+
+$(QOBJS): %.o: %.cpp moc_%.cpp
+ @$(MESSAGE) "Compiling (QT) $@"
+ @$(PRETTY) "$(CXX) $(CPPFLAGS) $(QT_INCLUDE) $(CXXFLAGS) -c $< -o $@"
+
+moc_%.cpp: %.h
+ @$(MESSAGE) "Generating MOC $@"
+ @$(PRETTY) "$(MOC) -i $< -o $@"
+
+# Generazione degli oggetti
+%.o: %.cpp
+ @$(MESSAGE) "Compiling $@"
+ @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@"
+
+# Generazione delle applicazioni
+$(BINDIR)/%: %.cpp $(OBJS) $(QOBJS)
+ @$(MESSAGE) "Linking application `basename $@`"
+ @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(OBJS) $(QOBJS) $< -L$(LIBDIR) $(LIBS) -o $@"
+
+#Regole per la generazione di tabelle o altri file creati automaticamente
+table_%.cpp: gen_table_%
+ @$(MESSAGE) "Generating $@"
+ @$(PRETTY) "./$< > $@"
+
+gen_table_%: gen_table_%.cpp
+ @$(MESSAGE) "Generating $@"
+ @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) $< -o $@"
+
+#Regole per la generazione delle dipendenze
+OBJDEPS=$(foreach module,$(basename $(OBJS) $(QOBJS)),$(module).d)
+
+$(OBJDEPS): %.d: %.cpp # ci va o no? %.h
+ @$(MESSAGE) "Generating dependecies $@"
+ @$(PRETTY) "$(CXX) $(CPPFLAGS) -MM -MG -MF $@ $<"
+
+ifneq ($(MAKECMDGOALS),clean)
+ifneq ($(MAKECMDGOALS),copy)
+-include $(OBJDEPS)
+endif
+endif
+
+doc:
+ rm -rf doc/$(PACKAGE)
+ifeq ($(strip $(DOCTITLE)),)
+ kdoc -L doc -d doc/$(PACKAGE) -n "Package $(PACKAGE) (lib$(PACKAGE).so)" $(HEADERS)
+else
+ kdoc -L doc -d doc/$(PACKAGE) -n "$(DOCTITLE) (lib$(PACKAGE).so)" $(HEADERS)
+endif
+
+clean:
+ @$(MESSAGE) "Cleaning $(PACKAGE)"
+ @$(PRETTY) "rm -f *.d *.o moc_*.cpp *.d core *~ table_*.cpp gen_table*[^.][^c][^p][^p] $(APPLICATIONS)"
+ @$(PRETTY) "rm -rf doc/$(PACKAGE)"
+
+copy: clean
+ tar -C .. -cvzf `date +../$(PACKAGE)-%d%b%y.tgz` $(PACKAGE)
Added: pkg/trunk/gmapping/build_tools/Makefile.generic-shared-object
===================================================================
--- pkg/trunk/gmapping/build_tools/Makefile.generic-shared-object (rev 0)
+++ pkg/trunk/gmapping/build_tools/Makefile.generic-shared-object 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,109 @@
+# Makefile generico per shared object
+export VERBOSE
+export CXX
+
+# Nome del package
+PACKAGE=$(notdir $(shell pwd))
+
+# Libreria da generare:
+# Se non si setta la variabile LIBNAME la libreria si chiama
+# come la directory
+ifndef LIBNAME
+LIBNAME=$(PACKAGE)
+endif
+
+ifeq ($(MACOSX),1)
+SONAME=$(LIBDIR)/lib$(LIBNAME).dylib
+endif
+
+ifeq ($(LINUX),1)
+SONAME=$(LIBDIR)/lib$(LIBNAME).so
+endif
+
+APPLICATIONS= $(foreach a, $(APPS),$(BINDIR)/$(a))
+INSTALL_SCRIPTS=$(foreach a, $(SCRIPTS),$(BINDIR)/$(a))
+
+all: $(SONAME) $(APPLICATIONS) $(INSTALL_SCRIPTS)
+
+.SECONDARY: $(OBJS) $(COBJS)
+.PHONY: all clean copy doc
+
+# Generazione della libreria
+$(SONAME): $(OBJS) $(COBJS)
+ @$(MESSAGE) "Creating library lib$(LIBNAME).so"
+ifeq ($(MACOSX),1)
+ @$(PRETTY) "$(CXX) $(LDFLAGS) -dynamiclib $(OBJS) $(COBJS) -L$(LIBDIR) $(LIBS) -install_name $@ -o $@"
+endif
+ifeq ($(LINUX),1)
+ @$(PRETTY) "$(CXX) $(LDFLAGS) -fPIC -shared $(OBJS) $(COBJS) -L $(LIBDIR) $(LIBS) -o $@"
+ @if ! $(PRETTY) "$(TESTLIB) $(SONAME)"; then $(MESSAGE) "Testing of $(SONAME) failed."; rm $(SONAME); exit 1; fi;
+endif
+
+# Generazione delle applicazioni
+$(BINDIR)/%: %.o $(SONAME)
+ @$(MESSAGE) "Linking application `basename "$@"`"
+ @$(PRETTY) "$(CXX) $(LDFLAGS) -L$(LIBDIR) $(LIBS) -l$(LIBNAME) $< -o $@"
+
+#Generazione dei moc files
+moc_%.cpp: %.h
+ @$(MESSAGE) "Compiling MOC $@"
+ @$(PRETTY) "$(MOC) -i $< -o $@"
+
+# Generazione degli oggetti
+%.o: %.cpp
+ @$(MESSAGE) "Compiling $<"
+ @$(PRETTY) "$(CXX) -fPIC $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@"
+
+%.o: %.c
+ @$(MESSAGE) "Compiling $<"
+ @$(PRETTY) "$(CC) -fPIC $(CPPFLAGS) $(CFLAGS) -c $< -o $@"
+
+#Regole per la generazione delle dipendenze
+OBJDEPS= $(foreach module,$(basename $(OBJS)),$(module).d) $(foreach a, $(APPS),$(a).d)
+COBJDEPS=$(foreach module,$(basename $(COBJS)),$(module).d)
+
+$(OBJDEPS): %.d: %.cpp
+ @$(MESSAGE) "Generating dependencies for $<"
+ @$(PRETTY) "$(CXX) $(CPPFLAGS) -MM -MG $< -MF $@"
+
+$(COBJDEPS): %.d: %.c
+ @$(MESSAGE) "Generating dependencies for $<"
+ @$(PRETTY) "$(CC) $(CPPFLAGS) -MM -MG $< -MF $@"
+
+#HEADERS=`ls *.h`
+#PRECOMPILED_HEADERS=$(foreach file,$(basename $(HEADERS)), $(file).pch)
+
+ifneq ($(MAKECMDGOALS),clean)
+ifneq ($(MAKECMDGOALS),copy)
+ifneq ($(MAKECMDGOALS),dep)
+-include $(OBJDEPS) $(COBJDEPS)
+endif
+endif
+endif
+
+dep: $(OBJDEPS) $(COBJDEPS)
+
+
+# GLi script vengono semplicemente copiati
+$(BINDIR)/%.sh: %.sh
+ @$(MESSAGE) "Installing script `basename "$@"`"
+ @$(PRETTY) "cp $< $@"
+ @$(PRETTY) "chmod +x $@"
+
+
+#doc:
+# rm -rf doc/$(PACKAGE)
+#ifeq ($(strip $(DOCTITLE)),)
+# kdoc -L doc -d doc/$(PACKAGE) -n "Package $(PACKAGE) (lib$(PACKAGE).so)" $(HEADERS)
+#else
+# kdoc -L doc -d doc/$(PACKAGE) -n "$(DOCTITLE) (lib$(PACKAGE).so)" $(HEADERS)
+#endif
+
+
+clean:
+ @$(MESSAGE) "Cleaning $(PACKAGE)"
+ @$(PRETTY) "rm -f $(SONAME) $(APPLICATIONS)"
+ @$(PRETTY) "rm -f *.o *.d core *~ moc_*.cpp"
+
+copy: clean
+ tar -C .. -cvzf `date +../$(PACKAGE)-%d%b%y.tgz` $(PACKAGE)
Added: pkg/trunk/gmapping/build_tools/Makefile.subdirs
===================================================================
--- pkg/trunk/gmapping/build_tools/Makefile.subdirs (rev 0)
+++ pkg/trunk/gmapping/build_tools/Makefile.subdirs 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,16 @@
+export VERBOSE
+
+.PHONY: clean, all
+
+ifeq ($(VERBOSE), 0)
+QUIET=--no-print-directory
+endif
+
+all:
+ @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; if ! $(MAKE) $(QUIET) -C $$subdir; then $(MESSAGE) "Compilation in $$subdir failed."; exit 1; fi; done
+
+clean:
+ @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir clean; done
+
+dep:
+ @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir dep; done
Added: pkg/trunk/gmapping/build_tools/generate_shared_object
===================================================================
--- pkg/trunk/gmapping/build_tools/generate_shared_object (rev 0)
+++ pkg/trunk/gmapping/build_tools/generate_shared_object 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,16 @@
+#!/bin/tcsh
+
+echo decompressing file $1
+
+set FILELIST=`ar -t $1`
+echo "Object files:"
+foreach i ($FILELIST)
+ echo $i
+end
+
+echo generating $1:r.so
+
+ar -x $1
+ld -shared -o $1:r.so $FILELIST
+
+rm $FILELIST
Property changes on: pkg/trunk/gmapping/build_tools/generate_shared_object
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/gmapping/build_tools/message
===================================================================
--- pkg/trunk/gmapping/build_tools/message (rev 0)
+++ pkg/trunk/gmapping/build_tools/message 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,14 @@
+#!/bin/sh
+
+#echo "message: verbose = $VERBOSE"
+
+if ($VERBOSE)
+then
+ exit 0;
+fi
+
+a=$MAKELEVEL
+
+while ((0<$a)); do echo -n " "; let "a = $a - 1";done
+
+echo $1
Property changes on: pkg/trunk/gmapping/build_tools/message
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/gmapping/build_tools/pretty_compiler
===================================================================
--- pkg/trunk/gmapping/build_tools/pretty_compiler (rev 0)
+++ pkg/trunk/gmapping/build_tools/pretty_compiler 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,26 @@
+#!/bin/sh
+
+
+#echo "pretty: verbose = $VERBOSE"
+
+if ($VERBOSE)
+then
+ echo $1;
+ if ! eval $1
+ then
+ echo "Failed command was:"
+ echo $1
+ echo "in directory " `pwd`
+ exit 1
+ fi
+else
+ if ! eval $1
+ then
+ echo "Failed command was:"
+ echo $1
+ echo "in directory " `pwd`
+ exit 1
+ fi
+fi
+
+exit 0
Property changes on: pkg/trunk/gmapping/build_tools/pretty_compiler
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/gmapping/build_tools/testlib
===================================================================
--- pkg/trunk/gmapping/build_tools/testlib (rev 0)
+++ pkg/trunk/gmapping/build_tools/testlib 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,23 @@
+#!/bin/bash
+if [ -z "$1" ]; then
+ echo "Syntax: rtestlib <library>"
+ exit 1
+fi
+FNAME=`mktemp rtestlibXXXXXX`
+echo "int main() { return 0; }" > $FNAME.cpp
+
+g++ $1 $FNAME.cpp -o $FNAME
+result=$?
+rm -f $FNAME.cpp $FNAME
+
+exit $result
+
+#if g++ $1 $FNAME.cpp -o $FNAME
+#then#
+# rm -f $FNAME.cpp $FNAME
+# exit 1
+#else
+# rm -f $FNAME.cpp $FNAME
+# exit 0
+#fi
+
Property changes on: pkg/trunk/gmapping/build_tools/testlib
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/gmapping/carmenwrapper/Makefile
===================================================================
--- pkg/trunk/gmapping/carmenwrapper/Makefile (rev 0)
+++ pkg/trunk/gmapping/carmenwrapper/Makefile 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,14 @@
+OBJS= carmenwrapper.o
+APPS=
+
+LIBS+= -L$(CARMEN_HOME)/lib -lnavigator_interface -lsimulator_interface -lrobot_interface -llaser_interface -lparam_interface -lglobal -lipc -lm -lutils -lsensor_range -llog -lscanmatcher -lpthread -lz
+CPPFLAGS+=-I../sensor -I$(CARMEN_HOME)/include
+
+-include ../global.mk
+
+ifeq ($(CARMENSUPPORT), 0)
+OBJS=
+ -include ../build_tools/Makefile.app
+else
+ -include ../build_tools/Makefile.generic-shared-object
+endif
Added: pkg/trunk/gmapping/carmenwrapper/carmenwrapper.cpp
===================================================================
--- pkg/trunk/gmapping/carmenwrapper/carmenwrapper.cpp (rev 0)
+++ pkg/trunk/gmapping/carmenwrapper/carmenwrapper.cpp 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,490 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti,
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons
+ * License (Attribution-NonCommercial-ShareAlike 2.0)"
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
+ * and Wolfram Burgard.
+ *
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ *
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.
+ *
+ *****************************************************************/
+
+
+#include "carmenwrapper.h"
+
+using namespace GMapping;
+using namespace std;
+
+//static vars for the carmenwrapper
+SensorMap CarmenWrapper::m_sensorMap;
+deque<RangeReading> CarmenWrapper::m_rangeDeque;
+pthread_mutex_t CarmenWrapper::m_mutex;
+sem_t CarmenWrapper::m_dequeSem;
+pthread_mutex_t CarmenWrapper::m_lock;
+pthread_t CarmenWrapper::m_readingThread;
+RangeSensor* CarmenWrapper::m_frontLaser=0;
+RangeSensor* CarmenWrapper::m_rearLaser=0;
+bool CarmenWrapper::m_threadRunning=false;
+OrientedPoint CarmenWrapper::m_truepos;
+bool CarmenWrapper::stopped=true;
+
+
+void CarmenWrapper::initializeIPC(const char* name) {
+ carmen_ipc_initialize(1,(char **)&name);
+}
+
+
+
+
+int CarmenWrapper::registerLocalizationMessages(){
+ lock();
+ IPC_RETURN_TYPE err;
+
+ /* register globalpos message */
+ err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_NAME, IPC_VARIABLE_LENGTH,
+ CARMEN_LOCALIZE_GLOBALPOS_FMT);
+ carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_GLOBALPOS_NAME);
+
+ /* register robot particle message */
+ err = IPC_defineMsg(CARMEN_LOCALIZE_PARTICLE_NAME, IPC_VARIABLE_LENGTH,
+ CARMEN_LOCALIZE_PARTICLE_FMT);
+ carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_PARTICLE_NAME);
+
+/*
+ carmen_localize_subscribe_initialize_placename_message(NULL,
+ (carmen_handler_t)
+ carmen_localize_initialize_placename_handler,
+ CARMEN_SUBSCRIBE_LATEST);
+
+ // register map request message
+ err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_QUERY_NAME, IPC_VARIABLE_LENGTH,
+ CARMEN_LOCALIZE_MAP_QUERY_FMT);
+ carmen_test_ipc_exit(err, "Could not define",
+ CARMEN_LOCALIZE_MAP_QUERY_NAME);
+
+ err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_NAME, IPC_VARIABLE_LENGTH,
+ CARMEN_LOCALIZE_MAP_FMT);
+ carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_MAP_NAME);
+
+ // subscribe to map request message
+ err = IPC_subscribe(CARMEN_LOCALIZE_MAP_QUERY_NAME, map_query_handler, NULL);
+ carmen_test_ipc(err, "Could not subscribe", CARMEN_LOCALIZE_MAP_QUERY_NAME);
+ IPC_setMsgQueueLength(CARMEN_LOCALIZE_MAP_QUERY_NAME, 1);
+
+
+ // register globalpos request message
+ err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME,
+ IPC_VARIABLE_LENGTH,
+ CARMEN_DEFAULT_MESSAGE_FMT);
+ carmen_test_ipc_exit(err, "Could not define",
+ CARMEN_LOCALIZE_MAP_QUERY_NAME);
+
+ // subscribe to globalpos request message
+ err = IPC_subscribe(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME,
+ globalpos_query_handler, NULL);
+ carmen_test_ipc(err, "Could not subscribe",
+ CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME);
+ IPC_setMsgQueueLength(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 1);
+*/
+ unlock();
+ return 0;
+}
+
+bool CarmenWrapper::start(const char* name){
+ if (m_threadRunning)
+ return false;
+ carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)robot_frontlaser_handler, CARMEN_SUBSCRIBE_LATEST);
+ carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)robot_rearlaser_handler, CARMEN_SUBSCRIBE_LATEST);
+ carmen_simulator_subscribe_truepos_message(NULL,(carmen_handler_t) simulator_truepos_handler, CARMEN_SUBSCRIBE_LATEST);
+
+ IPC_RETURN_TYPE err;
+
+ err = IPC_subscribe(CARMEN_NAVIGATOR_GO_NAME, navigator_go_handler, NULL);
+ carmen_test_ipc_exit(err, "Could not subscribe",
+ CARMEN_NAVIGATOR_GO_NAME);
+ IPC_setMsgQueueLength(CARMEN_NAVIGATOR_GO_NAME, 1);
+
+ err = IPC_subscribe(CARMEN_NAVIGATOR_STOP_NAME, navigator_stop_handler, NULL);
+ carmen_test_ipc_exit(err, "Could not subscribe",
+ CARMEN_NAVIGATOR_STOP_NAME);
+ IPC_setMsgQueueLength(CARMEN_NAVIGATOR_STOP_NAME, 1);
+
+
+
+ signal(SIGINT, shutdown_module);
+ pthread_mutex_init(&m_mutex, 0);
+ pthread_mutex_init(&m_lock, 0);
+ sem_init(&m_dequeSem, 0, 0);
+ m_threadRunning=true;
+ pthread_create (&m_readingThread,0,m_reading_function,0);
+ return true;
+}
+
+void CarmenWrapper::lock(){
+ //cerr <<"LOCK" << endl;
+ pthread_mutex_lock(&m_lock);
+}
+
+void CarmenWrapper::unlock(){
+ //cerr <<"UNLOCK" << endl;
+ pthread_mutex_unlock(&m_lock);
+}
+
+
+bool CarmenWrapper::sensorMapComputed(){
+ pthread_mutex_lock(&m_mutex);
+ bool smok=m_frontLaser;
+ pthread_mutex_unlock(&m_mutex);
+ return smok;
+}
+
+const SensorMap& CarmenWrapper::sensorMap(){
+ return m_sensorMap;
+}
+
+bool CarmenWrapper::isRunning(){
+ return m_threadRunning;
+}
+
+bool CarmenWrapper::isStopped(){
+ return stopped;
+}
+
+int CarmenWrapper::queueLength(){
+ int ql=0;
+ pthread_mutex_lock(&m_mutex);
+ ql=m_rangeDeque.size();
+ pthread_mutex_unlock(&m_mutex);
+ return ql;
+}
+
+OrientedPoint CarmenWrapper::getTruePos(){
+ return m_truepos;
+}
+
+bool CarmenWrapper::getReading(RangeReading& reading){
+ bool present=false;
+ sem_wait(&m_dequeSem);
+ pthread_mutex_lock(&m_mutex);
+ if (!m_rangeDeque.empty()){
+// cerr << __PRETTY_FUNCTION__ << ": queue size=" <<m_rangeDeque.size() << endl;
+ reading=m_rangeDeque.front();
+ m_rangeDeque.pop_front();
+ present=true;
+ }
+ int sval;
+ sem_getvalue(&m_dequeSem,&sval);
+// cerr << "fetch. elements= "<< m_rangeDeque.size() << " sval=" << sval <<endl;
+ pthread_mutex_unlock(&m_mutex);
+ return present;
+}
+
+void CarmenWrapper::addReading(RangeReading& reading){
+ pthread_mutex_lock(&m_mutex);
+ m_rangeDeque.push_back(reading);
+ pthread_mutex_unlock(&m_mutex);
+ sem_post(&m_dequeSem);
+ int sval;
+ sem_getvalue(&m_dequeSem,&sval);
+// cerr << "post. elements= "<< m_rangeDeque.size() << " sval=" << sval <<endl;
+}
+
+
+//RangeSensor::RangeSensor(std::string name, unsigned int beams_num, unsigned int res, const OrientedPoint& position, double span, double maxrange):
+
+void CarmenWrapper::robot_frontlaser_handler(carmen_robot_laser_message* frontlaser) {
+/* if (! m_rangeSensor){
+ double res=0;
+ if (frontlaser->num_readings==180 || frontlaser->num_readings==181)
+ res=M_PI/180;
+ if (frontlaser->num_readings==360 || frontlaser->num_readings==361)
+ res=M_PI/360;
+ assert(res>0);
+ m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9);
+ m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor));
+
+ cout << __PRETTY_FUNCTION__
+ << ": FrontLaser configured."
+ << " Readings " << m_rangeSensor->beams().size()
+ << " Resolution " << res << endl;
+ }
+
+ RangeReading reading(m_rangeSensor, frontlaser->timestamp);
+ reading.resize(m_rangeSensor->beams().size());
+ for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){
+ reading[i]=(double)frontlaser->range[i];
+ }
+ reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta));
+*/
+ RangeReading reading=carmen2reading(*frontlaser);
+ addReading(reading);
+}
+
+void CarmenWrapper::robot_rearlaser_handler(carmen_robot_laser_message* rearlaser) {
+/* if (! m_rangeSensor){
+ double res=0;
+ if (frontlaser->num_readings==180 || frontlaser->num_readings==181)
+ res=M_PI/180;
+ if (frontlaser->num_readings==360 || frontlaser->num_readings==361)
+ res=M_PI/360;
+ assert(res>0);
+ m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9);
+ m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor));
+
+ cout << __PRETTY_FUNCTION__
+ << ": FrontLaser configured."
+ << " Readings " << m_rangeSensor->beams().size()
+ << " Resolution " << res << endl;
+ }
+
+ RangeReading reading(m_rangeSensor, frontlaser->timestamp);
+ reading.resize(m_rangeSensor->beams().size());
+ for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){
+ reading[i]=(double)frontlaser->range[i];
+ }
+ reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta));
+*/
+ RangeReading reading=carmen2reading(*rearlaser);
+ addReading(reading);
+}
+
+
+
+
+void CarmenWrapper:: navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) {
+ carmen_navigator_go_message msg;
+ FORMATTER_PTR formatter;
+ IPC_RETURN_TYPE err;
+
+ formatter = IPC_msgInstanceFormatter(msgRef);
+ err = IPC_unmarshallData(formatter, callData, &msg,
+ sizeof(carmen_navigator_go_message));
+ IPC_freeByteArray(callData);
+
+ carmen_test_ipc_return
+ (err, "Could not unmarshall", IPC_msgInstanceName(msgRef));
+ cerr<<"go"<<endl;
+ stopped=false;
+}
+
+
+void CarmenWrapper:: navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) {
+ carmen_navigator_stop_message msg;
+ FORMATTER_PTR formatter;
+ IPC_RETURN_TYPE err;
+
+ formatter = IPC_msgInstanceFormatter(msgRef);
+ err = IPC_unmarshallData(formatter, callData, &msg,
+ sizeof(carmen_navigator_stop_message));
+ IPC_freeByteArray(callData);
+
+ carmen_test_ipc_return
+ (err, "Could not unmarshall", IPC_msgInstanceName(msgRef));
+ cerr<<"stop"<<endl;
+ stopped=true;
+}
+
+
+
+void CarmenWrapper::simulator_truepos_handler(carmen_simulator_truepos_message* truepos){
+ m_truepos.x=truepos->truepose.x;
+ m_truepos.y=truepos->truepose.y;
+ m_truepos.theta=truepos->truepose.theta;
+}
+
+RangeReading CarmenWrapper::carmen2reading(const carmen_robot_laser_message& msg){
+ //either front laser or rear laser
+ double dth=msg.laser_pose.theta-msg.robot_pose.theta;
+ dth=atan2(sin(dth), cos(dth));
+
+ if (msg.laser_pose.theta==msg.robot_pose.theta && !m_frontLaser){
+ double res=0;
+ res = msg.config.angular_resolution;
+// if (msg.num_readings==180 || msg.num_readings==181)
+// res=M_PI/180;
+// if (msg.num_readings==360 || msg.num_readings==361)
+// res=M_PI/360;
+ assert(res>0);
+ string sensorName="FLASER";
+ OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
+ OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
+ OrientedPoint dp=absoluteDifference(lpose, rpose);
+ m_frontLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0,
+ msg.config.maximum_range);
+ m_frontLaser->updateBeamsLookup();
+ m_sensorMap.insert(make_pair(sensorName, m_frontLaser));
+
+ cout << __PRETTY_FUNCTION__
+ << ": " << sensorName <<" configured."
+ << " Readings " << m_frontLaser->beams().size()
+ << " Resolution " << res << endl;
+ }
+ if (msg.laser_pose.theta!=msg.robot_pose.theta && !m_rearLaser){
+ double res=0;
+ res = msg.config.angular_resolution;
+// if (msg.num_readings==180 || msg.num_readings==181)
+// res=M_PI/180;
+// if (msg.num_readings==360 || msg.num_readings==361)
+// res=M_PI/360;
+ assert(res>0);
+ OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
+ OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
+ OrientedPoint dp=absoluteDifference(lpose, rpose);
+ string sensorName="RLASER";
+ m_rearLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0,
+ msg.config.maximum_range);
+ m_rearLaser->updateBeamsLookup();
+ m_sensorMap.insert(make_pair(sensorName, m_rearLaser));
+
+ cout << __PRETTY_FUNCTION__
+ << ": " << sensorName <<" configured."
+ << " Readings " << m_rearLaser->beams().size()
+ << " Resolution " << res << endl;
+ }
+
+ const RangeSensor * rs=(msg.laser_pose.theta==msg.robot_pose.theta)?m_frontLaser:m_rearLaser;
+ RangeReading reading(rs, msg.timestamp);
+ reading.resize(rs->beams().size());
+ for (unsigned int i=0; i< (unsigned int)msg.num_readings; i++){
+ reading[i]=(double)msg.range[i];
+ }
+ reading.setPose(OrientedPoint(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta));
+ return reading;
+}
+
+void CarmenWrapper::publish_globalpos(carmen_localize_summary_p summary)
+{
+ lock();
+ static carmen_localize_globalpos_message globalpos;
+ IPC_RETURN_TYPE err;
+
+ globalpos.timestamp = carmen_get_time();
+ globalpos.host = carmen_get_host();
+ globalpos.globalpos = summary->mean;
+ globalpos.globalpos_std = summary->std;
+ globalpos.globalpos_xy_cov = summary->xy_cov;
+ globalpos.odometrypos = summary->odometry_pos;
+ globalpos.converged = summary->converged;
+ err = IPC_publishData(CARMEN_LOCALIZE_GLOBALPOS_NAME, &globalpos);
+ carmen_test_ipc_exit(err, "Could not publish",
+ CARMEN_LOCALIZE_GLOBALPOS_NAME);
+ unlock();
+}
+
+/* publish a particle message */
+
+void CarmenWrapper::publish_particles(carmen_localize_particle_filter_p filter,
+ carmen_localize_summary_p summary)
+{
+ lock();
+ static carmen_localize_particle_message pmsg;
+ IPC_RETURN_TYPE err;
+
+ pmsg.timestamp = carmen_get_time();
+ pmsg.host = carmen_get_host();
+ pmsg.globalpos = summary->mean;
+ pmsg.globalpos_std = summary->mean;
+ pmsg.num_particles = filter->param->num_particles;
+ pmsg.particles = (carmen_localize_particle_ipc_p)filter->particles;
+ err = IPC_publishData(CARMEN_LOCALIZE_PARTICLE_NAME, &pmsg);
+ carmen_test_ipc_exit(err, "Could not publish",
+ CARMEN_LOCALIZE_PARTICLE_NAME);
+ fprintf(stderr, "P");
+ unlock();
+}
+
+
+
+
+
+void * CarmenWrapper::m_reading_function(void*){
+ while (true) {
+ lock();
+ IPC_listen(100);
+ unlock();
+ usleep(20000);
+ }
+ return 0;
+}
+
+void CarmenWrapper::shutdown_module(int sig){
+ if(sig == SIGINT) {
+ carmen_ipc_disconnect();
+
+ fprintf(stderr, "\nDisconnecting (shutdown_module(%d) called).\n",sig);
+ exit(0);
+ }
+}
+/*
+typedef struct {
+ int num_readings;
+ float *range;
+ char *tooclose;
+ double x, y, theta;//position of the laser on the robot
+ double odom_x, odom_y, odom_theta; //position of the center of the robot
+ double tv, rv;
+ double forward_safety_dist, side_safety_dist;
+ double turn_axis;
+ double timestamp;
+ char host[10];
+} carmen_robot_laser_message;
+*/
+
+carmen_robot_laser_message CarmenWrapper::reading2carmen(const RangeReading& reading){
+ carmen_robot_laser_message frontlaser;
+ frontlaser.num_readings=reading.size();
+ frontlaser.range = new float[frontlaser.num_readings];
+ frontlaser.tooclose=0;
+ frontlaser.laser_pose.x=frontlaser.robot_pose.x=reading.getPose().x;
+ frontlaser.laser_pose.y=frontlaser.robot_pose.y=reading.getPose().y;
+ frontlaser.laser_pose.theta=frontlaser.robot_pose.theta=reading.getPose().theta;
+ frontlaser.tv=frontlaser.rv=0;
+ frontlaser.forward_safety_dist=frontlaser.side_safety_dist=0;
+ frontlaser.turn_axis=0;
+ frontlaser.timestamp=reading.getTime();
+ for (unsigned int i=0; i< reading.size(); i++){
+ frontlaser.range[i]=(float)reading[i];
+ }
+ return frontlaser;
+}
+
+carmen_point_t CarmenWrapper::point2carmen (const OrientedPoint& p){
+ return (carmen_point_t){p.x,p.y,p.theta};
+}
+
+OrientedPoint CarmenWrapper::carmen2point (const carmen_point_t& p){
+ return OrientedPoint(p.x, p.y, p.theta);
+}
+
+
+/*
+int main (int argc, char** argv) {
+ CarmenWrapper::start(argc, argv);
+ while(1){
+ sleep(2);
+ RangeReading reading(0,0);
+ while(CarmenWrapper::getReading(reading)){
+ cout << "FLASER " << reading.size();
+ for (int i=0; i<reading.size(); i++)
+ cout << " " << reading[i];
+ cout << reading.getPose().x << " "
+ << reading.getPose().y << " "
+ << reading.getPose().theta << " 0 cazzo 0" << endl;
+ }
+ cout << endl;
+ }
+ return 1;
+}
+*/
+
Added: pkg/trunk/gmapping/carmenwrapper/carmenwrapper.h
===================================================================
--- pkg/trunk/gmapping/carmenwrapper/carmenwrapper.h (rev 0)
+++ pkg/trunk/gmapping/carmenwrapper/carmenwrapper.h 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,108 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti,
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons
+ * License (Attribution-NonCommercial-ShareAlike 2.0)"
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
+ * and Wolfram Burgard.
+ *
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ *
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.
+ *
+ *****************************************************************/
+
+
+#ifndef CARMENWRAPPER_H
+#define CARMENWRAPPER_H
+
+#include <iostream>
+#include <deque>
+#include <pthread.h>
+#include <semaphore.h>
+#include <carmen/carmen.h>
+#include <carmen/global.h>
+#include <sensor/sensor_base/sensor.h>
+#include <log/carmenconfiguration.h>
+#include <log/sensorstream.h>
+#include <log/sensorlog.h>
+#include <sensor/sensor_range/rangesensor.h>
+#include <sensor/sensor_range/rangereading.h>
+
+namespace GMapping{
+
+class CarmenWrapper {
+public:
+ static void initializeIPC(const char* name);
+ static bool start(const char* name);
+ static bool isRunning();
+ static void lock();
+ static void unlock();
+ static int registerLocalizationMessages();
+
+ static int queueLength();
+ static OrientedPoint getTruePos();
+ static bool getReading(RangeReading& reading);
+ static void addReading(RangeReading& reading);
+ static const SensorMap& sensorMap();
+ static bool sensorMapComputed();
+ static bool isStopped();
+
+// conversion function
+ static carmen_robot_laser_message reading2carmen(const RangeReading& reading);
+ static RangeReading carmen2reading(const carmen_robot_laser_message& msg);
+ static carmen_point_t point2carmen (const OrientedPoint& p);
+ static OrientedPoint carmen2point (const carmen_point_t& p);
+
+
+// carmen interaction
+ static void robot_frontlaser_handler(carmen_robot_laser_message* frontlaser);
+ static void robot_rearlaser_handler(carmen_robot_laser_message* frontlaser);
+ static void simulator_truepos_handler(carmen_simulator_truepos_message* truepos);
+ //babsi:
+ static void navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ;
+ static void navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ;
+
+ //babsi:
+ static void publish_globalpos(carmen_localize_summary_p summary);
+ static void publish_particles(carmen_localize_particle_filter_p filter,
+ carmen_localize_summary_p summary);
+
+ static void shutdown_module(int sig);
+
+ private:
+ static std::deque<RangeReading> m_rangeDeque;
+ static sem_t m_dequeSem;
+ static pthread_mutex_t m_mutex, m_lock;
+ static pthread_t m_readingThread;
+ static void * m_reading_function(void*);
+ static bool m_threadRunning;
+ static SensorMap m_sensorMap;
+ static RangeSensor* m_frontLaser, *m_rearLaser;
+ static OrientedPoint m_truepos;
+ static bool stopped;
+};
+
+} //end namespace
+
+
+
+#endif
+/*
+int main (int argc, char** argv) {
+
+ CarmenWrapper::init_carmen(argc, argv);
+ while (true) {
+ IPC_listenWait(100);
+ }
+ return 1;
+}
+*/
Added: pkg/trunk/gmapping/configfile/Makefile
===================================================================
--- pkg/trunk/gmapping/configfile/Makefile (rev 0)
+++ pkg/trunk/gmapping/configfile/Makefile 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,5 @@
+OBJS= configfile.o
+APPS= configfile_test
+
+-include ../global.mk
+-include ../build_tools/Makefile.generic-shared-object
Added: pkg/trunk/gmapping/configfile/configfile.cpp
===================================================================
--- pkg/trunk/gmapping/configfile/configfile.cpp (rev 0)
+++ pkg/trunk/gmapping/configfile/configfile.cpp 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,310 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti,
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons
+ * License (Attribution-NonCommercial-ShareAlike 2.0)"
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
+ * and Wolfram Burgard.
+ *
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ *
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.
+ *
+ *****************************************************************/
+
+
+#include "configfile.h"
+
+#include <fstream>
+#include <iostream>
+
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <ctype.h>
+
+namespace GMapping{
+using namespace std;
+
+AutoVal::AutoVal(const std::string& value) {
+ m_value=value;
+}
+
+AutoVal::AutoVal(const char* c) {
+ m_value=c;
+}
+
+AutoVal::AutoVal(double d) {
+ std::stringstream s;
+ s<<d;
+ m_value=s.str();
+}
+
+AutoVal::AutoVal(bool d) {
+ std::stringstream s;
+ if(d)
+ s << "on";
+ else
+ s << "off";
+ m_value=s.str();
+}
+
+
+AutoVal::AutoVal(int i) {
+ std::stringstream s;
+ s<<i;
+ m_value=s.str();
+}
+
+AutoVal::AutoVal(unsigned int i) {
+ std::stringstream s;
+ s<<i;
+ m_value=s.str();
+}
+
+AutoVal::AutoVal(const AutoVal& other) {
+ m_value=other.m_value;
+}
+
+AutoVal& AutoVal::operator=(const AutoVal& other) {
+ m_value=other.m_value;
+ return *this;
+}
+
+AutoVal& AutoVal::operator=(double d) {
+ std::stringstream s;
+ s << d;
+ m_value = s.str();
+ return *this;
+}
+
+AutoVal& AutoVal::operator=(bool d) {
+ std::stringstream s;
+ if(d)
+ s << "on";
+ else
+ s << "off";
+ m_value = s.str();
+ return *this;
+}
+
+
+AutoVal& AutoVal::operator=(int i) {
+ std::stringstream s;
+ s << i;
+ m_value = s.str();
+ return *this;
+}
+
+AutoVal& AutoVal::operator=(unsigned int i) {
+ std::stringstream s;
+ s << i;
+ m_value = s.str();
+ return *this;
+}
+
+
+AutoVal& AutoVal::operator=(const std::string& s) {
+ m_value=s;
+ return *this;
+}
+
+AutoVal::operator std::string() const {
+ return m_value;
+}
+
+AutoVal::operator double() const {
+ return atof(m_value.c_str());
+}
+
+AutoVal::operator int() const {
+ return atoi(m_value.c_str());
+}
+
+AutoVal::operator unsigned int() const {
+ return (unsigned int) atoi(m_value.c_str());
+}
+
+AutoVal::operator bool() const {
+ // stupid c++ does not allow compareNoCase...
+ if (toLower(m_value)=="on" || atoi(m_value.c_str()) == 1)
+ return true;
+ return false;
+}
+
+std::string AutoVal::toLower(const std::string& source) const {
+ std::string result(source);
+ char c='\0';
+ for (unsigned int i=0; i<result.length(); i++) {
+ c = result[i];
+ c = ::tolower(c);
+ result[i] = c;
+ }
+ return result;
+}
+
+
+//////////////////////////////////////////////////////////
+
+ConfigFile::ConfigFile() {
+}
+
+ConfigFile::ConfigFile(const std::string& configFile) {
+ read(configFile);
+}
+
+ConfigFile::ConfigFile(const char* configFile) {
+ read(configFile);
+}
+
+bool ConfigFile::read(const char* configFile) {
+ return read(std::string(configFile));
+}
+
+bool ConfigFile::read(const std::string& configFile) {
+ std::ifstream file(configFile.c_str());
+
+ if (!file || file.eof())
+ return false;
+
+ std::string line;
+ std::string name;
+ std::string val;
+ std::string inSection;
+ while (std::getline(file,line)) {
+
+ if (! line.length()) continue;
+ // cute the comments
+ if (line[0] == '#') continue;
+ line = truncate(line,"#");
+ line = trim(line);
+ if (! line.length()) continue;
+
+ if (line[0] == '[') {
+ inSection=trim(line.substr(1,line.find(']')-1));
+ continue;
+ }
+
+ istringstream lineStream(line);
+ lineStream >> name;
+ lineStream >> val;
+ insertValue(inSection,name,val);
+ }
+ return true;
+}
+
+void ConfigFile::insertValue(const std::string& section, const std::string& entry, const std::string& thevalue) {
+ m_content[toLower(section)+'/'+toLower(entry)]=AutoVal(thevalue);
+}
+
+const AutoVal& ConfigFile::value(const std::string& section, const std::string& entry) const {
+
+ std::map<std::string,AutoVal>::const_iterator ci =
+ m_content.find(toLower(section) + '/' + toLower(entry));
+ if (ci == m_content.end()) throw "entry does not exist";
+
+ return ci->second;
+}
+
+const AutoVal& ConfigFile::value(const std::string& section, const std::string& entry, double def) {
+ try {
+ return value(section, entry);
+ } catch(const char *) {
+ return m_content.insert(std::make_pair(section+'/'+entry, AutoVal(def))).first->second;
+ }
+}
+
+const AutoVal& ConfigFile::value(const std::string& section, const std::string& entry, bool def) {
+ try {
+ return value(section, entry);
+ } catch(const char *) {
+ return m_content.insert(std::make_pair(section+'/'+entry, AutoVal(def))).first->second;
+ }
+}
+
+const AutoVal& ConfigFile::value(const std::string& section, const std::string& entry, int def) {
+ try {
+ return value(section, entry);
+ } catch(const char *) {
+ return m_content.insert(std::make_pair(section+'/'+entry, AutoVal(def))).first->second;
+ }
+}
+
+const AutoVal& ConfigFile::value(const std::string& section, const std::string& entry, unsigned int def) {
+ try {
+ return value(section, entry);
+ } catch(const char *) {
+ return m_content.insert(std::make_pair(section+'/'+entry, AutoVal(def))).first->second;
+ }
+}
+
+const AutoVal& ConfigFile::value(const std::string& section, const std::string& entry, const std::string& def) {
+ try {
+ return value(section, entry);
+ } catch(const char *) {
+ return m_content.insert(std::make_pair(section+'/'+entry, AutoVal(def))).first->second;
+ }
+}
+
+const AutoVal& ConfigFile::value(const std::string& section, const std::string& entry, const char* def) {
+ try {
+ return value(section, entry);
+ } catch(const char *) {
+ return m_content.insert(std::make_pair(section+'/'+entry, AutoVal(def))).first->second;
+ }
+}
+
+void ConfigFile::dumpValues(std::ostream& out) {
+
+ for (std::map<std::string,AutoVal>::const_iterator it = m_content.begin();
+ it != m_content.end(); it++) {
+ out << (std::string) it->first << " " << (std::string)it->second << std::endl;
+ }
+}
+
+
+std::string ConfigFile::trim(const std::string& source, char const* delims) const {
+ std::string result(source);
+ std::string::size_type index = result.find_last_not_of(delims);
+ if(index != std::string::npos)
+ result.erase(++index);
+
+ index = result.find_first_not_of(delims);
+ if(index != std::string::npos)
+ result.erase(0, index);
+ else
+ result.erase();
+ return result;
+}
+
+std::string ConfigFile::truncate(const std::string& source, const char* atChar) const {
+ std::string::size_type index = source.find_first_of(atChar);
+
+ if(index == 0)
+ return "";
+ else if(index == std::string::npos) {
+ return source;
+ }
+ return
+ source.substr(0,index);
+}
+
+std::string ConfigFile::toLower(const std::string& source) const {
+ std::string result(source);
+ char c='\0';
+ for (unsigned int i=0; i<result.length(); i++) {
+ c = result[i];
+ c = ::tolower(c);
+ result[i] = c;
+ }
+ return result;
+}
+};
Added: pkg/trunk/gmapping/configfile/configfile.h
===================================================================
--- pkg/trunk/gmapping/configfile/configfile.h (rev 0)
+++ pkg/trunk/gmapping/configfile/configfile.h 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,117 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti,
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons
+ * License (Attribution-NonCommercial-ShareAlike 2.0)"
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
+ * and Wolfram Burgard.
+ *
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ *
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.
+ *
+ *****************************************************************/
+
+
+#ifndef CONFIGFILE_H
+#define CONFIGFILE_H
+
+#include <iostream>
+#include <string>
+#include <map>
+
+namespace GMapping{
+
+class AutoVal {
+public:
+ AutoVal() {};
+ explicit AutoVal(const std::string&);
+ explicit AutoVal(double);
+ explicit AutoVal(int);
+ explicit AutoVal(unsigned int);
+ explicit AutoVal(bool);
+ explicit AutoVal(const char*);
+
+ AutoVal(const AutoVal&);
+ AutoVal& operator=(const AutoVal&);
+
+ AutoVal& operator=(double);
+ AutoVal& operator=(int);
+ AutoVal& operator=(unsigned int);
+ AutoVal& operator=(bool);
+ AutoVal& operator=(const std::string&);
+
+public:
+ operator std::string() const;
+ operator double() const;
+ operator int() const;
+ operator unsigned int() const;
+ operator bool() const;
+
+protected:
+ std::string toLower(const std::string& source) const;
+
+private:
+ std::string m_value;
+};
+
+class ConfigFile {
+ std::map<std::string,AutoVal> m_content;
+
+public:
+ ConfigFile();
+ ConfigFile(const std::string& configFile);
+ ConfigFile(const char* configFile);
+
+ bool read(const std::string& configFile);
+ bool read(const char* configFile);
+
+
+ const AutoVal& value(const std::string& section,
+ const std::string& entry) const;
+
+ const AutoVal& value(const std::string& section,
+ const std::string& entry,
+ double def);
+
+ const AutoVal& value(const std::string& section,
+ const std::string& entry,
+ const char* def);
+
+ const AutoVal& value(const std::string& section,
+ const std::string& entry,
+ bool def);
+
+ const AutoVal& value(const std::string& section,
+ const std::string& entry,
+ int def);
+
+ const AutoVal& value(const std::string& section,
+ const std::string& entry,
+ unsigned int def);
+
+ const AutoVal& value(const std::string& section,
+ const std::string& entry,
+ const std::string& def);
+
+ void dumpValues(std::ostream& out);
+
+
+ protected:
+ std::string trim(const std::string& source, char const* delims = " \t\r\n") const;
+ std::string truncate(const std::string& source, const char* atChar) const;
+ std::string toLower(const std::string& source) const;
+ void insertValue(const std::string& section, const std::string& entry, const std::string& thevalue );
+};
+
+};
+
+#endif
Added: pkg/trunk/gmapping/configfile/configfile_test.cpp
===================================================================
--- pkg/trunk/gmapping/configfile/configfile_test.cpp (rev 0)
+++ pkg/trunk/gmapping/configfile/configfile_test.cpp 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,58 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti,
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons
+ * License (Attribution-NonCommercial-ShareAlike 2.0)"
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
+ * and Wolfram Burgard.
+ *
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ *
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.
+ *
+ *****************************************************************/
+
+
+
+#include <iostream>
+#include "configfile.h"
+
+using namespace std;
+using namespace GMapping;
+
+int main(int argc, char** argv) {
+
+ if (argc != 2) {
+ cerr << "Usage: " << argv[0] << " [initifle]" << endl;
+ exit(0);
+ }
+
+ ConfigFile cfg;
+ cfg.read(argv[argc-1]);
+
+ cout << "-- values from configfile --" << endl;
+ cfg.dumpValues(cout);
+
+ cout << "-- adding a value --" << endl;
+ cfg.value("unkown","unkown",std::string("the new value!"));
+
+
+
+
+
+ cout << "-- values from configfile & added values --" << endl;
+ cfg.dumpValues(cout);
+
+ if ( ((std::string) cfg.value("unkown","unkown",std::string("the new value!"))) != std::string("the new value!"))
+ cerr << "strange error, check strings" << endl;
+
+ return 0;
+}
Added: pkg/trunk/gmapping/configfile/demo.cfg
===================================================================
--- pkg/trunk/gmapping/configfile/demo.cfg (rev 0)
+++ pkg/trunk/gmapping/configfile/demo.cfg 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,19 @@
+[mysection]
+
+mykey1 string_value_1 # comment
+mykey2 string value 2
+
+ mykey3 string value 3
+
+# mykey4 string value 3
+
+ # mykey5 string value 3
+
+mykey6 1
+mykey7 1.7
+
+mykey8 On
+
+mykey9 off
+
+
Added: pkg/trunk/gmapping/configfile/test.ini
===================================================================
--- pkg/trunk/gmapping/configfile/test.ini (rev 0)
+++ pkg/trunk/gmapping/configfile/test.ini 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,10 @@
+
+[test]
+
+particles 30
+
+onLine on
+generateMap off
+
+teststr Hallo
+
Added: pkg/trunk/gmapping/configure
===================================================================
--- pkg/trunk/gmapping/configure (rev 0)
+++ pkg/trunk/gmapping/configure 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,243 @@
+#!/bin/bash
+#if [ "$UID" = 0 ]; then echo "Please don't run configure as root"; exit 1; fi
+
+LINUX=0
+MACOSX=0
+
+if [ `uname` = "Linux" ]; then
+ LINUX=1
+ OSTYPE=LINUX
+ CPPFLAGS="-DLINUX"
+fi
+if [ `uname` = "Darwin" ]; then
+ MACOSX=1
+ CPPFLAGS="-DMACOSX"
+ OSTYPE=MACOSX
+fi
+
+if [ ! $CXX ]; then
+ echo "No 'CXX' environment variable found, using g++.";
+ CXX="g++"
+fi
+
+if [ ! $CC ]; then
+ echo "No 'CC' environment variable found, using gcc.";
+ CC="gcc"
+fi
+
+if [ ! -x `which $CXX` ]; then
+ echo "Can't execute C++ compiler '$CXX'.";
+ exit 1;
+else
+ echo "Using C++ compiler: $CXX"
+fi
+
+if [ ! -x `which $CC` ]; then
+ echo "Can't execute C++ compiler '$CC'.";
+ exit 1;
+else
+ echo "Using C compiler: $CC"
+fi
+
+GCC_VERSION=`$CXX --version`
+
+echo -n "Checking for Qt 3.x ... "
+for GUESS_QTDIR in `ls /usr/lib/ | grep -E "qt3|qt-3"`; do
+ if [ -d /usr/lib/$GUESS_QTDIR/include -a -d /usr/lib/$GUESS_QTDIR/lib -a -f /usr/lib/$GUESS_QTDIR/bin/moc ]; then
+ QT_INCLUDE="-I /usr/lib/$GUESS_QTDIR/include"
+ QT_LIB="-L /usr/lib/$GUESS_QTDIR/lib -lqt-mt" ;
+ MOC="/usr/lib/$GUESS_QTDIR/bin/moc" ;
+ fi ;
+done ;
+if [ ! "$QT_INCLUDE" ]; then
+ echo -e "\n\n*** Qt 3.x not found please set QT_INCLUDE, QT_LIB, MOC by hand\n\a"
+ #exit 1
+else
+ echo "Ok" ;
+fi
+
+ARIASUPPORT="0"
+echo -n "Checking for Aria libs "
+for GUESS_ARIADIR in `ls /usr/local/ | grep -E "Aria"`; do
+ if [ -d /usr/local/$GUESS_ARIADIR/include -a -d /usr/local/$GUESS_ARIADIR/lib ]; then
+ ARIA_INCLUDE="-I/usr/local/$GUESS_ARIADIR/include"
+ ARIA_LIB="-L/usr/local/$GUESS_ARIADIR/lib -lAria"
+ ARIASUPPORT="1"
+ fi ;
+done ;
+
+if [ ! "$ARIA_INCLUDE" ]; then
+ echo -e "\n\n*** ARIA not found, please set ARIA_INCLUDE and ARIA_LIB by hand\n\a"
+else
+ echo "Ok" ;
+fi
+
+echo -n "Checking for Gsl libs "
+if [ "$OSTYPE" = "LINUX" ]; then
+ GSL_LIB="-lgsl -lgslcblas"
+ GSL_INCLUDE="-I/usr/include/"
+fi
+if [ "$OSTYPE" = "MACOSX" ]; then
+ GSL_LIB="-L/sw/lib -lgsl -lgslcblas"
+ GSL_INCLUDE="-I/sw/include"
+fi
+
+if [ ! "$GSL_INCLUDE" ]; then
+ echo -e "\n\n*** GSL not found, please set GSL_INCLUDE and GSL_LIB by hand\n\a"
+else
+ echo "Ok" ;
+fi
+
+
+# echo -n "Checking for KDE 3.x includes ... "
+# for GUESS_KDE_INCLUDE in /usr/include/kde /usr/include/kde3 /opt/kde3/include /opt/kde/include; do
+# if [ -d $GUESS_KDE_INCLUDE ]; then
+# KDE_INCLUDE="-I$GUESS_KDE_INCLUDE"
+# fi ;
+# done ;
+#
+# if [ ! "$KDE_INCLUDE" ]; then
+# echo -e "\n\n*** KDE 3.x includes not found please set KDE_INCLUDE by hand\n\a"
+# exit 1
+# else
+# echo "Ok" ;
+# fi
+#
+# echo -n "Checking for KDE 3.x libs ... "
+# for GUESS_KDE_LIB in /usr/lib/kde3 /opt/kde3/lib; do
+# if [ -d $GUESS_KDE_LIB ]; then
+# KDE_LIB="-L$GUESS_KDE_LIB -lkdeui"
+# fi ;
+# done ;
+#
+# if [ ! "$KDE_LIB" ]; then
+# echo -e "\n\n*** KDE 3.x libs not found please set KDE_LIBS by hand\n\a"
+# exit 1
+# else
+# echo "Ok" ;
+# fi
+
+
+# echo -n "Checking for uic ... "
+# for GUESS_UIC in `ls /usr/bin/ | grep -E "uic|uic3"`; do
+# if [ -f /usr/bin/$GUESS_UIC ]; then
+# UIC=$GUESS_UIC;
+# fi ;
+# done ;
+#
+# if [ ! "$UIC" ]; then
+# echo -e "\n\n*** uic not found please set UIC by hand\n\a"
+# exit 1
+# else
+# echo "Ok" ;
+# fi
+
+MAPPING_ROOT=`pwd`
+
+
+BINDIR=$MAPPING_ROOT/bin
+echo -n "Checking bin directory $BINDIR ... "
+if [ ! -d $BINDIR ]; then
+ mkdir $BINDIR
+ echo "created."
+else
+ echo "Ok."
+fi
+
+LIBDIR=$MAPPING_ROOT/lib
+echo -n "Checking lib directory $LIBDIR ... "
+if [ ! -d $LIBDIR ]; then
+ mkdir $LIBDIR
+ echo "created."
+else
+ echo "Ok."
+fi
+
+CARMENFLAG=""
+CARMENSUPPORT="0"
+CARMEN_LIBS="libnavigator_interface.a libipc.a librobot_interface.a liblaser_interface.a libsimulator_interface.a liblocalize_interface.a libreadlog.a libwritelog.a libglobal.a libipc.a"
+if [ ! "$CARMEN_HOME" ]; then
+ echo -e "Carmen NOT FOUND."
+ echo -e "If you have a carmen version installed please set the"
+ echo -e "CARMEN_HOME variable to the carmen path."
+else
+ if [ -d $CARMEN_HOME ]; then
+ echo -e "carmen found in $CARMEN_HOME, enabling support"
+ CARMENFLAG="-DCARMEN_SUPPORT"
+ echo -e "generating shared objects"
+ for CARMEN_OBJECT in $CARMEN_LIBS ; do
+ if [ -f $CARMEN_HOME/lib/$CARMEN_OBJECT ]; then
+ cp $CARMEN_HOME/lib/$CARMEN_OBJECT $LIBDIR
+ ./build_tools/generate_shared_object $LIBDIR/$CARMEN_OBJECT
+ rm $LIBDIR/$CARMEN_OBJECT
+ else
+ echo -e "Compile carmen first "$CARMEN_HOME/lib/$CARMEN_OBJECT" not found"
+ exit -1
+ fi
+ done;
+ CARMENSUPPORT="1"
+ else
+ echo -e "CARMEN_HOME=$CARMEN_HOME does not exist, disabling support\n"
+ fi
+fi
+
+
+CONFIG=global.mk
+rm -f $CONFIG
+
+cat << EOF > $CONFIG
+### You should not need to change anything below.
+LINUX=$LINUX
+MACOSX=$MACOSX
+
+# Compilers
+CC=$CC
+CXX=$CXX
+
+# Paths
+MAPPING_ROOT=$MAPPING_ROOT
+LIBDIR=$LIBDIR
+BINDIR=$BINDIR
+
+# Build tools
+PRETTY=$MAPPING_ROOT/build_tools/pretty_compiler
+MESSAGE=$MAPPING_ROOT/build_tools/message
+TESTLIB=$MAPPING_ROOT/build_tools/testlib
+
+# QT support
+MOC=$MOC
+QT_LIB=$QT_LIB
+QT_INCLUDE=$QT_INCLUDE
+
+# ARIA support
+ARIA_LIB=$ARIA_LIB
+ARIA_INCLUDE=$ARIA_INCLUDE
+
+
+# # KDE support
+# KDE_LIB=$KDE_LIB
+# KDE_INCLUDE=$KDE_INCLUDE
+# UIC=$UIC
+
+# Generic makefiles
+MAKEFILE_GENERIC=$MAPPING_ROOT/build_tools/Makefile.generic-shared-object
+MAKEFILE_APP=$MAPPING_ROOT/build_tools/Makefile.app
+MAKEFILE_SUBDIRS=$MAPPING_ROOT/build_tools/Makefile.subdirs
+
+
+# Flags
+CPPFLAGS+=$CPPFLAGS -I$MAPPING_ROOT $CARMENFLAG
+CXXFLAGS+=$CXXFLAGS
+LDFLAGS+=$LDFLAGS
+CARMENSUPPORT=$CARMENSUPPORT
+ARIASUPPORT=$ARIASUPPORT
+
+GSL_LIB=$GSL_LIB
+GSL_INCLUDE=$GSL_INCLUDE
+
+$OTHER
+
+include $MAPPING_ROOT/manual.mk
+
+EOF
+
Property changes on: pkg/trunk/gmapping/configure
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/gmapping/docs/Instructions.txt
===================================================================
--- pkg/trunk/gmapping/docs/Instructions.txt (rev 0)
+++ pkg/trunk/gmapping/docs/Instructions.txt 2008-04-17 19:49:41 UTC (rev 116)
@@ -0,0 +1,146 @@
+**WHAT IS THIS
+
+ Mapping is a suite for studying sonar and laser based mapping robotic mapping algorithms.
+ The name is only an attempt, and it will FOR SURE change in the next releases.
+ All of the code has been developed by Giorgio Grisetti <gri...@di...>, and is
+ released under the GPL license.
+ The software is...
[truncated message content] |
|
From: <tf...@us...> - 2008-04-17 04:30:08
|
Revision: 115
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=115&view=rev
Author: tfoote
Date: 2008-04-16 21:30:14 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
updating test program to work with new time. And adding DHparams input ticket:16 and testing basic functionality.
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
pkg/trunk/libTF/simple/testQuat.cc
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-17 04:30:14 UTC (rev 115)
@@ -92,7 +92,19 @@
getFrame(frameID)->setParamsEulers(a,b,c,d,e,f,time);
}
+void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d, unsigned long long time)
+{
+ if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES || frameID == NO_PARENT || frameID == ROOT_FRAME)
+ throw InvalidFrame;
+
+ if (frames[frameID] == NULL)
+ frames[frameID] = new RefFrame();
+
+ getFrame(frameID)->setParent(parentID);
+ getFrame(frameID)->setParamsDH(a,b,c,d,time);
+}
+
NEWMAT::Matrix TransformReference::get(unsigned int target_frame, unsigned int source_frame, unsigned long long time)
{
NEWMAT::Matrix myMat(4,4);
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-17 04:30:14 UTC (rev 115)
@@ -90,6 +90,7 @@
/********** Mutators **************/
/* Set a new frame or update an old one. */
void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double,unsigned long long time);
+ void set(unsigned int framid, unsigned int parentid, double,double,double,double,unsigned long long time);
// Possible exceptions TransformReference::LookupException
/*********** Accessors *************/
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-17 04:30:14 UTC (rev 115)
@@ -18,14 +18,16 @@
//Fill in some transforms
- mTR.set(10,2,1,1,1,dyaw,dp,dr,atime);
+ // mTR.set(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
+ mTR.set(10,2,1,1,1,dyaw,atime);
mTR.set(2,3,1,1,1,dyaw,dp,dr+1,atime-1000);
mTR.set(2,3,1,1,1,dyaw,dp,dr-1,atime+1000);
mTR.set(3,5,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(5,1,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(6,5,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(7,6,1,1,1,dyaw,dp,dr,atime);
- mTR.set(8,7,1,1,1,dyaw,dp,dr,atime);
+ mTR.set(8,7,1,1,1,dyaw,atime);
+ // mTR.set(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
//Demonstrate InvalidFrame LookupException
Modified: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc 2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/testQuat.cc 2008-04-17 04:30:14 UTC (rev 115)
@@ -37,13 +37,13 @@
int main()
{
- Quaternion3D myquat(1,1,1,1,1,1,1);
+ Quaternion3D myquat(1,1,1,1,1,1,1,1000101010);
- Quaternion3D my2ndquat(myquat.asMatrix());
+ Quaternion3D my2ndquat(myquat.asMatrix(1000101010),1000101010);
std::cout << "Quat1"<<std::endl;
- myquat.printMatrix();
+ myquat.printMatrix(1000101010);
std::cout << "Quat2"<<std::endl;
- my2ndquat.printMatrix();
+ my2ndquat.printMatrix(1000101010);
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 02:27:06
|
Revision: 114
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=114&view=rev
Author: tfoote
Date: 2008-04-16 19:27:14 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
removing debugging output, and demonstrating interpolation working
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 02:09:56 UTC (rev 113)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 02:27:14 UTC (rev 114)
@@ -334,7 +334,7 @@
//Base case empty list
if (first == NULL)
{
- cout << "Base case" << endl;
+ //cout << "Base case" << endl;
first = new data_LL;
first->data = new_val;
first->next = NULL;
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-17 02:09:56 UTC (rev 113)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-17 02:27:14 UTC (rev 114)
@@ -19,7 +19,8 @@
//Fill in some transforms
mTR.set(10,2,1,1,1,dyaw,dp,dr,atime);
- mTR.set(2,3,1,1,1,dyaw,dp,dr,atime);
+ mTR.set(2,3,1,1,1,dyaw,dp,dr+1,atime-1000);
+ mTR.set(2,3,1,1,1,dyaw,dp,dr-1,atime+1000);
mTR.set(3,5,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(5,1,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(6,5,dx,dy,dz,dyaw,dp,dr,atime);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 02:09:50
|
Revision: 113
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=113&view=rev
Author: tfoote
Date: 2008-04-16 19:09:56 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
still working using timestamps everywhere
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 01:46:36 UTC (rev 112)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 02:09:56 UTC (rev 113)
@@ -39,35 +39,49 @@
};
-Quaternion3D::Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w):
+Quaternion3D::Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time):
xt(_xt),yt(_yt),zt(_zt),xr(_xr),yr(_yr),zr(_zr),w(_w),
max_storage_time(MAX_STORAGE_TIME),
first(NULL),
last(NULL)
{
+
pthread_mutex_init( &linked_list_mutex, NULL);
Normalize();
return;
};
-Quaternion3D::Quaternion3D(NEWMAT::Matrix matrixIn):
+Quaternion3D::Quaternion3D(NEWMAT::Matrix matrixIn, unsigned long long time):
max_storage_time(MAX_STORAGE_TIME),
first(NULL),
last(NULL)
{
pthread_mutex_init( &linked_list_mutex, NULL);
- fromMatrix(matrixIn);
+ fromMatrix(matrixIn, time);
};
-void Quaternion3D::fromMatrix(NEWMAT::Matrix matIn)
+void Quaternion3D::Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time)
+{xt = _xt; yt = _yt; zt = _zt; xr = _xr; yr = _yr; zr = _zr; w = _w;
+
+ Quaternion3DStorage temp;
+ temp.xt = _xt; temp.yt = _yt; temp.zt = _zt; temp.xr = _xr; temp.yr = _yr; temp.zr = _zr; temp.w = _w; temp.time = time;
+
+ add_value(temp);
+
+} ;
+
+
+void Quaternion3D::fromMatrix(NEWMAT::Matrix matIn, unsigned long long time)
{
// math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+ Quaternion3DStorage temp;
+ temp.time = time;
double * mat = matIn.Store();
//Get the translations
- xt = mat[3];
- yt = mat[7];
- zt = mat[11];
+ temp.xt = mat[3];
+ temp.yt = mat[7];
+ temp.zt = mat[11];
//TODO ASSERT others are zero and one as they should be
@@ -82,10 +96,10 @@
if ( T > 0.00000001 ) //to avoid large distortions!
{
double S = sqrt(T) * 2;
- xr = ( mat[9] - mat[6] ) / S;
- yr = ( mat[2] - mat[8] ) / S;
- zr = ( mat[4] - mat[1] ) / S;
- w = 0.25 * S;
+ temp.xr = ( mat[9] - mat[6] ) / S;
+ temp.yr = ( mat[2] - mat[8] ) / S;
+ temp.zr = ( mat[4] - mat[1] ) / S;
+ temp.w = 0.25 * S;
}
//If the trace of the matrix is equal to zero then identify
// which major diagonal element has the greatest value.
@@ -93,34 +107,36 @@
if ( mat[0] > mat[5] && mat[0] > mat[10] ) {// Column 0:
double S = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
- xr = 0.25 * S;
- yr = (mat[1] + mat[4] ) / S;
- zr = (mat[8] + mat[2] ) / S;
- w = (mat[6] - mat[9] ) / S;
+ temp.xr = 0.25 * S;
+ temp.yr = (mat[1] + mat[4] ) / S;
+ temp.zr = (mat[8] + mat[2] ) / S;
+ temp.w = (mat[6] - mat[9] ) / S;
} else if ( mat[5] > mat[10] ) {// Column 1:
double S = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
- xr = (mat[1] + mat[4] ) / S;
- yr = 0.25 * S;
- zr = (mat[6] + mat[9] ) / S;
- w = (mat[8] - mat[2] ) / S;
+ temp.xr = (mat[1] + mat[4] ) / S;
+ temp.yr = 0.25 * S;
+ temp.zr = (mat[6] + mat[9] ) / S;
+ temp.w = (mat[8] - mat[2] ) / S;
} else {// Column 2:
double S = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
- xr = (mat[8] + mat[2] ) / S;
- yr = (mat[6] + mat[9] ) / S;
- zr = 0.25 * S;
- w = (mat[1] - mat[4] ) / S;
+ temp.xr = (mat[8] + mat[2] ) / S;
+ temp.yr = (mat[6] + mat[9] ) / S;
+ temp.zr = 0.25 * S;
+ temp.w = (mat[1] - mat[4] ) / S;
}
+
+ add_value(temp);
};
-void Quaternion3D::fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
+void Quaternion3D::fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll, unsigned long long time)
{
- fromMatrix(matrixFromEuler(_x,_y,_z,_yaw,_pitch,_roll));
+ fromMatrix(matrixFromEuler(_x,_y,_z,_yaw,_pitch,_roll),time);
};
void Quaternion3D::fromDH(double theta,
- double length, double distance, double alpha)
+ double length, double distance, double alpha, unsigned long long time)
{
- fromMatrix(matrixFromDH(theta, length, distance, alpha));
+ fromMatrix(matrixFromDH(theta, length, distance, alpha),time);
};
@@ -211,7 +227,7 @@
};
-NEWMAT::Matrix Quaternion3D::asMatrix()
+NEWMAT::Matrix Quaternion3D::asMatrix(unsigned long long time)
{
NEWMAT::Matrix outMat(4,4);
@@ -247,9 +263,9 @@
};
-void Quaternion3D::printMatrix()
+void Quaternion3D::printMatrix(unsigned long long time)
{
- std::cout << asMatrix();
+ std::cout << asMatrix(time);
};
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 01:46:36 UTC (rev 112)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 02:09:56 UTC (rev 113)
@@ -62,21 +62,20 @@
/** Constructors **/
// Standard constructor which takes in 7 doubles
- Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
+ Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
// Constructor from Matrix
- Quaternion3D(NEWMAT::Matrix matrixIn);
+ Quaternion3D(NEWMAT::Matrix matrixIn, unsigned long long time);
/** Mutators **/
// Set the values manually
- inline void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w)
- {xt = _xt; yt = _yt; zt = _zt; xr = _xr; yr = _yr; zr = _zr; w = _w;} ;
+ void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
//Set the values from a matrix
- void fromMatrix(NEWMAT::Matrix matIn);
+ void fromMatrix(NEWMAT::Matrix matIn, unsigned long long time);
// Set the values using Euler angles
- void fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
+ void fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll, unsigned long long time);
// Set the values using DH Parameters
- void fromDH(double theta, double length, double distance, double alpha);
+ void fromDH(double theta, double length, double distance, double alpha, unsigned long long time);
/**** Utility Functions ****/
@@ -92,11 +91,13 @@
/** Accessors **/
// Return a Matrix
- NEWMAT::Matrix asMatrix();
+ NEWMAT::Matrix asMatrix(unsigned long long time);
//Print as a matrix
- void printMatrix();
+ void printMatrix(unsigned long long time);
+ // this is a function to return the current time in microseconds from the beginning of 1970
+ static unsigned long long Qgettime(void);
private:
//Quaternion Storage
@@ -114,8 +115,6 @@
bool getValue(Quaternion3DStorage& buff, unsigned long long time, long long &time_diff);
void add_value(Quaternion3DStorage);//todo fixme finish implementing this
- // this is a function to return the current time in microseconds from the beginning of 1970
- unsigned long long Qgettime(void);
// insert a node into the sorted linked list
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-17 01:46:36 UTC (rev 112)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-17 02:09:56 UTC (rev 113)
@@ -34,38 +34,38 @@
RefFrame::RefFrame() :
parent(0),
- myQuat(0,0,0,0,0,0,1)
+ myQuat(0,0,0,0,0,0,1,111110000)
{
return;
}
/* Quaternion 3D version */
-void RefFrame::setParamsQuaternion3D(double a,double b,double c,double d,double e,double f, double g)
+void RefFrame::setParamsQuaternion3D(double a,double b,double c,double d,double e,double f, double g, unsigned long long time)
{
- myQuat.Set(a,b,c,d,e,f,g);
+ myQuat.Set(a,b,c,d,e,f,g,time);
};
/* Six DOF version */
-void RefFrame::setParamsEulers(double a,double b,double c,double d,double e,double f)
+void RefFrame::setParamsEulers(double a,double b,double c,double d,double e,double f, unsigned long long time)
{
- myQuat.fromEuler(a,b,c,d,e,f);
+ myQuat.fromEuler(a,b,c,d,e,f,time) ;
}
/* DH Params version */
-void RefFrame::setParamsDH(double a,double b,double c,double d)
+void RefFrame::setParamsDH(double a,double b,double c,double d, unsigned long long time)
{
- myQuat.fromDH(a,b,c,d);
+ myQuat.fromDH(a,b,c,d,time);
}
-NEWMAT::Matrix RefFrame::getMatrix()
+NEWMAT::Matrix RefFrame::getMatrix(unsigned long long time)
{
- return myQuat.asMatrix();
+ return myQuat.asMatrix(time);
}
-NEWMAT::Matrix RefFrame::getInverseMatrix()
+NEWMAT::Matrix RefFrame::getInverseMatrix(unsigned long long time)
{
- return myQuat.asMatrix().i();
+ return myQuat.asMatrix(time).i();
};
@@ -80,7 +80,7 @@
}
-void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f)
+void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f, unsigned long long time)
{
if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES || frameID == NO_PARENT || frameID == ROOT_FRAME)
throw InvalidFrame;
@@ -89,15 +89,15 @@
frames[frameID] = new RefFrame();
getFrame(frameID)->setParent(parentID);
- getFrame(frameID)->setParamsEulers(a,b,c,d,e,f);
+ getFrame(frameID)->setParamsEulers(a,b,c,d,e,f,time);
}
-NEWMAT::Matrix TransformReference::get(unsigned int target_frame, unsigned int source_frame)
+NEWMAT::Matrix TransformReference::get(unsigned int target_frame, unsigned int source_frame, unsigned long long time)
{
NEWMAT::Matrix myMat(4,4);
TransformLists lists = lookUpList(target_frame, source_frame);
- myMat = computeTransformFromList(lists);
+ myMat = computeTransformFromList(lists,time);
// std::cout << myMat;
return myMat;
}
@@ -175,7 +175,7 @@
}
-NEWMAT::Matrix TransformReference::computeTransformFromList(TransformLists lists)
+NEWMAT::Matrix TransformReference::computeTransformFromList(TransformLists lists, unsigned long long time)
{
NEWMAT::Matrix retMat(4,4);
retMat << 1 << 0 << 0 << 0
@@ -185,13 +185,13 @@
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
- retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix();
+ retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix(time);
// std::cout <<"Multiplying by " << std::endl << frames[lists.inverseTransforms[i]].getInverseMatrix() << std::endl;
//std::cout <<"Result "<<std::endl << retMat << std::endl;
}
for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
{
- retMat *= getFrame(lists.forwardTransforms[lists.forwardTransforms.size() -1 - i])->getMatrix(); //Do this list backwards for it was generated traveling the wrong way
+ retMat *= getFrame(lists.forwardTransforms[lists.forwardTransforms.size() -1 - i])->getMatrix(time); //Do this list backwards for it was generated traveling the wrong way
// std::cout <<"Multiplying by "<<std::endl << frames[lists.forwardTransforms[i]].getMatrix() << std::endl;
//std::cout <<"Result "<<std::endl << retMat << std::endl;
}
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-17 01:46:36 UTC (rev 112)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-17 02:09:56 UTC (rev 113)
@@ -48,9 +48,9 @@
RefFrame();
/* Set the parameters for this frame */
- void setParamsQuaternion3D(double, double, double, double, double, double, double);
- void setParamsEulers(double, double, double, double, double, double);
- void setParamsDH(double, double, double, double);
+ void setParamsQuaternion3D(double, double, double, double, double, double, double, unsigned long long time);
+ void setParamsEulers(double, double, double, double, double, double, unsigned long long time);
+ void setParamsDH(double, double, double, double, unsigned long long time);
/* Get the parent node */
inline unsigned int getParent(){return parent;};
@@ -59,10 +59,10 @@
inline void setParent(unsigned int parentID){parent = parentID;};
/* Generate and return the transform associated with gettingn into this frame */
- NEWMAT::Matrix getMatrix();
+ NEWMAT::Matrix getMatrix(unsigned long long time);
/* Generate and return the transform associated with getting out of this frame. */
- NEWMAT::Matrix getInverseMatrix();
+ NEWMAT::Matrix getInverseMatrix(unsigned long long time);
private:
@@ -89,13 +89,13 @@
/********** Mutators **************/
/* Set a new frame or update an old one. */
- void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double);
+ void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double,unsigned long long time);
// Possible exceptions TransformReference::LookupException
/*********** Accessors *************/
/* Get the transform between two frames by frame ID. */
- NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame);
+ NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame, unsigned long long time);
// Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
// TransformReference::MaxDepthException
@@ -158,7 +158,7 @@
TransformLists lookUpList(unsigned int target_frame, unsigned int source_frame);
/* Compute the transform based on the list of frames */
- NEWMAT::Matrix computeTransformFromList(TransformLists list);
+ NEWMAT::Matrix computeTransformFromList(TransformLists list, unsigned long long time);
};
#endif //LIBTF_HH
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-17 01:46:36 UTC (rev 112)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-17 02:09:56 UTC (rev 113)
@@ -14,15 +14,17 @@
dx = dy= dz = 0;
dyaw = dp = dr = .1;
+ unsigned long long atime = Quaternion3D::Qgettime();
+
//Fill in some transforms
- mTR.set(10,2,1,1,1,dyaw,dp,dr);
- mTR.set(2,3,1,1,1,dyaw,dp,dr);
- mTR.set(3,5,dx,dy,dz,dyaw,dp,dr);
- mTR.set(5,1,dx,dy,dz,dyaw,dp,dr);
- mTR.set(6,5,dx,dy,dz,dyaw,dp,dr);
- mTR.set(7,6,1,1,1,dyaw,dp,dr);
- mTR.set(8,7,1,1,1,dyaw,dp,dr);
+ mTR.set(10,2,1,1,1,dyaw,dp,dr,atime);
+ mTR.set(2,3,1,1,1,dyaw,dp,dr,atime);
+ mTR.set(3,5,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.set(5,1,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.set(6,5,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.set(7,6,1,1,1,dyaw,dp,dr,atime);
+ mTR.set(8,7,1,1,1,dyaw,dp,dr,atime);
//Demonstrate InvalidFrame LookupException
@@ -44,14 +46,14 @@
//See the resultant transform
std::cout <<"Calling get(10,8)"<<std::endl;
// NEWMAT::Matrix mat = mTR.get(1,1);
- NEWMAT::Matrix mat = mTR.get(10,8);
+ NEWMAT::Matrix mat = mTR.get(10,8,atime);
- std::cout << "Result of get(10,8):" << std::endl << mat<< std::endl;
+ std::cout << "Result of get(10,8,atime):" << std::endl << mat<< std::endl;
//Break the graph, making it loop and demonstrate catching MaxDepthException
- mTR.set(6,7,dx,dy,dz,dyaw,dp,dr);
+ mTR.set(6,7,dx,dy,dz,dyaw,dp,dr,atime);
try {
mTR.view(10,8);
@@ -62,7 +64,7 @@
}
//Break the graph, making it disconnected, and demonstrate catching ConnectivityException
- mTR.set(6,0,dx,dy,dz,dyaw,dp,dr);
+ mTR.set(6,0,dx,dy,dz,dyaw,dp,dr,atime);
try {
mTR.view(10,8);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 01:46:29
|
Revision: 112
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=112&view=rev
Author: tfoote
Date: 2008-04-16 18:46:36 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
adding interpolation linked list and methods
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 01:46:21 UTC (rev 111)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 01:46:36 UTC (rev 112)
@@ -40,14 +40,22 @@
Quaternion3D::Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w):
- xt(_xt),yt(_yt),zt(_zt),xr(_xr),yr(_yr),zr(_zr),w(_w)
+ xt(_xt),yt(_yt),zt(_zt),xr(_xr),yr(_yr),zr(_zr),w(_w),
+ max_storage_time(MAX_STORAGE_TIME),
+ first(NULL),
+ last(NULL)
{
+ pthread_mutex_init( &linked_list_mutex, NULL);
Normalize();
return;
};
-Quaternion3D::Quaternion3D(NEWMAT::Matrix matrixIn)
+Quaternion3D::Quaternion3D(NEWMAT::Matrix matrixIn):
+ max_storage_time(MAX_STORAGE_TIME),
+ first(NULL),
+ last(NULL)
{
+ pthread_mutex_init( &linked_list_mutex, NULL);
fromMatrix(matrixIn);
};
@@ -244,3 +252,227 @@
std::cout << asMatrix();
};
+
+
+unsigned long long Quaternion3D::Qgettime()
+{
+ timeval temp_time_struct;
+ gettimeofday(&temp_time_struct,NULL);
+ return temp_time_struct.tv_sec * 1000000ULL + (unsigned long long)temp_time_struct.tv_usec;
+}
+
+
+bool Quaternion3D::getValue(Quaternion3DStorage& buff, unsigned long long time, long long &time_diff)
+{
+ Quaternion3DStorage p_temp_1;
+ Quaternion3DStorage p_temp_2;
+ // long long temp_time;
+ int num_nodes;
+
+ bool retval = false;
+
+ pthread_mutex_lock(&linked_list_mutex);
+ num_nodes = findClosest(p_temp_1,p_temp_2, time, time_diff);
+
+ if (num_nodes == 0)
+ retval= false;
+ else if (num_nodes == 1)
+ {
+ memcpy(&buff, &p_temp_1, sizeof(Quaternion3DStorage));
+ retval = true;
+ }
+ else
+ {
+ interpolate(p_temp_1, p_temp_2, time, buff);
+ retval = true;
+ }
+
+ pthread_mutex_unlock(&linked_list_mutex);
+
+
+ return retval;
+
+};
+
+void Quaternion3D::add_value(Quaternion3DStorage dataIn)
+{
+ Quaternion3DStorage temp;
+ //cout << "started thread" << endl;
+
+ pthread_mutex_lock(&linked_list_mutex);
+ insertNode(dataIn);
+ pruneList();
+ pthread_mutex_unlock(&linked_list_mutex);
+
+
+};
+
+
+void Quaternion3D::insertNode(Quaternion3DStorage new_val)
+{
+ data_LL* p_current;
+ data_LL* p_old;
+
+ // cout << "Inserting Node" << endl;
+
+ //Base case empty list
+ if (first == NULL)
+ {
+ cout << "Base case" << endl;
+ first = new data_LL;
+ first->data = new_val;
+ first->next = NULL;
+ first->previous = NULL;
+ last = first;
+ }
+ else
+ {
+ //Increment through until at the end of the list or in the right spot
+ p_current = first;
+ while (p_current != NULL && first->data.time > new_val.time)
+ {
+ //cout << "passed beyond " << p_current->data.time << endl;
+ p_current = p_current->next;
+ }
+
+ //THis means we hit the end of the list so just append the node
+ if (p_current == NULL)
+ {
+ //cout << "Appending node to the end" << endl;
+ p_current = new data_LL;
+ p_current->data = new_val;
+ p_current->previous = last;
+ p_current->next = NULL;
+
+ last = p_current;
+ }
+ else
+ {
+
+ // cout << "Found a place to put data into the list" << endl;
+
+ // Insert the new node
+ // Record where the old first node was
+ p_old = p_current;
+ //Fill in the new node
+ p_current = new data_LL;
+ p_current->data = new_val;
+ p_current->next = p_old;
+ p_current->previous = p_old->previous;
+
+ //point the old to the new
+ p_old->previous = p_current;
+
+ //If at the top of the list make sure we're not
+ if (p_current->previous == NULL)
+ first = p_current;
+ }
+
+
+
+ }
+};
+
+void Quaternion3D::pruneList()
+{
+ unsigned long long current_time = Qgettime();
+ data_LL* p_current = last;
+
+ // cout << "Pruning List" << endl;
+
+ //Empty Set
+ if (last == NULL) return;
+
+ //While time stamps too old
+ while (p_current->data.time + max_storage_time < current_time)
+ {
+ // cout << "Age of node " << (double)(-p_current->data.time + current_time)/1000000.0 << endl;
+ // Make sure that there's at least two elements in the list
+ if (p_current->previous != NULL)
+ {
+ if (p_current->previous->previous != NULL)
+ {
+ // Remove the last node
+ p_current->previous->next = NULL;
+ last = p_current->previous;
+ delete p_current;
+ p_current = last;
+ // cout << " Pruning Node" << endl;
+ }
+ else
+ break;
+ }
+ else
+ break;
+
+ }
+
+};
+
+
+
+int Quaternion3D::findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, unsigned long long target_time, long long &time_diff)
+{
+
+ unsigned long long current_time = Qgettime();
+ data_LL* p_current = first;
+
+
+ // Base case no list
+ if (first == NULL)
+ {
+ return 0;
+ }
+
+ //Case one element list
+ else if (first->next == NULL)
+ {
+ one = first->data;
+ time_diff = current_time - first->data.time;
+ return 1;
+ }
+
+ else
+ {
+ //Two or more elements
+ //Find the one that just exceeds the time or hits the end
+ //and then take the previous one
+ p_current = first->next; //Start on the 2nd element so if we fail we fall back to the first one
+ while (p_current->next != NULL && p_current->data.time > target_time)
+ {
+ p_current = p_current->next;
+ }
+
+ one = p_current->data;
+ two = p_current->previous->data;
+
+ //FIXME this should be the min distance not just one random one.
+ time_diff = target_time - two.time;
+ return 2;
+ }
+};
+
+
+void Quaternion3D::interpolate(Quaternion3DStorage &one, Quaternion3DStorage &two,unsigned long long target_time, Quaternion3DStorage& output)
+{
+ //fixme do a proper interpolatioln here!!!
+ output.time = target_time;
+
+ output.xt = interpolateDouble(one.xt, one.time, two.xt, two.time, target_time);
+ output.yt = interpolateDouble(one.yt, one.time, two.yt, two.time, target_time);
+ output.zt = interpolateDouble(one.zt, one.time, two.zt, two.time, target_time);
+ output.xr = interpolateDouble(one.xr, one.time, two.xr, two.time, target_time);
+ output.yr = interpolateDouble(one.yr, one.time, two.yr, two.time, target_time);
+ output.zr = interpolateDouble(one.zr, one.time, two.zr, two.time, target_time);
+ output.w = interpolateDouble(one.w, one.time, two.w, two.time, target_time);
+
+};
+
+double Quaternion3D::interpolateDouble(double first, unsigned long long first_time, double second, unsigned long long second_time, unsigned long long target_time)
+{
+ if ( first_time == second_time ) {
+ return first;
+ } else {
+ return first + (second-first)* (double)((target_time - first_time)/(second_time - first_time));
+ }
+};
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 01:46:21 UTC (rev 111)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 01:46:36 UTC (rev 112)
@@ -37,6 +37,8 @@
#include <newmat/newmat.h>
#include <newmat/newmatio.h>
#include <math.h>
+#include <pthread.h>
+#include <sys/time.h>
class Euler3D {
public:
@@ -45,13 +47,19 @@
//Storage
double x,y,z,yaw,pitch,roll;
+};
-
-};
+class Quaternion3D {
-class Quaternion3D {
public:
+ // Storage
+ struct Quaternion3DStorage
+ {
+ double xt, yt, zt, xr, yr, zr, w;
+ unsigned long long time;
+ };
+
/** Constructors **/
// Standard constructor which takes in 7 doubles
Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
@@ -62,9 +70,12 @@
// Set the values manually
inline void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w)
{xt = _xt; yt = _yt; zt = _zt; xr = _xr; yr = _yr; zr = _zr; w = _w;} ;
+
//Set the values from a matrix
void fromMatrix(NEWMAT::Matrix matIn);
+ // Set the values using Euler angles
void fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
+ // Set the values using DH Parameters
void fromDH(double theta, double length, double distance, double alpha);
@@ -82,7 +93,7 @@
/** Accessors **/
// Return a Matrix
NEWMAT::Matrix asMatrix();
-
+
//Print as a matrix
void printMatrix();
@@ -91,8 +102,51 @@
//Quaternion Storage
double xt,yt,zt,xr,yr,zr,w;
+ /**** Linked List stuff ****/
+ static const unsigned int MAX_STORAGE_TIME = 100000000; // max of 100 seconds storage
+
+ struct data_LL{
+ Quaternion3DStorage data;
+ data_LL * next;
+ data_LL * previous;
+ };
+ bool getValue(Quaternion3DStorage& buff, unsigned long long time, long long &time_diff);
+ void add_value(Quaternion3DStorage);//todo fixme finish implementing this
+ // this is a function to return the current time in microseconds from the beginning of 1970
+ unsigned long long Qgettime(void);
+
+
+ // insert a node into the sorted linked list
+ void insertNode(Quaternion3DStorage);
+ // prune data older than max_storage_time from the list
+ void pruneList();
+
+ //Find the closest two points in the list
+ //Return the distance to the closest one
+ int findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, unsigned long long target_time, long long &time_diff);
+
+ //Interpolate between two nodes and return the interpolated value
+ // This must always take two valid points!!!!!!
+ // Only Cpose version implemented
+ void interpolate(Quaternion3DStorage &one, Quaternion3DStorage &two, unsigned long long target_time, Quaternion3DStorage& output);
+
+ //Used by interpolate to interpolate between double values
+ double interpolateDouble(double, unsigned long long, double, unsigned long long, unsigned long long);
+
+ //How long to cache incoming values
+ unsigned long long max_storage_time;
+
+ //A mutex to prevent linked list collisions
+ pthread_mutex_t linked_list_mutex;
+
+ //Pointers for the start and end of a sorted linked list.
+ data_LL* first;
+ data_LL* last;
+
+
+
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 01:46:17
|
Revision: 111
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=111&view=rev
Author: tfoote
Date: 2008-04-16 18:46:21 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
adding a set quaternion method
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-17 00:03:10 UTC (rev 110)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-17 01:46:21 UTC (rev 111)
@@ -39,6 +39,12 @@
return;
}
+/* Quaternion 3D version */
+void RefFrame::setParamsQuaternion3D(double a,double b,double c,double d,double e,double f, double g)
+{
+ myQuat.Set(a,b,c,d,e,f,g);
+};
+
/* Six DOF version */
void RefFrame::setParamsEulers(double a,double b,double c,double d,double e,double f)
{
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-17 00:03:10 UTC (rev 110)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-17 01:46:21 UTC (rev 111)
@@ -48,6 +48,7 @@
RefFrame();
/* Set the parameters for this frame */
+ void setParamsQuaternion3D(double, double, double, double, double, double, double);
void setParamsEulers(double, double, double, double, double, double);
void setParamsDH(double, double, double, double);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 00:03:02
|
Revision: 110
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=110&view=rev
Author: tfoote
Date: 2008-04-16 17:03:10 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
stripping out stuff that has been replaced
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-16 23:55:11 UTC (rev 109)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-17 00:03:10 UTC (rev 110)
@@ -34,70 +34,33 @@
RefFrame::RefFrame() :
parent(0),
- myQuat(0,0,0,0,0,0,0)
+ myQuat(0,0,0,0,0,0,1)
{
return;
}
/* Six DOF version */
-void RefFrame::setParamsXYZYPR(double a,double b,double c,double d,double e,double f)
+void RefFrame::setParamsEulers(double a,double b,double c,double d,double e,double f)
{
- paramType = SIXDOF;
- params[0]=a;
- params[1]=b;
- params[2]=c;
- params[3]=d;
- params[4]=e;
- params[5]=f;
-
myQuat.fromEuler(a,b,c,d,e,f);
}
/* DH Params version */
void RefFrame::setParamsDH(double a,double b,double c,double d)
{
- paramType = DH;
- params[0]=a;
- params[1]=b;
- params[2]=c;
- params[3]=d;
+ myQuat.fromDH(a,b,c,d);
}
NEWMAT::Matrix RefFrame::getMatrix()
{
- NEWMAT::Matrix mMat(4,4);
- switch ( paramType)
- {
- case SIXDOF:
- //temp insert
- return myQuat.asMatrix();
- // fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
- break;
- case DH:
- fill_transformation_matrix_from_dh(mMat,params[0],params[1],params[2],params[3]);
- }
- return mMat;
+ return myQuat.asMatrix();
}
NEWMAT::Matrix RefFrame::getInverseMatrix()
{
- NEWMAT::Matrix mMat(4,4);
- switch(paramType)
- {
- case SIXDOF:
- return myQuat.asMatrix().i();
- fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
- //todo create a fill_inverse_transform_matrix call to be more efficient
- return mMat.i();
- break;
- case DH:
- fill_transformation_matrix_from_dh(mMat,params[0],params[1],params[2],params[3]);
- //todo create a fill_inverse_transform_matrix call to be more efficient
- return mMat.i();
- break;
- }
-}
+ return myQuat.asMatrix().i();
+};
TransformReference::TransformReference()
@@ -120,7 +83,7 @@
frames[frameID] = new RefFrame();
getFrame(frameID)->setParent(parentID);
- getFrame(frameID)->setParamsXYZYPR(a,b,c,d,e,f);
+ getFrame(frameID)->setParamsEulers(a,b,c,d,e,f);
}
@@ -134,81 +97,6 @@
}
-
-
-bool RefFrame::fill_transformation_matrix(NEWMAT::Matrix & matrix, double ax,
- double ay, double az, double yaw,
- double pitch, double roll)
-{
- double ca = cos(yaw);
- double sa = sin(yaw);
- double cb = cos(pitch);
- double sb = sin(pitch);
- double cg = cos(roll);
- double sg = sin(roll);
- double sbsg = sb*sg;
- double sbcg = sb*cg;
-
-
- double* matrix_pointer = matrix.Store();
- if (matrix.Storage() != 16)
- return false;
-
- matrix_pointer[0] = ca*cb;
- matrix_pointer[1] = (ca*sbsg)-(sa*cg);
- matrix_pointer[2] = (ca*sbcg)+(sa*sg);
- matrix_pointer[3] = ax;
- matrix_pointer[4] = sa*cb;
- matrix_pointer[5] = (sa*sbsg)+(ca*cg);
- matrix_pointer[6] = (sa*sbcg)-(ca*sg);
- matrix_pointer[7] = ay;
- matrix_pointer[8] = -sb;
- matrix_pointer[9] = cb*sg;
- matrix_pointer[10] = cb*cg;
- matrix_pointer[11] = az;
- matrix_pointer[12] = 0.0;
- matrix_pointer[13] = 0.0;
- matrix_pointer[14] = 0.0;
- matrix_pointer[15] = 1.0;
-
- return true;
-};
-
-// Math from http://en.wikipedia.org/wiki/Robotics_conventions
-bool RefFrame::fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
- double length, double distance, double alpha)
-{
- double ca = cos(alpha);
- double sa = sin(alpha);
- double ct = cos(theta);
- double st = sin(theta);
-
- double* matrix_pointer = matrix.Store();
- if (matrix.Storage() != 16)
- return false;
-
- matrix_pointer[0] = ct;
- matrix_pointer[1] = -st*ca;
- matrix_pointer[2] = st*sa;
- matrix_pointer[3] = distance * ct;
- matrix_pointer[4] = st;
- matrix_pointer[5] = ct*ca;
- matrix_pointer[6] = -ct*sa;
- matrix_pointer[7] = distance*st;
- matrix_pointer[8] = 0;
- matrix_pointer[9] = sa;
- matrix_pointer[10] = ca;
- matrix_pointer[11] = length;
- matrix_pointer[12] = 0.0;
- matrix_pointer[13] = 0.0;
- matrix_pointer[14] = 0.0;
- matrix_pointer[15] = 1.0;
-
- return true;
-};
-
-
-
TransformReference::TransformLists TransformReference::lookUpList(unsigned int target_frame, unsigned int source_frame)
{
TransformLists mTfLs;
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-16 23:55:11 UTC (rev 109)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-17 00:03:10 UTC (rev 110)
@@ -44,15 +44,11 @@
{
public:
- /* The type of frame
- * This determines how many different parameters to expect to be updated, versus fixed */
- enum ParamTypeEnum {SIXDOF, DH };//how to tell which mode it's in //todo add this process i'm going to start with 6dof only
-
/* Constructor */
RefFrame();
/* Set the parameters for this frame */
- void setParamsXYZYPR(double, double, double, double, double, double);
+ void setParamsEulers(double, double, double, double, double, double);
void setParamsDH(double, double, double, double);
/* Get the parent node */
@@ -69,26 +65,12 @@
private:
- /* Storage of the parametsrs
- * NOTE: Depending on if this is a 6dof or DH parameter the storage will be different. */
- double params[6];
-
+ /* Storage of the parametsrs */
Quaternion3D myQuat;
- /* A helper function to build a homogeneous transform based on 6dof parameters */
- static bool fill_transformation_matrix(NEWMAT::Matrix& matrix_pointer, double ax,
- double ay, double az, double yaw,
- double pitch, double roll);
-
- /* A helper function to build a homogeneous transform based on DH parameters */
- static bool fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
- double length, double distance, double alpha);
-
/* Storage of the parent */
unsigned int parent;
- ParamTypeEnum paramType;
-
};
class TransformReference
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-16 23:55:03
|
Revision: 109
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=109&view=rev
Author: tfoote
Date: 2008-04-16 16:55:11 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
adding licensing
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/testQuat.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 23:53:39 UTC (rev 108)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 23:55:11 UTC (rev 109)
@@ -1,3 +1,35 @@
+// 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 "Quaternion3D.hh"
Euler3D::Euler3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll) :
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 23:53:39 UTC (rev 108)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 23:55:11 UTC (rev 109)
@@ -1,3 +1,35 @@
+// 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 QUATERNION3D_HH
#define QUATERNION3D_HH
Modified: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc 2008-04-16 23:53:39 UTC (rev 108)
+++ pkg/trunk/libTF/simple/testQuat.cc 2008-04-16 23:55:11 UTC (rev 109)
@@ -1,3 +1,35 @@
+// 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 "Quaternion3D.hh"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-16 23:53:33
|
Revision: 108
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=108&view=rev
Author: tfoote
Date: 2008-04-16 16:53:39 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
adding fromDH Params for Quaternion, and starting to switch to the new class
Modified Paths:
--------------
pkg/trunk/libTF/simple/Makefile
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/Makefile
===================================================================
--- pkg/trunk/libTF/simple/Makefile 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/Makefile 2008-04-16 23:53:39 UTC (rev 108)
@@ -10,7 +10,7 @@
quat: testQuat.cc Quaternion3D.o
g++ -g -O2 $^ -o quat -lnewmat
-test: main.cc libTF.o
+test: main.cc libTF.o Quaternion3D.o
g++ -g -O2 $^ -o test -lnewmat
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 23:53:39 UTC (rev 108)
@@ -14,8 +14,13 @@
return;
};
-Quaternion3D::Quaternion3D(NEWMAT::Matrix matIn)
+Quaternion3D::Quaternion3D(NEWMAT::Matrix matrixIn)
{
+ fromMatrix(matrixIn);
+};
+
+void Quaternion3D::fromMatrix(NEWMAT::Matrix matIn)
+{
// math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
double * mat = matIn.Store();
@@ -67,7 +72,90 @@
}
};
+void Quaternion3D::fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
+{
+ fromMatrix(matrixFromEuler(_x,_y,_z,_yaw,_pitch,_roll));
+};
+void Quaternion3D::fromDH(double theta,
+ double length, double distance, double alpha)
+{
+ fromMatrix(matrixFromDH(theta, length, distance, alpha));
+};
+
+
+NEWMAT::Matrix Quaternion3D::matrixFromEuler(double ax,
+ double ay, double az, double yaw,
+ double pitch, double roll)
+{
+ NEWMAT::Matrix matrix(4,4);
+ double ca = cos(yaw);
+ double sa = sin(yaw);
+ double cb = cos(pitch);
+ double sb = sin(pitch);
+ double cg = cos(roll);
+ double sg = sin(roll);
+ double sbsg = sb*sg;
+ double sbcg = sb*cg;
+
+
+ double* matrix_pointer = matrix.Store();
+
+ matrix_pointer[0] = ca*cb;
+ matrix_pointer[1] = (ca*sbsg)-(sa*cg);
+ matrix_pointer[2] = (ca*sbcg)+(sa*sg);
+ matrix_pointer[3] = ax;
+ matrix_pointer[4] = sa*cb;
+ matrix_pointer[5] = (sa*sbsg)+(ca*cg);
+ matrix_pointer[6] = (sa*sbcg)-(ca*sg);
+ matrix_pointer[7] = ay;
+ matrix_pointer[8] = -sb;
+ matrix_pointer[9] = cb*sg;
+ matrix_pointer[10] = cb*cg;
+ matrix_pointer[11] = az;
+ matrix_pointer[12] = 0.0;
+ matrix_pointer[13] = 0.0;
+ matrix_pointer[14] = 0.0;
+ matrix_pointer[15] = 1.0;
+
+ return matrix;
+};
+
+
+// Math from http://en.wikipedia.org/wiki/Robotics_conventions
+NEWMAT::Matrix Quaternion3D::matrixFromDH(double theta,
+ double length, double distance, double alpha)
+{
+ NEWMAT::Matrix matrix(4,4);
+
+ double ca = cos(alpha);
+ double sa = sin(alpha);
+ double ct = cos(theta);
+ double st = sin(theta);
+
+ double* matrix_pointer = matrix.Store();
+
+ matrix_pointer[0] = ct;
+ matrix_pointer[1] = -st*ca;
+ matrix_pointer[2] = st*sa;
+ matrix_pointer[3] = distance * ct;
+ matrix_pointer[4] = st;
+ matrix_pointer[5] = ct*ca;
+ matrix_pointer[6] = -ct*sa;
+ matrix_pointer[7] = distance*st;
+ matrix_pointer[8] = 0;
+ matrix_pointer[9] = sa;
+ matrix_pointer[10] = ca;
+ matrix_pointer[11] = length;
+ matrix_pointer[12] = 0.0;
+ matrix_pointer[13] = 0.0;
+ matrix_pointer[14] = 0.0;
+ matrix_pointer[15] = 1.0;
+
+ return matrix;
+};
+
+
void Quaternion3D::Normalize()
{
double mag = getMagnitude();
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 23:53:39 UTC (rev 108)
@@ -20,17 +20,38 @@
class Quaternion3D {
public:
- //Constructor
+ /** Constructors **/
+ // Standard constructor which takes in 7 doubles
Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
- Quaternion3D(NEWMAT::Matrix matIn);
-
+ // Constructor from Matrix
+ Quaternion3D(NEWMAT::Matrix matrixIn);
+
+ /** Mutators **/
+ // Set the values manually
inline void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w)
{xt = _xt; yt = _yt; zt = _zt; xr = _xr; yr = _yr; zr = _zr; w = _w;} ;
+ //Set the values from a matrix
+ void fromMatrix(NEWMAT::Matrix matIn);
+ void fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
+ void fromDH(double theta, double length, double distance, double alpha);
+
+ /**** Utility Functions ****/
+ static NEWMAT::Matrix matrixFromDH(double theta,
+ double length, double distance, double alpha);
+ static NEWMAT::Matrix matrixFromEuler(double ax,
+ double ay, double az, double yaw,
+ double pitch, double roll);
+
+ // Utility functions to normalize and get magnitude.
void Normalize();
double getMagnitude();
+ /** Accessors **/
+ // Return a Matrix
NEWMAT::Matrix asMatrix();
+
+ //Print as a matrix
void printMatrix();
@@ -39,6 +60,7 @@
double xt,yt,zt,xr,yr,zr,w;
+
};
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-16 23:53:39 UTC (rev 108)
@@ -33,7 +33,8 @@
#include "libTF.hh"
RefFrame::RefFrame() :
- parent(0)
+ parent(0),
+ myQuat(0,0,0,0,0,0,0)
{
return;
}
@@ -48,7 +49,8 @@
params[3]=d;
params[4]=e;
params[5]=f;
-
+
+ myQuat.fromEuler(a,b,c,d,e,f);
}
/* DH Params version */
@@ -68,7 +70,9 @@
switch ( paramType)
{
case SIXDOF:
- fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
+ //temp insert
+ return myQuat.asMatrix();
+ // fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
break;
case DH:
fill_transformation_matrix_from_dh(mMat,params[0],params[1],params[2],params[3]);
@@ -82,6 +86,7 @@
switch(paramType)
{
case SIXDOF:
+ return myQuat.asMatrix().i();
fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
//todo create a fill_inverse_transform_matrix call to be more efficient
return mMat.i();
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-16 23:53:39 UTC (rev 108)
@@ -38,6 +38,7 @@
#include <newmat/newmatio.h>
#include <math.h>
#include <vector>
+#include "Quaternion3D.hh"
class RefFrame
{
@@ -72,6 +73,8 @@
* NOTE: Depending on if this is a 6dof or DH parameter the storage will be different. */
double params[6];
+ Quaternion3D myQuat;
+
/* A helper function to build a homogeneous transform based on 6dof parameters */
static bool fill_transformation_matrix(NEWMAT::Matrix& matrix_pointer, double ax,
double ay, double az, double yaw,
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-16 23:53:39 UTC (rev 108)
@@ -1,3 +1,4 @@
+#include "Quaternion3D.hh"
#include "libTF.hh"
#include <time.h>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-16 22:39:40
|
Revision: 107
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=107&view=rev
Author: tfoote
Date: 2008-04-16 15:39:37 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
adding an import from homogeneous method
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/testQuat.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 22:16:17 UTC (rev 106)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 22:39:37 UTC (rev 107)
@@ -14,7 +14,60 @@
return;
};
+Quaternion3D::Quaternion3D(NEWMAT::Matrix matIn)
+{
+ // math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+ double * mat = matIn.Store();
+ //Get the translations
+ xt = mat[3];
+ yt = mat[7];
+ zt = mat[11];
+
+ //TODO ASSERT others are zero and one as they should be
+
+
+ double T = 1 + mat[0] + mat[5] + mat[10];
+
+
+ // If the trace of the matrix is greater than zero, then
+ // perform an "instant" calculation.
+ // Important note wrt. rouning errors:
+
+ if ( T > 0.00000001 ) //to avoid large distortions!
+ {
+ double S = sqrt(T) * 2;
+ xr = ( mat[9] - mat[6] ) / S;
+ yr = ( mat[2] - mat[8] ) / S;
+ zr = ( mat[4] - mat[1] ) / S;
+ w = 0.25 * S;
+ }
+ //If the trace of the matrix is equal to zero then identify
+ // which major diagonal element has the greatest value.
+ // Depending on this, calculate the following:
+
+ if ( mat[0] > mat[5] && mat[0] > mat[10] ) {// Column 0:
+ double S = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
+ xr = 0.25 * S;
+ yr = (mat[1] + mat[4] ) / S;
+ zr = (mat[8] + mat[2] ) / S;
+ w = (mat[6] - mat[9] ) / S;
+ } else if ( mat[5] > mat[10] ) {// Column 1:
+ double S = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
+ xr = (mat[1] + mat[4] ) / S;
+ yr = 0.25 * S;
+ zr = (mat[6] + mat[9] ) / S;
+ w = (mat[8] - mat[2] ) / S;
+ } else {// Column 2:
+ double S = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
+ xr = (mat[8] + mat[2] ) / S;
+ yr = (mat[6] + mat[9] ) / S;
+ zr = 0.25 * S;
+ w = (mat[1] - mat[4] ) / S;
+ }
+};
+
+
void Quaternion3D::Normalize()
{
double mag = getMagnitude();
@@ -36,7 +89,7 @@
double * mat = outMat.Store();
- // math from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+ // math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
double xx = xr * xr;
double xy = xr * yr;
double xz = xr * zr;
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 22:16:17 UTC (rev 106)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 22:39:37 UTC (rev 107)
@@ -22,6 +22,7 @@
public:
//Constructor
Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
+ Quaternion3D(NEWMAT::Matrix matIn);
inline void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w)
{xt = _xt; yt = _yt; zt = _zt; xr = _xr; yr = _yr; zr = _zr; w = _w;} ;
Modified: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc 2008-04-16 22:16:17 UTC (rev 106)
+++ pkg/trunk/libTF/simple/testQuat.cc 2008-04-16 22:39:37 UTC (rev 107)
@@ -7,7 +7,11 @@
{
Quaternion3D myquat(1,1,1,1,1,1,1);
+ Quaternion3D my2ndquat(myquat.asMatrix());
+ std::cout << "Quat1"<<std::endl;
myquat.printMatrix();
+ std::cout << "Quat2"<<std::endl;
+ my2ndquat.printMatrix();
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-16 22:26:54
|
Revision: 106
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=106&view=rev
Author: tfoote
Date: 2008-04-16 15:16:17 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
first cut at quaternion class
Modified Paths:
--------------
pkg/trunk/libTF/simple/Makefile
Added Paths:
-----------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/testQuat.cc
Modified: pkg/trunk/libTF/simple/Makefile
===================================================================
--- pkg/trunk/libTF/simple/Makefile 2008-04-16 21:16:16 UTC (rev 105)
+++ pkg/trunk/libTF/simple/Makefile 2008-04-16 22:16:17 UTC (rev 106)
@@ -3,8 +3,13 @@
all: test
+#%.o:%.cc
+#%.o:%.hh
+quat: testQuat.cc Quaternion3D.o
+ g++ -g -O2 $^ -o quat -lnewmat
+
test: main.cc libTF.o
g++ -g -O2 $^ -o test -lnewmat
Added: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc (rev 0)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 22:16:17 UTC (rev 106)
@@ -0,0 +1,73 @@
+#include "Quaternion3D.hh"
+
+Euler3D::Euler3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll) :
+ x(_x),y(_y),z(_z),yaw(_yaw),pitch(_pitch),roll(_roll)
+{
+ return;
+};
+
+
+Quaternion3D::Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w):
+ xt(_xt),yt(_yt),zt(_zt),xr(_xr),yr(_yr),zr(_zr),w(_w)
+{
+ Normalize();
+ return;
+};
+
+
+void Quaternion3D::Normalize()
+{
+ double mag = getMagnitude();
+ xr /= mag;
+ yr /= mag;
+ zr /= mag;
+ w /= mag;
+};
+
+double Quaternion3D::getMagnitude()
+{
+ return sqrt(xr*xr + yr*yr + zr*zr + w*w);
+};
+
+
+NEWMAT::Matrix Quaternion3D::asMatrix()
+{
+ NEWMAT::Matrix outMat(4,4);
+
+ double * mat = outMat.Store();
+
+ // math from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+ double xx = xr * xr;
+ double xy = xr * yr;
+ double xz = xr * zr;
+ double xw = xr * w;
+ double yy = yr * yr;
+ double yz = yr * zr;
+ double yw = yr * w;
+ double zz = zr * zr;
+ double zw = zr * w;
+ mat[0] = 1 - 2 * ( yy + zz );
+ mat[4] = 2 * ( xy - zw );
+ mat[8] = 2 * ( xz + yw );
+ mat[1] = 2 * ( xy + zw );
+ mat[5] = 1 - 2 * ( xx + zz );
+ mat[9] = 2 * ( yz - xw );
+ mat[2] = 2 * ( xz - yw );
+ mat[6] = 2 * ( yz + xw );
+ mat[10] = 1 - 2 * ( xx + yy );
+ mat[12] = mat[13] = mat[14] = 0;
+ mat[3] = xt;
+ mat[7] = yt;
+ mat[11] = zt;
+ mat[15] = 1;
+
+
+ return outMat;
+};
+
+
+void Quaternion3D::printMatrix()
+{
+ std::cout << asMatrix();
+
+};
Added: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh (rev 0)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 22:16:17 UTC (rev 106)
@@ -0,0 +1,54 @@
+#ifndef QUATERNION3D_HH
+#define QUATERNION3D_HH
+
+#include <iostream>
+#include <newmat/newmat.h>
+#include <newmat/newmatio.h>
+#include <math.h>
+
+class Euler3D {
+public:
+ //Constructor
+ Euler3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
+
+ //Storage
+ double x,y,z,yaw,pitch,roll;
+
+
+
+};
+
+class Quaternion3D {
+public:
+ //Constructor
+ Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
+
+ inline void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w)
+ {xt = _xt; yt = _yt; zt = _zt; xr = _xr; yr = _yr; zr = _zr; w = _w;} ;
+
+ void Normalize();
+ double getMagnitude();
+
+ NEWMAT::Matrix asMatrix();
+ void printMatrix();
+
+
+private:
+ //Quaternion Storage
+ double xt,yt,zt,xr,yr,zr,w;
+
+
+};
+
+
+
+
+
+
+
+
+
+
+
+
+#endif //QUATERNION3D_HH
Added: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc (rev 0)
+++ pkg/trunk/libTF/simple/testQuat.cc 2008-04-16 22:16:17 UTC (rev 106)
@@ -0,0 +1,13 @@
+
+
+#include "Quaternion3D.hh"
+
+
+int main()
+{
+ Quaternion3D myquat(1,1,1,1,1,1,1);
+
+ myquat.printMatrix();
+
+ 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-16 21:16:12
|
Revision: 105
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=105&view=rev
Author: jleibs
Date: 2008-04-16 14:16:16 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
Actual node to configure and spew out elphel jpeg images.
Modified Paths:
--------------
pkg/trunk/elphel_cam/build.yaml
pkg/trunk/elphel_cam/manifest.xml
pkg/trunk/elphel_cam/src/libelphel_cam/elphel_cam.cpp
pkg/trunk/elphel_cam/test/test_cam/test_cam.cpp
Added Paths:
-----------
pkg/trunk/elphel_cam/nodes/test_elphel_cam
pkg/trunk/elphel_cam/src/elphel_cam/
pkg/trunk/elphel_cam/src/elphel_cam/Makefile
pkg/trunk/elphel_cam/src/elphel_cam/elphel_cam.cpp
Modified: pkg/trunk/elphel_cam/build.yaml
===================================================================
--- pkg/trunk/elphel_cam/build.yaml 2008-04-16 01:35:21 UTC (rev 104)
+++ pkg/trunk/elphel_cam/build.yaml 2008-04-16 21:16:16 UTC (rev 105)
@@ -1,4 +1,5 @@
cpp:
make:
- src/libelphel_cam
+ - src/elphel_cam
- test/test_cam
Modified: pkg/trunk/elphel_cam/manifest.xml
===================================================================
--- pkg/trunk/elphel_cam/manifest.xml 2008-04-16 01:35:21 UTC (rev 104)
+++ pkg/trunk/elphel_cam/manifest.xml 2008-04-16 21:16:16 UTC (rev 105)
@@ -8,6 +8,6 @@
<url>http://pr.willowgarage.com</url>
<depend package="common_flows"/>
<depend package="thread_utils"/>
-<depend package="image_viewer"/>
+<depend package="simple_sdl_gui"/>
</package>
Added: pkg/trunk/elphel_cam/nodes/test_elphel_cam
===================================================================
--- pkg/trunk/elphel_cam/nodes/test_elphel_cam (rev 0)
+++ pkg/trunk/elphel_cam/nodes/test_elphel_cam 2008-04-16 21:16:16 UTC (rev 105)
@@ -0,0 +1,13 @@
+#!/usr/bin/env ruby
+require "#{`#{ENV['ROS_ROOT']}/rospack latest yamlgraph`}/lib/yamlgraph/ygl.rb"
+g = YAMLGraph.new
+g.param 'elphel_cam.host', '192.168.0.9'
+g.param 'elphel_cam.fps', '80'
+g.param 'elphel_cam.im_dec', '4'
+g.node 'elphel_cam/elphel_cam', {'launch'=>'xterm'}
+g.node 'simple_sdl_gui/gui', {'launch'=>'xterm'}
+#g.node 'vacuum/vacuum', {'launch' => 'valgrind'}
+g.flow 'elphel_cam:image', 'gui:image'
+#g.flow 'elphel_cam:image', 'vacuum:hose'
+YAMLGraphLauncher.new.launch g
+
Property changes on: pkg/trunk/elphel_cam/nodes/test_elphel_cam
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/elphel_cam/src/elphel_cam/Makefile
===================================================================
--- pkg/trunk/elphel_cam/src/elphel_cam/Makefile (rev 0)
+++ pkg/trunk/elphel_cam/src/elphel_cam/Makefile 2008-04-16 21:16:16 UTC (rev 105)
@@ -0,0 +1,4 @@
+SRC = elphel_cam.cpp
+OUT = ../../nodes/elphel_cam
+PKG = elphel_cam
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/elphel_cam/src/elphel_cam/elphel_cam.cpp
===================================================================
--- pkg/trunk/elphel_cam/src/elphel_cam/elphel_cam.cpp (rev 0)
+++ pkg/trunk/elphel_cam/src/elphel_cam/elphel_cam.cpp 2008-04-16 21:16:16 UTC (rev 105)
@@ -0,0 +1,134 @@
+///////////////////////////////////////////////////////////////////////////////
+// The axis_cam package provides a library that talks to Axis IP-based cameras
+// as well as ROS nodes which use these libraries
+//
+// 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/ImageCodec.h"
+#include "elphel_cam/elphel_cam.h"
+
+class Elphel_Cam_Node : public ROS_Slave
+{
+public:
+ FlowImage *image;
+ ImageCodec<FlowImage> *codec;
+ string elphel_host;
+ Elphel_Cam *cam;
+ int frame_id;
+ double fps;
+ int im_dec;
+
+ Elphel_Cam_Node() : ROS_Slave(), cam(NULL), frame_id(0)
+ {
+ register_source(image = new FlowImage("image"));
+ codec = new ImageCodec<FlowImage>(image);
+
+ register_with_master();
+ if (!get_string_param(".host", elphel_host))
+ elphel_host = "192.168.0.9";
+ printf("elphel_cam host set to [%s]\n", elphel_host.c_str());
+
+ if (!get_int_param(".frame_id", &frame_id))
+ frame_id = -1;
+ printf("elphel_cam frame_id set to [%d]\n", frame_id);
+
+ if (!get_double_param(".fps", &fps))
+ fps = 20;
+ printf("elphel_cam fps set to [%d]\n", fps);
+
+ if (!get_int_param(".im_dec", &im_dec))
+ im_dec = 4;
+ printf("elphel_cam image decimation and binning set to [%d]\n", im_dec);
+
+ cam = new Elphel_Cam(elphel_host);
+
+ cam->init(fps, im_dec, im_dec);
+
+ cam->start();
+ }
+ virtual ~Elphel_Cam_Node()
+ {
+ if (cam)
+ delete cam;
+ }
+ bool take_and_send_image()
+ {
+ uint8_t *jpeg;
+ uint32_t jpeg_size;
+ if (!cam->next_jpeg(&jpeg, &jpeg_size))
+ {
+ log(ROS::ERROR, "Elphel_Cam::next_jpeg returned an error");
+ return false;
+ }
+
+ // Sometimes things break for no great reason. When this happens
+ // the http server returns a 1x1 GIF.
+ // Skip failures and re-init cam if we get too many of them
+ int failcount = 0;
+ while (jpeg[0] == 0x47 && jpeg[1] == 0x49) { //THIS IS A GIF
+ if (failcount++ > 10) {
+ printf("Received too many failures... restarting cam\n");
+ cam->init(fps, im_dec, im_dec);
+ cam->start();
+ failcount = 0;
+ }
+ if (!cam->next_jpeg(&jpeg, &jpeg_size))
+ {
+ log(ROS::ERROR, "Elphel_Cam::next_jpeg returned an error");
+ return false;
+ }
+ }
+
+ image->set_data_size(jpeg_size);
+ memcpy(image->data, jpeg, jpeg_size);
+
+ image->compression = "jpeg";
+ image->colorspace = "rgb24";
+
+ codec->extract_header_to_flow();
+
+ image->frame_id = frame_id;
+
+ printf("Sending %d x %d\n", image->width, image->height);
+
+ image->publish();
+ return true;
+ }
+};
+
+int main(int argc, char **argv)
+{
+ Elphel_Cam_Node a;
+ while (a.happy())
+ if (!a.take_and_send_image())
+ {
+ a.log(ROS::ERROR,"couldn't take image.");
+ break;
+ }
+ return 0;
+}
Modified: pkg/trunk/elphel_cam/src/libelphel_cam/elphel_cam.cpp
===================================================================
--- pkg/trunk/elphel_cam/src/libelphel_cam/elphel_cam.cpp 2008-04-16 01:35:21 UTC (rev 104)
+++ pkg/trunk/elphel_cam/src/libelphel_cam/elphel_cam.cpp 2008-04-16 21:16:16 UTC (rev 105)
@@ -38,22 +38,25 @@
jpeg_buf_size = 0;
curl_global_init(0);
+ //URL to get next image, and increment pointer
ostringstream oss;
oss << "http://" << ip << ":8081/torp/wait/img/next/save";
image_url = new char[oss.str().length()+1];
strcpy(image_url, oss.str().c_str());
-
+ //URL to initialize pointer
oss.str("");
- oss << "http://" << ip << ":8081/towp/save";
+ oss << "http://" << ip << ":8081/towp/wait/next/save";
towp_url = new char[oss.str().length()+1];
strcpy(towp_url, oss.str().c_str());
+ //URL to control camera adjustments
oss.str("");
oss << "http://" << ip << "/admin-bin/ccam.cgi?";
ccam_url = new char[oss.str().length()+1];
strcpy(ccam_url, oss.str().c_str());
+ //URL to turn on/off the hardware compressor
oss.str("");
oss << "http://" << ip << ":81/compressor.php?";
comp_url = new char[oss.str().length()+1];
@@ -65,10 +68,8 @@
curl_easy_setopt(jpeg_curl, CURLOPT_WRITEDATA, this);
config_curl = curl_easy_init();
- // curl_easy_setopt(config_curl, CURLOPT_URL, config_url);
curl_easy_setopt(config_curl, CURLOPT_WRITEFUNCTION, Elphel_Cam::config_write);
curl_easy_setopt(config_curl, CURLOPT_WRITEDATA, this);
-
}
@@ -127,8 +128,6 @@
char urlbuf[512];
strcpy(urlbuf, oss.str().c_str());
- // printf("Executing command: %s\n", urlbuf);
-
curl_easy_setopt(config_curl, CURLOPT_URL, urlbuf);
CURLcode code;
@@ -158,20 +157,26 @@
uint8_t *jpeg;
uint32_t jpeg_size;
- for (int i = 0; i < 2; i++) { //loop twice
+ // We loop twice here and grab 10 images per loop This is fairly
+ // heuristic and involves the fact that the autogain stuff doesn't
+ // work right until the camera is actually capturing images. We
+ // want this to work if the camera has just powered on without being
+ // manually configured.
+ for (int i = 0; i < 2; i++) {
if (!compressor_cmd("cmd=reset"))
return false;
- //magic incantation:
- //http://wiki.elphel.com/index.php?title=Ccam.cgi
+ //These are the dark fairly undocumented incantations that
+ //initialize the camera well:
+ //"Documentation" can be found at: http://wiki.elphel.com/index.php?title=Ccam.cgi
ostringstream oss;
oss << "opt=vhcxyu+!-"
<< "&dh=" << im_dec << "&dv=" << im_dec
<< "&bh=" << im_bin << "&bh=" << im_bin
<< "&iq=90"
- << "&fps=" << fps << "&fpslm=3"
- << "&kgm=6&sens=2&bit=8&gam=50&pxl=10&csb=200&csr=200&rscale=auto&bscale=auto";
+ << "&fps=" << fps << ".0&fpslm=3"
+ << "&sens=4&bit=8&gam=50&pxl=10&csb=200&csr=200&rscale=auto&bscale=auto";
if (!ccam_cmd(oss.str()))
return false;
@@ -197,12 +202,21 @@
bool Elphel_Cam::start() {
+
+ //We put in sleeps here (*cringe*) because otherwise if we start reading too fast things break.
+
+ usleep(100000);
+
if (!compressor_cmd("cmd=run"))
return false;
+ usleep(100000);
+
if (!towp())
return false;
+ usleep(100000);
+
return true;
}
@@ -221,7 +235,6 @@
{
// overalloc
a->jpeg_buf_size = 2 * (a->jpeg_file_size + (size*nmemb));
- //printf("jpeg_buf_size is now %d\n", a->jpeg_buf_size);
if (a->jpeg_buf)
delete[] a->jpeg_buf;
a->jpeg_buf = new uint8_t[a->jpeg_buf_size];
@@ -239,9 +252,6 @@
Elphel_Cam *a = (Elphel_Cam *)userp;
a->config_ss_mutex.lock();
a->config_ss << string((char *)buf, size*nmemb);
- //printf("writing %d bytes\n", size*nmemb);
- //cout << a->ptz_ss.str() << endl;
a->config_ss_mutex.unlock();
- //cout << string((char *)buf, size*nmemb);
return size*nmemb;
}
Modified: pkg/trunk/elphel_cam/test/test_cam/test_cam.cpp
===================================================================
--- pkg/trunk/elphel_cam/test/test_cam/test_cam.cpp 2008-04-16 01:35:21 UTC (rev 104)
+++ pkg/trunk/elphel_cam/test/test_cam/test_cam.cpp 2008-04-16 21:16:16 UTC (rev 105)
@@ -45,7 +45,7 @@
uint8_t* jpeg;
uint32_t jpeg_size;
- e.init(80, 4, 4);
+ e.init(20, 2, 2);
e.start();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-04-16 01:35:16
|
Revision: 104
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=104&view=rev
Author: jleibs
Date: 2008-04-15 18:35:21 -0700 (Tue, 15 Apr 2008)
Log Message:
-----------
Check in library for reading from elphel camera.
Added Paths:
-----------
pkg/trunk/elphel_cam/
pkg/trunk/elphel_cam/build.yaml
pkg/trunk/elphel_cam/include/
pkg/trunk/elphel_cam/include/elphel_cam/
pkg/trunk/elphel_cam/include/elphel_cam/elphel_cam.h
pkg/trunk/elphel_cam/lib/
pkg/trunk/elphel_cam/manifest.xml
pkg/trunk/elphel_cam/nodes/
pkg/trunk/elphel_cam/rosbuild
pkg/trunk/elphel_cam/src/
pkg/trunk/elphel_cam/src/libelphel_cam/
pkg/trunk/elphel_cam/src/libelphel_cam/Makefile
pkg/trunk/elphel_cam/src/libelphel_cam/elphel_cam.cpp
pkg/trunk/elphel_cam/test/
pkg/trunk/elphel_cam/test/test_cam/
pkg/trunk/elphel_cam/test/test_cam/Makefile
pkg/trunk/elphel_cam/test/test_cam/test_cam.cpp
Added: pkg/trunk/elphel_cam/build.yaml
===================================================================
--- pkg/trunk/elphel_cam/build.yaml (rev 0)
+++ pkg/trunk/elphel_cam/build.yaml 2008-04-16 01:35:21 UTC (rev 104)
@@ -0,0 +1,4 @@
+cpp:
+ make:
+ - src/libelphel_cam
+ - test/test_cam
Added: pkg/trunk/elphel_cam/include/elphel_cam/elphel_cam.h
===================================================================
--- pkg/trunk/elphel_cam/include/elphel_cam/elphel_cam.h (rev 0)
+++ pkg/trunk/elphel_cam/include/elphel_cam/elphel_cam.h 2008-04-16 01:35:21 UTC (rev 104)
@@ -0,0 +1,78 @@
+///////////////////////////////////////////////////////////////////////////////
+// The axis_cam package provides a library that talks to Axis IP-based cameras
+// as well as ROS nodes which use these libraries
+//
+// Copyright (C) 2008, Morgan Quigley, Stanford Univerity
+// Jeremy Leibs, Willow Garage
+//
+// 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, 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 ELPHEL_CAM_ELPHEL_CAM_H
+#define ELPHEL_CAM_ELPHEL_CAM_H
+
+#include <curl/curl.h>
+#include <string>
+#include <sstream>
+#include "thread_utils/mutex.h"
+
+using namespace std;
+
+class Elphel_Cam
+{
+public:
+ Elphel_Cam(string ip);
+ ~Elphel_Cam();
+
+ bool next_jpeg(uint8_t ** const fetch_jpeg_buf, uint32_t *fetch_buf_size);
+
+ bool config_cmd(string url, string cmd);
+
+ bool compressor_cmd(string cmd);
+ bool ccam_cmd(string cmd);
+ bool towp();
+
+ bool start();
+ bool stop();
+
+ bool init(float = 10, int = 4, int = 4);
+
+private:
+ string ip;
+ uint8_t *jpeg_buf, ;
+ uint32_t jpeg_buf_size, jpeg_file_size;
+
+ CURL *jpeg_curl, *config_curl;
+
+ char *image_url, *towp_url, *ccam_url, *comp_url;
+
+ stringstream config_ss;
+ ThreadUtils::Mutex config_ss_mutex;
+
+ static size_t jpeg_write(void *buf, size_t size, size_t nmemb, void *userp);
+ static size_t config_write(void *buf, size_t size, size_t nmemb, void *userp);
+
+};
+
+#endif
+
Added: pkg/trunk/elphel_cam/manifest.xml
===================================================================
--- pkg/trunk/elphel_cam/manifest.xml (rev 0)
+++ pkg/trunk/elphel_cam/manifest.xml 2008-04-16 01:35:21 UTC (rev 104)
@@ -0,0 +1,13 @@
+<package>
+<description brief="Controlling and acquiring data from the Elphel IP-based cameras">
+A package to configure and grab images from the Elphel IP-based camera.
+
+</description>
+<author>Jeremy Leibs (email: le...@wi...)</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<depend package="common_flows"/>
+<depend package="thread_utils"/>
+<depend package="image_viewer"/>
+</package>
+
Added: pkg/trunk/elphel_cam/rosbuild
===================================================================
--- pkg/trunk/elphel_cam/rosbuild (rev 0)
+++ pkg/trunk/elphel_cam/rosbuild 2008-04-16 01:35:21 UTC (rev 104)
@@ -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/elphel_cam/rosbuild
___________________________________________________________________
Name: svn:executable
+ *
Added: pkg/trunk/elphel_cam/src/libelphel_cam/Makefile
===================================================================
--- pkg/trunk/elphel_cam/src/libelphel_cam/Makefile (rev 0)
+++ pkg/trunk/elphel_cam/src/libelphel_cam/Makefile 2008-04-16 01:35:21 UTC (rev 104)
@@ -0,0 +1,4 @@
+SRC = elphel_cam.cpp
+OUT = ../../lib/libelphel_cam.a
+PKG = elphel_cam
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/lib.mk
Added: pkg/trunk/elphel_cam/src/libelphel_cam/elphel_cam.cpp
===================================================================
--- pkg/trunk/elphel_cam/src/libelphel_cam/elphel_cam.cpp (rev 0)
+++ pkg/trunk/elphel_cam/src/libelphel_cam/elphel_cam.cpp 2008-04-16 01:35:21 UTC (rev 104)
@@ -0,0 +1,247 @@
+///////////////////////////////////////////////////////////////////////////////
+// The axis_cam package provides a library that talks to Axis IP-based cameras
+// as well as ROS nodes which use these libraries
+//
+// Copyright (C) 2008, Morgan Quigley, Stanford Univerity
+// Jeremy Leibs, Willow Garage
+//
+// 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, 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 <sstream>
+#include <iostream>
+#include "elphel_cam/elphel_cam.h"
+
+Elphel_Cam::Elphel_Cam(string ip) : ip(ip)
+{
+ jpeg_buf = NULL;
+ jpeg_buf_size = 0;
+ curl_global_init(0);
+
+ ostringstream oss;
+ oss << "http://" << ip << ":8081/torp/wait/img/next/save";
+ image_url = new char[oss.str().length()+1];
+ strcpy(image_url, oss.str().c_str());
+
+
+ oss.str("");
+ oss << "http://" << ip << ":8081/towp/save";
+ towp_url = new char[oss.str().length()+1];
+ strcpy(towp_url, oss.str().c_str());
+
+ oss.str("");
+ oss << "http://" << ip << "/admin-bin/ccam.cgi?";
+ ccam_url = new char[oss.str().length()+1];
+ strcpy(ccam_url, oss.str().c_str());
+
+ oss.str("");
+ oss << "http://" << ip << ":81/compressor.php?";
+ comp_url = new char[oss.str().length()+1];
+ strcpy(comp_url, oss.str().c_str());
+
+ jpeg_curl = curl_easy_init();
+ curl_easy_setopt(jpeg_curl, CURLOPT_URL, image_url);
+ curl_easy_setopt(jpeg_curl, CURLOPT_WRITEFUNCTION, Elphel_Cam::jpeg_write);
+ curl_easy_setopt(jpeg_curl, CURLOPT_WRITEDATA, this);
+
+ config_curl = curl_easy_init();
+ // curl_easy_setopt(config_curl, CURLOPT_URL, config_url);
+ curl_easy_setopt(config_curl, CURLOPT_WRITEFUNCTION, Elphel_Cam::config_write);
+ curl_easy_setopt(config_curl, CURLOPT_WRITEDATA, this);
+
+
+}
+
+Elphel_Cam::~Elphel_Cam()
+{
+ delete[] image_url;
+ delete[] towp_url;
+ delete[] ccam_url;
+ delete[] comp_url;
+ if (jpeg_buf)
+ delete[] jpeg_buf;
+ jpeg_buf = NULL;
+ curl_global_cleanup();
+}
+
+bool Elphel_Cam::next_jpeg(uint8_t ** const fetch_jpeg_buf, uint32_t *fetch_buf_size)
+{
+ if (fetch_jpeg_buf && fetch_buf_size)
+ {
+ *fetch_jpeg_buf = NULL;
+ *fetch_buf_size = 0;
+ }
+ else
+ {
+ printf("woah! bad input parameters\n");
+ return false; // don't make me crash
+ }
+ CURLcode code;
+ do
+ {
+ jpeg_file_size = 0;
+ if (code = curl_easy_perform(jpeg_curl))
+ {
+ printf("woah! curl error: [%s]\n", curl_easy_strerror(code));
+ return false;
+ }
+ if (jpeg_buf[0] == 0 && jpeg_buf[1] == 0)
+ printf("[Elphel_Cam] ODD...first two bytes are zero...\n");
+ } while (jpeg_buf[0] == 0 && jpeg_buf[1] == 0);
+ *fetch_jpeg_buf = jpeg_buf;
+ *fetch_buf_size = jpeg_file_size;
+ return true;
+}
+
+
+bool Elphel_Cam::config_cmd(string url, string cmd)
+{
+ config_ss_mutex.lock();
+ config_ss.clear(); // reset stringstream state so we can insert into it again
+ config_ss.str("");
+ config_ss_mutex.unlock();
+
+ ostringstream oss;
+ oss << url << cmd;
+
+ char urlbuf[512];
+ strcpy(urlbuf, oss.str().c_str());
+
+ // printf("Executing command: %s\n", urlbuf);
+
+ curl_easy_setopt(config_curl, CURLOPT_URL, urlbuf);
+
+ CURLcode code;
+ if (code = curl_easy_perform(config_curl))
+ {
+ printf("woah! curl error: [%s]\n", curl_easy_strerror(code));
+ return false;
+ }
+ return true;
+}
+
+
+bool Elphel_Cam::compressor_cmd(string cmd) {
+ return config_cmd(comp_url, cmd);
+}
+
+bool Elphel_Cam::ccam_cmd(string cmd) {
+ return config_cmd(ccam_url, cmd);
+}
+
+bool Elphel_Cam::towp() {
+ return config_cmd(towp_url, string(""));
+}
+
+bool Elphel_Cam::init(float fps, int im_dec, int im_bin) {
+
+ uint8_t *jpeg;
+ uint32_t jpeg_size;
+
+ for (int i = 0; i < 2; i++) { //loop twice
+
+ if (!compressor_cmd("cmd=reset"))
+ return false;
+
+ //magic incantation:
+ //http://wiki.elphel.com/index.php?title=Ccam.cgi
+ ostringstream oss;
+ oss << "opt=vhcxyu+!-"
+ << "&dh=" << im_dec << "&dv=" << im_dec
+ << "&bh=" << im_bin << "&bh=" << im_bin
+ << "&iq=90"
+ << "&fps=" << fps << "&fpslm=3"
+ << "&kgm=6&sens=2&bit=8&gam=50&pxl=10&csb=200&csr=200&rscale=auto&bscale=auto";
+
+ if (!ccam_cmd(oss.str()))
+ return false;
+
+ if (!compressor_cmd("cmd=run"))
+ return false;
+
+ if (!towp())
+ return false;
+
+ for (int j = 0; j < 10;j++) {
+ if (!next_jpeg(&jpeg, &jpeg_size))
+ return false;
+ }
+
+ if (!compressor_cmd("cmd=stop"))
+ return false;
+ }
+
+ return true;
+
+}
+
+
+bool Elphel_Cam::start() {
+ if (!compressor_cmd("cmd=run"))
+ return false;
+
+ if (!towp())
+ return false;
+
+ return true;
+
+}
+
+bool Elphel_Cam::stop() {
+ return compressor_cmd("cmd=stop");
+}
+
+
+size_t Elphel_Cam::jpeg_write(void *buf, size_t size, size_t nmemb, void *userp)
+{
+ if (size * nmemb == 0)
+ return 0;
+ Elphel_Cam *a = (Elphel_Cam *)userp;
+ if (a->jpeg_file_size + size*nmemb >= a->jpeg_buf_size)
+ {
+ // overalloc
+ a->jpeg_buf_size = 2 * (a->jpeg_file_size + (size*nmemb));
+ //printf("jpeg_buf_size is now %d\n", a->jpeg_buf_size);
+ if (a->jpeg_buf)
+ delete[] a->jpeg_buf;
+ a->jpeg_buf = new uint8_t[a->jpeg_buf_size];
+ }
+ memcpy(a->jpeg_buf + a->jpeg_file_size, buf, size*nmemb);
+ a->jpeg_file_size += size*nmemb;
+ return size*nmemb;
+}
+
+
+size_t Elphel_Cam::config_write(void *buf, size_t size, size_t nmemb, void *userp)
+{
+ if (size * nmemb == 0)
+ return 0;
+ Elphel_Cam *a = (Elphel_Cam *)userp;
+ a->config_ss_mutex.lock();
+ a->config_ss << string((char *)buf, size*nmemb);
+ //printf("writing %d bytes\n", size*nmemb);
+ //cout << a->ptz_ss.str() << endl;
+ a->config_ss_mutex.unlock();
+ //cout << string((char *)buf, size*nmemb);
+ return size*nmemb;
+}
Added: pkg/trunk/elphel_cam/test/test_cam/Makefile
===================================================================
--- pkg/trunk/elphel_cam/test/test_cam/Makefile (rev 0)
+++ pkg/trunk/elphel_cam/test/test_cam/Makefile 2008-04-16 01:35:21 UTC (rev 104)
@@ -0,0 +1,4 @@
+SRC = test_cam.cpp
+OUT = test_cam
+PKG = elphel_cam
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/elphel_cam/test/test_cam/test_cam.cpp
===================================================================
--- pkg/trunk/elphel_cam/test/test_cam/test_cam.cpp (rev 0)
+++ pkg/trunk/elphel_cam/test/test_cam/test_cam.cpp 2008-04-16 01:35:21 UTC (rev 104)
@@ -0,0 +1,64 @@
+/*********************************************************************
+* 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 <iostream>
+#include <fstream>
+#include "elphel_cam/elphel_cam.h"
+
+using namespace std;
+
+int main() {
+
+ Elphel_Cam e("192.168.0.9");
+
+ uint8_t* jpeg;
+ uint32_t jpeg_size;
+
+ e.init(80, 4, 4);
+
+ e.start();
+
+ for (int i = 0; i < 200;i++) {
+ ostringstream oss;
+ oss << "img" << i << ".jpg";
+
+ ofstream outfile(oss.str().c_str(),ofstream::binary);
+ e.next_jpeg(&jpeg, &jpeg_size);
+ outfile.write((char*)jpeg, jpeg_size);
+
+ outfile.close();
+ }
+
+ e.stop();
+}
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-12 03:18:12
|
Revision: 102
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=102&view=rev
Author: jleibs
Date: 2008-04-11 20:18:18 -0700 (Fri, 11 Apr 2008)
Log Message:
-----------
All necessary driver_axis213 functionality is now in axis_cam. Burn this now before we end up with redundant code again.
Removed Paths:
-------------
pkg/trunk/driver_axis213/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-04-12 03:17:10
|
Revision: 101
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=101&view=rev
Author: jleibs
Date: 2008-04-11 20:17:15 -0700 (Fri, 11 Apr 2008)
Log Message:
-----------
Added axis_ptz node to axis_cam. Flows for this are in unstable_flows until we decide a good representation. Camera_calibration now uses axis_cam.
Modified Paths:
--------------
pkg/trunk/axis_cam/build.yaml
pkg/trunk/axis_cam/include/axis_cam/axis_cam.h
pkg/trunk/axis_cam/manifest.xml
pkg/trunk/axis_cam/src/libaxis_cam/axis_cam.cpp
pkg/trunk/camera_calibration/manifest.xml
pkg/trunk/camera_calibration/nodes/run_cal
pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp
Added Paths:
-----------
pkg/trunk/axis_cam/src/axis_ptz/
pkg/trunk/axis_cam/src/axis_ptz/Makefile
pkg/trunk/axis_cam/src/axis_ptz/axis_ptz.cpp
pkg/trunk/unstable_flows/
pkg/trunk/unstable_flows/build.yaml
pkg/trunk/unstable_flows/flows/
pkg/trunk/unstable_flows/flows/Actuator.flow
pkg/trunk/unstable_flows/flows/LensActuator.flow
pkg/trunk/unstable_flows/flows/LensActuatorNoSub.flow
pkg/trunk/unstable_flows/flows/PTZActuator.flow
pkg/trunk/unstable_flows/flows/PTZActuatorNoSub.flow
pkg/trunk/unstable_flows/manifest.xml
pkg/trunk/unstable_flows/rosbuild
Modified: pkg/trunk/axis_cam/build.yaml
===================================================================
--- pkg/trunk/axis_cam/build.yaml 2008-04-11 18:29:37 UTC (rev 100)
+++ pkg/trunk/axis_cam/build.yaml 2008-04-12 03:17:15 UTC (rev 101)
@@ -2,6 +2,7 @@
make:
- src/libaxis_cam
- src/axis_cam
+ - src/axis_ptz
- src/axis_cam_polled
- test/test_get
- test/test_ptz
Modified: pkg/trunk/axis_cam/include/axis_cam/axis_cam.h
===================================================================
--- pkg/trunk/axis_cam/include/axis_cam/axis_cam.h 2008-04-11 18:29:37 UTC (rev 100)
+++ pkg/trunk/axis_cam/include/axis_cam/axis_cam.h 2008-04-12 03:17:15 UTC (rev 101)
@@ -48,8 +48,16 @@
int get_focus();
bool set_iris(int iris = 0, bool relative = false, bool blocking = true);
int get_iris();
+
+ bool send_params(string params);
+ bool query_params();
+
void print_params();
+ int last_iris, last_focus;
+ double last_pan, last_tilt, last_zoom;
+ bool last_autofocus_enabled, last_autoiris_enabled;
+
private:
string ip;
uint8_t *jpeg_buf;
@@ -64,11 +72,7 @@
}
static size_t jpeg_write(void *buf, size_t size, size_t nmemb, void *userp);
static size_t ptz_write(void *buf, size_t size, size_t nmemb, void *userp);
- bool send_params(string params);
- bool query_params();
- int last_iris, last_focus;
- double last_pan, last_tilt, last_zoom;
- bool last_autofocus_enabled, last_autoiris_enabled;
+
};
#endif
Modified: pkg/trunk/axis_cam/manifest.xml
===================================================================
--- pkg/trunk/axis_cam/manifest.xml 2008-04-11 18:29:37 UTC (rev 100)
+++ pkg/trunk/axis_cam/manifest.xml 2008-04-12 03:17:15 UTC (rev 101)
@@ -15,5 +15,6 @@
<depend package="image_viewer"/>
<depend package="thread_utils"/>
<depend package="string_utils"/>
+<depend package="unstable_flows"/>
</package>
Added: pkg/trunk/axis_cam/src/axis_ptz/Makefile
===================================================================
--- pkg/trunk/axis_cam/src/axis_ptz/Makefile (rev 0)
+++ pkg/trunk/axis_cam/src/axis_ptz/Makefile 2008-04-12 03:17:15 UTC (rev 101)
@@ -0,0 +1,4 @@
+SRC = axis_ptz.cpp
+OUT = ../../nodes/axis_ptz
+PKG = axis_cam
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/axis_cam/src/axis_ptz/axis_ptz.cpp
===================================================================
--- pkg/trunk/axis_cam/src/axis_ptz/axis_ptz.cpp (rev 0)
+++ pkg/trunk/axis_cam/src/axis_ptz/axis_ptz.cpp 2008-04-12 03:17:15 UTC (rev 101)
@@ -0,0 +1,223 @@
+///////////////////////////////////////////////////////////////////////////////
+// The axis_cam package provides a library that talks to Axis IP-based cameras
+// as well as ROS nodes which use these libraries
+//
+// 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 "unstable_flows/FlowPTZActuatorNoSub.h"
+#include "axis_cam/axis_cam.h"
+
+class Axis_PTZ : public ROS_Slave
+{
+public:
+ FlowPTZActuatorNoSub *ptz;
+ FlowPTZActuatorNoSub *ptz_control;
+
+ string axis_host;
+ AxisCam *cam;
+ int frame_id;
+
+ float pan;
+ bool pan_rel;
+ bool pan_valid;
+
+ float tilt;
+ bool tilt_rel;
+ bool tilt_valid;
+
+ float zoom;
+ bool zoom_rel;
+ bool zoom_valid;
+
+ float focus;
+ bool focus_rel;
+ bool focus_valid;
+
+ float iris;
+ bool iris_rel;
+ bool iris_valid;
+
+ ROS_Mutex control_mutex;
+
+ Axis_PTZ() : ROS_Slave(), cam(NULL), frame_id(0)
+ {
+ register_source(ptz = new FlowPTZActuatorNoSub("ptz"));
+ register_sink(ptz_control = new FlowPTZActuatorNoSub("ptz_control"), ROS_CALLBACK(Axis_PTZ, ptz_callback));
+ register_with_master();
+ if (!get_string_param(".host", axis_host))
+ {
+ printf("host parameter not specified; defaulting to 192.168.0.90\n");
+ axis_host = "192.168.0.90";
+ }
+ printf("axis_cam host set to [%s]\n", axis_host.c_str());
+ get_int_param(".frame_id", &frame_id);
+ cam = new AxisCam(axis_host);
+ printf("package path is [%s]\n", get_my_package_path().c_str());
+ }
+
+ virtual ~Axis_PTZ()
+ {
+ if (cam)
+ delete cam;
+ }
+
+ bool get_and_send_ptz()
+ {
+ if (!cam->query_params()) {
+ return false;
+ }
+
+ ptz->frame_id = frame_id;
+
+ ptz->pan_val = cam->last_pan;
+ ptz->pan_rel = false;
+ ptz->pan_valid = true;
+
+ ptz->tilt_val = cam->last_tilt;
+ ptz->tilt_rel = false;
+ ptz->tilt_valid = true;
+
+ ptz->lens_zoom_val = cam->last_zoom;
+ ptz->lens_zoom_rel = false;
+ ptz->lens_zoom_valid = true;
+
+ ptz->lens_focus_val = cam->last_focus;
+ if (cam->last_autofocus_enabled)
+ ptz->lens_focus_val = -1;
+ ptz->lens_focus_rel = false;
+ ptz->lens_focus_valid = true;
+
+ ptz->lens_iris_val = cam->last_iris;
+ if (cam->last_autoiris_enabled == true)
+ ptz->lens_iris_val = -1;
+ ptz->lens_iris_rel = false;
+ ptz->lens_iris_valid = true;
+
+ ptz->publish();
+ return true;
+ }
+
+ void ptz_callback()
+ {
+ control_mutex.lock();
+
+ if (ptz_control->pan_valid) {
+ pan = ptz_control->pan_val;
+ pan_rel = ptz_control->pan_rel;
+ pan_valid = true;
+ }
+
+ if (ptz_control->tilt_valid) {
+ tilt = ptz_control->tilt_val;
+ tilt_rel = ptz_control->tilt_rel;
+ tilt_valid = true;
+ }
+
+ if (ptz_control->lens_zoom_valid) {
+ zoom = ptz_control->lens_zoom_val;
+ zoom_rel = ptz_control->lens_zoom_rel;
+ zoom_valid = true;
+ }
+
+ if (ptz_control->lens_focus_valid) {
+ focus = ptz_control->lens_focus_val;
+ focus_rel = ptz_control->lens_focus_rel;
+ focus_valid = true;
+ }
+
+ if (ptz_control->lens_focus_valid) {
+ focus = ptz_control->lens_focus_val;
+ focus_rel = ptz_control->lens_focus_rel;
+ focus_valid = true;
+ }
+
+ if (ptz_control->lens_iris_valid) {
+ iris = ptz_control->lens_iris_val;
+ iris_rel = ptz_control->lens_iris_rel;
+ iris_valid = true;
+ }
+
+ control_mutex.unlock();
+ }
+
+ bool do_ptz_control() {
+
+ control_mutex.lock();
+
+ ostringstream oss;
+
+ if (pan_valid)
+ oss << (pan_rel ? "r" : "") << string("pan=") << pan << string("&");
+ if (tilt_valid)
+ oss << (tilt_rel ? "r" : "") << string("tilt=") << tilt << string("&");
+ if (zoom_valid)
+ oss << (zoom_rel ? "r" : "") << string("zoom=") << zoom << string("&");
+ if (focus_valid) {
+ if (!focus_rel && focus <= 0)
+ oss << string("autofocus=on&");
+ else
+ oss << string("autofocus=off&") << (focus_rel ? "r" : "") << string("focus=") << focus << string("&");
+ }
+ if (iris_valid) {
+ if (!iris_rel && iris <= 0)
+ oss << string("autoiris=on&");
+ else
+ oss << string("autoiris=off&") << (iris_rel ? "r" : "") << string("iris=") << iris << string("&");
+ }
+
+ if (oss.str().size() > 0) {
+ if (!cam->send_params(oss.str()))
+ return false;
+ pan_valid = tilt_valid = zoom_valid = focus_valid = iris_valid = false;
+ }
+
+ control_mutex.unlock();
+
+ return true;
+ }
+
+
+};
+
+int main(int argc, char **argv)
+{
+ Axis_PTZ a;
+ while (a.happy()) {
+ if (!a.get_and_send_ptz())
+ {
+ a.log(ROS::ERROR,"Couldn't acquire ptz info.");
+ break;
+ }
+ if (!a.do_ptz_control())
+ {
+ a.log(ROS::ERROR,"Couldn't command ptz.");
+ break;
+ }
+ }
+ return 0;
+}
+
Modified: pkg/trunk/axis_cam/src/libaxis_cam/axis_cam.cpp
===================================================================
--- pkg/trunk/axis_cam/src/libaxis_cam/axis_cam.cpp 2008-04-11 18:29:37 UTC (rev 100)
+++ pkg/trunk/axis_cam/src/libaxis_cam/axis_cam.cpp 2008-04-12 03:17:15 UTC (rev 101)
@@ -266,16 +266,18 @@
else if (tokens[0] == string("iris"))
last_iris = atoi(tokens[1].c_str());
else if (tokens[0] == string("autofocus"))
- last_autofocus_enabled = (tokens[1] == string("on") ? true : false);
+ last_autofocus_enabled = (tokens[1].substr(0,2) == string("on") ? true : false);
else if (tokens[0] == string("autoiris"))
- last_autoiris_enabled = (tokens[1] == string("on") ? true : false);
-/*
+ last_autoiris_enabled = (tokens[1].substr(0,2) == string("on") ? true : false);
+
+ /*
printf("line has %d tokens:\n", tokens.size());
for (int i = 0; i < tokens.size(); i++)
- printf(" [%s] ", tokens[i].c_str());
+ printf(" '%s' ", tokens[i].substr(0,2).c_str());
printf("\n");
-*/
+ */
}
+
ptz_ss_mutex.unlock();
return true;
}
Modified: pkg/trunk/camera_calibration/manifest.xml
===================================================================
--- pkg/trunk/camera_calibration/manifest.xml 2008-04-11 18:29:37 UTC (rev 100)
+++ pkg/trunk/camera_calibration/manifest.xml 2008-04-12 03:17:15 UTC (rev 101)
@@ -5,8 +5,9 @@
<author>Jeremy Leibs (email: le...@wi...)</author>
<license>BSD</license>
<depend package='roscpp'/>
-<depend package='driver_axis213'/>
+<depend package='axis_cam'/>
<depend package='common_flows'/>
+<depend package='unstable_flows'/>
<depend package='yamlgraph'/>
<depend package='sdl'/>
<depend package='simple_sdl_gui'/>
Modified: pkg/trunk/camera_calibration/nodes/run_cal
===================================================================
--- pkg/trunk/camera_calibration/nodes/run_cal 2008-04-11 18:29:37 UTC (rev 100)
+++ pkg/trunk/camera_calibration/nodes/run_cal 2008-04-12 03:17:15 UTC (rev 101)
@@ -2,15 +2,15 @@
(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.node 'driver_axis213/axis213_cam', {'launch'=>'xterm', }
-g.node 'driver_axis213/axis213_ptz', {'launch'=>'xterm'}
-g.param 'axis213_cam.host', '10.0.0.150'
-g.param 'axis213_ptz.host', '10.0.0.150'
+g.node 'axis_cam/axis_cam', {'launch'=>'xterm', }
+g.node 'axis_cam/axis_ptz', {'launch'=>'xterm'}
+g.param 'axis_cam.host', '10.0.0.150'
+g.param 'axis_ptz.host', '10.0.0.150'
g.node 'camera_calibration/calibrator', {'launch'=>'xterm'}
g.node 'simple_sdl_gui/gui', {'launch'=>'xterm'}
-g.flow 'axis213_cam:image_all', 'calibrator:image_in'
-g.flow 'axis213_ptz:ptz_observe_all', 'calibrator:observe'
-g.flow 'calibrator:control', 'axis213_ptz:ptz_control'
+g.flow 'axis_cam:image', 'calibrator:image_in'
+g.flow 'axis_ptz:ptz', 'calibrator:observe'
+g.flow 'calibrator:control', 'axis_ptz:ptz_control'
g.flow 'calibrator:image_out', 'gui:image'
g.flow 'gui:key', 'calibrator:key'
YAMLGraphLauncher.new.launch g
Modified: pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp
===================================================================
--- pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp 2008-04-11 18:29:37 UTC (rev 100)
+++ pkg/trunk/camera_calibration/src/calibrator/calibrator.cpp 2008-04-12 03:17:15 UTC (rev 101)
@@ -47,7 +47,7 @@
#include "ros/ros_slave.h"
#include "common_flows/FlowImage.h"
#include "common_flows/ImageCodec.h"
-#include "driver_axis213/FlowPTZPosition.h"
+#include "unstable_flows/FlowPTZActuatorNoSub.h"
#include "simple_sdl_gui/FlowSDLKeyEvent.h"
void matToScreen(CvMat *mat, const char* matname) {
@@ -70,8 +70,8 @@
FlowImage *image_out;
ImageCodec<FlowImage> *codec_out;
- FlowPTZPosition *control;
- FlowPTZPosition *observe;
+ FlowPTZActuatorNoSub *control;
+ FlowPTZActuatorNoSub *observe;
FlowSDLKeyEvent *key;
CvMat *cvimage_in;
@@ -99,8 +99,8 @@
register_source(image_out = new 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"));
+ register_sink(observe = new FlowPTZActuatorNoSub("observe"), ROS_CALLBACK(Calibrator, ptz_received));
+ register_source(control = new FlowPTZActuatorNoSub("control"));
register_sink(key = new FlowSDLKeyEvent("key"), ROS_CALLBACK(Calibrator, key_received));
register_with_master();
@@ -143,11 +143,11 @@
virtual ~Calibrator() { }
void key_received() {
- control->pan = 0;
- control->tilt = 0;
- control->zoom = 0;
- control->focus = 0;
- control->relative = true;
+ control->pan_valid = false;
+ control->tilt_valid = false;
+ control->lens_zoom_valid = false;
+ control->lens_focus_valid = false;
+ control->lens_iris_valid = false;
// std::cout << "Got keypress: " << key->data << std::endl;
@@ -155,35 +155,57 @@
if (key->state == SDL_PRESSED) {
switch (key->sym) {
case SDLK_UP:
- control->tilt += 1;
+ control->tilt_val = 1;
+ control->tilt_rel = true;
+ control->tilt_valid = true;
break;
case SDLK_DOWN:
- control->tilt -= 1;
+ control->tilt_val = -1;
+ control->tilt_rel = true;
+ control->tilt_valid = true;
break;
case SDLK_LEFT:
- control->pan -= 1;
+ control->pan_val = -1;
+ control->pan_rel = true;
+ control->pan_valid = true;
break;
case SDLK_RIGHT:
- control->pan += 1;
+ control->pan_val = 1;
+ control->pan_rel = true;
+ control->pan_valid = true;
break;
case 61:
- control->zoom += 100;
+ control->lens_zoom_val = 100;
+ control->lens_zoom_rel = true;
+ control->lens_zoom_valid = true;
break;
case 45:
- control->zoom -= 100;
+ control->lens_zoom_val = -100;
+ control->lens_zoom_rel = true;
+ control->lens_zoom_valid = true;
break;
case SDLK_RIGHTBRACKET:
- control->focus += 100;
+ control->lens_focus_val = 100;
+ control->lens_focus_rel = true;
+ control->lens_focus_valid = true;
break;
case SDLK_LEFTBRACKET:
- control->focus -= 100;
+ control->lens_focus_val = -100;
+ control->lens_focus_rel = true;
+ control->lens_focus_valid = true;
break;
case SDLK_SPACE:
- control->pan = 0;
- control->tilt = 0;
- control->zoom = 5000;
- control->focus = -1;
- control->relative = false;
+ control->pan_val = 0;
+ control->pan_rel = false;
+ control->pan_valid = true;
+
+ control->tilt_val = 0;
+ control->tilt_rel = false;
+ control->tilt_valid = true;
+
+ control->lens_zoom_val = 5000;
+ control->lens_zoom_rel = false;
+ control->lens_zoom_valid = true;
break;
case SDLK_c:
centering = !centering;
@@ -193,17 +215,17 @@
break;
case SDLK_f:
observe->lock_atom();
- control->pan = observe->pan;
- control->tilt = observe->tilt;
- control->zoom = observe->zoom;
- tmp_focus = observe->focus;
+ tmp_focus = observe->lens_focus_val;
observe->unlock_atom();
- control->relative = false;
- if (tmp_focus > 1) {
- control->focus = -1;
+ if (tmp_focus > 0) {
+ control->lens_focus_val = -1;
+ control->lens_focus_rel = false;
+ control->lens_focus_valid = true;
} else {
- control->focus = 0;
+ control->lens_focus_val = 0;
+ control->lens_focus_rel = true;
+ control->lens_focus_valid = true;
}
break;
case SDLK_RETURN:
@@ -279,10 +301,10 @@
std::ofstream posfile(ss.str().c_str());
observe->lock_atom();
- posfile << "P: " << observe->pan << std::endl
- << "T: " << observe->tilt << std::endl
- << "Z: " << observe->zoom << std::endl
- << "F: " << observe->focus;
+ posfile << "P: " << observe->pan_val << std::endl
+ << "T: " << observe->tilt_val << std::endl
+ << "Z: " << observe->lens_zoom_val << std::endl
+ << "F: " << observe->lens_focus_val;
observe->unlock_atom();
posfile.close();
@@ -303,10 +325,10 @@
std::stringstream ss;
observe->lock_atom();
- ss << "P: " << observe->pan;
- ss << " T: " << observe->tilt;
- ss << " Z: " << observe->zoom;
- ss << " F: " << observe->focus;
+ ss << "P: " << observe->pan_val;
+ ss << " T: " << observe->tilt_val;
+ ss << " Z: " << observe->lens_zoom_val;
+ ss << " F: " << observe->lens_focus_val;
observe->unlock_atom();
cvPutText(cvimage_undistort, ss.str().c_str(), cvPoint(15,30), &font, CV_RGB(255,0,0));
@@ -347,11 +369,14 @@
rel_pan = (COM.x - 354.0) * .001;
rel_tilt = -(COM.y - 240.0) * .001;
- control->pan = rel_pan;
- control->tilt = rel_tilt;
- control->zoom = 0;
- control->focus = 0;
- control->relative = true;
+ control->pan_val = rel_pan;
+ control->pan_rel = true;
+ control->pan_valid = true;
+
+ control->tilt_val = rel_tilt;
+ control->tilt_rel = true;
+ control->tilt_valid = true;
+
control->publish();
}
Added: pkg/trunk/unstable_flows/build.yaml
===================================================================
Added: pkg/trunk/unstable_flows/flows/Actuator.flow
===================================================================
--- pkg/trunk/unstable_flows/flows/Actuator.flow (rev 0)
+++ pkg/trunk/unstable_flows/flows/Actuator.flow 2008-04-12 03:17:15 UTC (rev 101)
@@ -0,0 +1,3 @@
+float32 val
+byte rel
+byte valid
Added: pkg/trunk/unstable_flows/flows/LensActuator.flow
===================================================================
--- pkg/trunk/unstable_flows/flows/LensActuator.flow (rev 0)
+++ pkg/trunk/unstable_flows/flows/LensActuator.flow 2008-04-12 03:17:15 UTC (rev 101)
@@ -0,0 +1,3 @@
+Actuator zoom
+Actuator focus
+Actuator iris
\ No newline at end of file
Added: pkg/trunk/unstable_flows/flows/LensActuatorNoSub.flow
===================================================================
--- pkg/trunk/unstable_flows/flows/LensActuatorNoSub.flow (rev 0)
+++ pkg/trunk/unstable_flows/flows/LensActuatorNoSub.flow 2008-04-12 03:17:15 UTC (rev 101)
@@ -0,0 +1,9 @@
+float32 zoom_val
+byte zoom_rel
+byte zoom_valid
+float32 focus_val
+byte focus_rel
+byte focus_valid
+float32 iris_val
+byte iris_rel
+byte iris_valid
Added: pkg/trunk/unstable_flows/flows/PTZActuator.flow
===================================================================
--- pkg/trunk/unstable_flows/flows/PTZActuator.flow (rev 0)
+++ pkg/trunk/unstable_flows/flows/PTZActuator.flow 2008-04-12 03:17:15 UTC (rev 101)
@@ -0,0 +1,4 @@
+Actuator pan
+Actuator tilt
+LensActuator lens
+
Added: pkg/trunk/unstable_flows/flows/PTZActuatorNoSub.flow
===================================================================
--- pkg/trunk/unstable_flows/flows/PTZActuatorNoSub.flow (rev 0)
+++ pkg/trunk/unstable_flows/flows/PTZActuatorNoSub.flow 2008-04-12 03:17:15 UTC (rev 101)
@@ -0,0 +1,17 @@
+float32 pan_val
+byte pan_rel
+byte pan_valid
+float32 tilt_val
+byte tilt_rel
+byte tilt_valid
+float32 lens_zoom_val
+byte lens_zoom_rel
+byte lens_zoom_valid
+float32 lens_focus_val
+byte lens_focus_rel
+byte lens_focus_valid
+float32 lens_iris_val
+byte lens_iris_rel
+byte lens_iris_valid
+
+
Added: pkg/trunk/unstable_flows/manifest.xml
===================================================================
--- pkg/trunk/unstable_flows/manifest.xml (rev 0)
+++ pkg/trunk/unstable_flows/manifest.xml 2008-04-12 03:17:15 UTC (rev 101)
@@ -0,0 +1,12 @@
+<package>
+ <description brief="Flows that have not yet been elevated to the status common_flows">
+This is a place to put flows that are more general than the module that needs to use them, but
+havn't been tested thoroughly enough to put in common_flows. Depending on these flows is
+likely to break things since they are subject to refactoring and/or moving at any time.
+ </description>
+ <author>Jeremy Leibs (email: le...@wi...)</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com</url>
+ <depend package="common_flows"/>
+</package>
+
Added: pkg/trunk/unstable_flows/rosbuild
===================================================================
--- pkg/trunk/unstable_flows/rosbuild (rev 0)
+++ pkg/trunk/unstable_flows/rosbuild 2008-04-12 03:17:15 UTC (rev 101)
@@ -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/unstable_flows/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-11 18:29:32
|
Revision: 100
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=100&view=rev
Author: morgan_quigley
Date: 2008-04-11 11:29:37 -0700 (Fri, 11 Apr 2008)
Log Message:
-----------
tweaks to the image_viewer program so that pressing <enter> saves the current image (priceless for taking checkerboard calibration photos)
Modified Paths:
--------------
pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp
Modified: pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp
===================================================================
--- pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp 2008-04-11 17:55:41 UTC (rev 99)
+++ pkg/trunk/image_viewer/src/image_viewer/image_viewer.cpp 2008-04-11 18:29:37 UTC (rev 100)
@@ -37,8 +37,11 @@
FlowImage *image;
ImageCodec<FlowImage> *codec;
SDL_Surface *screen, *blit_prep;
+ bool save_next_image;
+ int save_number;
- ImageViewer() : ROS_Slave(), blit_prep(NULL)
+ ImageViewer() : ROS_Slave(), blit_prep(NULL),
+ save_next_image(false), save_number(1)
{
register_sink(image = new FlowImage("image"),
ROS_CALLBACK(ImageViewer, image_cb));
@@ -51,8 +54,31 @@
screen = SDL_SetVideoMode(320, 240, 24, 0);
return (screen ? true : false);
}
+ void request_save_image() { save_next_image = true; }
+ void save_image()
+ {
+ save_next_image = false;
+ if (image->compression != string("jpeg"))
+ printf("I am only smart enough to save a jpeg image\n");
+ else
+ {
+ char fnamebuf[100];
+ sprintf(fnamebuf, "image%04d.jpg", save_number++);
+ FILE *f = fopen(fnamebuf, "wb");
+ if (!f)
+ {
+ printf("couldn't open [%s]\n", fnamebuf);
+ return; // bummer
+ }
+ fwrite(image->data, 1, image->get_data_size(), f);
+ fclose(f);
+ printf("saved %s\n", fnamebuf);
+ }
+ }
void image_cb()
{
+ if (save_next_image)
+ save_image();
if (!screen)
return; // paranoia. shouldn't happen. we should have bailed by now.
if (screen->h != image->height || screen->w != image->width)
@@ -124,7 +150,11 @@
exit(1);
}
while (v.happy())
- usleep(1000000);
+ {
+ getc(stdin);
+ v.save_next_image = true;
+ //usleep(1000000);
+ }
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-04-11 17:55:41
|
Revision: 99
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=99&view=rev
Author: morgan_quigley
Date: 2008-04-11 10:55:41 -0700 (Fri, 11 Apr 2008)
Log Message:
-----------
a ppm-writing class and a simple checkerboard generator program
Added Paths:
-----------
pkg/trunk/image_utils/include/image_utils/ppm_wrapper.h
pkg/trunk/image_utils/src/
pkg/trunk/image_utils/src/create_chessboard/
pkg/trunk/image_utils/src/create_chessboard/Makefile
pkg/trunk/image_utils/src/create_chessboard/create_chessboard.cpp
pkg/trunk/image_utils/test/ppm/
pkg/trunk/image_utils/test/ppm/Makefile
pkg/trunk/image_utils/test/ppm/ppm.cpp
Added: pkg/trunk/image_utils/include/image_utils/ppm_wrapper.h
===================================================================
--- pkg/trunk/image_utils/include/image_utils/ppm_wrapper.h (rev 0)
+++ pkg/trunk/image_utils/include/image_utils/ppm_wrapper.h 2008-04-11 17:55:41 UTC (rev 99)
@@ -0,0 +1,48 @@
+#ifndef IMAGE_UTILS_PPM_WRAPPER_H
+#define IMAGE_UTILS_PPM_WRAPPER_H
+
+// this guy encapsulates reading / writing ppm images
+#include <string>
+#include <cstdio>
+#include <cassert>
+using std::string;
+
+class PpmWrapper
+{
+public:
+ PpmWrapper();
+ ~PpmWrapper();
+
+ static char *write_file(string filename, int width, int height,
+ string colorspace, uint8_t *raster)
+ {
+ if (colorspace != string("rgb24") && colorspace != string("bgr24"))
+ return "woah! PpmWrapper can only handle rgb24 or bgr24 images";
+ FILE *f = fopen(filename.c_str(), "wb");
+ if (!f)
+ return "couldn't open file to write ppm";
+ fprintf(f, "P6%d %d\n255\n", width, height);
+ if (colorspace == string("rgb24"))
+ fwrite(raster, 1, width*height*3, f);
+ else if (colorspace == string("bgr24"))
+ {
+ uint8_t *bgr = new uint8_t[width*height*3];
+ for (int y = 0; y < height; y++)
+ for(int x = 0; x < height; x++)
+ {
+ uint8_t *p = raster + y * width * 3 + x * 3;
+ uint8_t *q = bgr + y * width * 3 + x * 3;
+ q[0] = p[2]; q[1] = p[1]; q[2] = p[0];
+ }
+ fwrite(bgr, 1, width * height * 3, f);
+ delete[] bgr;
+ }
+ else
+ assert(0);
+ fclose(f);
+ return NULL; // no error
+ }
+};
+
+#endif
+
Added: pkg/trunk/image_utils/src/create_chessboard/Makefile
===================================================================
--- pkg/trunk/image_utils/src/create_chessboard/Makefile (rev 0)
+++ pkg/trunk/image_utils/src/create_chessboard/Makefile 2008-04-11 17:55:41 UTC (rev 99)
@@ -0,0 +1,4 @@
+SRC = create_chessboard.cpp
+OUT = create_chessboard
+PKG = image_utils
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/image_utils/src/create_chessboard/create_chessboard.cpp
===================================================================
--- pkg/trunk/image_utils/src/create_chessboard/create_chessboard.cpp (rev 0)
+++ pkg/trunk/image_utils/src/create_chessboard/create_chessboard.cpp 2008-04-11 17:55:41 UTC (rev 99)
@@ -0,0 +1,36 @@
+#include "image_utils/ppm_wrapper.h"
+#include <cstdio>
+
+int main(int argc, char **argv)
+{
+ if (argc < 3)
+ {
+ printf("usage: create_chessboard WIDTH HEIGHT [SIZE]\n");
+ printf(" where WIDTH, HEIGHT are the number of square, and\n");
+ printf(" SIZE is the pixel length of the side of each square.\n");
+ return 1;
+ }
+
+ int w = atoi(argv[1]), h = atoi(argv[2]);
+ int s = 200;
+ if (argc >= 4)
+ s = atoi(argv[3]);
+ printf("creating %d by %d board with %d-pixel squares\n", w, h, s);
+
+ int img_width = w*s, img_height = h*s;
+ uint8_t *img = new uint8_t[img_width*img_height*3];
+ for (int y = 0; y < img_height; y++)
+ for (int x = 0; x < img_width; x++)
+ {
+ uint8_t *p = img + y * img_width * 3 + x * 3;
+ uint8_t xc = (uint8_t)(x / s) % 2;
+ uint8_t yc = (uint8_t)(y / s) % 2;
+ uint8_t c = (xc ^ yc ? 255 : 0);
+ p[0] = p[1] = p[2] = c;
+ }
+
+ PpmWrapper::write_file("board.ppm", img_width, img_height, "rgb24", img);
+ delete[] img;
+
+ return 0;
+}
Added: pkg/trunk/image_utils/test/ppm/Makefile
===================================================================
--- pkg/trunk/image_utils/test/ppm/Makefile (rev 0)
+++ pkg/trunk/image_utils/test/ppm/Makefile 2008-04-11 17:55:41 UTC (rev 99)
@@ -0,0 +1,4 @@
+SRC = ppm.cpp
+OUT = ppm
+PKG = image_utils
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/image_utils/test/ppm/ppm.cpp
===================================================================
--- pkg/trunk/image_utils/test/ppm/ppm.cpp (rev 0)
+++ pkg/trunk/image_utils/test/ppm/ppm.cpp 2008-04-11 17:55:41 UTC (rev 99)
@@ -0,0 +1,48 @@
+///////////////////////////////////////////////////////////////////////////////
+// The image_flows package provides image transport, codec wrapping, and
+// various handy image utilities.
+//
+// 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 "image_utils/ppm_wrapper.h"
+
+int main(int argc, char **argv)
+{
+ int width = 640, height = 480;
+ uint8_t *raster = new uint8_t[width*height*3];
+ for (int y = 0; y < height; y++)
+ for (int x = 0; x < width; x++)
+ {
+ raster[y*width*3 + x*3 ] = rand() % 255;
+ raster[y*width*3 + x*3 + 1] = rand() % 255;
+ raster[y*width*3 + x*3 + 2] = rand() % 255;
+ }
+ PpmWrapper::write_file("test.ppm", width, height, "rgb24", raster);
+
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-04-10 21:31:04
|
Revision: 98
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=98&view=rev
Author: morgan_quigley
Date: 2008-04-10 14:31:07 -0700 (Thu, 10 Apr 2008)
Log Message:
-----------
register with master explicitly
Modified Paths:
--------------
pkg/trunk/axis_cam/src/axis_cam/axis_cam.cpp
Modified: pkg/trunk/axis_cam/src/axis_cam/axis_cam.cpp
===================================================================
--- pkg/trunk/axis_cam/src/axis_cam/axis_cam.cpp 2008-04-10 19:55:56 UTC (rev 97)
+++ pkg/trunk/axis_cam/src/axis_cam/axis_cam.cpp 2008-04-10 21:31:07 UTC (rev 98)
@@ -42,6 +42,7 @@
AxisCamNode() : ROS_Slave(), cam(NULL), frame_id(0)
{
register_source(image = new FlowImage("image"));
+ register_with_master();
if (!get_string_param(".host", axis_host))
{
printf("host parameter not specified; defaulting to 192.168.0.90\n");
@@ -66,6 +67,7 @@
log(ROS::ERROR, "woah! AxisCam::get_jpeg returned an error");
return false;
}
+ //printf("sending %d-byte jpeg block\n", jpeg_size);
image->set_data_size(jpeg_size);
memcpy(image->data, jpeg, jpeg_size);
image->width = 704;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-10 19:55:54
|
Revision: 97
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=97&view=rev
Author: tfoote
Date: 2008-04-10 12:55:56 -0700 (Thu, 10 Apr 2008)
Log Message:
-----------
adding license to script to complete ticket:13
Modified Paths:
--------------
pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/parseIBPS.py
Modified: pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/parseIBPS.py
===================================================================
--- pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/parseIBPS.py 2008-04-10 18:46:41 UTC (rev 96)
+++ pkg/trunk/IBPSBatteryInterface/src/IBPSBatteryInterface/parseIBPS.py 2008-04-10 19:55:56 UTC (rev 97)
@@ -1,3 +1,36 @@
+# 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.
+#
+
import operator
import select
import time
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-10 18:47:05
|
Revision: 96
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=96&view=rev
Author: tfoote
Date: 2008-04-10 11:46:41 -0700 (Thu, 10 Apr 2008)
Log Message:
-----------
adding license
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-10 17:08:27 UTC (rev 95)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-10 18:46:41 UTC (rev 96)
@@ -1,3 +1,35 @@
+//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 "libTF.hh"
RefFrame::RefFrame() :
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-10 17:08:27 UTC (rev 95)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-10 18:46:41 UTC (rev 96)
@@ -1,3 +1,35 @@
+//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 LIBTF_HH
#define LIBTF_HH
#include <iostream>
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: <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 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-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.
|
