Menu
▾
▴
personalrobots-commit
|
From: <rdi...@us...> - 2009-01-06 10:55:02
|
Revision: 8878
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8878&view=rev
Author: rdiankov
Date: 2009-01-06 10:54:58 +0000 (Tue, 06 Jan 2009)
Log Message:
-----------
updated prf with new cameras, added checkerboard detection launch file
Modified Paths:
--------------
pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml
pkg/trunk/robot_descriptions/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/control_laser_tilt_roslaunch.xml
Added Paths:
-----------
pkg/trunk/calibration/laser_camera_calibration/startcalib.ros.xml
Added: pkg/trunk/calibration/laser_camera_calibration/startcalib.ros.xml
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/startcalib.ros.xml (rev 0)
+++ pkg/trunk/calibration/laser_camera_calibration/startcalib.ros.xml 2009-01-06 10:54:58 UTC (rev 8878)
@@ -0,0 +1,24 @@
+<launch>
+ <machine name="localhost_cameras" address="localhost" default="false"/>
+
+<!-- <node machine="cameras" name="dcam" pkg="dcam" type="dcam" respawn="false">
+ <param name="video_mode" type="str" value="640x480_videre_rectified"/>
+ <param name="do_rectify" type="bool" value="False"/>
+ <param name="do_stereo" type="bool" value="False"/>
+ <param name="do_calc_points" type="bool" value="False"/>
+ </node> -->
+ <group ns="checkerdetector" clear_params="true">
+ <param name="display" type="int" value="1"/>
+ <param name="frame_id" type="string" value="head_tilt_link"/>
+ <param name="rect0_size_x" type="double" value="0.10740"/>
+ <param name="rect0_size_y" type="double" value="0.10757"/>
+ <param name="grid0_size_x" type="int" value="6"/>
+ <param name="grid0_size_y" type="int" value="8"/>
+ <param name="type0" type="string" value="data/ricebox.kinbody.xml"/>
+ <node machine="localhost_cameras" pkg="checkerboard_detector" type="checkerboard_detector" respawn="true" output="screen">
+ <remap from="CamInfo" to="/stereo/left/cam_info"/>
+ <remap from="Image" to="/stereo/left/image_rect"/>
+<!-- <env name="DISPLAY" value=":0.0"/> -->
+ </node>
+ </group>
+</launch>
Modified: pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml 2009-01-06 10:53:48 UTC (rev 8877)
+++ pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml 2009-01-06 10:54:58 UTC (rev 8878)
@@ -16,8 +16,8 @@
<param name="grid0_size_y" type="int" value="3"/>
<param name="type0" type="string" value="data/ricebox.kinbody.xml"/>
<node machine="localhost_cameras" pkg="checkerboard_detector" type="checkerboard_detector" respawn="true" output="screen">
- <remap from="CamInfo" to="/dcam/left/cam_info"/>
- <remap from="Image" to="/dcam/left/image_rect"/>
+ <remap from="CamInfo" to="/stereo/left/cam_info"/>
+ <remap from="Image" to="/stereo/left/image_rect"/>
<!-- <env name="DISPLAY" value=":0.0"/> -->
</node>
</group>
Modified: pkg/trunk/robot_descriptions/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2_alpha/prf.launch 2009-01-06 10:53:48 UTC (rev 8877)
+++ pkg/trunk/robot_descriptions/pr2_alpha/prf.launch 2009-01-06 10:54:58 UTC (rev 8878)
@@ -63,8 +63,8 @@
-->
<!-- DCAM-->
-<node machine="three" name="dcam" pkg="dcam" type="dcam" respawn="false">
- <!-- video_mode should be one of the following:
+ <group ns="stereo">
+ <!-- video_mode should be one of the following:
640x480_videre_rectified: Provides rectified images from the hw
Provides: left mono image
640x480_videre_disparity: Disparity and rectification done on chip.
@@ -74,12 +74,16 @@
640x480_videre_none: No stereo on chip (all processing done in software).
Provides: all 3 images available
-->
- <param name="video_mode" type="str" value="640x480_videre_rectified"/>
- <param name="do_rectify" type="bool" value="False"/>
- <param name="do_stereo" type="bool" value="False"/>
- <param name="do_calc_points" type="bool" value="False"/>
-</node>
+ <param name="videre_mode" type="str" value="rectified"/>
+ <param name="do_colorize" type="bool" value="False"/>
+ <param name="do_rectify" type="bool" value="False"/>
+ <param name="do_stereo" type="bool" value="False"/>
+ <param name="do_calc_points" type="bool" value="False"/>
+ </group>
+ <node machine="three" pkg="dcam" type="stereodcam" respawn="false"/>
+ <node machine="three" pkg="stereo_image_proc" type="stereoproc" respawn="false"/>
+
<!-- Runtime Diagnostics Logging -->
<node pkg="rosrecord" type="rosrecord" args="-f /hwlog/prf_runtime_automatic /diagnostics" />
Modified: pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/control_laser_tilt_roslaunch.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/control_laser_tilt_roslaunch.xml 2009-01-06 10:53:48 UTC (rev 8877)
+++ pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/control_laser_tilt_roslaunch.xml 2009-01-06 10:54:58 UTC (rev 8878)
@@ -1,4 +1,4 @@
<launch>
- <node pkg="mechanism_control" type="spawner.py" args="laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 15 .75 .25" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find wg_robot_description)/laser_tilt/laser_tilt_controller.xml" />
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 10 .75 .25" />
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ve...@us...> - 2009-01-06 19:15:20
|
Revision: 8886
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8886&view=rev
Author: veedee
Date: 2009-01-06 19:15:19 +0000 (Tue, 06 Jan 2009)
Log Message:
-----------
added ANN as a dependency, changef cloud_kdtree to use ANN for now (more stable)
Modified Paths:
--------------
pkg/trunk/mapping/cloud_kdtree/include/cloud_kdtree/kdtree.h
pkg/trunk/mapping/cloud_kdtree/manifest.xml
Added Paths:
-----------
pkg/trunk/3rdparty/ANN/
pkg/trunk/3rdparty/ANN/Makefile
pkg/trunk/3rdparty/ANN/gcc43_shared.patch
pkg/trunk/3rdparty/ANN/manifest.xml
Added: pkg/trunk/3rdparty/ANN/Makefile
===================================================================
--- pkg/trunk/3rdparty/ANN/Makefile (rev 0)
+++ pkg/trunk/3rdparty/ANN/Makefile 2009-01-06 19:15:19 UTC (rev 8886)
@@ -0,0 +1,15 @@
+all: ann
+
+TARBALL = build/ann_1.1.1.tar.gz
+TARBALL_URL = http://www.cs.umd.edu/~mount/ANN/Files/1.1.1/ann_1.1.1.tar.gz
+UNPACK_CMD = tar xfz
+SOURCE_DIR = build/ann_1.1.1
+TARBALL_PATCH = gcc43_shared.patch
+
+include $(shell rospack find mk)/download_unpack_build.mk
+
+ann: $(SOURCE_DIR)
+ echo "Building FL-ANN..."
+ cd build/ann_1.1.1 && make linux-g++
+wipe:
+ rm -rf build
Added: pkg/trunk/3rdparty/ANN/gcc43_shared.patch
===================================================================
--- pkg/trunk/3rdparty/ANN/gcc43_shared.patch (rev 0)
+++ pkg/trunk/3rdparty/ANN/gcc43_shared.patch 2009-01-06 19:15:19 UTC (rev 8886)
@@ -0,0 +1,128 @@
+--- ./test/ann_test.cpp.orig 2009-01-06 10:58:37.000000000 -0800
++++ ./test/ann_test.cpp 2009-01-06 10:59:11.000000000 -0800
+@@ -37,6 +37,7 @@
+ #include <ctime> // clock
+ #include <cmath> // math routines
+ #include <string> // C string ops
++#include <string.h>
+ #include <fstream> // file I/O
+
+ #include <ANN/ANN.h> // ANN declarations
+@@ -370,7 +371,7 @@
+ //----------------------------------------------------------------------
+
+ void Error( // error routine
+- char *msg, // error message
++ const char *msg, // error message
+ ANNerr level) // abort afterwards
+ {
+ if (level == ANNabort) {
+--- ./Makefile.orig 2009-01-06 11:03:59.000000000 -0800
++++ ./Makefile 2009-01-06 11:04:29.000000000 -0800
+@@ -42,6 +42,7 @@
+ default:
+ @echo "Enter one of the following:"
+ @echo " make linux-g++ for Linux and g++"
++ @echo " make linux-shared-g++ for Linux and g++ (shared)"
+ @echo " make macosx-g++ for Mac OS X and g++"
+ @echo " make sunos5 for Sun with SunOS 5.x"
+ @echo " make sunos5-sl for Sun with SunOS 5.x, make shared libs"
+@@ -56,7 +57,7 @@
+ #-----------------------------------------------------------------------------
+ # main make entry point
+ #-----------------------------------------------------------------------------
+-alpha-g++ macosx-g++ linux-g++ sgi sunos4 sunos4-g++ sunos5 sunos5-g++ sunos5-g++-sl authors-debug authors-perf:
++alpha-g++ macosx-g++ linux-g++ linux-shared-g++ sgi sunos4 sunos4-g++ sunos5 sunos5-g++ sunos5-g++-sl authors-debug authors-perf:
+ cd src ; $(MAKE) $@
+ cd test ; $(MAKE) $@
+ cd sample ; $(MAKE) $@
+--- ./src/kd_dump.cpp.orig 2009-01-06 10:56:58.000000000 -0800
++++ ./src/kd_dump.cpp 2009-01-06 10:57:29.000000000 -0800
+@@ -31,6 +31,8 @@
+ // desired.)
+ //----------------------------------------------------------------------
+
++#include <stdlib.h>
++#include <string.h>
+ #include "kd_tree.h" // kd-tree declarations
+ #include "bd_tree.h" // bd-tree declarations
+
+--- ./src/perf.cpp.orig 2009-01-06 10:57:54.000000000 -0800
++++ ./src/perf.cpp 2009-01-06 10:57:59.000000000 -0800
+@@ -102,7 +102,7 @@
+ }
+
+ // print a single statistic
+-void print_one_stat(char *title, ANNsampStat s, double div)
++void print_one_stat(const char *title, ANNsampStat s, double div)
+ {
+ cout << title << "= [ ";
+ cout.width(9); cout << s.mean()/div << " : ";
+--- ./src/ANN.cpp.orig 2009-01-06 10:55:10.000000000 -0800
++++ ./src/ANN.cpp 2009-01-06 11:00:32.000000000 -0800
+@@ -24,6 +24,7 @@
+ // Added performance counting to annDist()
+ //----------------------------------------------------------------------
+
++#include <stdlib.h>
+ #include <ANN/ANNx.h> // all ANN includes
+ #include <ANN/ANNperf.h> // ANN performance
+
+@@ -163,7 +164,7 @@
+ // Error handler
+ //----------------------------------------------------------------------
+
+-void annError(char *msg, ANNerr level)
++void annError(const char *msg, ANNerr level)
+ {
+ if (level == ANNabort) {
+ cerr << "ANN: ERROR------->" << msg << "<-------------ERROR\n";
+--- ./ann2fig/ann2fig.cpp.orig 2009-01-06 11:01:08.000000000 -0800
++++ ./ann2fig/ann2fig.cpp 2009-01-06 11:01:35.000000000 -0800
+@@ -40,6 +40,8 @@
+ // arguments.
+ //----------------------------------------------------------------------
+
++#include <stdlib.h>
++#include <string.h>
+ #include <cstdio> // C standard I/O
+ #include <fstream> // file I/O
+ #include <string> // string manipulation
+@@ -92,7 +94,7 @@
+ // Error handler
+ //----------------------------------------------------------------------
+
+-void Error(char *msg, ANNerr level)
++void Error(const char *msg, ANNerr level)
+ {
+ if (level == ANNabort) {
+ cerr << "ann2fig: ERROR------->" << msg << "<-------------ERROR\n";
+--- ./include/ANN/ANNx.h.orig 2009-01-06 10:56:33.000000000 -0800
++++ ./include/ANN/ANNx.h 2009-01-06 10:56:40.000000000 -0800
+@@ -61,7 +61,7 @@
+ //----------------------------------------------------------------------
+
+ void annError( // ANN error routine
+- char *msg, // error message
++ const char *msg, // error message
+ ANNerr level); // level of error
+
+ void annPrintPt( // print a point
+--- ./Make-config.orig 2009-01-06 11:02:38.000000000 -0800
++++ ./Make-config 2009-01-06 11:04:36.000000000 -0800
+@@ -76,6 +76,15 @@
+ "MAKELIB = ar ruv" \
+ "RANLIB = true"
+
++# Linux using g++ shared
++linux-shared-g++:
++ $(MAKE) targets \
++ "ANNLIB = libANN.so" \
++ "C++ = g++" \
++ "CFLAGS = -O3" \
++ "MAKELIB = g++ -shared -o" \
++ "RANLIB = true"
++
+ # Mac OS X using g++
+ macosx-g++:
+ $(MAKE) targets \
Added: pkg/trunk/3rdparty/ANN/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/ANN/manifest.xml (rev 0)
+++ pkg/trunk/3rdparty/ANN/manifest.xml 2009-01-06 19:15:19 UTC (rev 8886)
@@ -0,0 +1,43 @@
+<package>
+ <description brief="ANN">
+
+ ANN is a library written in C++, which supports data structures and
+ algorithms for both exact and approximate nearest neighbor searching in
+ arbitrarily high dimensions.
+
+ In the nearest neighbor problem a set of data points in d-dimensional
+ space is given. These points are preprocessed into a data structure, so
+ that given any query point q, the nearest or generally k nearest points of
+ P to q can be reported efficiently. The distance between two points can be
+ defined in many ways. ANN assumes that distances are measured using any
+ class of distance functions called Minkowski metrics. These include the
+ well known Euclidean distance, Manhattan distance, and max distance.
+
+ Based on our own experience, ANN performs quite efficiently for point sets
+ ranging in size from thousands to hundreds of thousands, and in dimensions
+ as high as 20. (For applications in significantly higher dimensions, the
+ results are rather spotty, but you might try it anyway.)
+
+ The library implements a number of different data structures, based on
+ kd-trees and box-decomposition trees, and employs a couple of different
+ search strategies.
+
+ The library also comes with test programs for measuring the quality of
+ performance of ANN on any particular data sets, as well as programs for
+ visualizing the structure of the geometric data structures.
+
+ </description>
+
+ <author>Sunil Arya and David Mount</author>
+ <license>LGPL</license>
+ <review status="3rdparty" notes=""/>
+ <url>http://www.cs.umd.edu/~mount/ANN/</url>
+ <export>
+ <cpp cflags="-I${prefix}/build/ann_1.1.1/include" lflags="-Wl,-rpath,${prefix}/build/ann_1.1.1/lib -L${prefix}/build/ann_1.1.1/lib -lANN" />
+ <doxymaker external="http://www.cs.umd.edu/~mount/ANN/Files/1.1.1/ANNmanual_1.1.1.pdf"/>
+ </export>
+
+ <sysdepend os="ubuntu" version="7.04-feisty" package="wget"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="wget"/>
+
+</package>
Modified: pkg/trunk/mapping/cloud_kdtree/include/cloud_kdtree/kdtree.h
===================================================================
--- pkg/trunk/mapping/cloud_kdtree/include/cloud_kdtree/kdtree.h 2009-01-06 18:57:42 UTC (rev 8885)
+++ pkg/trunk/mapping/cloud_kdtree/include/cloud_kdtree/kdtree.h 2009-01-06 19:15:19 UTC (rev 8886)
@@ -39,6 +39,8 @@
#include <stdlib.h>
+#define USE_ANN
+
#ifdef USE_ANN
#include <ANN/ANN.h>
#else
@@ -88,9 +90,9 @@
bucket_size_ = 30; // default bucket size value
// Allocate enough data
+ nr_points_ = convertCloudToArray (points, points_);
nn_idx_ = new ANNidx [nr_points_];
nn_dists_ = new ANNdist [nr_points_];
- nr_points_ = convertCloudToArray (points, points_);
// Create the kd_tree representation
#ifdef USE_ANN
ann_kd_tree_ = new ANNkd_tree (points_, nr_points_, dim_, bucket_size_);
@@ -113,9 +115,9 @@
dim_ = 3 + dim; // default number of dimensions (3 = xyz) + the extra channels
// Allocate enough data
+ nr_points_ = convertCloudToArray (points, dim, points_);
nn_idx_ = new ANNidx [nr_points_];
nn_dists_ = new ANNdist [nr_points_];
- nr_points_ = convertCloudToArray (points, dim, points_);
// Create the kd_tree representation
#ifdef USE_ANN
ann_kd_tree_ = new ANNkd_tree (points_, nr_points_, dim_, bucket_size_);
Modified: pkg/trunk/mapping/cloud_kdtree/manifest.xml
===================================================================
--- pkg/trunk/mapping/cloud_kdtree/manifest.xml 2009-01-06 18:57:42 UTC (rev 8885)
+++ pkg/trunk/mapping/cloud_kdtree/manifest.xml 2009-01-06 19:15:19 UTC (rev 8886)
@@ -10,8 +10,8 @@
<depend package="roscpp" />
<depend package="std_msgs" />
-<!-- <depend package="ANN" />-->
- <depend package="FL-ANN" />
+ <depend package="ANN" />
+<!-- <depend package="FL-ANN" />-->
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcloud_kdtree"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-01-06 20:05:54
|
Revision: 8902
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8902&view=rev
Author: tpratkanis
Date: 2009-01-06 20:05:53 +0000 (Tue, 06 Jan 2009)
Log Message:
-----------
Make it so that jam can run in parallel.
Modified Paths:
--------------
pkg/trunk/3rdparty/trex/make_trex
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/run-jam
pkg/trunk/highlevel/test_executive_trex_pr2/run-jam
Modified: pkg/trunk/3rdparty/trex/make_trex
===================================================================
--- pkg/trunk/3rdparty/trex/make_trex 2009-01-06 20:05:23 UTC (rev 8901)
+++ pkg/trunk/3rdparty/trex/make_trex 2009-01-06 20:05:53 UTC (rev 8902)
@@ -10,7 +10,7 @@
cd TREX
# Build the nddl jar file used to parse input files
-jam -q nddl.jar
+jam $PARALLEL_JOBS -q nddl.jar
if [ $? -ne 0 ] ; then
echo "Jam of nddl.jar failed."
@@ -19,9 +19,9 @@
# Build the debug version
if [ `uname` = Darwin ]; then
- jam -q libTREX_g.dylib
+ jam $PARALLEL_JOBS -q libTREX_g.dylib
else
- jam -q libTREX_g.so
+ jam $PARALLEL_JOBS -q libTREX_g.so
fi
if [ $? -ne 0 ] ; then
@@ -31,9 +31,9 @@
# Build the optimized version
if [ `uname` = Darwin ]; then
- jam -sVARIANTS=OPTIMIZED -q libTREX_o.dylib
+ jam $PARALLEL_JOBS -sVARIANTS=OPTIMIZED -q libTREX_o.dylib
else
- jam -sVARIANTS=OPTIMIZED -q libTREX_o.so
+ jam $PARALLEL_JOBS -sVARIANTS=OPTIMIZED -q libTREX_o.so
fi
if [ $? -ne 0 ] ; then
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/run-jam
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/run-jam 2009-01-06 20:05:23 UTC (rev 8901)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/run-jam 2009-01-06 20:05:53 UTC (rev 8902)
@@ -4,5 +4,5 @@
cd `rospack find executive_trex_pr2`
PLASMA_HOME=`rospack find trex`/PLASMA/
-jam exec_inputs
+jam $PARALLEL_JOBS exec_inputs
Modified: pkg/trunk/highlevel/test_executive_trex_pr2/run-jam
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/run-jam 2009-01-06 20:05:23 UTC (rev 8901)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/run-jam 2009-01-06 20:05:53 UTC (rev 8902)
@@ -4,5 +4,5 @@
cd `rospack find test_executive_trex_pr2`
PLASMA_HOME=`rospack find trex`/PLASMA/
-jam exec_test_inputs
+jam $PARALLEL_JOBS exec_test_inputs
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-01-06 22:36:11
|
Revision: 8919
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8919&view=rev
Author: hsujohnhsu
Date: 2009-01-06 22:36:09 +0000 (Tue, 06 Jan 2009)
Log Message:
-----------
* move mech.py spawn from test directory to demo.
* update arm orientation check to use quaternions.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/arm_no_x.launch
pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.py
pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml
Modified: pkg/trunk/demos/arm_gazebo/arm_no_x.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm_no_x.launch 2009-01-06 22:34:45 UTC (rev 8918)
+++ pkg/trunk/demos/arm_gazebo/arm_no_x.launch 2009-01-06 22:36:09 UTC (rev 8919)
@@ -15,10 +15,10 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- start arm controller -->
- <!--node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/controllers.xml" respawn="false" /--> <!-- load default arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" /> <!-- load default arm controller -->
<!-- send arm a command -->
- <!--node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
<!-- for visualization -->
<!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.py 2009-01-06 22:34:45 UTC (rev 8918)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.py 2009-01-06 22:36:09 UTC (rev 8919)
@@ -49,41 +49,52 @@
rostools.update_path('robot_msgs')
rostools.update_path('rostest')
rostools.update_path('rospy')
+rostools.update_path('transformations')
+rostools.update_path('numpy')
-
import sys, unittest
import os, os.path, threading, time
import rospy, rostest
from std_msgs.msg import *
from pr2_mechanism_controllers.msg import *
+from transformations import *
+from numpy import *
-CMD_POS = 0.5
+ARM_JNT_NAMES = ['l_shoulder_pan_joint','l_shoulder_lift_joint', \
+ 'l_upper_arm_roll_joint','l_elbow_flex_joint', \
+ 'l_forearm_roll_joint','l_wrist_flex_joint', \
+ 'l_wrist_roll_joint']
+CMD_POS = [0.5,0.5,0.5,-0.5,0.5,0.5,0.5]
+CMD_VEL = [0,0,0,0,0,0,0]
+GRP_CMD_POS = 0.3
+
TARGET_DURATION = 1.0
-TARGET_TOL = 0.08 #empirical test result john - 20081029
+ROT_TARGET_TOL = 0.01 #empirical test result john - 20090106
+POS_TARGET_TOL = 0.01 #empirical test result john - 20090106
TEST_TIMEOUT = 30.0
# pre-recorded poses for above commands
-TARGET_ARM_TX = 0.70167519
-TARGET_ARM_TY = 0.27016711
-TARGET_ARM_TZ = 0.78997955
-TARGET_ARM_QX = 0.23727666
-TARGET_ARM_QY = -0.18292768
-TARGET_ARM_QZ = 0.19008174
-TARGET_ARM_QW = 0.93493646
-TARGET_GRIPPER_TX = 0.76451604
-TARGET_GRIPPER_TY = 0.29929084
-TARGET_GRIPPER_TZ = 0.82737631
-TARGET_GRIPPER_QX = 0.22177592
-TARGET_GRIPPER_QY = -0.20146688
-TARGET_GRIPPER_QZ = 0.26480912
-TARGET_GRIPPER_QW = 0.91654743
-
+TARGET_ARM_TX = 0.703305819503
+TARGET_ARM_TY = 0.270579779085
+TARGET_ARM_TZ = 0.789835186655
+TARGET_ARM_QX = -0.23728153595
+TARGET_ARM_QY = 0.182723242802
+TARGET_ARM_QZ = -0.190178621726
+TARGET_ARM_QW = -0.934955496842
+TARGET_GRIPPER_TX = 0.765601972524
+TARGET_GRIPPER_TY = 0.299927644187
+TARGET_GRIPPER_TZ = 0.827281715659
+TARGET_GRIPPER_QX = -0.201821946334
+TARGET_GRIPPER_QY = 0.221139617347
+TARGET_GRIPPER_QZ = -0.350190747038
+TARGET_GRIPPER_QW = -0.887542456622
class ArmTest(unittest.TestCase):
def __init__(self, *args):
super(ArmTest, self).__init__(*args)
- self.success = False
+ self.arm_success = False
+ self.grp_success = False
self.reached_target_arm = False
self.reached_target_gripper = False
self.duration_start_gripper = 0
@@ -108,53 +119,83 @@
def gripperP3dInput(self, p3d):
i = 0
- error = abs(p3d.pos.position.x - TARGET_GRIPPER_TX) + \
- abs(p3d.pos.position.y - TARGET_GRIPPER_TY) + \
- abs(p3d.pos.position.z - TARGET_GRIPPER_TZ) + \
- abs(p3d.pos.orientation.x - TARGET_GRIPPER_QX) + \
- abs(p3d.pos.orientation.y - TARGET_GRIPPER_QY) + \
- abs(p3d.pos.orientation.z - TARGET_GRIPPER_QZ) + \
- abs(p3d.pos.orientation.w - TARGET_GRIPPER_QW)
- print " gripper Error: " + str(error)
+ pos_error = abs(p3d.pos.position.x - TARGET_GRIPPER_TX) + \
+ abs(p3d.pos.position.y - TARGET_GRIPPER_TY) + \
+ abs(p3d.pos.position.z - TARGET_GRIPPER_TZ)
+
+ target_rm = rotation_matrix_from_quaternion([TARGET_GRIPPER_QX \
+ ,TARGET_GRIPPER_QY \
+ ,TARGET_GRIPPER_QZ \
+ ,TARGET_GRIPPER_QW])
+
+ p3d_q = [p3d.pos.orientation.x \
+ ,p3d.pos.orientation.y \
+ ,p3d.pos.orientation.z \
+ ,p3d.pos.orientation.w]
+
+ target_q_inv = quaternion_from_rotation_matrix( linalg.inv(target_rm) )
+
+ rot_euler = euler_from_quaternion( quaternion_multiply(p3d_q, target_q_inv) )
+
+ rot_error = abs( rot_euler[0] ) + \
+ abs( rot_euler[1] ) + \
+ abs( rot_euler[2] )
+
+ print " gripper Error pos: " + str(pos_error) + " rot: " + str(rot_error)
#self.printP3D(p3d)
# has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
if self.reached_target_gripper:
print " gripper duration: " + str(time.time() - self.duration_start_gripper)
- if error < TARGET_TOL:
+ if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
if time.time() - self.duration_start_gripper > TARGET_DURATION:
- self.success = True
+ self.grp_success = True
else:
# failed to maintain target vw, reset duration
- self.success = False
+ self.grp_success = False
self.reached_target_gripper = False
else:
- if error < TARGET_TOL:
+ if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
self.reached_target_gripper = True
self.duration_start_gripper = time.time()
def armP3dInput(self, p3d):
i = 0
- error = abs(p3d.pos.position.x - TARGET_ARM_TX) + \
- abs(p3d.pos.position.y - TARGET_ARM_TY) + \
- abs(p3d.pos.position.z - TARGET_ARM_TZ) + \
- abs(p3d.pos.orientation.x - TARGET_ARM_QX) + \
- abs(p3d.pos.orientation.y - TARGET_ARM_QY) + \
- abs(p3d.pos.orientation.z - TARGET_ARM_QZ) + \
- abs(p3d.pos.orientation.w - TARGET_ARM_QW)
- print " arm Error: " + str(error)
+ pos_error = abs(p3d.pos.position.x - TARGET_ARM_TX) + \
+ abs(p3d.pos.position.y - TARGET_ARM_TY) + \
+ abs(p3d.pos.position.z - TARGET_ARM_TZ)
+
+ target_rm = rotation_matrix_from_quaternion([TARGET_ARM_QX \
+ ,TARGET_ARM_QY \
+ ,TARGET_ARM_QZ \
+ ,TARGET_ARM_QW])
+
+ p3d_q = [p3d.pos.orientation.x \
+ ,p3d.pos.orientation.y \
+ ,p3d.pos.orientation.z \
+ ,p3d.pos.orientation.w]
+
+ target_q_inv = quaternion_from_rotation_matrix( linalg.inv(target_rm) )
+
+ rot_euler = euler_from_quaternion( quaternion_multiply(p3d_q, target_q_inv) )
+
+ rot_error = abs( rot_euler[0] ) + \
+ abs( rot_euler[1] ) + \
+ abs( rot_euler[2] )
+
+ print " arm Error pos: " + str(pos_error) + " rot: " + str(rot_error)
#self.printP3D(p3d)
# has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
if self.reached_target_arm:
print " arm duration: " + str(time.time() - self.duration_start_arm)
- if error < TARGET_TOL:
+ if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
if time.time() - self.duration_start_arm > TARGET_DURATION:
- self.success = True
+ self.arm_success = True
else:
# failed to maintain target vw, reset duration
- self.success = False
+ self.arm_success = False
self.reached_target_arm = False
else:
- if error < TARGET_TOL:
+ if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
self.reached_target_arm = True
self.duration_start_arm = time.time()
@@ -166,13 +207,11 @@
rospy.Subscriber("l_gripper_l_finger_pose_ground_truth", PoseWithRatesStamped, self.gripperP3dInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + TEST_TIMEOUT
- while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
- pub_arm.publish(JointPosCmd(['l_shoulder_pan_joint','l_shoulder_lift_joint','l_upper_arm_roll_joint','l_elbow_flex_joint','l_forearm_roll_joint','l_wrist_flex_joint','l_wrist_roll_joint'],[CMD_POS,CMD_POS,CMD_POS,CMD_POS,CMD_POS,CMD_POS,CMD_POS],[0,0,0,0,0,0,0],0))
- pub_gripper.publish(Float64(CMD_POS))
- time.sleep(0.5)
- self.assert_(self.success)
-
+ while not rospy.is_shutdown() and (not self.arm_success or not self.grp_success) and time.time() < timeout_t:
+ pub_arm.publish(JointPosCmd(ARM_JNT_NAMES,CMD_POS,CMD_VEL,0))
+ pub_gripper.publish(Float64(GRP_CMD_POS))
+ time.sleep(1.0)
+ self.assert_(self.arm_success and self.grp_success)
if __name__ == '__main__':
rostest.run(PKG, sys.argv[0], ArmTest, sys.argv) #, text_mode=True)
-
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml 2009-01-06 22:34:45 UTC (rev 8918)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml 2009-01-06 22:36:09 UTC (rev 8919)
@@ -4,13 +4,6 @@
<!-- send single_link.xml to param server -->
<include file="$(find arm_gazebo)/arm_no_x.launch" />
- <!-- start arm controller -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" /> <!-- load default arm controller -->
-
- <!-- send arm a command -->
- <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
-
-
<!--<node pkg="gazebo_plugin" type="groundtruthtransform" args="" respawn="true" />-->
<test test-name="gazebo_plugin_test_arm" pkg="gazebo_plugin" type="test_arm.py" />
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-01-07 00:18:51
|
Revision: 8928
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8928&view=rev
Author: tpratkanis
Date: 2009-01-07 00:18:47 +0000 (Wed, 07 Jan 2009)
Log Message:
-----------
Move planning_node_util to robot_model in the world_models directory.
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/world_models/robot_model/CMakeLists.txt
pkg/trunk/world_models/robot_model/include/robot_model/cnode.h
pkg/trunk/world_models/robot_model/include/robot_model/knode.h
pkg/trunk/world_models/world_3d_map/manifest.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Added Paths:
-----------
pkg/trunk/world_models/robot_model/
pkg/trunk/world_models/robot_model/include/robot_model/
Removed Paths:
-------------
pkg/trunk/motion_planning/planning_node_util/
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-07 00:10:40 UTC (rev 8927)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-07 00:18:47 UTC (rev 8928)
@@ -8,7 +8,7 @@
<depend package="wg_robot_description" />
<depend package="robot_msgs" />
<depend package="robot_srvs" />
- <depend package="planning_node_util" />
+ <depend package="robot_model" />
<depend package="ompl" />
<depend package="profiling_utils" />
<depend package="string_utils" />
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-07 00:10:40 UTC (rev 8927)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-07 00:18:47 UTC (rev 8928)
@@ -130,7 +130,7 @@
**/
-#include <planning_node_util/cnode.h>
+#include <robot_model/cnode.h>
#include <std_msgs/String.h>
#include "RKPModel.h"
@@ -140,12 +140,12 @@
#include <robot_srvs/NamedKinematicPlanState.h>
class KinematicPlanning : public ros::node,
- public planning_node_util::NodeCollisionModel
+ public robot_model::NodeCollisionModel
{
public:
KinematicPlanning(const std::string &robot_model) : ros::node("kinematic_planning"),
- planning_node_util::NodeCollisionModel(dynamic_cast<ros::node*>(this),
+ robot_model::NodeCollisionModel(dynamic_cast<ros::node*>(this),
robot_model)
{
advertise_service("plan_kinematic_path_state", &KinematicPlanning::planToState);
@@ -345,7 +345,7 @@
virtual void setRobotDescription(robot_desc::URDF *file)
{
- planning_node_util::NodeCollisionModel::setRobotDescription(file);
+ robot_model::NodeCollisionModel::setRobotDescription(file);
defaultPosition();
printf("=======================================\n");
@@ -400,6 +400,7 @@
const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "RRT");
}
+
model->addRRT(options);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-07 00:10:40 UTC (rev 8927)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-07 00:18:47 UTC (rev 8928)
@@ -88,7 +88,7 @@
**/
-#include <planning_node_util/cnode.h>
+#include <robot_model/cnode.h>
#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
#include <robot_srvs/ValidateKinematicPath.h>
@@ -101,7 +101,7 @@
#include <map>
class MotionValidator : public ros::node,
- public planning_node_util::NodeCollisionModel
+ public robot_model::NodeCollisionModel
{
public:
@@ -127,7 +127,7 @@
};
MotionValidator(const std::string &robot_model) : ros::node("motion_validator"),
- planning_node_util::NodeCollisionModel(dynamic_cast<ros::node*>(this),
+ robot_model::NodeCollisionModel(dynamic_cast<ros::node*>(this),
robot_model)
{
advertise_service("validate_path", &MotionValidator::validatePath);
@@ -205,7 +205,7 @@
virtual void setRobotDescription(robot_desc::URDF *file)
{
- planning_node_util::NodeCollisionModel::setRobotDescription(file);
+ robot_model::NodeCollisionModel::setRobotDescription(file);
printf("=======================================\n");
m_kmodel->printModelInfo();
Modified: pkg/trunk/world_models/robot_model/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/CMakeLists.txt 2009-01-06 02:42:45 UTC (rev 8864)
+++ pkg/trunk/world_models/robot_model/CMakeLists.txt 2009-01-07 00:18:47 UTC (rev 8928)
@@ -1,3 +1,3 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
-rospack(planning_node_util)
+rospack(robot_model)
Modified: pkg/trunk/world_models/robot_model/include/robot_model/cnode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2009-01-06 22:09:07 UTC (rev 8914)
+++ pkg/trunk/world_models/robot_model/include/robot_model/cnode.h 2009-01-07 00:18:47 UTC (rev 8928)
@@ -34,12 +34,12 @@
/** \author Ioan Sucan */
-#include <planning_node_util/knode.h>
+#include <robot_model/knode.h>
#include <std_msgs/PointCloud.h>
#include <collision_space/environmentODE.h>
/** Main namespace */
-namespace planning_node_util
+namespace robot_model
{
/**
Modified: pkg/trunk/world_models/robot_model/include/robot_model/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2009-01-06 22:09:07 UTC (rev 8914)
+++ pkg/trunk/world_models/robot_model/include/robot_model/knode.h 2009-01-07 00:18:47 UTC (rev 8928)
@@ -54,7 +54,7 @@
#include <robot_msgs/MechanismState.h>
/** Main namespace */
-namespace planning_node_util
+namespace robot_model
{
/**
@b NodeRobotModel is a class that is also of a given robot model and
@@ -88,7 +88,7 @@
public:
- NodeRobotModel(ros::node *node, const std::string &robot_model) : m_tf(*node, true, 1000000000ULL)
+ NodeRobotModel(ros::node *node, const std::string &robot_model_name) : m_tf(*node, true, 1000000000ULL)
{
m_tf.setExtrapolationLimit(ros::Duration().fromSec(10));
m_urdf = NULL;
@@ -98,7 +98,7 @@
m_robotStateSimple = NULL;
m_node = node;
m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
- m_robotModelName = robot_model;
+ m_robotModelName = robot_model_name;
m_haveState = false;
m_haveMechanismState = false;
m_haveBasePos = false;
Modified: pkg/trunk/world_models/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/manifest.xml 2009-01-07 00:10:40 UTC (rev 8927)
+++ pkg/trunk/world_models/world_3d_map/manifest.xml 2009-01-07 00:18:47 UTC (rev 8928)
@@ -12,5 +12,5 @@
<depend package="wg_robot_description_parser" />
<depend package="planning_models" />
<depend package="collision_space" />
-<depend package="planning_node_util" />
+<depend package="robot_model" />
</package>
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2009-01-07 00:10:40 UTC (rev 8927)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2009-01-07 00:18:47 UTC (rev 8928)
@@ -102,7 +102,7 @@
- @b "world_3d_map/verbosity_level" : @b [int] sets the verbosity level (default 1)
**/
-#include <planning_node_util/knode.h>
+#include <robot_model/knode.h>
#include <rosthread/member_thread.h>
#include <rosthread/mutex.h>
@@ -120,12 +120,12 @@
#include <cmath>
class World3DMap : public ros::node,
- public planning_node_util::NodeRobotModel
+ public robot_model::NodeRobotModel
{
public:
- World3DMap(const std::string &robot_model) : ros::node("world_3d_map"),
- planning_node_util::NodeRobotModel(dynamic_cast<ros::node*>(this), robot_model)
+ World3DMap(const std::string &robot_model_name) : ros::node("world_3d_map"),
+ robot_model::NodeRobotModel(dynamic_cast<ros::node*>(this), robot_model_name)
{
advertise<std_msgs::PointCloud>("world_3d_map", 1);
@@ -163,7 +163,7 @@
virtual void setRobotDescription(robot_desc::URDF *file)
{
- planning_node_util::NodeRobotModel::setRobotDescription(file);
+ robot_model::NodeRobotModel::setRobotDescription(file);
addSelfSeeBodies();
}
@@ -188,7 +188,7 @@
return;
}
//m_robotState->print();
- planning_node_util::NodeRobotModel::stateUpdate();
+ robot_model::NodeRobotModel::stateUpdate();
if (m_kmodel)
m_kmodel->computeTransforms(m_robotState->getParams());
if (m_kmodelSimple)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-01-07 01:05:01
|
Revision: 8944
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8944&view=rev
Author: hsujohnhsu
Date: 2009-01-07 01:04:57 +0000 (Wed, 07 Jan 2009)
Log Message:
-----------
* rename arm.launch to l_arm.launch and arm_no_x.launch to l_arm_no_x.launch, similarly added r_arm.launch and r_arm_no_x.launch with ArmTrajectoryController
* in pr2_arm_test: rename pr2_arm.xacro.xml to pr2_l_arm.xacro.xml. rename groups_arm.xml to groups_l_arm.xml. similarly added right arm xacro files.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/arm_grav.launch
pkg/trunk/demos/arm_gazebo/test_launch_order.tcsh
pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml
Added Paths:
-----------
pkg/trunk/demos/arm_gazebo/l_arm.launch
pkg/trunk/demos/arm_gazebo/l_arm_dynamic_controller.xml
pkg/trunk/demos/arm_gazebo/l_arm_no_x.launch
pkg/trunk/demos/arm_gazebo/r_arm.launch
pkg/trunk/demos/arm_gazebo/r_arm_no_x.launch
pkg/trunk/demos/arm_gazebo/r_arm_trajectory_controller.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_l_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_r_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_l_arm.xacro.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_r_arm.xacro.xml
Removed Paths:
-------------
pkg/trunk/demos/arm_gazebo/arm.launch
pkg/trunk/demos/arm_gazebo/arm_no_x.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xacro.xml
Deleted: pkg/trunk/demos/arm_gazebo/arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm.launch 2009-01-07 01:02:31 UTC (rev 8943)
+++ pkg/trunk/demos/arm_gazebo/arm.launch 2009-01-07 01:04:57 UTC (rev 8944)
@@ -1,27 +0,0 @@
-<launch>
- <group name="wg">
- <!-- send pr2_arm.xml to param server -->
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_arm.xacro.xml'" />
-
- <!-- -g flag runs gazebo in gui-less mode -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- </node>
-
- <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
-
- <!-- start arm controller -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" /> <!-- load default arm controller -->
-
- <!-- send arm a command -->
- <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
-
- <!-- for visualization -->
- <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
- </group>
-</launch>
-
Modified: pkg/trunk/demos/arm_gazebo/arm_grav.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm_grav.launch 2009-01-07 01:02:31 UTC (rev 8943)
+++ pkg/trunk/demos/arm_gazebo/arm_grav.launch 2009-01-07 01:04:57 UTC (rev 8944)
@@ -1,6 +1,6 @@
<launch>
<group name="wg">
- <!-- send pr2_arm.xml to param server -->
+ <!-- send pr2_grav_arm.xml to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_grav_arm.xacro.xml'" />
<!-- -g flag runs gazebo in gui-less mode -->
Deleted: pkg/trunk/demos/arm_gazebo/arm_no_x.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm_no_x.launch 2009-01-07 01:02:31 UTC (rev 8943)
+++ pkg/trunk/demos/arm_gazebo/arm_no_x.launch 2009-01-07 01:04:57 UTC (rev 8944)
@@ -1,27 +0,0 @@
-<launch>
- <group name="wg">
- <!-- send pr2_arm.xml to param server -->
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_arm.xacro.xml'" />
-
- <!-- -g flag runs gazebo in gui-less mode -->
- <node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib:$(env LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- </node>
-
- <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
-
- <!-- start arm controller -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" /> <!-- load default arm controller -->
-
- <!-- send arm a command -->
- <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
-
- <!-- for visualization -->
- <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
- </group>
-</launch>
-
Added: pkg/trunk/demos/arm_gazebo/l_arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/l_arm.launch 2009-01-07 01:04:57 UTC (rev 8944)
@@ -0,0 +1,27 @@
+<launch>
+ <group name="wg">
+ <!-- send pr2_l_arm.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_l_arm.xacro.xml'" />
+
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
+
+ <!-- start arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" /> <!-- load default arm controller -->
+
+ <!-- send arm a command -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+
+ <!-- for visualization -->
+ <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
+ </group>
+</launch>
+
Added: pkg/trunk/demos/arm_gazebo/l_arm_dynamic_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm_dynamic_controller.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/l_arm_dynamic_controller.xml 2009-01-07 01:04:57 UTC (rev 8944)
@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+
+<controllers>
+ <!-- ========================================= -->
+ <!-- right arm array -->
+ <controller name="left_arm_controller" type="ArmDynamicsControllerNode">
+ <listen_topic name="left_arm_commands" />
+ <kinematics>
+ <elem key="kdl_chain_name">left_arm</elem>
+ </kinematics>
+ <map name="controller_param">
+ <elem key="kdl_chain_name">left_arm</elem>
+ </map>
+ <controller name="l_shoulder_pan_controller" topic="l_shoulder_pan_controller" type="JointEffortController">
+ <joint name="l_shoulder_pan_joint" >
+ <pid p="0" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="l_shoulder_lift_controller" topic="l_shoulder_lift_controller" type="JointEffortController">
+ <joint name="l_shoulder_lift_joint" >
+ <pid p="0" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="l_upper_arm_roll_controller" topic="l_upper_arm_roll_controller" type="JointEffortController">
+ <joint name="l_upper_arm_roll_joint" >
+ <pid p="0" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="l_elbow_flex_controller" topic="l_elbow_flex_controller" type="JointEffortController">
+ <joint name="l_elbow_flex_joint" >
+ <pid p="0" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="l_forearm_roll_controller" topic="l_forearm_roll_controller" type="JointEffortController">
+ <joint name="l_forearm_roll_joint" >
+ <pid p="0" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="l_wrist_flex_controller" topic="l_wrist_flex_controller" type="JointEffortController">
+ <joint name="l_wrist_flex_joint" >
+ <pid p="0" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="l_wrist_roll_controller" topic="l_wrist_roll_controller" type="JointEffortController">
+ <joint name="l_wrist_roll_joint" >
+ <pid p="0" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ </controller>
+ <controller name="l_gripper_controller" topic="l_gripper_controller" type="JointPositionControllerNode">
+ <joint name="l_gripper_l_finger_joint">
+ <pid p="0.5" d="0.000001" i="0.001" iClamp="0.01" />
+ </joint>
+ </controller>
+</controllers>
Added: pkg/trunk/demos/arm_gazebo/l_arm_no_x.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm_no_x.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/l_arm_no_x.launch 2009-01-07 01:04:57 UTC (rev 8944)
@@ -0,0 +1,27 @@
+<launch>
+ <group name="wg">
+ <!-- send pr2_l_arm.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_l_arm.xacro.xml'" />
+
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib:$(env LD_LIBRARY_PATH)" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
+ <!-- start arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" /> <!-- load default arm controller -->
+
+ <!-- send arm a command -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+
+ <!-- for visualization -->
+ <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
+ </group>
+</launch>
+
Copied: pkg/trunk/demos/arm_gazebo/r_arm.launch (from rev 8893, pkg/trunk/demos/arm_gazebo/arm.launch)
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/r_arm.launch 2009-01-07 01:04:57 UTC (rev 8944)
@@ -0,0 +1,30 @@
+<launch>
+ <group name="wg">
+ <!-- send pr2_r_arm.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_r_arm.xacro.xml'" />
+
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
+
+ <!-- start arm controller -->
+ <!--
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_default_controller.xml" respawn="false" />
+ -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_trajectory_controller.xml" respawn="false" />
+
+ <!-- send arm a command -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set r_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+
+ <!-- for visualization -->
+ <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
+ </group>
+</launch>
+
Copied: pkg/trunk/demos/arm_gazebo/r_arm_no_x.launch (from rev 8929, pkg/trunk/demos/arm_gazebo/arm_no_x.launch)
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_no_x.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/r_arm_no_x.launch 2009-01-07 01:04:57 UTC (rev 8944)
@@ -0,0 +1,30 @@
+<launch>
+ <group name="wg">
+ <!-- send pr2_r_arm.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_r_arm.xacro.xml'" />
+
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib:$(env LD_LIBRARY_PATH)" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
+ <!-- start arm controller -->
+ <!--
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_default_controller.xml" respawn="false" />
+ -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_trajectory_controller.xml" respawn="false" />
+
+ <!-- send arm a command -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set r_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+
+ <!-- for visualization -->
+ <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
+ </group>
+</launch>
+
Added: pkg/trunk/demos/arm_gazebo/r_arm_trajectory_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_trajectory_controller.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/r_arm_trajectory_controller.xml 2009-01-07 01:04:57 UTC (rev 8944)
@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+
+<controllers>
+ <!-- ========================================= -->
+ <!-- right arm array -->
+ <controller name="right_arm_controller" type="ArmTrajectoryControllerNode">
+ <listen_topic name="arm_trajectory_command" />
+ <kinematics>
+ <elem key="kdl_chain_name">right_arm</elem>
+ </kinematics>
+ <map name="controller_param">
+ <elem key="kdl_chain_name">right_arm</elem>
+ </map>
+ <controller name="r_shoulder_pan_controller" topic="r_shoulder_pan_controller" type="JointPositionController">
+ <joint name="r_shoulder_pan_joint" >
+ <pid p="10.0" d="2.0" i="1.0" iClamp="4.0" />
+ </joint>
+ </controller>
+ <controller name="r_shoulder_lift_controller" topic="r_shoulder_pitch_controller" type="JointPositionController">
+ <joint name="r_shoulder_lift_joint" >
+ <pid p="10.0" d="2.0" i="1.0" iClamp="4.0" />
+ </joint>
+ </controller>
+ <controller name="r_upperarm_roll_controller" topic="r_upperarm_roll_controller" type="JointPositionController">
+ <joint name="r_upper_arm_roll_joint" >
+ <pid p="4.0" d="1.0" i="0.5" iClamp="1.0" />
+ </joint>
+ </controller>
+ <controller name="r_elbow_controller" topic="r_elbow_flex_controller" type="JointPositionController">
+ <joint name="r_elbow_flex_joint" >
+ <pid p="10.0" d="2.0" i="1.0" iClamp="4.0" />
+ </joint>
+ </controller>
+ <controller name="r_forearm_roll_controller" topic="r_forearm_roll_controller" type="JointPositionController">
+ <joint name="r_forearm_roll_joint" >
+ <pid p="4.0" d="1.0" i="0.5" iClamp="1.0" />
+ </joint>
+ </controller>
+
+ <controller name="r_wrist_flex_controller" type="JointPositionController">
+ <joint name="r_wrist_flex_joint">
+ <pid p="4" i="0.5" d="2.0" iClamp="4.0" />
+ </joint>
+ </controller>
+
+ <controller name="r_wrist_roll_controller" type="JointPositionController">
+ <joint name="r_wrist_roll_joint">
+ <pid p="4" i="0.5" d="2.0" iClamp="4.0" />
+ </joint>
+ </controller>
+
+
+ <trajectory interpolation="linear" />
+
+ </controller>
+</controllers>
Modified: pkg/trunk/demos/arm_gazebo/test_launch_order.tcsh
===================================================================
--- pkg/trunk/demos/arm_gazebo/test_launch_order.tcsh 2009-01-07 01:02:31 UTC (rev 8943)
+++ pkg/trunk/demos/arm_gazebo/test_launch_order.tcsh 2009-01-07 01:04:57 UTC (rev 8944)
@@ -13,7 +13,7 @@
echo "----------------roslaunch xml"
-`rospack find xacro`/xacro.py `rospack find wg_robot_description`/pr2_arm_test/pr2_arm.xacro.xml > pr2_arm.xml
+`rospack find xacro`/xacro.py `rospack find wg_robot_description`/pr2_arm_test/pr2_r_arm.xacro.xml > pr2_arm.xml
#$ROS_ROOT/bin/rosparam set robotdesc/pr2
python ./setparam.py
@@ -21,9 +21,9 @@
`rospack find gazebo_plugin`/urdf2factory robotdesc/pr2
echo "----------------spawn controller"
-`rospack find mechanism_control`/scripts/mech.py sp `rospack find arm_gazebo`/l_arm_default_controller.xml
+`rospack find mechanism_control`/scripts/mech.py sp `rospack find arm_gazebo`/r_arm_default_controller.xml
echo "----------------set gripper gap"
-`rospack find robot_mechanism_controllers`/scripts/control.py set l_gripper_controller 0.0
+`rospack find robot_mechanism_controllers`/scripts/control.py set r_gripper_controller 0.0
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml 2009-01-07 01:02:31 UTC (rev 8943)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml 2009-01-07 01:04:57 UTC (rev 8944)
@@ -2,7 +2,7 @@
<master auto="start" />
<!-- send single_link.xml to param server -->
- <include file="$(find arm_gazebo)/arm_no_x.launch" />
+ <include file="$(find arm_gazebo)/l_arm_no_x.launch" />
<!--<node pkg="gazebo_plugin" type="groundtruthtransform" args="" respawn="true" />-->
<test test-name="gazebo_plugin_test_arm" pkg="gazebo_plugin" type="test_arm.py" />
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_arm.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_arm.xml 2009-01-07 01:02:31 UTC (rev 8943)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_arm.xml 2009-01-07 01:04:57 UTC (rev 8944)
@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-
-<robot name="pr2"><!-- name of the robot-->
-
- <group name="left_arm" flags="planning kinematic">
- l_shoulder_pan_link
- l_shoulder_lift_link
- l_upper_arm_roll_link
- l_elbow_flex_link
- l_forearm_roll_link
- l_wrist_flex_link
- l_wrist_roll_link
-
- <map name="RRT" flag="planning">
- <elem key="range" value="0.75" />
- </map>
-
- <map name="LazyRRT" flag="planning">
- <elem key="range" value="0.75" />
- </map>
- </group>
-
-</robot>
Copied: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_l_arm.xml (from rev 8893, pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_arm.xml)
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_l_arm.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_l_arm.xml 2009-01-07 01:04:57 UTC (rev 8944)
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+
+<robot name="pr2"><!-- name of the robot-->
+
+ <group name="left_arm" flags="planning kinematic">
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ l_upper_arm_roll_link
+ l_elbow_flex_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+
+ <map name="RRT" flag="planning">
+ <elem key="range" value="0.75" />
+ </map>
+
+ <map name="LazyRRT" flag="planning">
+ <elem key="range" value="0.75" />
+ </map>
+ </group>
+
+</robot>
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_r_arm.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_r_arm.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_r_arm.xml 2009-01-07 01:04:57 UTC (rev 8944)
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+
+<robot name="pr2"><!-- name of the robot-->
+
+ <group name="right_arm" flags="planning kinematic">
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ r_upper_arm_roll_link
+ r_elbow_flex_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+
+ <map name="RRT" flag="planning">
+ <elem key="range" value="0.75" />
+ </map>
+
+ <map name="LazyRRT" flag="planning">
+ <elem key="range" value="0.75" />
+ </map>
+ </group>
+
+</robot>
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xacro.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xacro.xml 2009-01-07 01:02:31 UTC (rev 8943)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xacro.xml 2009-01-07 01:04:57 UTC (rev 8944)
@@ -1,77 +0,0 @@
-<?xml version="1.0"?>
-<robot name="pr2"
- xmlns:xi="http://www.w3.org/2001/XInclude"
- xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
- xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
- xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
- xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
- xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
- xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
- xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
- xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
- xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
- xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
- xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
-
-
- <!-- declare the path where all models/textures/materials are stored -->
- <resource location="ros-pkg://wg_robot_description/models/pr2"/>
-
- <include filename="../pr2_robot_defs/arm_defs.xml" />
- <include filename="../pr2_robot_defs/gripper_defs.xml" />
- <include filename="../pr2_robot_defs/gazebo_defs.xml" />
- <include filename="./groups_arm.xml" />
-
- <pr2_arm side="l" reflect="1" parent="base">
- <origin xyz="0.0 0.0 1.0" rpy="0 0 0" />
- </pr2_arm>
- <pr2_gripper side="l" parent="l_wrist_roll" />
-
- <!-- Solid Base -->
- <joint name="base_joint" type="planar">
- </joint>
- <link name="base_link"><!-- link specifying the base of the robot -->
- <parent name=" world" />
- <!-- rotation of a local coordinate frame attached to the link with respect to a global coordinate frame -->
- <origin xyz=" 0 0 0.002 " rpy=" 0 0 0" /> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
- <joint name="base_joint" />
- <inertial>
- <mass value="1000" />
- <com xyz=" 0 0 0 " /> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
- <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
- <map name="gazebo_material" flag="gazebo">
- <elem key="material">Gazebo/White</elem>
- </map>
- <geometry name="pr2_base_mesh_file">
- <mesh scale="20 20 0.01" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="0 0 0" rpy="0.0 0.0 0.0 " /> <!-- default box is centered at the origin -->
- <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
- <geometry name="base_collision_geom"> <!-- think about putting mesh here as well -->
- <box size="20 20 0.01" />
- </geometry>
- </collision>
- </link>
-
- <map name="gazebo_material" flag="gazebo">
- <verbatim>
- <!-- ros_p3d for position groundtruth -->
- <controller:ros_p3d name="p3d_l_wrist_controller" plugin="libros_p3d.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <bodyName>l_gripper_palm_link</bodyName>
- <topicName>l_gripper_palm_pose_ground_truth</topicName>
- <frameName>map</frameName>
- <interface:position name="p3d_l_wrist_position"/>
- </controller:ros_p3d>
- </verbatim>
- </map>
-
-
-</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml 2009-01-07 01:02:31 UTC (rev 8943)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml 2009-01-07 01:04:57 UTC (rev 8944)
@@ -20,7 +20,7 @@
<include filename="../pr2_robot_defs/arm_grav_defs.xml" />
<include filename="../pr2_robot_defs/gripper_defs.xml" />
<include filename="../pr2_robot_defs/gazebo_defs.xml" />
- <include filename="./groups_arm.xml" />
+ <include filename="./groups_l_arm.xml" />
<pr2_arm side="l" reflect="1" parent="base">
<origin xyz="0.0 0.0 1.0" rpy="0 0 0" />
Copied: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_l_arm.xacro.xml (from rev 8893, pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xacro.xml)
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_l_arm.xacro.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_l_arm.xacro.xml 2009-01-07 01:04:57 UTC (rev 8944)
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot name="pr2"
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#contro...
[truncated message content] |
|
From: <tf...@us...> - 2009-01-07 01:57:33
|
Revision: 8959
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8959&view=rev
Author: tfoote
Date: 2009-01-07 01:53:02 +0000 (Wed, 07 Jan 2009)
Log Message:
-----------
ros::mutex to boost::mutex
Modified Paths:
--------------
pkg/trunk/nav/wavefront_player/manifest.xml
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/nav/wavefront_player/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront_player/manifest.xml 2009-01-07 01:50:53 UTC (rev 8958)
+++ pkg/trunk/nav/wavefront_player/manifest.xml 2009-01-07 01:53:02 UTC (rev 8959)
@@ -4,6 +4,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
+ <depend package="boost" />
<depend package="player" />
<depend package="laser_scan" />
<depend package="std_srvs" />
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2009-01-07 01:50:53 UTC (rev 8958)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2009-01-07 01:53:02 UTC (rev 8959)
@@ -83,7 +83,7 @@
// roscpp
#include <ros/node.h>
-#include <rosthread/mutex.h>
+#include "boost/thread/mutex.hpp"
#include <std_msgs/LaserScan.h>
#include <std_msgs/RobotBase2DOdom.h>
#include <std_msgs/PoseWithRatesStamped.h>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-07 08:16:18
|
Revision: 8982
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8982&view=rev
Author: rdiankov
Date: 2009-01-07 08:16:13 +0000 (Wed, 07 Jan 2009)
Log Message:
-----------
forgot some openrave stuff
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-07 08:15:10 UTC (rev 8981)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-07 08:16:13 UTC (rev 8982)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 597
+SVN_REVISION = -r 598
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2009-01-07 08:15:10 UTC (rev 8981)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2009-01-07 08:16:13 UTC (rev 8982)
@@ -41,7 +41,7 @@
end
end
-if( ~exist('sessionserver','var') )
+if( ~exist('sessionserver','var') || isempty(sessionserver) )
sessionserver = 'openrave_session';
end
openraveros_globalsession = openraveros_createsession(sessionserver);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-08 01:14:46
|
Revision: 9026
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9026&view=rev
Author: rdiankov
Date: 2009-01-08 01:14:36 +0000 (Thu, 08 Jan 2009)
Log Message:
-----------
fixed several bugs in the laser/camera calibration
Modified Paths:
--------------
pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml
pkg/trunk/calibration/laser_camera_calibration/octave/QuatFromRotationMatrix.m
pkg/trunk/calibration/laser_camera_calibration/octave/SegmentLines.m
pkg/trunk/calibration/laser_camera_calibration/octave/calibratevalues.m
pkg/trunk/calibration/laser_camera_calibration/octave/runcalibration.m
pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
Modified: pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml 2009-01-08 01:14:36 UTC (rev 9026)
@@ -1,12 +1,6 @@
<launch>
<machine name="localhost_cameras" address="localhost" default="false"/>
-<!-- <node machine="cameras" name="dcam" pkg="dcam" type="dcam" respawn="false">
- <param name="video_mode" type="str" value="640x480_videre_rectified"/>
- <param name="do_rectify" type="bool" value="False"/>
- <param name="do_stereo" type="bool" value="False"/>
- <param name="do_calc_points" type="bool" value="False"/>
- </node> -->
<group ns="checkerdetector" clear_params="true">
<param name="display" type="int" value="1"/>
<param name="frame_id" type="string" value="head_tilt_link"/>
@@ -18,20 +12,31 @@
<node machine="localhost_cameras" pkg="checkerboard_detector" type="checkerboard_detector" respawn="true" output="screen">
<remap from="CamInfo" to="/stereo/left/cam_info"/>
<remap from="Image" to="/stereo/left/image_rect"/>
-<!-- <env name="DISPLAY" value=":0.0"/> -->
+ <!-- <env name="DISPLAY" value=":0.0"/> -->
</node>
</group>
-
+
+ <!-- start nodding laser -->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find wg_robot_description)/laser_tilt/laser_tilt_controller.xml" />
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 10 .75 .25" />
+
<!-- start the filtering node -->
<node machine="localhost_cameras" pkg="laser_camera_calibration" type="gatherdata.py" output="screen">
<remap from="ObjectDetection" to="/checkerdetector/ObjectDetection"/>
</node>
- <!-- start nodding laser -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find wg_robot_description)/laser_tilt/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 10 .75 .25" />
-
<!-- start recording -->
<node pkg="rosrecord" type="rosrecord" args="-f $(find laser_camera_calibration)/lasercamcalib /new_ObjectDetection /new_tilt_scan /new_mechanism_state"/>
+
+ <!-- DCAM - this should be brough up with the robot, but in case they are not, comment this out -->
+<!-- <group ns="stereo">
+ <param name="videre_mode" type="str" value="rectified"/>
+ <param name="do_colorize" type="bool" value="False"/>
+ <param name="do_rectify" type="bool" value="False"/>
+ <param name="do_stereo" type="bool" value="False"/>
+ <param name="do_calc_points" type="bool" value="False"/>
+ </group>
+
+ <node machine="camera-machine" pkg="dcam" type="stereodcam" respawn="false"/>
+ <node machine="camera-machine" pkg="stereo_image_proc" type="stereoproc" respawn="false"/> -->
</launch>
-
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/QuatFromRotationMatrix.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/QuatFromRotationMatrix.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/QuatFromRotationMatrix.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -41,22 +41,22 @@
switch(convcase)
case 1
s = sqrt((R(1,1) - (R(2,2) + R(3,3))) + 1);
- quat(2) = (dReal)(0.5) * s;
- s = (dReal)(0.5) / s;
+ quat(2) = 0.5 * s;
+ s = 0.5 / s;
quat(3) = (R(1,2) + R(2,1)) * s;
quat(4) = (R(3,1) + R(1,3)) * s;
quat(1) = (R(3,2) - R(2,3)) * s;
case 2
- s = RaveSqrt((R(2,2) - (R(3,3) + R(1,1))) + 1);
- quat(3) = (dReal)(0.5) * s;
- s = (dReal)(0.5) / s;
+ s = sqrt((R(2,2) - (R(3,3) + R(1,1))) + 1);
+ quat(3) = 0.5 * s;
+ s = 0.5 / s;
quat(4) = (R(2,3) + R(3,2)) * s;
quat(2) = (R(1,2) + R(2,1)) * s;
quat(1) = (R(1,3) - R(3,1)) * s;
case 3
- s = RaveSqrt((R(3,3) - (R(1,1) + R(2,2))) + 1);
- quat(4) = (dReal)(0.5) * s;
- s = (dReal)(0.5) / s;
+ s = sqrt((R(3,3) - (R(1,1) + R(2,2))) + 1);
+ quat(4) = 0.5 * s;
+ s = 0.5 / s;
quat(2) = (R(3,1) + R(1,3)) * s;
quat(3) = (R(2,3) + R(3,2)) * s;
quat(1) = (R(2,1) - R(1,2)) * s;
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/SegmentLines.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/SegmentLines.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/SegmentLines.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -38,7 +38,7 @@
pointthresh = 1;
end
-thresh = 0.015;
+thresh = 0.025;
linedata = [];
% dists = polar_points(1,:);
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/calibratevalues.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/calibratevalues.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/calibratevalues.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -1,7 +1,9 @@
-%% [Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot)
+%% [Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot, Tcamerainit, Tlaserinit)
%%
-%% Needs openrave to perform the kinematics
-function [Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot)
+%% The main optimization loop to calibrate the values, needs openraveros to perform the kinematics
+%% Tcamerainit (optional) - 3x4 matrix of the initial camera frame
+%% Tlaserinit (optional) - 3x4 matrix of the initial laser frame
+function [Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot, Tcamerainit, Tlaserinit)
tic;
links = orBodyGetLinks(robot.id);
@@ -16,7 +18,7 @@
end
% joint offsets to calibrate
-jointnames = {};%'head_pan_joint'};
+jointnames = {'head_pan_joint'};
joints = [];
for i = 1:length(jointnames)
joints(i) = find(cellfun(@(x) strcmp(x,jointnames{i}), robot.jointnames),1,'first');
@@ -32,16 +34,22 @@
end
%% get initial values
-Tlaser = inv([reshape(links(:,laser_tilt_mount_link),[3 4]); 0 0 0 1]) * [reshape(links(:,laser_tilt_link),[3 4]); 0 0 0 1];
-Tinvcamera = inv([reshape(links(:,stereo_link),[3 4]); 0 0 0 1]) * [reshape(links(:,head_tilt_link),[3 4]); 0 0 0 1];
-Xinit = [AxisAngleFromRotation(Tinvcamera(1:3,1:3));Tinvcamera(1:3,4);...
- AxisAngleFromRotation(Tlaser(1:3,1:3));Tlaser(1:3,4);zeros(size(joints))];
+if( ~exist('Tcamerainit','var') || isempty(Tcamerainit) )
+ Tcamerainit = inv([reshape(links(:,head_tilt_link),[3 4]); 0 0 0 1]) * [reshape(links(:,stereo_link),[3 4]); 0 0 0 1];
+end
+if( ~exist('Tlaserinit','var') || isempty(Tlaserinit) )
+ Tlaserinit = inv([reshape(links(:,laser_tilt_mount_link),[3 4]); 0 0 0 1]) * [reshape(links(:,laser_tilt_link),[3 4]); 0 0 0 1];
+end
+
+Tcamerainitinv = inv([Tcamerainit(1:3,1:4); 0 0 0 1]);
+Xinit = [AxisAngleFromRotation(Tcamerainitinv(1:3,1:3));Tcamerainitinv(1:3,4);...
+ AxisAngleFromRotation(Tlaserinit(1:3,1:3));Tlaserinit(1:3,4);zeros(size(joints))];
+
x = zeros(length(calibdata),1);
y = x;
display('starting to optimize');
-[f,X, kvg, iter] = leasqr(x,y, Xinit, @(data,X) calibdist(X,robot,calibdata,joints,laser_tilt_mount_link,head_tilt_link), 0.0001, 100);
-f
+[f,X, kvg, iter] = leasqr(x,y, Xinit, @(data,X) calibdist(X,robot,calibdata,joints,laser_tilt_mount_link,head_tilt_link), 0.0001, 400);
Tcamera = inv([RotationMatrixFromAxisAngle(X(1:3)) X(4:6); 0 0 0 1]);
Tlaser = [RotationMatrixFromAxisAngle(X(7:9)) X(10:12); 0 0 0 1];
jointoffsets = zeros(robot.dof,1);
@@ -71,7 +79,9 @@
end
Nlaserplane = transpose(Tinvcamera * inv(Thead_tilt_link) * Tlaser_tilt_mount_link * Tlaser) * calibdata{i}.Ncamera;
- Nlaserplane = Nlaserplane / norm(Nlaserplane(1:3));
+ if( abs(norm(Nlaserplane(1:3))-1) > 0.01 )
+ error('wtf');
+ end
%% find the 'area' under the line using the plane as the 0
%% first transform the line in plane space so that y axis is plane
@@ -99,9 +109,15 @@
dist = s1(2) * (s1(1)-s2(1));
end
- F(i) = min(dist,1000);
+% dist
+% clf();
+% plot([s1(1) s2(1)],[0 0],'r'); hold on;
+% plot([s1(1) s2(1)],[s1(2) s2(2)],'b','linewidth',3);
+% drawnow;
+ F(i) = max(min(dist,1000),-1000);
else
F(i) = 1000;
end
end
-sum(F.^2)
+%F'
+sqrt(sum(F.^2))
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/runcalibration.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/runcalibration.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/runcalibration.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -15,9 +15,12 @@
robot = orEnvGetRobots(robotid);
-calibdata = startgathering(robot);
+calibdata = startgathering(robot);
-[Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot); % compute the calibration values
+Tcamerainit = [0 0 1 0.05;
+ -1 0 0 0.05;
+ 0 -1 0 0.095];
+[Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot, Tcamerainit); % compute the calibration values
Tcamera
Tlaser
jointoffsets
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -16,7 +16,7 @@
rosoct_add_msgs('checkerboard_detector');
rosoct_add_msgs('robot_msgs');
-queuesize = 1000;
+queuesize = 1000; % need big buffer
__calibdata = {};
lastobjdet = {};
__iterationcount = 0;
@@ -28,7 +28,7 @@
lastlaserscan = {};
__rosoct_msg_unsubscribe("new_tile_scan");
-success = rosoct_subscribe("new_tile_scan", @std_msgs_LaserScan, @(msg) laserscancb(msg,robot),queuesize);
+success = rosoct_subscribe("new_tilt_scan", @std_msgs_LaserScan, @(msg) laserscancb(msg,robot),queuesize);
if( ~success )
error('subscribe failed');
end
@@ -55,7 +55,7 @@
__rosoct_msg_unsubscribe("new_tile_scan");
__rosoct_msg_unsubscribe("new_mechanism_state");
-display('gathering calibration done');
+display(sprintf('gathering calibration done, total extracted: %d, total parsed: %d', length(__calibdata), __iterationcount));
calibdata = __calibdata;
__calibdata = {};
@@ -125,10 +125,11 @@
end
laserpoints = laserpoints(:,validpoints);
-linedata = SegmentLines(laserpoints, laserscanmsg.range_max, 0);
+linedata = SegmentLines(laserpoints, laserscanmsg.range_max, 1);
%% take the line intersecting with x-axis (middle of laser range)
-laserline = linedata(:,find( sign(linedata(2,:)) ~= sign(linedata(4,:)), 1, 'first'));
+ilaserline = find( sign(linedata(2,:)) ~= sign(linedata(4,:)), 1, 'first');
+laserline = linedata(:,ilaserline);
if( isempty(laserline) )
display('could not find laser line');
return;
@@ -136,17 +137,61 @@
%% threshold length of line
laserdir = laserline(1:2)-laserline(3:4);
-if( norm(laserdir) < 0.8 )
- display(sprintf('line not long enough %f', norm(laserdir)));
+if( norm(laserdir) < 0.6 || norm(laserdir) > 1.5 )
+ display(sprintf('line not right size %f', norm(laserdir)));
return;
end
-if( min(norm(laserline(1:2)),norm(laserline(3:4))) > 1.5 )
+if( min(norm(laserline(1:2)),norm(laserline(3:4))) > 2 )
display('line is too far');
return;
end
+%% ground can still be detected as checkerboard, so look for discontinuities
+Nlaser = [laserdir(2);-laserdir(1);0]/norm(laserdir);
+Nlaser(3) = -dot(Nlaser(1:2),laserline(1:2));
+
+testpoints = [];
+iprevline = ilaserline-1;
+while(iprevline > 0)
+ if( norm(linedata(1:2,iprevline)-linedata(3:4,iprevline)) > 0.2 )
+ break;
+ end
+ iprevline = iprevline-1;
+end
+
+if( iprevline > 0 )
+ testpoints = [testpoints [reshape(linedata(:,iprevline), [2 2]); 1 1]];
+end
+
+inextline = ilaserline+1;
+while(inextline < size(linedata,2))
+ if( norm(linedata(1:2,inextline)-linedata(3:4,inextline)) > 0.2 )
+ break;
+ end
+ inextline = inextline+1;
+end
+
+if( inextline < size(linedata,2) )
+ testpoints = [testpoints [reshape(linedata(:,inextline), [2 2]); 1 1]];
+end
+
+if( isempty(testpoints) )
+ display('no points to compare displacement to');
+ return;
+end
+
+maxdisplacement = max(abs(transpose(Nlaser)*testpoints));
+if( maxdisplacement < 0.4 )
+ maxdisplacement
+ plot(laserline([1 3]),laserline([2 4]),'g','linewidth',5); drawnow;
+ display('displacement is off');
+ return;
+end
+
+plot(laserline([1 3]),laserline([2 4]),'r','linewidth',5); drawnow;
data.laserline = laserline;
__calibdata{end+1} = data;
display(sprintf('adding data %d', length(__calibdata)));
+pause(4);
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -30,8 +30,16 @@
function openraveros_restart(sessionserver,viewer)
global openraveros_globalsession
-openraveros_startup();
+if( ~exist('sessionserver','var') || isempty(sessionserver) )
+ sessionserver = 'openrave_session';
+end
+if( ~exist('viewer','var') )
+ viewer = 'qtcoin';
+end
+
+openraveros_startup(sessionserver, 1, viewer);
+
if( ~isempty(openraveros_globalsession) )
%% send a dummy env_set command
res = rosoct_session_call(openraveros_globalsession.id,'env_set',openraveros_env_set());
@@ -41,19 +49,14 @@
end
end
-if( ~exist('sessionserver','var') || isempty(sessionserver) )
- sessionserver = 'openrave_session';
-end
openraveros_globalsession = openraveros_createsession(sessionserver);
-if( ~exist('viewer','var') )
- viewer = 'qtcoin';
-end
-
if( ~isempty(viewer) && ~isempty(openraveros_globalsession) )
%% set the viewer
reqset = openraveros_env_set();
- reqset.setmask = reqset.Set_Viewer();
- reqset.viewer = viewer;
+ if( ~isempty(viewer) )
+ reqset.setmask = reqset.Set_Viewer();
+ reqset.viewer = viewer;
+ end
resset = rosoct_session_call(openraveros_globalsession.id,'env_set',reqset);
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -26,7 +26,7 @@
%% POSSIBILITY OF SUCH DAMAGE.
%%
%% author: Rosen Diankov
-function openraveros_startup(sessionserver,createsession, veiwer)
+function openraveros_startup(sessionserver,createsession, viewer)
global openraveros_globalsession
persistent openraveros_initialized
@@ -46,15 +46,15 @@
openraveros_initialized = 1;
end
-if( ~exist('sessionserver','var') )
- sessionserver = 'openrave_session';
-end
+if( createsession && isempty(openraveros_globalsession) )
+ if( ~exist('sessionserver','var') )
+ sessionserver = 'openrave_session';
+ end
+
+ if( ~exist('viewer','var') )
+ viewer = 'qtcoin';
+ end
-if( ~exist('viewer','var') )
- viewer = 'qtcoin';
-end
-
-if( createsession && isempty(openraveros_globalsession) )
req = openraveros_openrave_session();
req.viewer = viewer; % default viewer
while(1)
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -8,9 +8,9 @@
robot = SetupTableScene('data/pr2table_real.env.xml',1);
-Tcamera = [0 0 1 -0.05;
- -1 0 0 -0.05;
- 0 -1 0 -0.095];
+Tcamera = [0 0 1 0.05;
+ -1 0 0 0.05;
+ 0 -1 0 0.095];
out = orProblemSendCommand(['createsystem ObjectTransform topic /checkerdetector/ObjectDetection thresh 0.1 robot ' sprintf('%d ', robot.id) ' matrixoffset ' sprintf('%f ', Tcamera(1:3,1:4))],probs.task);
if( isempty(out) )
error('failed to create checkerboard detector');
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-08 01:14:36 UTC (rev 9026)
@@ -12,5 +12,6 @@
<depend package="boost"/>
<depend package="checkerboard_detector"/>
<depend package="robot_msgs"/>
+ <depend package="pr2_mechanism_controllers"/>
<depend package="tf"/>
</package>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-08 01:14:36 UTC (rev 9026)
@@ -1,5 +1,5 @@
// Software License Agreement (BSD License)
-// Copyright (c) 2008, Willow Garage, Inc.
+// Copyright (c) 2008, Rosen Diankov
// 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,
@@ -21,14 +21,15 @@
// 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.
-//
-// author: Rosen Diankov
#ifndef RAVE_ROS_ROBOT_CONTROLLER
#define RAVE_ROS_ROBOT_CONTROLLER
#include <robot_msgs/MechanismState.h>
+#include <pr2_mechanism_controllers/StartTrajectory.h>
+#include <pr2_mechanism_controllers/CancelTrajectory.h>
+#include <pr2_mechanism_controllers/WaitTrajectory.h>
+#include <pr2_mechanism_controllers/QueryTrajectory.h>
-// controller for the SSC-32 board
class ROSRobotController : public ControllerBase
{
enum ControllerState{
@@ -127,6 +128,7 @@
virtual bool SetDesired(const dReal* pValues)
{
+ // set a path between the current and desired positions
return true;
}
@@ -307,6 +309,11 @@
// do some monitoring of the joint state (try to look for stalls)
}
+ void ControllerThread()
+ {
+
+ }
+
RobotBase* _probot; ///< robot owning this controller
string _topic;
@@ -323,6 +330,8 @@
double _fTimeCommandStarted;
const Trajectory* _ptraj;
+ // trajectory services
+ service::ServiceHandlePtr srvStartTrajectory, srvCancelTrajectory, srvWaitTrajectory, srvQueryTrajectory;
bool _bIsDone;
bool _bSendTimestamps; ///< if true, will send timestamps along with traj
bool _bSubscribed; ///< if true, succesfully subscribed to the mechanism state msgs
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-08 02:22:11
|
Revision: 9028
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9028&view=rev
Author: rdiankov
Date: 2009-01-08 01:38:57 +0000 (Thu, 08 Jan 2009)
Log Message:
-----------
trajectory services
Modified Paths:
--------------
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
Added Paths:
-----------
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryCancel.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryWait.srv
Removed Paths:
-------------
pkg/trunk/controllers/pr2_mechanism_controllers/srv/CancelTrajectory.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/QueryTrajectory.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/StartTrajectory.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/WaitTrajectory.srv
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/CancelTrajectory.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/CancelTrajectory.srv 2009-01-08 01:19:09 UTC (rev 9027)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/CancelTrajectory.srv 2009-01-08 01:38:57 UTC (rev 9028)
@@ -1,6 +0,0 @@
-## cancels a trajectory(ies)
-
-# if 0, cancels all trajectories, otherwise cancels the trajectory with the specified id
-# if no such trajectory exists return fail
-uint32 trajectoryid
----
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/QueryTrajectory.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/QueryTrajectory.srv 2009-01-08 01:19:09 UTC (rev 9027)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/QueryTrajectory.srv 2009-01-08 01:38:57 UTC (rev 9028)
@@ -1,5 +0,0 @@
-## queries where the current trajectory is along its path
-uint32 trajectoryid
----
-uint8 done # 1 if trajectory is done, 0 if ongoing
-float32 trajectorytime # the current timestamp the trajectory is at. If the trajectory is finished, specifies the total time of the trajectory
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/StartTrajectory.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/StartTrajectory.srv 2009-01-08 01:19:09 UTC (rev 9027)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/StartTrajectory.srv 2009-01-08 01:38:57 UTC (rev 9028)
@@ -1,6 +0,0 @@
-## Starts a trajectory
-JointTraj traj
-uint8 requesttiming # if 1, send back the timestamps for every trajectory point (first point starts at 0)
----
-uint32 trajectoryid # unique trajectory id to be used for later referencing
-float32[] timestamps # trajectory timestamps if requested
Copied: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryCancel.srv (from rev 9023, pkg/trunk/controllers/pr2_mechanism_controllers/srv/CancelTrajectory.srv)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryCancel.srv (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryCancel.srv 2009-01-08 01:38:57 UTC (rev 9028)
@@ -0,0 +1,6 @@
+## cancels a trajectory(ies)
+
+# if 0, cancels all trajectories, otherwise cancels the trajectory with the specified id
+# if no such trajectory exists return fail
+uint32 trajectoryid
+---
Copied: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv (from rev 9024, pkg/trunk/controllers/pr2_mechanism_controllers/srv/QueryTrajectory.srv)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv 2009-01-08 01:38:57 UTC (rev 9028)
@@ -0,0 +1,10 @@
+## queries where the current trajectory is along its path
+
+# if trajectoryid is 0, returns the timestamp of the current trajectory
+# and done is filled whenever all the trajectories have been finished
+uint32 trajectoryid
+---
+uint8 done # 1 if trajectory is done, 0 if ongoing
+float32 trajectorytime # the current timestamp the trajectory is at. If the trajectory is finished, specifies the total time of the trajectory
+
+string[] jointnames # names of the joints that the controller expects
Copied: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv (from rev 9018, pkg/trunk/controllers/pr2_mechanism_controllers/srv/StartTrajectory.srv)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv 2009-01-08 01:38:57 UTC (rev 9028)
@@ -0,0 +1,6 @@
+## Starts a trajectory
+JointTraj traj
+uint8 requesttiming # if 1, send back the timestamps for every trajectory point (first point starts at 0)
+---
+uint32 trajectoryid # unique trajectory id to be used for later referencing
+float32[] timestamps # trajectory timestamps if requested
Copied: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryWait.srv (from rev 9023, pkg/trunk/controllers/pr2_mechanism_controllers/srv/WaitTrajectory.srv)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryWait.srv (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryWait.srv 2009-01-08 01:38:57 UTC (rev 9028)
@@ -0,0 +1,8 @@
+## waits for a trajectory to finish or times out
+
+# if 0, waits for all trajectories, otherwise waits for the trajectory with the specified id
+# if no such trajectory exists return fail, if such a trajectory exists and is done already return immediately
+uint32 trajectoryid
+float32 timeout # if 0, waits indefinitely
+---
+uint8 timedout # 1 if timed out, 0 if any other error occurs
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/WaitTrajectory.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/WaitTrajectory.srv 2009-01-08 01:19:09 UTC (rev 9027)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/WaitTrajectory.srv 2009-01-08 01:38:57 UTC (rev 9028)
@@ -1,8 +0,0 @@
-## waits for a trajectory to finish or times out
-
-# if 0, waits for all trajectories, otherwise waits for the trajectory with the specified id
-# if no such trajectory exists return fail, if such a trajectory exists and is done already return immediately
-uint32 trajectoryid
-float32 timeout # if 0, waits indefinitely
----
-uint8 timedout # 1 if timed out, 0 if any other error occurs
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-08 01:19:09 UTC (rev 9027)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-08 01:38:57 UTC (rev 9028)
@@ -25,10 +25,10 @@
#define RAVE_ROS_ROBOT_CONTROLLER
#include <robot_msgs/MechanismState.h>
-#include <pr2_mechanism_controllers/StartTrajectory.h>
-#include <pr2_mechanism_controllers/CancelTrajectory.h>
-#include <pr2_mechanism_controllers/WaitTrajectory.h>
-#include <pr2_mechanism_controllers/QueryTrajectory.h>
+#include <pr2_mechanism_controllers/TrajectoryStart.h>
+#include <pr2_mechanism_controllers/TrajectoryCancel.h>
+#include <pr2_mechanism_controllers/TrajectoryWait.h>
+#include <pr2_mechanism_controllers/TrajectoryQuery.h>
class ROSRobotController : public ControllerBase
{
@@ -309,11 +309,6 @@
// do some monitoring of the joint state (try to look for stalls)
}
- void ControllerThread()
- {
-
- }
-
RobotBase* _probot; ///< robot owning this controller
string _topic;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-01-08 04:18:32
|
Revision: 9035
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9035&view=rev
Author: hsujohnhsu
Date: 2009-01-08 04:18:27 +0000 (Thu, 08 Jan 2009)
Log Message:
-----------
arm constraint controller working in sim.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
Added Paths:
-----------
pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch
pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-01-08 03:54:17 UTC (rev 9034)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-01-08 04:18:27 UTC (rev 9035)
@@ -47,6 +47,7 @@
#include "Eigen/SVD"
#include "Eigen/Core"
#include "robot_kinematics/robot_kinematics.h"
+#include <std_msgs/VisualizationMarker.h>
namespace controller {
@@ -76,6 +77,16 @@
Eigen::Matrix<float,6,6> constraint_jac_;
Eigen::Matrix<float,6,1> constraint_wrench_;
KDL::Frame endeffector_frame_;
+
+ // some parameters to define the constraint
+ double wall_x; /// @todo: hardcoded x wall location
+ double threshold_x; //@todo: hardcoded wall threshold, activate constraint force if closer than this
+ double wall_r; /// @todo: hardcoded wall_radius
+ double threshold_r; //@todo: hardcoded wall threshold, activate constraint force if closer than this
+ double f_x_max;
+ double f_y_max;
+ double f_z_max;
+
};
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-08 03:54:17 UTC (rev 9034)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-08 04:18:27 UTC (rev 9035)
@@ -84,6 +84,15 @@
if (serial_chain == NULL)
fprintf(stderr, "Got NULL Chain\n") ;
+ // some parameters
+ node->param("constraint/wall_x", wall_x, 0.75) ;
+ node->param("constraint/threshold_x", threshold_x, 0.2) ;
+ node->param("constraint/wall_r", wall_r, 0.2) ;
+ node->param("constraint/threshold_r", threshold_r, 0.1) ;
+ node->param("constraint/f_x_max", f_x_max, 20.0) ;
+ node->param("constraint/f_y_max", f_y_max, 20.0) ;
+ node->param("constraint/f_z_max", f_z_max, 20.0) ;
+
// convert description to KDL chain
chain_ = serial_chain->chain;
num_joints_ = chain_.getNrOfJoints();
@@ -183,8 +192,8 @@
jnt_torq(i) = 0;
for (unsigned int j=0; j<6; j++)
{
- //jnt_torq(i) += (jacobian(j,i) * wrench_desi_(j));
jnt_torq(i) += (jacobian(j,i) * constraint_wrench_(j));
+ jnt_torq(i) += (jacobian(j,i) * wrench_desi_(j)); /// todo: project into null space first before adding
}
joints_[i]->commanded_effort_ = jnt_torq(i);
}
@@ -207,7 +216,7 @@
double df_dx = -1.0; // we are describing a wall at constant x, x- side is allowed
double df_dy = -cos(ee_theta); // radial lines toward origin
- double df_dz = sin(ee_theta); // radial lines toward origin
+ double df_dz = -sin(ee_theta); // radial lines toward origin
// Constraint Jacobian (normals to the constraint surface)
constraint_jac_(0,0)= df_dx;
@@ -221,42 +230,53 @@
////////////////////////////////////////////
// x-direction force is a function of endeffector distance from the wall
// @todo: FIXME: hardcoded wall at x=1
- double x_wall = 1.0; /// @todo: hardcoded x wall location
- double x_distance = x_wall - endeffector_frame_.p(0);
- double x_threshold = 0.2; //@todo: hardcoded wall threshold, activate constraint force if closer than this
- double f_x = 0;
- double f_x_scale = 10; //@todo: f_x_scale * exp(distance) = force applied in x direction
+ double x_distance = wall_x - endeffector_frame_.p(0);
+ double f_x;
// assign x-direction constraint force f_x if within range of the wall
- if (x_distance >0 && x_distance < x_threshold)
+ if (x_distance >0 && x_distance < threshold_x)
{
- f_x = exp(x_distance) * f_x_scale * df_dx; /// @todo: FIXME, some exponential function
+ f_x = x_distance/threshold_x * f_x_max; /// @todo: FIXME, replace with some exponential function
}
else if (x_distance <= 0)
+ {
+ f_x = f_x_max;
ROS_ERROR("wall x breach! by: %f m\n",x_distance);
+ }
+ else
+ {
+ f_x = 0;
+ }
/// yz-force magnitude is a function of endeffector distance from unit circle
double ee_radius = sqrt( endeffector_frame_.p(1)*endeffector_frame_.p(1) + endeffector_frame_.p(2)*endeffector_frame_.p(2) );
- double wall_radius = 1.0; /// @todo: hardcoded wall_radius
- double r_distance = wall_radius - ee_radius;
- double r_threshold = 0.2; //@todo: hardcoded wall threshold, activate constraint force if closer than this
- double f_y_scale = 10; //@todo: f_y_scale * exp(distance) = force applied in y direction
- double f_z_scale = 10; //@todo: f_z_scale * exp(distance) = force applied in z direction
- double f_y = 0;
- double f_z = 0;
+ double r_distance = wall_r - ee_radius;
+ double f_y, f_z;
// assign x-direction constraint force f_x if within range of the wall
- if (r_distance > 0 && r_distance < r_threshold)
+ if (r_distance > 0 && r_distance < threshold_r)
{
- f_y = exp(r_distance) * f_y_scale * df_dy; /// @todo: FIXME, some exponential function
- f_z = exp(r_distance) * f_z_scale * df_dz; /// @todo: FIXME, some exponential function
+ f_y = r_distance/threshold_r * f_y_max; /// @todo: FIXME, replace with some exponential function
+ f_z = r_distance/threshold_r * f_z_max; /// @todo: FIXME, replace with some exponential function
}
else if (r_distance <= 0)
+ {
+ f_y = f_y_max; /// @todo: FIXME, some exponential function
+ f_z = f_z_max; /// @todo: FIXME, some exponential function
ROS_ERROR("wall radius breach! by: %f m\n",r_distance);
+ }
+ else
+ {
+ f_y = 0;
+ f_z = 0;
+ }
constraint_wrench_(0) = f_x;
constraint_wrench_(1) = f_y;
constraint_wrench_(2) = f_z;
+
+ ROS_WARN("force magnitude (%f, %f, %f)\n",f_x,f_y,f_z);
+
}
@@ -291,6 +311,30 @@
&EndeffectorConstraintControllerNode::command, this, 1);
guard_command_.set(topic_ + "/command");
+ node->advertise<std_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+
+ // visualization not working yet
+ std_msgs::VisualizationMarker marker;
+ marker.header.frame_id = "base_link";
+ marker.id = 0;
+ marker.type = 2;
+ marker.action = 0;
+ marker.x = 0.7;
+ marker.y = 0;
+ marker.z = 0;
+ marker.yaw = 0;
+ marker.pitch = 0;
+ marker.roll = 0.0;
+ marker.xScale = 0.01;
+ marker.yScale = 0.3;
+ marker.zScale = 0.3;
+ marker.alpha = 100;
+ marker.r = 0;
+ marker.g = 255;
+ marker.b = 0;
+ node->publish("visualizationMarker", marker );
+
+
return true;
}
Added: pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch 2009-01-08 04:18:27 UTC (rev 9035)
@@ -0,0 +1,39 @@
+<launch>
+ <group name="wg">
+ <!-- send pr2_r_arm.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_r_arm.xacro.xml'" />
+
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
+
+
+ <!-- start arm controller -->
+ <!--
+ <node pkg="ogre_visualizer" type="standalone_visualizer.py" respawn="false" output="screen" />
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_default_controller.xml" respawn="false" />
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_cartesian_pos_controller.xml" respawn="false" />
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_trajectory_controller.xml" respawn="false" />
+ -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_constraint_controller.xml" respawn="false" />
+
+ <!-- send arm a command -->
+ <!-- open gripper .5 radians
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set r_gripper_controller 0.5" respawn="false" output="screen" />
+ -->
+ <!-- tests
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set r_gripper_controller 0.5" respawn="false" output="screen" />
+ -->
+
+ <!-- for visualization -->
+ <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
+ </group>
+</launch>
+
Added: pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml 2009-01-08 04:18:27 UTC (rev 9035)
@@ -0,0 +1,5 @@
+<controllers>
+ <controller type="EndeffectorConstraintControllerNode" name="arm_constraint" topic="arm_constraint">
+ <chain root="base_link" tip="r_wrist_roll_link" offset="0.0 0 0" />
+ </controller>
+</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-08 08:11:47
|
Revision: 9041
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9041&view=rev
Author: rdiankov
Date: 2009-01-08 08:11:42 +0000 (Thu, 08 Jan 2009)
Log Message:
-----------
openrave robot controller now uses the pr2_mechanism controllers services to send trajectories, added a testtrajectories script for testing the services directly with rosoct (bypassing the openrave controller)
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
Added Paths:
-----------
pkg/trunk/openrave_planning/ormanipulation/octave/GetInterpolatedHandTrajectory.m
pkg/trunk/openrave_planning/ormanipulation/octave/QuatFromRotationMatrix.m
pkg/trunk/openrave_planning/ormanipulation/octave/QuatSlerp.m
pkg/trunk/openrave_planning/ormanipulation/octave/RotationMatrixFromQuat.m
pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv 2009-01-08 07:46:06 UTC (rev 9040)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv 2009-01-08 08:11:42 UTC (rev 9041)
@@ -1,5 +1,6 @@
## Starts a trajectory
JointTraj traj
+uint8 hastiming # if 1, use timestamps of trajectory points, otherwise do internal retiming
uint8 requesttiming # if 1, send back the timestamps for every trajectory point (first point starts at 0)
---
uint32 trajectoryid # unique trajectory id to be used for later referencing
Added: pkg/trunk/openrave_planning/ormanipulation/octave/GetInterpolatedHandTrajectory.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GetInterpolatedHandTrajectory.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GetInterpolatedHandTrajectory.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -0,0 +1,31 @@
+%% Ttrajectory = GetInterpolatedHandTrajectory(T0,T1,deltat, deltaq)
+%%
+%% Ttrajerctory is a 12xN array of 3x4 matrices
+function Ttrajectory = GetInterpolatedHandTrajectory(T0,T1,deltat, deltaq)
+
+if( ~exist('deltat','var') )
+ deltat = 0.01;
+end
+
+if( ~exist('deltaq','var') )
+ deltaq = 0.05;
+end
+
+q0 = QuatFromRotationMatrix(T0(1:3,1:3));
+q1 = QuatFromRotationMatrix(T1(1:3,1:3));
+t0 = T0(1:3,4);
+t1 = T1(1:3,4);
+
+if( norm(q0+q1) < norm(q0-q1) )
+ q1 = -q1;
+end
+
+numsegments = 1+max(norm(q0-q1)/deltaq, norm(t0-t1)/deltat);
+times = 0:(1/numsegments):1;
+
+Ttrajectory = zeros(12,length(times));
+for i = 1:length(times)
+ curtime = times(i);
+ Ttrajectory(1:9,i) = reshape(RotationMatrixFromQuat(QuatSlerp(q0,q1,curtime,1e-6)),[9 1]);
+ Ttrajectory(10:12,i) = t0*(1-curtime)+t1*curtime;
+end
Added: pkg/trunk/openrave_planning/ormanipulation/octave/QuatFromRotationMatrix.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/QuatFromRotationMatrix.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/QuatFromRotationMatrix.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -0,0 +1,64 @@
+% quat = QuatFromRotationMatrix(R)
+%
+% R - 3x3 orthogonal rotation matrix
+% quat - the format is [cos(angle/2) axis*sin(angle/2)]
+
+% Software License Agreement (BSD License)
+% Copyright (c) 2008, Rosen Diankov
+% 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.
+function quat = QuatFromRotationMatrix(R)
+
+quat = zeros(4,1);
+tr = R(1,1) + R(2,2) + R(3,3);
+if (tr >= 0)
+ s = sqrt(tr + 1);
+ quat(1) = 0.5*s;
+ quat(2:4) = [R(3,2)-R(2,3); R(1,3)-R(3,1); R(2,1) - R(1,2)]*0.5/s;
+else
+ %% find the largest diagonal element and jump to the appropriate case
+ [rmax, convcase] = max([R(1,1) R(2,2) R(3,3)]);
+ switch(convcase)
+ case 1
+ s = sqrt((R(1,1) - (R(2,2) + R(3,3))) + 1);
+ quat(2) = 0.5 * s;
+ s = 0.5 / s;
+ quat(3) = (R(1,2) + R(2,1)) * s;
+ quat(4) = (R(3,1) + R(1,3)) * s;
+ quat(1) = (R(3,2) - R(2,3)) * s;
+ case 2
+ s = sqrt((R(2,2) - (R(3,3) + R(1,1))) + 1);
+ quat(3) = 0.5 * s;
+ s = 0.5 / s;
+ quat(4) = (R(2,3) + R(3,2)) * s;
+ quat(2) = (R(1,2) + R(2,1)) * s;
+ quat(1) = (R(1,3) - R(3,1)) * s;
+ case 3
+ s = sqrt((R(3,3) - (R(1,1) + R(2,2))) + 1);
+ quat(4) = 0.5 * s;
+ s = 0.5 / s;
+ quat(2) = (R(3,1) + R(1,3)) * s;
+ quat(3) = (R(2,3) + R(3,2)) * s;
+ quat(1) = (R(2,1) - R(1,2)) * s;
+ end
+end
Added: pkg/trunk/openrave_planning/ormanipulation/octave/QuatSlerp.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/QuatSlerp.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/QuatSlerp.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -0,0 +1,74 @@
+% [qm] = QuatSlerp(qi, qn, t, eps)
+%
+% where qi=[w1 x1 y1 z1] - start unit quaternions
+% qn=[w2 x2 y2 z2] - end unit quaternions
+% t=[0 to 1]
+% eps=threshold value
+
+
+% Sagi Dalyot %
+
+% This routine aims for calculating a unit quaternion, describing a rotation matrix,
+% which lies between two known unit quaternions - q1 and q2,
+% using a spherical linear interpolation - Slerp.
+% Slerp follow the shortest great arc on a unit sphere,
+% hence, the shortest possible interpolation path.
+% Consequently, Slerp has constant angular velocity,
+% so it is the optimal interpolation curve between two rotations.
+% (first published by Sheomake K., 1985 - Animating Rotation with Quaternion Curves)
+
+% end of file -> explnation of rotation matrix and quaternions
+
+% in general:
+% slerp(q1, q2, t) = q1*(sin(1-t)*teta)/sin(t) + q2*(sin(t*teta))/sin(teta)
+% where teta is the angle between the two unit quaternions,
+% and t is between [0,1]
+
+% two border cases will be delt:
+% 1: where q1 = q2 (or close by eps)
+% 2: where q1 = -q2 (angle between unit quaternions is 180 degrees).
+% in general, if q1=q2 then Slerp(q; q; t) == q
+
+function [qm] = QuatSlerp(qi, qn, t, eps)
+
+if t==0 % saving calculation time -> where qm=qi
+ qm=qi;
+
+elseif t==1 % saving calculation time -> where qm=qn
+ qm=qn;
+
+else
+
+ C=dot(qi,qn); % Calculating the angle beteen the unit quaternions by dot product
+
+ teta=acos(C);
+
+ if (1 - C) <= eps % if angle teta is close by epsilon to 0 degrees -> calculate by linear interpolation
+ qm=qi*(1-t)+qn*t; % avoiding divisions by number close to 0
+
+ elseif (1 + C) <= eps % when teta is close by epsilon to 180 degrees the result is undefined -> no shortest direction to rotate
+ q2(1) = qi(4); q2(2) = -qi(3); q2(3)= qi(2); q2(4) = -qi(1); % rotating one of the unit quaternions by 90 degrees -> q2
+ qm=qi*(sin((1-t)*(pi/2)))+q2*sin(t*(pi/2));
+
+ else
+ qm=qi*(sin((1-t)*teta))/sin(teta)+qn*sin(t*teta)/sin(teta);
+ end
+end
+
+
+% eof
+% q = [w, (x, y, z)] quaternion definition
+%
+% where
+% R = [1-2*y^2-2*z^2 2*x*y-2*w*z 2*x*z+2*w*y R is function of 3 euler rotation angles
+% 2*x*y+2*w*z 1-2*x^2-2*z^2 2*y*z-2*w*x
+% 2*x*z-2*w*y 2*y*z+2*w*x 1-2*x^2-2*y^2]
+% => R = [M00 M01 M02
+% M10 M11 M12
+% M20 M21 M22]
+% => trace(R) = 4 - 4*(x^2+y^2+z^2), and x^2+y^2+z^2+w^2==1
+% => w=(trace)^.5
+% => x=(M21-M12)/4*w
+% => y=(M02-M21)/4*w
+% => x=(M10-M01)/4*w
+% => q = [w, (x, y, z)]
Added: pkg/trunk/openrave_planning/ormanipulation/octave/RotationMatrixFromQuat.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RotationMatrixFromQuat.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RotationMatrixFromQuat.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -0,0 +1,43 @@
+% R = RotationMatrixFromQuat(quat)
+%
+% quat - the format is [cos(angle/2) axis*sin(angle/2)]
+
+% Software License Agreement (BSD License)
+% Copyright (c) 2008, Rosen Diankov
+% 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.
+function R = RotationMatrixFromQuat(quat)
+
+R = zeros(3,3);
+qq1 = 2*quat(2)*quat(2);
+qq2 = 2*quat(3)*quat(3);
+qq3 = 2*quat(4)*quat(4);
+R(1,1) = 1 - qq2 - qq3;
+R(1,2) = 2*(quat(2)*quat(3) - quat(1)*quat(4));
+R(1,3) = 2*(quat(2)*quat(4) + quat(1)*quat(3));
+R(2,1) = 2*(quat(2)*quat(3) + quat(1)*quat(4));
+R(2,2) = 1 - qq1 - qq3;
+R(2,3) = 2*(quat(3)*quat(4) - quat(1)*quat(2));
+R(3,1) = 2*(quat(2)*quat(4) - quat(1)*quat(3));
+R(3,2) = 2*(quat(3)*quat(4) + quat(1)*quat(2));
+R(3,3) = 1 - qq1 - qq2;
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-08 07:46:06 UTC (rev 9040)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -4,8 +4,6 @@
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
startup;
-%orEnvSetOptions('debug debug');
-
robot = SetupTableScene('data/pr2table_real.env.xml',1);
Tcamera = [0 0 1 0.05;
Added: pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -0,0 +1,58 @@
+#!/usr/bin/env octave
+global updir probs
+
+cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
+
+startup;
+orEnvLoadScene('',1);
+robotid = orEnvCreateRobot('pr2','robots/pr2full.robot.xml');
+robot = orEnvGetRobots(robotid);
+orBodySetJointValues(robot.id,[ 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.05 0.000000 0.000000 0.000000 0.000000 0.000000 1.582704 1.081165 0.000000 -2.299995 -0.000000 0.000000 0.000000 -0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.979114 -0.400000 0.000000 -1.535151 0.000000 0.000000 0.000000 0.000000 0.000000]);
+
+%orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /']);
+
+probs.manip = orEnvCreateProblem('BaseManipulation',robot.name);
+imanipulator = 2;
+orProblemSendCommand(sprintf('setactivemanip %d', imanipulator-1), probs.manip);
+
+Tgripperstart = [1.00000 0.00000 0.00000 0.5
+ 0.00000 1.00000 0.00000 -0.04616
+ 0.00000 0.00000 1.00000 0.7];
+Tgripperend = [1.00000 0.00000 0.00000 0.5
+ 0.00000 1.00000 0.00000 -0.04616
+ 0.00000 0.00000 1.00000 1.2];
+
+Tgrippertraj = GetInterpolatedHandTrajectory(Tgripperstart, Tgripperend);
+trajectory = [];
+for i = 1:size(Tgrippertraj,2)
+ iksol = orProblemSendCommand(['iktest matrix ' sprintf('%f ', Tgrippertraj(:,i))],probs.manip)
+ if( isempty(iksol) )
+ display(sprintf('failed to find ik %d/%d', i, size(Tgrippertraj,2)));
+ break;
+ end
+
+ trajectory = [trajectory sscanf(iksol, '%f ')];
+ pause(0.1);
+end
+
+%% make a service call
+rosoct_add_srvs('pr2_mechanism_controllers');
+req = pr2_mechanism_controllers_TrajectoryStart();
+req.traj.points = {};
+for i = 1:size(trajectory,2)
+ req.traj.points{i} = pr2_mechanism_controllers_JointTrajPoint();
+ req.traj.points{i}.positions = trajectory(:,i);
+end
+res = rosoct_service_call('/TrajectoryStart',req);
+
+%% play it out in simulation
+for i = 1:size(trajectory,2)
+ orBodySetJointValues(robot.id,trajectory(:,i),robot.manips{imanipulator}.armjoints);
+ pause(0.1);
+end
+
+robothand = RobotCreatePR2Hand('pr2gripper');
+for i = 1:size(Tgrippertraj,2)
+ orBodySetTransform(robothand.id,Tgrippertraj(:,i));
+ pause(0.1);
+end
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-08 07:46:06 UTC (rev 9040)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-08 08:11:42 UTC (rev 9041)
@@ -32,20 +32,104 @@
class ROSRobotController : public ControllerBase
{
- enum ControllerState{
+ enum ControllerState {
None = 0,
Servo, // done when servoed to position and the position is held
Traj, // done when reaches last point
Velocity // done when joints stop moving
};
+ class TrajectoryController
+ {
+ public:
+ TrajectoryController(RobotBase* probot, const string& strTrajectoryServiceDir) : _probot(probot), _strTrajectoryServiceDir(strTrajectoryServiceDir) {}
+ virtual ~TrajectoryController() {
+ Destroy();
+ }
+
+ bool Init(ros::node* pnode)
+ {
+ assert(pnode != NULL);
+ Destroy();
+
+ _srvTrajectoryStart = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryStart::request, pr2_mechanism_controllers::TrajectoryStart::response>(_strTrajectoryServiceDir+"TrajectoryStart", true);
+ _srvTrajectoryCancel = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryCancel::request, pr2_mechanism_controllers::TrajectoryCancel::response>(_strTrajectoryServiceDir+"TrajectoryCancel", true);
+ _srvTrajectoryWait = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryWait::request, pr2_mechanism_controllers::TrajectoryWait::response>(_strTrajectoryServiceDir+"TrajectoryWait", true);
+ _srvTrajectoryQuery = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryQuery::request, pr2_mechanism_controllers::TrajectoryQuery::response>(_strTrajectoryServiceDir+"TrajectoryQuery", true);
+
+ if( !_srvTrajectoryQuery ) {
+ RAVELOG_ERRORA("failed to find %s service\n", (_strTrajectoryServiceDir+"TrajectoryQuery").c_str());
+ return false;
+ }
+
+ pr2_mechanism_controllers::TrajectoryQuery::request req;
+ pr2_mechanism_controllers::TrajectoryQuery::response res;
+ if( !_srvTrajectoryQuery->call(req,res) ) {
+ RAVELOG_ERRORA("failed to query trajectory service %s", _strTrajectoryServiceDir.c_str());
+ return false;
+ }
+
+ if( res.jointnames.size() == 0 ) {
+ RAVELOG_ERRORA("no joint names for trajectory service %s", _strTrajectoryServiceDir.c_str());
+ return false;
+ }
+
+ vector<string> vrobotjoints(_probot->GetJoints().size());
+ for(size_t i = 0; i < _probot->GetJoints().size(); ++i)
+ vrobotjoints[i] = _stdwcstombs(_probot->GetJoints()[i]->GetName());
+
+ _vjointmap.reserve(res.jointnames.size());
+ FOREACH(itname, res.jointnames) {
+ vector<string>::iterator itindex = find(vrobotjoints.begin(), vrobotjoints.end(), *itname);
+ if( itindex == vrobotjoints.end() ) {
+ RAVELOG_ERRORA("failed to find joint %s\n", itname->c_str());
+ Destroy();
+ return false;
+ }
+ _vjointmap.push_back((int)(itindex-vrobotjoints.begin()));
+ }
+
+ return true;
+ }
+
+ bool Destroy()
+ {
+ _vjointmap.clear();
+ _listTrajectories.clear();
+ _srvTrajectoryStart.reset();
+ _srvTrajectoryCancel.reset();
+ _srvTrajectoryWait.reset();
+ _srvTrajectoryQuery.reset();
+ return true;
+ }
+
+ void GetTrajPoint(const vector<dReal>& vrobotvalues, pr2_mechanism_controllers::JointTrajPoint& pt)
+ {
+ pt.positions.resize(_vjointmap.size());
+ typeof(pt.positions.begin()) itcontrollerpos = pt.positions.begin();
+ FOREACH(itindex, _vjointmap)
+ *itcontrollerpos++ = vrobotvalues[*itindex];
+ }
+
+ // trajectory services
+ RobotBase* _probot;
+ string _strTrajectoryServiceDir;
+ service::ServiceHandlePtr _srvTrajectoryStart, _srvTrajectoryCancel, _srvTrajectoryWait, _srvTrajectoryQuery;
+ vector<int> _vjointmap;
+ list<uint32_t> _listTrajectories; ///< trajectories currently pending for completion
+ };
+
public:
ROSRobotController(EnvironmentBase* penv) : ControllerBase(penv), _topic("mechanism_state"), _fTimeCommandStarted(0),
- _ptraj(NULL), _bIsDone(false), _bSendTimestamps(false), _bSubscribed(false), _bCalibrated(false) {
+ _ptraj(NULL), _bIsDone(false), _bSendTimestamps(false), _bSubscribed(false), _bCalibrated(false), _bDestroyThread(false) {
+ _threadTrajectories = boost::thread(boost::bind(&ROSRobotController::_TrajectoryThread,this));
}
virtual ~ROSRobotController() {
Destroy();
+
+ _bDestroyThread = true;
+ _threadTrajectories.join();
}
/// args format: host port [proxytype index]
@@ -92,6 +176,12 @@
break;
}
+ else if( stricmp(cmd.c_str(), "trajectoryservice") == 0 ) {
+ string servicedir;
+ ss >> servicedir;
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+ _listControllers.push_back(boost::shared_ptr<TrajectoryController>(new TrajectoryController(_probot, servicedir)));
+ }
else break;
if( !ss ) {
@@ -120,6 +210,11 @@
_probot = NULL;
_bIsDone = false;
_setEnabledJoints.clear();
+
+ {
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+ _listControllers.clear();
+ }
}
virtual void Reset(int options)
@@ -128,13 +223,89 @@
virtual bool SetDesired(const dReal* pValues)
{
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+
// set a path between the current and desired positions
+ bool bSuccess = true;
+ pr2_mechanism_controllers::TrajectoryStart::request req;
+ pr2_mechanism_controllers::TrajectoryStart::response res;
+
+ vector<dReal> vcurvalues, vnewvalues;
+ _probot->GetJointValues(vcurvalues);
+
+ {
+ RobotBase::RobotStateSaver saver(_probot);
+ _probot->SetJointValues(NULL, NULL, pValues,true);
+ _probot->GetJointValues(vcurvalues);
+ }
+
+ FOREACH(ittrajcontroller, _listControllers) {
+ if( !(*ittrajcontroller)->_srvTrajectoryStart ) {
+ RAVELOG_ERRORA("no start trajectory service\n");
+ bSuccess = false;
+ continue;
+ }
+
+ //req.requesttiming = 1; // request back the timestamps
+ req.hastiming = 0;
+ req.traj.points.resize(2);
+ (*ittrajcontroller)->GetTrajPoint(vcurvalues, req.traj.points[0]);
+ (*ittrajcontroller)->GetTrajPoint(vnewvalues, req.traj.points[1]);
+
+ if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
+ (*ittrajcontroller)->_listTrajectories.push_back(res.trajectoryid);
+ else {
+ RAVELOG_ERRORA("failed to start trajectory\n");
+ bSuccess = false;
+ continue;
+ }
+
+ RAVELOG_DEBUGA("started trajectory %d\n", res.trajectoryid);
+ //res.timestamps // final timestamps
+ }
+
return true;
}
virtual bool SetPath(const Trajectory* ptraj)
{
- return true;
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+
+ bool bSuccess = true;
+ pr2_mechanism_controllers::TrajectoryStart::request req;
+ pr2_mechanism_controllers::TrajectoryStart::response res;
+
+ FOREACH(ittrajcontroller, _listControllers) {
+ if( !(*ittrajcontroller)->_srvTrajectoryStart ) {
+ RAVELOG_ERRORA("no start trajectory service\n");
+ bSuccess = false;
+ continue;
+ }
+
+ //req.requesttiming = 1; // request back the timestamps
+ req.hastiming = 0;
+ req.traj.points.resize(ptraj->GetPoints().size());
+ typeof(req.traj.points.begin()) ittraj = req.traj.points.begin();
+ FOREACHC(itpoint, ptraj->GetPoints()) {
+ ittraj->positions.reserve(itpoint->q.size()); ittraj->positions.resize(0);
+ FOREACHC(itq, itpoint->q)
+ ittraj->positions.push_back(*itq);
+ ++ittraj;
+ }
+
+ if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
+ (*ittrajcontroller)->_listTrajectories.push_back(res.trajectoryid);
+ else {
+ RAVELOG_ERRORA("failed to start trajectory\n");
+ bSuccess = false;
+ continue;
+ }
+
+ RAVELOG_DEBUGA("started trajectory %d\n", res.trajectoryid);
+ //res.timestamps // final timestamps
+ }
+
+ return bSuccess;
}
virtual bool SetPath(const Trajectory* ptraj, int nTrajectoryId, float fDivergenceTime)
@@ -143,8 +314,6 @@
return false;
}
- virtual int GetDOF() { return _probot != NULL ? _probot->GetDOF() : 0; }
-
virtual bool SimulationStep(float fTimeElapsed)
{
if( !_bSubscribed )
@@ -248,6 +417,12 @@
RAVELOG_DEBUGA("subscribed to %s\n", _topic.c_str());
else
RAVELOG_ERRORA("failed to subscribe to %s\n", _topic.c_str());
+
+ if( _bSubscribed ) {
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+ FOREACH(it, _listControllers)
+ (*it)->Init(pnode);
+ }
}
}
@@ -258,6 +433,10 @@
if( pnode != NULL ) {
pnode->unsubscribe(_topic.c_str());
RAVELOG_DEBUGA("unsubscribe from %s\n", _topic.c_str());
+
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+ FOREACH(it, _listControllers)
+ (*it)->Destroy();
}
_bSubscribed = false;
}
@@ -309,9 +488,58 @@
// do some monitoring of the joint state (try to look for stalls)
}
+ void _TrajectoryThread()
+ {
+ while(!_bDestroyThread) {
+
+ pr2_mechanism_controllers::TrajectoryQuery::request req;
+ pr2_mechanism_controllers::TrajectoryQuery::response res;
+
+ // check if the first trajectory is done
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+ uint32_t trajectoryid;
+ bool bDone = true;
+
+ if( _listControllers.size() > 0 ) {
+ bool bPopTrajectory = true;
+
+ // check if done
+ FOREACH(ittraj, _listControllers) {
+ assert( (*ittraj)->_listTrajectories.size() == _listControllers.front()->_listTrajectories.size());
+ if( (*ittraj)->_listTrajectories.size() == 0 ) {
+ bPopTrajectory = false;
+ break;
+ }
+ req.trajectoryid = (*ittraj)->_listTrajectories.front();
+ if( !(*ittraj)->_srvTrajectoryQuery->call(req,res) ) {
+ RAVELOG_ERRORA("trajectory query failed\n");
+ bPopTrajectory = false;
+ }
+
+ if( !res.done ) {
+ bPopTrajectory = false;
+ break;
+ }
+ }
+
+ if( bPopTrajectory ) {
+ RAVELOG_DEBUGA("robot trajectory finished, left: %d\n", _listControllers.front()->_listTrajectories.size());
+ FOREACH(ittraj, _listControllers)
+ (*ittraj)->_listTrajectories.pop_front();
+ }
+
+ bDone = _listControllers.front()->_listTrajectories.size()==0;
+ }
+
+ _bIsDone = bDone;
+ usleep(10000); // query every 10ms
+ }
+ }
+
RobotBase* _probot; ///< robot owning this controller
string _topic;
+
robot_msgs::MechanismState _mstate_cb, _mstate;
vector<dReal> _vecdesired;
set< pair<string, int> > _setEnabledJoints; // set of enabled joints and their indices
@@ -321,16 +549,19 @@
int logid;
map<int, int> _mapjoints; ///< maps mechanism state joints to robot joints
+ list<boost::shared_ptr<TrajectoryController> > _listControllers;
double _fTimeCommandStarted;
const Trajectory* _ptraj;
- // trajectory services
- service::ServiceHandlePtr srvStartTrajectory, srvCancelTrajectory, srvWaitTrajectory, srvQueryTrajectory;
+ boost::mutex _mutexTrajectories;
+ boost::thread _threadTrajectories;
+
bool _bIsDone;
bool _bSendTimestamps; ///< if true, will send timestamps along with traj
bool _bSubscribed; ///< if true, succesfully subscribed to the mechanism state msgs
bool _bCalibrated; ///< if true, mechanism state matches robot
+ bool _bDestroyThread; /// if true, destroy the thread
};
#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2009-01-08 07:46:06 UTC (rev 9040)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2009-01-08 08:11:42 UTC (rev 9041)
@@ -35,7 +35,7 @@
public:
ROSSensorSystem(EnvironmentBase* penv) : SimpleSensorSystem<XMLID>(penv), _bSubscribed(false)
{
-
+
}
virtual ~ROSSensorSystem() {
Destroy();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sac...@us...> - 2009-01-09 01:27:00
|
Revision: 9098
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9098&view=rev
Author: sachinchitta
Date: 2009-01-09 01:26:53 +0000 (Fri, 09 Jan 2009)
Log Message:
-----------
arm joint space trajectory bug fixes
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
pkg/trunk/robot_descriptions/arm_cart/run_trajectory_controller.launch
pkg/trunk/robot_descriptions/arm_cart/trajectory_controllers.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-01-09 01:06:35 UTC (rev 9097)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-01-09 01:26:53 UTC (rev 9098)
@@ -40,3 +40,7 @@
rospack_add_executable(testTraj test/test_arm_trajectory_controller.cpp)
target_link_libraries(testTraj pr2_mechanism_controllers)
+
+rospack_add_executable(testTrajServ test/test_arm_trajectory_service.cpp)
+target_link_libraries(testTrajServ pr2_mechanism_controllers)
+
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-01-09 01:06:35 UTC (rev 9097)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-01-09 01:26:53 UTC (rev 9098)
@@ -419,13 +419,31 @@
bool ArmTrajectoryControllerNode::queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::request &req,
pr2_mechanism_controllers::TrajectoryQuery::response &resp)
{
+ resp.set_jointnames_size(c_->dimension_);
+ for(int i=0; i < c_->dimension_; i++)
+ {
+ resp.jointnames[i] = c_->joint_pd_controllers_[i]->getJointName();
+
+ }
+
+ if(req.trajectoryid == 0)
+ {
+ resp.trajectorytime = 0;
+ if(joint_trajectory_vector_.size() == 0)
+ resp.done = 1;
+ else
+ resp.done = 0;
+ return true;
+ }
+
std::map<int, int>::const_iterator it = joint_trajectory_status_.find((int)req.trajectoryid);
if(it == joint_trajectory_status_.end())
return false;
+ else
+ resp.done = it->second;
+
- resp.done = it->second;
-
- if(current_trajectory_id_ == (int)req.trajectoryid)
+if(current_trajectory_id_ == (int)req.trajectoryid)
{
if((int) resp.done == 1)
resp.trajectorytime = c_->trajectory_end_time_ - c_->trajectory_start_time_;
@@ -440,12 +458,8 @@
resp.trajectorytime = it_time->second;
}
- resp.set_jointnames_size(c_->dimension_);
- for(int i=0; i < c_->dimension_; i++)
- {
- resp.jointnames[i] = c_->joint_pd_controllers_[i]->getJointName();
-
- }
+
+
return true;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp 2009-01-09 01:06:35 UTC (rev 9097)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp 2009-01-09 01:26:53 UTC (rev 9098)
@@ -98,21 +98,29 @@
req.traj.points[2].time = 0.0;
- if (ros::service::call("set_arm_traj_srv", req, res))
+ if (ros::service::call("TrajectoryStart", req, res))
{
ROS_INFO("Done");
}
- req_q.trajectoryid = 1;
+ req_q.trajectoryid = atoi(argv[1]);
- if(ros::service::call("query_arm_traj_srv", req_q, res_q))
+ if(ros::service::call("TrajectoryQuery", req_q, res_q))
{
ROS_INFO("response:: %f, %d",res_q.trajectorytime,res_q.done);
}
- sleep(10);
- if(ros::service::call("query_arm_traj_srv", req_q, res_q))
+ else
+ {
+ ROS_INFO("service call failed");
+ }
+ sleep(4);
+ if(ros::service::call("TrajectoryQuery", req_q, res_q))
{
ROS_INFO("response:: %f, %d",res_q.trajectorytime,res_q.done);
}
+ else
+ {
+ ROS_INFO("service call failed");
+ }
}
Modified: pkg/trunk/robot_descriptions/arm_cart/run_trajectory_controller.launch
===================================================================
--- pkg/trunk/robot_descriptions/arm_cart/run_trajectory_controller.launch 2009-01-09 01:06:35 UTC (rev 9097)
+++ pkg/trunk/robot_descriptions/arm_cart/run_trajectory_controller.launch 2009-01-09 01:26:53 UTC (rev 9098)
@@ -1,7 +1,7 @@
<launch>
<!-- Arm trajectory controller -->
- <param name="right_arm_controller/velocity_scaling_factor" type="double" value="0.25"/>
+ <param name="right_arm_trajectory_controller/velocity_scaling_factor" type="double" value="0.25"/>
<node pkg="mechanism_control" type="spawner.py" args="$(find arm_cart)/trajectory_controllers.xml" />
Modified: pkg/trunk/robot_descriptions/arm_cart/trajectory_controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/arm_cart/trajectory_controllers.xml 2009-01-09 01:06:35 UTC (rev 9097)
+++ pkg/trunk/robot_descriptions/arm_cart/trajectory_controllers.xml 2009-01-09 01:26:53 UTC (rev 9098)
@@ -3,7 +3,7 @@
<controllers>
<!-- ========================================= -->
<!-- right arm array -->
- <controller name="right_arm_controller" type="ArmTrajectoryControllerNode">
+ <controller name="right_arm_trajectory_controller" type="ArmTrajectoryControllerNode">
<listen_topic name="arm_trajectory_command" />
<kinematics>
<elem key="kdl_chain_name">right_arm</elem>
@@ -49,6 +49,11 @@
</joint>
</controller>
+ <controller name="r_gripper_l_finger_controller" type="JointPDController">
+ <joint name="r_gripper_l_finger_joint">
+ <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
+ </joint>
+ </controller>
<trajectory interpolation="cubic" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-09 13:44:45
|
Revision: 9128
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9128&view=rev
Author: rdiankov
Date: 2009-01-09 13:44:38 +0000 (Fri, 09 Jan 2009)
Log Message:
-----------
several openrave manipulation changes and bug fixes, openrave can now control the real robot and go through a part of the manipulation script with it
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m
pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m
pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
pkg/trunk/openrave_planning/ormanipulation/octave/rungrasptables.m
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-09 13:44:38 UTC (rev 9128)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 599
+SVN_REVISION = -r 600
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -7,4 +7,8 @@
function values = orBodyGetTransform(bodyid)
links = orBodyGetLinks(bodyid);
-values = links(:,1);
\ No newline at end of file
+if( ~isempty(links) )
+ values = links(:,1);
+else
+ values = [];
+end
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-01-09 13:44:38 UTC (rev 9128)
@@ -201,9 +201,12 @@
// destroy environment specific state: problems, planners, figures
_mapFigureIds.clear();
- FOREACH(itprob, _mapproblems)
- itprob->second->GetEnv()->RemoveProblem(itprob->second.get());
- _mapproblems.clear();
+ {
+ boost::mutex::scoped_lock lock(_mutexProblems);
+ FOREACH(itprob, _mapproblems)
+ itprob->second->GetEnv()->RemoveProblem(itprob->second.get());
+ _mapproblems.clear();
+ }
_mapplanners.clear();
}
@@ -616,6 +619,8 @@
if( !pproblem )
return false;
+ boost::mutex::scoped_lock lock(_mutexProblems);
+
if( req.destroyduplicates ) {
map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.begin();
while(itprob != _mapproblems.end()) {
@@ -666,6 +671,7 @@
bool env_destroyproblem_srv(env_destroyproblem::request& req, env_destroyproblem::response& res)
{
+ boost::mutex::scoped_lock lock(_mutexProblems);
map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.find(req.problemid);
if( itprob == _mapproblems.end() )
return false;
@@ -1012,6 +1018,7 @@
bool env_set_srv(env_set::request& req, env_set::response& res)
{
if( req.setmask & env_set::request::Set_Simulation ) {
+ LockEnvironment envlock(this);
switch(req.sim_action) {
case env_set::request::SimAction_Start:
if( req.sim_timestep > 0 )
@@ -1196,6 +1203,7 @@
bool problem_sendcommand_srv(problem_sendcommand::request& req, problem_sendcommand::response& res)
{
+ boost::mutex::scoped_lock lock(_mutexProblems);
map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.find(req.problemid);
if( itprob == _mapproblems.end() )
return false;
@@ -1563,7 +1571,7 @@
/// viewer control variables
boost::shared_ptr<RaveViewerBase> _pviewer;
boost::thread _threadviewer;
- boost::mutex _mutexViewer;
+ boost::mutex _mutexViewer, _mutexProblems;
boost::condition _conditionViewer;
/// workers
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2009-01-09 13:44:38 UTC (rev 9128)
@@ -18,6 +18,7 @@
<Kinbody name="floor">
<Body>
+ <translation>0 0 -0.03</translation>
<Geom type="box">
<extents>4 4 0.01</extents>
<translation>0 0 -0.01</translation>
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -83,7 +83,7 @@
[dests, surfaceplane] = scenedata.GetDestsFn(); % compute a set of destinations
-offsetfromtable = 0.02; %% set destination a little above the table
+offsetfromtable = 0.04; %% set destination a little above the table
distup = surfaceplane(1:3)*(transpose(surfaceplane(1:3))*curobj.info.T(1:3,4) + surfaceplane(4) + offsetfromtable);
curobj.dests = zeros(size(dests));
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -76,13 +76,11 @@
robotid = robot.id;
while(curgrasp < size(grasps,1))
-
- %% C++ grasp testing (fast)
g = transpose(grasps(curgrasp:end,:));
offset = 0.02;
basetime = toc;
- cmd = ['testallgrasps execute 0 outputtraj palmdir ' sprintf('%f ', handrobot.palmdir) ...
+ cmd = ['testallgrasps maxiter 1000 execute 0 outputtraj palmdir ' sprintf('%f ', handrobot.palmdir) ...
' seedik 1 seedgrasps 5 seeddests 8 randomdests 1 randomgrasps 1 ' ...
' target ' curobj.info.name ' robothand ' handrobot.name ...
' robothandjoints ' sprintf('%d ', length(handjoints), handjoints) ...
@@ -175,7 +173,8 @@
% orRobotControllerSend(robotid, 'ignoreproxy 1');
% else
display('closing fingers');
- [trajdata, success] = orProblemSendCommand(['CloseFingers execute 0 outputtraj offset ' sprintf('%f ', 0.04*ones(size(handjoints)))] , probs.manip);
+ closeoffset = 0.12;
+ [trajdata, success] = orProblemSendCommand(['CloseFingers execute 0 outputtraj offset ' sprintf('%f ', closeoffset*ones(size(handjoints)))] , probs.manip);
if( ~success )
error('failed to movehandstraight');
end
@@ -295,7 +294,13 @@
%% cannot wait forever since hand might get stuck
if( ~success )
- error('failed to release fingers');
+ warning('problems releasing, releasing target first');
+ orProblemSendCommand('releaseall', probs.manip);
+ [trajdata, success] = orProblemSendCommand(['ReleaseFingers execute 0 outputtraj target ' curobj.info.name ...
+ ' movingdir ' sprintf('%f ', handrobot.releasedir)], probs.manip);
+ if( ~success )
+ error('failed to release fingers');
+ end
end
success = StartTrajectory(robotid,trajdata,4);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -64,7 +64,7 @@
orBodySetTransform(Target.id, [0 0 0], [1 0 0 0]); % identity
-standoffs = [0.01 0.025];
+standoffs = [0.01];
rolls = [0 pi/2]; % hand is symmetric
% start simulating grasps
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -30,9 +30,9 @@
global updir probs realsession
setrealsession();
-if( ~simulation )
- DeleteObjects(scenedata.TargetObjPattern);
-end
+% if( ~simulation )
+% DeleteObjects(scenedata.TargetObjPattern);
+% end
MySwitchModels = @(x) SwitchModels(scenedata.SwitchModelPatterns, x);
%MySwitchModels = @(x) 0;
@@ -66,7 +66,7 @@
curobj.info = orEnvGetBodies(curobj.id);
if( isempty(curobj.info) )
- display(sprintf('failed to get info for obj %d (might have been deleted)', curobj.info.name));
+ display(sprintf('failed to get info for obj %d (might have been deleted)', curobj.id));
continue;
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -28,6 +28,7 @@
function [robot, scenedata] = SetupTableScene(scene,realrobot,randomize)
global updir probs robothome
+setrealsession();
if( ~exist('realrobot','var') )
realrobot = 0;
@@ -119,7 +120,7 @@
enabledjoints([robot.manips{1}.armjoints; robot.manips{1}.handjoints]) = [];
jointnames_cell = transpose(robot.jointnames(enabledjoints+1));
jointnames_str = cell2mat (cellfun(@(x) [x ' '], jointnames_cell,'uniformoutput',false));
- orRobotControllerSet(robot.id, 'ROSRobot', ['joints ' jointnames_str]);
+ orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice / joints ' jointnames_str]);
end
%% dests is a 12xN array where every column is a 3x4 matrix
@@ -153,12 +154,12 @@
end
Nx = 4;
-Ny = 5;
+Ny = 10;
X = [];
Y = [];
for x = 0:(Nx-1)
X = [X 0.5*rand(1,Ny)/(Nx+1) + (x+1)/(Nx+1)];
- Y = [Y 0.5*rand(1,Ny)/(Ny+1) + ([0:(Ny-1)]+0.5)/(Ny+1)];
+ Y = [Y 0.5*rand(1,Ny)/(Ny+1) + 2*([0:(Ny-1)]-Ny/2)/(Ny+1)];
end
offset = [ab(1,1)-ab(1,2);ab(2,1); ab(3,1)+ab(3,2)];
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
===================================================================
(Binary files differ)
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/rungrasptables.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/rungrasptables.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/rungrasptables.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -10,4 +10,5 @@
%% loop forever
while(1)
RunGrasps('grasp_pr2_ricebox.mat');
+ pause(0.1);
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -4,11 +4,14 @@
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
startup;
-robot = SetupTableScene('data/pr2table_real.env.xml',1);
+[robot, scenedata] = SetupTableScene('data/pr2table_real.env.xml',1);
-Tcamera = [0 0 1 0.05;
- -1 0 0 0.05;
- 0 -1 0 0.095];
+Tlaser = [1.00000 0.00062 0.00232 0.01577
+ -0.00066 0.99988 0.01554 -0.00208
+ -0.00231 -0.01555 0.99988 0.03934];
+Tcamera = [0.01611 0.01631 0.99974 0.02890
+ -0.99966 -0.02020 0.01644 0.03236
+ 0.02047 -0.99966 0.01598 0.06732];
out = orProblemSendCommand(['createsystem ObjectTransform topic /checkerdetector/ObjectDetection thresh 0.1 robot ' sprintf('%d ', robot.id) ' matrixoffset ' sprintf('%f ', Tcamera(1:3,1:4))],probs.task);
if( isempty(out) )
error('failed to create checkerboard detector');
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -14,5 +14,6 @@
setrealsession();
openraveros_restart();
orEnvSetOptions('wdims 800 600');
+orEnvSetOptions('simulation timestep 0.001');
%orEnvSetOptions('collision bullet');
%orEnvSetOptions('debug debug');
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -9,7 +9,7 @@
robot = orEnvGetRobots(robotid);
orBodySetJointValues(robot.id,[ 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.05 0.000000 0.000000 0.000000 0.000000 0.000000 1.582704 1.081165 0.000000 -2.299995 -0.000000 0.000000 0.000000 -0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.979114 -0.400000 0.000000 -1.535151 0.000000 0.000000 0.000000 0.000000 0.000000]);
-%orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /']);
+orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /']);
probs.manip = orEnvCreateProblem('BaseManipulation',robot.name);
imanipulator = 2;
@@ -34,14 +34,17 @@
trajectory = [trajectory sscanf(iksol, '%f ')];
pause(0.1);
end
+trajectory = [trajectory trajectory(:,end:-1:1)];
%% make a service call
rosoct_add_srvs('pr2_mechanism_controllers');
+resquery = rosoct_service_call('/TrajectoryQuery',pr2_mechanism_controllers_TrajectoryQuery());
+
req = pr2_mechanism_controllers_TrajectoryStart();
req.traj.points = {};
for i = 1:size(trajectory,2)
req.traj.points{i} = pr2_mechanism_controllers_JointTrajPoint();
- req.traj.points{i}.positions = trajectory(:,i);
+ req.traj.points{i}.positions = [trajectory(:,i);0.4];
end
res = rosoct_service_call('/TrajectoryStart',req);
Modified: pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2009-01-09 13:44:38 UTC (rev 9128)
@@ -95,6 +95,7 @@
ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>::startsubscriptions();
if( _bSubscribed ) {
+ boost::mutex::scoped_lock lock(_mutex);
ros::node* pnode = check_roscpp_nocreate();
if( pnode != NULL ) {
double tf_cache_time_secs;
@@ -155,7 +156,7 @@
Transform tnew;
// if on robot, have to find the global transformation
- if( bHasRobotTransform ) {
+ if( bHasRobotTransform && !!_tf ) {
posestamped.pose = GetPose(_toffset * GetTransform(itobj->pose));
posestamped.header = _topicmsg.header;
@@ -164,8 +165,17 @@
tnew = trobot * GetTransform(poseout.pose);
}
catch(tf::TransformException& ex) {
- RAVELOG_WARNA("failed to get tf frames %s (body link:%s) for object %s\n",posestamped.header.frame_id.c_str(), strrobotbaselink.c_str(), itobj->type.c_str());
- tnew = GetTransform(itobj->pose);
+
+ try {
+ // try getting the latest value by passing a 0 timestamp
+ posestamped.header.stamp = ros::Time();
+ _tf->transformPose(strrobotbaselink, posestamped, poseout);
+ tnew = trobot * GetTransform(poseout.pose);
+ }
+ catch(tf::TransformException& ex) {
+ RAVELOG_WARNA("failed to get tf frames %s (body link:%s) for object %s\n",posestamped.header.frame_id.c_str(), strrobotbaselink.c_str(), itobj->type.c_str());
+ tnew = GetTransform(itobj->pose);
+ }
}
}
else
@@ -222,12 +232,16 @@
continue;
}
- if( AddKinBody(pbody, NULL) == NULL ) {
+ BODY* b = AddKinBody(pbody, NULL);
+ if( b == NULL ) {
delete pbody;
continue;
}
- pbody->SetTransform(itobj->second);
+ b->tnew = itobj->second;
+
+ // put somewhere at infinity until UpdateBodies thread gets to it
+ pbody->SetTransform(Transform(Vector(1,0,0,0), Vector(10000,10000,10000)));
}
GetEnv()->LockPhysics(false);
}
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-09 13:44:38 UTC (rev 9128)
@@ -51,17 +51,17 @@
{
assert(pnode != NULL);
Destroy();
-
- _srvTrajectoryStart = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryStart::request, pr2_mechanism_controllers::TrajectoryStart::response>(_strTrajectoryServiceDir+"TrajectoryStart", true);
- _srvTrajectoryCancel = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryCancel::request, pr2_mechanism_controllers::TrajectoryCancel::response>(_strTrajectoryServiceDir+"TrajectoryCancel", true);
- _srvTrajectoryWait = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryWait::request, pr2_mechanism_controllers::TrajectoryWait::response>(_strTrajectoryServiceDir+"TrajectoryWait", true);
- _srvTrajectoryQuery = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryQuery::request, pr2_mechanism_controllers::TrajectoryQuery::response>(_strTrajectoryServiceDir+"TrajectoryQuery", true);
+
+ _srvTrajectoryStart = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryStart::request, pr2_mechanism_controllers::TrajectoryStart::response>(_strTrajectoryServiceDir+"TrajectoryStart", false);
+ _srvTrajectoryCancel = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryCancel::request, pr2_mechanism_controllers::TrajectoryCancel::response>(_strTrajectoryServiceDir+"TrajectoryCancel", false);
+ _srvTrajectoryWait = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryWait::request, pr2_mechanism_controllers::TrajectoryWait::response>(_strTrajectoryServiceDir+"TrajectoryWait", false);
+ _srvTrajectoryQuery = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryQuery::request, pr2_mechanism_controllers::TrajectoryQuery::response>(_strTrajectoryServiceDir+"TrajectoryQuery", false);
if( !_srvTrajectoryQuery ) {
RAVELOG_ERRORA("failed to find %s service\n", (_strTrajectoryServiceDir+"TrajectoryQuery").c_str());
return false;
}
-
+
pr2_mechanism_controllers::TrajectoryQuery::request req;
pr2_mechanism_controllers::TrajectoryQuery::response res;
if( !_srvTrajectoryQuery->call(req,res) ) {
@@ -230,15 +230,32 @@
pr2_mechanism_controllers::TrajectoryStart::request req;
pr2_mechanism_controllers::TrajectoryStart::response res;
- vector<dReal> vcurvalues, vnewvalues;
- _probot->GetJointValues(vcurvalues);
+ vector<dReal> vnewvalues;
{
RobotBase::RobotStateSaver saver(_probot);
_probot->SetJointValues(NULL, NULL, pValues,true);
- _probot->GetJointValues(vcurvalues);
+ _probot->GetJointValues(vnewvalues);
}
+ // check if values are sufficiently different
+ if( _vcurvalues.size() == vnewvalues.size() ) {
+ dReal fthresh = 0.01;
+ bool bSendTraj = false;
+ for(size_t i = 0; i < _vcurvalues.size(); ++i) {
+ if( RaveFabs(_vcurvalues[i] - vnewvalues[i]) > fthresh ) {
+ bSendTraj = true;
+ }
+ }
+
+ if( !bSendTraj ) {
+ RAVELOG_VERBOSEA("setdesired sent same joint value, ignoring\n");
+ return false;
+ }
+ }
+ else
+ RAVELOG_WARNA("number of current values (%d) != desird values (%d)\n", _vcurvalues.size(), vnewvalues.size());
+
FOREACH(ittrajcontroller, _listControllers) {
if( !(*ittrajcontroller)->_srvTrajectoryStart ) {
RAVELOG_ERRORA("no start trajectory service\n");
@@ -249,7 +266,7 @@
//req.requesttiming = 1; // request back the timestamps
req.hastiming = 0;
req.traj.points.resize(2);
- (*ittrajcontroller)->GetTrajPoint(vcurvalues, req.traj.points[0]);
+ (*ittrajcontroller)->GetTrajPoint(_vcurvalues, req.traj.points[0]);
(*ittrajcontroller)->GetTrajPoint(vnewvalues, req.traj.points[1]);
if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
@@ -269,6 +286,9 @@
virtual bool SetPath(const Trajectory* ptraj)
{
+ if( ptraj == NULL )
+ return false;
+
boost::mutex::scoped_lock lock(_mutexTrajectories);
bool bSuccess = true;
@@ -286,12 +306,8 @@
req.hastiming = 0;
req.traj.points.resize(ptraj->GetPoints().size());
typeof(req.traj.points.begin()) ittraj = req.traj.points.begin();
- FOREACHC(itpoint, ptraj->GetPoints()) {
- ittraj->positions.reserve(itpoint->q.size()); ittraj->positions.resize(0);
- FOREACHC(itq, itpoint->q)
- ittraj->positions.push_back(*itq);
- ++ittraj;
- }
+ FOREACHC(itpoint, ptraj->GetPoints())
+ (*ittrajcontroller)->GetTrajPoint(itpoint->q, *ittraj++);
if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
(*ittrajcontroller)->_listTrajectories.push_back(res.trajectoryid);
@@ -329,7 +345,7 @@
FOREACHC(itj, _mapjoints) {
values[itj->second] = _mstate.joint_states[itj->first].position;
while(values[itj->second] > vupper[itj->second] ) {
- if( values[itj->second]-vupper[itj->second] > PI )
+ if( values[itj->second]-2*PI >= vlower[itj->second] )
values[itj->second] -= 2*PI;
else {
values[itj->second] = vupper[itj->second];
@@ -337,7 +353,7 @@
}
}
while(values[itj->second] < vlower[itj->second] ) {
- if( values[itj->second]-vlower[itj->second] < -PI )
+ if( values[itj->second]+2*PI <= vupper[itj->second] )
values[itj->second] += 2*PI;
else {
values[itj->second] = vlower[itj->second];
@@ -347,6 +363,7 @@
}
ROS_ASSERT( (int)values.size() == _probot->GetDOF() );
+ _vcurvalues = values;
_probot->SetJointValues(NULL, NULL, &values[0], true);
}
}
@@ -444,47 +461,48 @@
virtual void mechanismstatecb()
{
- if( !_bCalibrated ) {
- // check the robot joint/link names
- do {
- _mapjoints.clear();
- FOREACH(itj, _setEnabledJoints) {
- bool bAdded = false;
- for(size_t j = 0; j < _mstate_cb.get_joint_states_size(); ++j) {
- if( itj->first == _mstate_cb.joint_states[j].name ) {
- _mapjoints[j] = itj->second;
- bAdded = true;
+ {
+ boost::mutex::scoped_lock lock(_mutexstate);
+
+ if( !_bCalibrated ) {
+ // check the robot joint/link names
+ do {
+ _mapjoints.clear();
+ FOREACH(itj, _setEnabledJoints) {
+ bool bAdded = false;
+ for(size_t j = 0; j < _mstate_cb.get_joint_states_size(); ++j) {
+ if( itj->first == _mstate_cb.joint_states[j].name ) {
+ _mapjoints[j] = itj->second;
+ bAdded = true;
+ break;
+ }
+ }
+
+ if( !bAdded ) {
+ RAVELOG_WARNA("could not find robot joint %s in mechanism state\n", itj->first.c_str());
break;
}
}
-
- if( !bAdded ) {
- RAVELOG_WARNA("could not find robot joint %s in mechanism state\n", itj->first.c_str());
+
+ if( _mapjoints.size() != _setEnabledJoints.size() ) {
+ _mapjoints.clear();
break;
}
- }
- if( _mapjoints.size() != _setEnabledJoints.size() ) {
- _mapjoints.clear();
- break;
- }
+ _bCalibrated = true;
+ } while(0);
+ }
+ else {
+ if( _probot->GetDOF() != (int)_mstate_cb.get_joint_states_size() )
+ _bCalibrated = false;
+ }
- _bCalibrated = true;
- } while(0);
- }
- else {
- if( _probot->GetDOF() != (int)_mstate_cb.get_joint_states_size() )
- _bCalibrated = false;
- }
+ if( !_bCalibrated )
+ return;
- if( !_bCalibrated )
- return;
-
- {
- boost::mutex::scoped_lock lock(_mutexstate);
_mstate = _mstate_cb;
}
-
+
// do some monitoring of the joint state (try to look for stalls)
}
@@ -497,7 +515,6 @@
// check if the first trajectory is done
boost::mutex::scoped_lock lock(_mutexTrajectories);
- uint32_t trajectoryid;
bool bDone = true;
if( _listControllers.size() > 0 ) {
@@ -523,9 +540,9 @@
}
if( bPopTrajectory ) {
- RAVELOG_DEBUGA("robot trajectory finished, left: %d\n", _listControllers.front()->_listTrajectories.size());
FOREACH(ittraj, _listControllers)
(*ittraj)->_listTrajectories.pop_front();
+ RAVELOG_DEBUGA("robot trajectory finished, left: %d\n", _listControllers.front()->_listTrajectories.size());
}
bDone = _listControllers.front()->_listTrajectories.size()==0;
@@ -544,6 +561,7 @@
vector<dReal> _vecdesired;
set< pair<string, int> > _setEnabledJoints; // set of enabled joints and their indices
mutable boost::mutex _mutexstate;
+ vector<dReal> _vcurvalues; ///< current robot values
ofstream flog;
int logid;
@@ -561,7 +579,7 @@
bool _bSendTimestamps; ///< if true, will send timestamps along with traj
bool _bSubscribed; ///< if true, succesfully subscribed to the mechanism state msgs
bool _bCalibrated; ///< if true, mechanism state matches robot
- bool _bDestroyThread; /// if true, destroy the thread
+ bool _bDestroyThread; ///< if true, destroy the thread
};
#endif
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-10 02:01:21
|
Revision: 9162
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9162&view=rev
Author: rdiankov
Date: 2009-01-10 02:01:15 +0000 (Sat, 10 Jan 2009)
Log Message:
-----------
ormanipulation work, robot demo becoming more stable
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m
pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
Added Paths:
-----------
pkg/trunk/openrave_planning/ormanipulation/octave/ParseTrajData.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-10 02:01:15 UTC (rev 9162)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 600
+SVN_REVISION = -r 601
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-01-10 02:01:15 UTC (rev 9162)
@@ -272,8 +272,8 @@
node_->unadvertise_service(service_prefix_ + "/get_command");
node_->unadvertise_service(service_prefix_ + "/set_target");
*/
- node_->unadvertise_service(service_prefix_ + "/TrajectoryStart");
- node_->unadvertise_service(service_prefix_ + "/TrajectoryQuery");
+ node_->unadvertise_service(service_prefix_ + "TrajectoryStart");
+ node_->unadvertise_service(service_prefix_ + "TrajectoryQuery");
if(topic_name_ptr_ && topic_name_.c_str())
{
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -1,6 +1,8 @@
-%% openraveros_restart(sessionserver, viewer)
+%% openraveros_restart(sessionserver, viewer, destroyall)
%%
%% restars an openraveros session if the current one is invalid
+%% if destroyall is 1 and openraveros cannot find a currently existing session,
+%% destroys all other sessions before creating a new one
%% Software License Agreement (BSD License)
%% Copyright (c) 2008, Willow Garage, Inc.
@@ -27,9 +29,13 @@
%% POSSIBILITY OF SUCH DAMAGE.
%%
%% author: Rosen Diankov
-function openraveros_restart(sessionserver,viewer)
+function openraveros_restart(sessionserver,viewer,destroyall)
global openraveros_globalsession
+if( ~exist('destroyall','var') )
+ destroyall = 0;
+end
+
if( ~exist('sessionserver','var') || isempty(sessionserver) )
sessionserver = 'openrave_session';
end
@@ -38,7 +44,7 @@
viewer = 'qtcoin';
end
-openraveros_startup(sessionserver, 1, viewer);
+openraveros_startup(sessionserver, 1, viewer,destroyall);
if( ~isempty(openraveros_globalsession) )
%% send a dummy env_set command
@@ -49,6 +55,15 @@
end
end
+if( destroyall )
+ reqdestroy = openraveros_openrave_session();
+ reqdestroy.sessionid = -1;
+ resdestroy = rosoct_service_call(sessionserver,reqdestroy);
+ if( isempty(resdestroy) )
+ warning('failed to destroy sessions');
+ end
+end
+
openraveros_globalsession = openraveros_createsession(sessionserver);
if( ~isempty(viewer) && ~isempty(openraveros_globalsession) )
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -1,5 +1,8 @@
-%% openraveros_startup(sessionserver, createsession, viewer)
+%% openraveros_startup(sessionserver, createsession, viewer, destroyall)
+%%
%% adds all the necessary paths for the openraveros octave client
+%% if destroyall is 1 and openraveros cannot find a currently existing session,
+%% destroys all other sessions before creating a new one
%% Software License Agreement (BSD License)
%% Copyright (c) 2008, Willow Garage, Inc.
@@ -26,13 +29,16 @@
%% POSSIBILITY OF SUCH DAMAGE.
%%
%% author: Rosen Diankov
-function openraveros_startup(sessionserver,createsession, viewer)
+function openraveros_startup(sessionserver,createsession, viewer, destroyall)
global openraveros_globalsession
persistent openraveros_initialized
if( ~exist('createsession','var') )
createsession = 1;
end
+if( ~exist('destroyall','var') )
+ destroyall = 0;
+end
if( isempty(openraveros_initialized))
[status,rosoctpath] = system('rospack find rosoct');
@@ -55,6 +61,15 @@
viewer = 'qtcoin';
end
+
+ if( destroyall )
+ reqdestroy = openraveros_openrave_session();
+ reqdestroy.sessionid = -1;
+ resdestroy = rosoct_service_call(sessionserver,reqdestroy);
+ if( isempty(resdestroy) )
+ warning('failed to destroy sessions');
+ end
+ end
req = openraveros_openrave_session();
req.viewer = viewer; % default viewer
while(1)
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-10 02:01:15 UTC (rev 9162)
@@ -331,15 +331,23 @@
bool session_callback(openrave_session::request& req, openrave_session::response& res)
{
if( req.sessionid != 0 ) {
- // destory the session
boost::mutex::scoped_lock lock(_mutexsession);
- if( _mapsessions.find(req.sessionid) != _mapsessions.end() ) {
+
+ if( req.sessionid == -1 ) {
+ // destroy all sessions
+ RAVELOG_DEBUGA("destroying all sessions\n");
+ _mapsessions.clear();
+ }
+ else {
+ // destory the session
+ if( _mapsessions.find(req.sessionid) == _mapsessions.end() )
+ return false;
+
_mapsessions.erase(req.sessionid);
RAVELOG_INFOA("destroyed openrave session: %d\n", req.sessionid);
- return true;
}
-
- return false;
+
+ return true;
}
int id = rand();
Modified: pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv 2009-01-10 02:01:15 UTC (rev 9162)
@@ -2,7 +2,7 @@
# that gets loaded.
# if 0, creates an openrave session
-# if -1, destroys all other session instances and creates an openrave session
+# if -1, destroys all other session instances
# otherwise, destroy the sessionid
int32 sessionid
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2009-01-10 02:01:15 UTC (rev 9162)
@@ -5,7 +5,7 @@
<Robot file="robots/pr2full.robot.xml">
<translation>-0.64 0.31 0.0102</translation>
- <jointvalues> 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.05 0.000000 0.000000 0.000000 0.000000 0.000000 1.582704 1.081165 0.000000 -2.299995 -0.000000 0.000000 0.000000 -0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.979114 -0.400000 0.000000 -1.535151 0.000000 0.000000 0.000000 0.000000 0.000000</jointvalues>
+ <jointvalues> 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.05 0.000000 0.000000 0.000000 0.000000 0.000000 1.582704 1.081165 0.000000 -2.299995 -0.000000 0.000000 0.000000 -0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.979114 -0.400000 0.000000 -1.535151 0.000000 0.000000 0.000000 0.000000 0.500000</jointvalues>
</Robot>
<Kinbody name="floor">
@@ -18,7 +18,9 @@
</Body>
</Kinbody>
- <Kinbody file="willow_table.kinbody.xml"/>
+ <Kinbody file="willow_table.kinbody.xml">
+ <translation>2 0 0</translation>
+ </Kinbody>
<Kinbody name="ricebox0" file="ricebox.kinbody.xml">
<rotationmat>0 0 -1 0 1 0 1 0 0</rotationmat>
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2009-01-10 02:01:15 UTC (rev 9162)
@@ -11,7 +11,7 @@
<prerotationaxis>0 0 1 180</prerotationaxis>
</phasespace>
<!-- default values to get left arm out of the way -->
- <jointvalues>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.05 0.000000 0.000000 0.000000 0.000000 0.000000 1.582704 1.081165 0.000000 -2.299995 -0.000000 0.000000 0.000000 -0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.979114 -0.400000 0.000000 -1.535151 0.000000 0.000000 0.000000 0.000000 0.000000</jointvalues>
+ <jointvalues>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.05 0.000000 0.000000 0.000000 0.000000 0.000000 1.582704 1.081165 0.000000 -2.299995 -0.000000 0.000000 0.000000 -0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.979114 -0.400000 0.000000 -1.535151 0.000000 0.000000 0.000000 0.000000 0.500000</jointvalues>
</Robot>
<Kinbody file="willow_table.kinbody.xml"/>
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -315,9 +315,10 @@
if( squeezesuccess > 0 & putsuccess > 0 )
display('success, putting down');
+ orBodySetJointValues(robotid,open_config,handjoints);
% only break when succeeded
%orProblemSendCommand(['MoveHandStraight stepsize 0.003 minsteps ' sprintf('%f ', 90) ' maxsteps ' sprintf('%f ', 100) ' direction ' sprintf('%f ', updir')]);
- %RobotGoInitial(robot);
+ RobotGoInitial(robot);
if( MySwitchModels(0) )
curobj.id = orEnvGetBody(curobj.info.name);
Added: pkg/trunk/openrave_planning/ormanipulation/octave/ParseTrajData.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/ParseTrajData.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/ParseTrajData.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -0,0 +1,12 @@
+%% [trajvals,transvals,timevals] = ParseTrajData(trajdata)
+%%
+%% parses a trajectory file
+function [trajvals,transvals,timevals] = ParseTrajData(trajdata)
+vals = sscanf(trajdata, '%f');
+numpts = vals(1);
+numdof = vals(2);
+
+newvals = reshape(vals(4:(3+numpts*(numdof+8))),[numdof+8 numpts]);
+timevals = newvals(1,:);
+transvals = newvals((2+numdof):end,:);
+trajvals = newvals(2:(1+numdof),:);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -29,11 +29,13 @@
display('moving hand');
prevsession = openraveros_getglobalsession();
+prevprobs = probs;
setrealsession();
-orRobotSetDOFValues(robotid,home(handjoints),handjoints);
+orRobotSetDOFValues(robotid,home(handjoints+1),handjoints);
pause(0.4); % pause a little to give a chance for controller to start
success = WaitForRobot(robotid);
setclonesession(prevsession);
+probs = prevprobs;
%orRobotSetActiveDOFs(robotid,0:(robot.dof-1));
%curvalues = orRobotGetDOFValues(robotid);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -83,5 +83,10 @@
end
% switch back to real
+ sessionclone = openraveros_getglobalsession();
setrealsession();
+ if( sessionclone.id ~= openraveros_getglobalsession().id )
+ %destroy
+ openraveros_destroysession(sessionclone);
+ end
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -92,9 +92,23 @@
orProblemSendCommand('setactivemanip 1',probs.manip); % right arm
robot.activemanip = 2; % update
+tableaabb = [];
+tablepattern = '^willow_table$';
+bodies = orEnvGetBodies(0,openraveros_BodyInfo().Req_Names());
+for i = 1:length(bodies)
+ if( regexp(bodies{i}.name, tablepattern) )
+ Tidentity = eye(4);
+ Ttable = bodies{i}.T;
+ orBodySetTransform(bodies{i}.id,Tidentity(1:3,1:4));
+ ab = orBodyGetAABB(bodies{i}.id);
+ orBodySetTransform(bodies{i}.id,Ttable);
+ break;
+ end
+end
+
scenedata.TargetObjPattern = TargetObjPattern;
scenedata.SwitchModelPatterns = SwitchModelPatterns;
-scenedata.GetDestsFn = @() GetDests('^willow_table$');
+scenedata.GetDestsFn = @() GetDests(tablepattern, tableaabb);
scenedata.SetupCloneFn = @() SetupClone(robot.name);
updir = [0;0;1];
@@ -120,11 +134,11 @@
enabledjoints([robot.manips{1}.armjoints; robot.manips{1}.handjoints]) = [];
jointnames_cell = transpose(robot.jointnames(enabledjoints+1));
jointnames_str = cell2mat (cellfun(@(x) [x ' '], jointnames_cell,'uniformoutput',false));
- orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice / joints ' jointnames_str]);
+ orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /right_arm_trajectory_controller joints ' jointnames_str]);
end
%% dests is a 12xN array where every column is a 3x4 matrix
-function [dests, surfaceplane] = GetDests(tablepattern)
+function [dests, surfaceplane] = GetDests(tablepattern, ab)
dests = [];
surfaceplane = [];
@@ -146,8 +160,12 @@
end
%% table up is assumed to be +z, sample the +y axis of the table
-ab = orBodyGetAABB(id);
if( isempty(ab) )
+ ab = orBodyGetAABB(id);
+ ab(1:3,1) = ab(1:3,1) - Ttable(1:3,4);
+end
+
+if( isempty(ab) )
display('missed table, trying again');
dests = GetDests(tablepattern);
return;
@@ -159,11 +177,11 @@
Y = [];
for x = 0:(Nx-1)
X = [X 0.5*rand(1,Ny)/(Nx+1) + (x+1)/(Nx+1)];
- Y = [Y 0.5*rand(1,Ny)/(Ny+1) + 2*([0:(Ny-1)]-Ny/2)/(Ny+1)];
+ Y = [Y 0.5*rand(1,Ny)/(Ny+1) + ([0:(Ny-1)]+0.5)/(Ny+1)];
end
-offset = [ab(1,1)-ab(1,2);ab(2,1); ab(3,1)+ab(3,2)];
-trans = [offset(1)+2*ab(1,2)*X; offset(2)+ab(2,2)*Y; repmat(offset(3),size(X))];
+offset = [ab(1,1)-ab(1,2);ab(2,1)-ab(2,2); ab(3,1)+ab(3,2)];
+trans = [offset(1)+2*ab(1,2)*X; offset(2)+2*ab(2,2)*Y; repmat(offset(3),size(X))];
trans = Ttable*[trans;ones(size(X))];
Rots = [];
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -19,9 +19,10 @@
setrealsession();
[out,trajsuc] = orProblemSendCommand(['traj stream ' trajdata],probs.manip);
if( ~trajsuc )
+ display('trajectory failed');
success = 0;
setclonesession(prevsession);
- probs = prevprobs;
+ probs = prevprobs
return;
end
@@ -46,7 +47,8 @@
end
end
+display('wait ended');
newjointconfig = orBodyGetJointValues(robotid);
setclonesession(prevsession);
-probs = prevprobs;
+probs = prevprobs
orBodySetJointValues(robotid,newjointconfig);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -1,8 +1,8 @@
#!/usr/bin/env octave
+cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
+
global updir probs
-cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
-
startup;
[robot, scenedata] = SetupTableScene('data/pr2table_real.env.xml',1);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -12,7 +12,7 @@
end
setrealsession();
-openraveros_restart();
+openraveros_restart('openrave_session','qtcoin',1);
orEnvSetOptions('wdims 800 600');
orEnvSetOptions('simulation timestep 0.001');
%orEnvSetOptions('collision bullet');
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-01-10 07:58:53
|
Revision: 9174
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9174&view=rev
Author: tfoote
Date: 2009-01-10 07:58:49 +0000 (Sat, 10 Jan 2009)
Log Message:
-----------
moving shadow and intensity filters into laser_scan package, and decreasing our package count by 2 :-)
Modified Paths:
--------------
pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
pkg/trunk/util/laser_scan/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp
pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp
Removed Paths:
-------------
pkg/trunk/drivers/laser/scan_intensity_filter/
pkg/trunk/drivers/laser/scan_shadows_filter/
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml 2009-01-10 02:59:08 UTC (rev 9173)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml 2009-01-10 07:58:49 UTC (rev 9174)
@@ -72,7 +72,7 @@
<param name="/scan_shadows_filter/filter_min_angle" value="10"/>
<param name="/scan_shadows_filter/filter_max_angle" value="170"/>
<param name="/scan_shadows_filter/filter_window" value="2"/>
- <node pkg="scan_shadows_filter" type="scan_shadows_filter_node" respawn="false" output="screen" />
+ <node pkg="laser_scan" type="scan_shadows_filter_node" respawn="false" output="screen" />
<!-- move_base_sbpl settings -->
<!-- for the moment use e.g.: ./shparam set move_base/plannerType string ADPlanner -->
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml 2009-01-10 02:59:08 UTC (rev 9173)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml 2009-01-10 07:58:49 UTC (rev 9174)
@@ -21,7 +21,7 @@
<param name="/scan_shadows_filter/filter_min_angle" value="10"/>
<param name="/scan_shadows_filter/filter_max_angle" value="170"/>
<param name="/scan_shadows_filter/filter_window" value="2"/>
- <node pkg="scan_shadows_filter" type="scan_shadows_filter_node" respawn="false" output="screen" />
+ <node pkg="laser_scan" type="scan_shadows_filter_node" respawn="false" output="screen" />
<!-- wavefront_player settings -->
<!--
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-01-10 02:59:08 UTC (rev 9173)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-01-10 07:58:49 UTC (rev 9174)
@@ -21,7 +21,6 @@
<depend package="pr2_gazebo"/>
<depend package="pr2_prototype1_gazebo"/>
<depend package="transformations"/>
- <depend package="scan_shadows_filter"/>
<depend package="ransac_ground_plane_extraction"/>
<depend package="point_cloud_assembler"/>
</package>
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml 2009-01-10 02:59:08 UTC (rev 9173)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml 2009-01-10 07:58:49 UTC (rev 9174)
@@ -16,7 +16,6 @@
# For Testing
<depend package="rostest"/>
<depend package="gtest"/>
- <depend package="scan_shadows_filter"/>
<depend package="rosrecord" />
</package>
Modified: pkg/trunk/util/laser_scan/CMakeLists.txt
===================================================================
--- pkg/trunk/util/laser_scan/CMakeLists.txt 2009-01-10 02:59:08 UTC (rev 9173)
+++ pkg/trunk/util/laser_scan/CMakeLists.txt 2009-01-10 07:58:49 UTC (rev 9174)
@@ -5,4 +5,10 @@
rospack_add_library(laser_median_filter src/median_filter.cpp )
rospack_add_executable(median_node src/median_filter_node.cpp)
-target_link_libraries(median_node laser_median_filter)
\ No newline at end of file
+target_link_libraries(median_node laser_median_filter)
+
+rospack_add_executable(scan_shadows_filter_node src/scan_shadows_filter.cpp)
+target_link_libraries (scan_shadows_filter_node laser_scan)
+
+rospack_add_executable(scan_intensity_filter src/scan_intensity_filter.cpp)
+target_link_libraries (scan_intensity_filter laser_scan)
Copied: pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp (from rev 5430, pkg/trunk/drivers/laser/scan_intensity_filter/scan_intensity_filter.cpp)
===================================================================
--- pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp (rev 0)
+++ pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp 2009-01-10 07:58:49 UTC (rev 9174)
@@ -0,0 +1,126 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+\author Vijay Pradeep
+@b ScanIntensityFilter takes input scans and fiters out that are not within the specified range. The filtered out readings are set at >max_range \
+in order to invalidate them.
+
+**/
+
+
+#include "ros/node.h"
+#include "std_msgs/LaserScan.h"
+
+using namespace std_msgs;
+
+class ScanIntensityFilter : public ros::node
+{
+public:
+
+ LaserScan cur_scan_ ;
+ double lower_threshold_ ;
+ double upper_threshold_ ;
+ int disp_hist_ ;
+
+ ScanIntensityFilter() : ros::node("scan_intensity_filter")
+ {
+ advertise<LaserScan>("filtered_scan", 1) ;
+ subscribe("input_scan", cur_scan_, &ScanIntensityFilter::scans_callback, 40) ;
+
+ param("~lower_threshold", lower_threshold_, 8000.0) ;
+ param("~upper_threshold", upper_threshold_, 100000.0) ;
+ param("~disp_histogram", disp_hist_, 1) ;
+ }
+
+ virtual ~ScanIntensityFilter()
+ {
+
+ }
+
+ void scans_callback()
+ {
+ const double hist_max = 4*12000.0 ;
+ const int num_buckets = 24 ;
+ int histogram[num_buckets] ;
+ for (int i=0; i < num_buckets; i++)
+ histogram[i] = 0 ;
+
+ LaserScan filtered_scan ;
+ filtered_scan = cur_scan_ ;
+
+ for (unsigned int i=0; i < cur_scan_.ranges.size(); i++) // Need to check ever reading in the current scan
+ {
+ if (filtered_scan.intensities[i] <= lower_threshold_ || // Is this reading below our lower threshold?
+ filtered_scan.intensities[i] >= upper_threshold_) // Is this reading above our upper threshold?
+ {
+ filtered_scan.ranges[i] = cur_scan_.range_max + 1.0 ; // If so, then make it a value bigger than the max range
+ }
+
+ int cur_bucket = (int) ((filtered_scan.intensities[i]/hist_max)*num_buckets) ;
+ if (cur_bucket >= num_buckets-1)
+ cur_bucket = num_buckets-1 ;
+ histogram[cur_bucket]++ ;
+ }
+
+ if (disp_hist_ > 0) // Display Histogram
+ {
+ printf("********** SCAN **********\n") ;
+ for (int i=0; i < num_buckets; i++)
+ {
+ printf("%u - %u: %u\n", (unsigned int) hist_max/num_buckets*i,
+ (unsigned int) hist_max/num_buckets*(i+1),
+ histogram[i]) ;
+ }
+ }
+
+ publish("filtered_scan", filtered_scan) ; // Publish the filtered data
+ }
+} ;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv) ;
+ ScanIntensityFilter filter ;
+ filter.spin() ;
+ ros::fini() ;
+ return 0 ;
+}
+
Copied: pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp (from rev 8873, pkg/trunk/drivers/laser/scan_shadows_filter/src/scan_shadows_filter.cpp)
===================================================================
--- pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp (rev 0)
+++ pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp 2009-01-10 07:58:49 UTC (rev 9174)
@@ -0,0 +1,258 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <ru...@cs...>
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * $Id: scan_shadows_filter.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/*
+\author Radu Bogdan Rusu <ru...@cs...>
+
+
+ */
+
+#include <ros/node.h>
+#include <std_msgs/PointCloud.h>
+#include <std_msgs/LaserScan.h>
+
+#include <float.h>
+
+// Laser projection
+#include <laser_scan/laser_scan.h>
+
+using namespace std_msgs;
+
+/** @b ScanShadowsFilter is a simple node that filters shadow points in a laser scan line and publishes the results in a cloud.
+ */
+class ScanShadowsFilter : public ros::node
+{
+ public:
+
+ // ROS related
+ LaserScan tilt_scan_msg_; // Filled by subscriber with new tilte laser scans
+ laser_scan::LaserProjection projector_; // Used to project laser scans
+
+ double tilt_laser_max_range_; // Used in laser scan projection
+ double min_angle_, max_angle_; // Filter angle threshold
+ int window_;
+
+ ////////////////////////////////////////////////////////////////////////////////
+ ScanShadowsFilter () : ros::node ("scan_shadows_filter"), tilt_laser_max_range_ (DBL_MAX)
+ {
+ subscribe("tilt_scan", tilt_scan_msg_, &ScanShadowsFilter::tiltScanCallback, 10);
+
+ advertise<PointCloud> ("tilt_laser_cloud_filtered", 10);
+
+ param ("~filter_min_angle", min_angle_, 10.0);
+ param ("~filter_max_angle", max_angle_, 170.0);
+ param ("~filter_window", window_, 2);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ virtual ~ScanShadowsFilter () { }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /**
+ * \brief Computes the angle between the two lines created from 2 points and the
+ * viewpoint. Returns the angle (in degrees).
+ * \param px X coordinate for the first point
+ * \param py Y coordinate for the first point
+ * \param pz Z coordinate for the first point
+ * \param qx X coordinate for the second point
+ * \param qy Y coordinate for the second point
+ * \param qz Z coordinate for the second point
+ */
+ inline double
+ getAngleWithViewpoint (float px, float py, float pz, float qx, float qy, float qz)
+ {
+ double dir_a[3], dir_b[3];
+ dir_a[0] = - px; dir_a[1] = - py; dir_a[2] = - pz; // Assume viewpoint is 0,0,0
+ dir_b[0] = qx - px; dir_b[1] = qy - py; dir_b[2] = qz - pz;
+
+ // sqrt (sqr (x) + sqr (y) + sqr (z))
+ double norm_a = sqrt (dir_a[0]*dir_a[0] + dir_a[1]*dir_a[1] + dir_a[2]*dir_a[2]);
+ // Check for bogus 0,0,0 points
+ if (norm_a == 0) return (0);
+ double norm_b = sqrt (dir_b[0]*dir_b[0] + dir_b[1]*dir_b[1] + dir_b[2]*dir_b[2]);
+ if (norm_b == 0) return (0);
+ // dot_product (x, y)
+ double dot_pr = dir_a[0]*dir_b[0] + dir_a[1]*dir_b[1] + dir_a[2]*dir_b[2];
+ if (dot_pr != dot_pr) // Check for NaNs
+ return (0);
+
+ return ( acos (dot_pr / (norm_a * norm_b) ) * 180.0 / M_PI);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Given a PointCloud representing a single laser scan (usually obtained
+ * after LaserProjection's projectLaser(), and the index of the channel
+ * representing the true measurement "index", create a complete PointCloud
+ * representation which replaces the invalid measurements with 0 values.
+ * \param c_idx the channel index for the "index"
+ * \param cloud_in the input PointCloud message
+ * \param cloud_out the output PointCloud message
+ */
+ void
+ constructCompleteLaserScanCloud (int c_idx, PointCloud cloud_in, PointCloud &cloud_out)
+ {
+ // Use the index to revert to a full laser scan cloud (inefficient locally, but efficient globally)
+ int idx = 0;
+ for (unsigned int i = 0; i < cloud_out.pts.size (); i++)
+ {
+ unsigned int j = (int)cloud_in.chan[c_idx].vals[idx]; // Find out the true index value
+ if (i == j)
+ {
+ // Copy relevant data
+ cloud_out.pts[i].x = cloud_in.pts[idx].x;
+ cloud_out.pts[i].y = cloud_in.pts[idx].y;
+ cloud_out.pts[i].z = cloud_in.pts[idx].z;
+ for (unsigned int d = 0; d < cloud_out.get_chan_size (); d++)
+ cloud_out.chan[d].vals[i] = cloud_in.chan[d].vals[idx];
+
+ idx++; // Assume chan['index'] is sorted (which should be true)
+ if (idx >= (int)cloud_in.chan[c_idx].vals.size ()) idx = cloud_in.chan[c_idx].vals.size () - 1;
+ }
+ else
+ {
+ // Bogus XYZ entry. No need to copy channels.
+ cloud_out.pts[i].x = cloud_out.pts[i].y = cloud_out.pts[i].z = 0;
+ }
+ }
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Filter shadow points based on 3 global parameters: min_angle, max_angle
+ * and window. {min,max}_angle specify the allowed angle interval (in degrees)
+ * between the created lines (see getAngleWithViewPoint). Window specifies how many
+ * consecutive measurements to take into account for one point.
+ * \param cloud_in the input PointCloud message
+ * \param cloud_out the output PointCloud message
+ */
+ void
+ filterShadowPoints (PointCloud cloud_in, PointCloud &cloud_out)
+ {
+ // For each point in the current line scan
+ int n_pts_filtered = 0;
+ for (unsigned int i = 0; i < cloud_in.pts.size (); i++)
+ {
+ bool valid_point = true;
+ for (int y = -window_; y < window_ + 1; y++)
+ {
+ int j = i + y;
+ if ( j < 0 || j >= (int)cloud_in.pts.size () || (int)i == j ) // Out of scan bounds or itself
+ continue;
+
+ double angle = getAngleWithViewpoint (cloud_in.pts[i].x, cloud_in.pts[i].y, cloud_in.pts[i].z,
+ cloud_in.pts[j].x, cloud_in.pts[j].y, cloud_in.pts[j].z);
+ if (angle < min_angle_ || angle > max_angle_)
+ valid_point = false;
+ }
+
+ // If point found as 'ok', copy the relevant data
+ if (valid_point)
+ {
+ cloud_out.pts[n_pts_filtered].x = cloud_in.pts[i].x;
+ cloud_out.pts[n_pts_filtered].y = cloud_in.pts[i].y;
+ cloud_out.pts[n_pts_filtered].z = cloud_in.pts[i].z;
+
+ for (unsigned int d = 0; d < cloud_out.get_chan_size (); d++)
+ cloud_out.chan[d].vals[n_pts_filtered] = cloud_in.chan[d].vals[i];
+
+ n_pts_filtered++;
+ }
+ }
+
+ // Resize output vectors
+ cloud_out.pts.resize (n_pts_filtered);
+ for (unsigned int d = 0; d < cloud_out.get_chan_size (); d++)
+ cloud_out.chan[d].vals.resize (n_pts_filtered);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ void
+ tiltScanCallback ()
+ {
+ // Project laser into point cloud
+ PointCloud tilt_cloud;
+ int n_scan = tilt_scan_msg_.ranges.size (); // Save the number of measurements
+
+ // Transform into a PointCloud message
+ int mask = laser_scan::MASK_INTENSITY + laser_scan::MASK_DISTANCE + laser_scan::MASK_INDEX;
+ projector_.projectLaser (tilt_scan_msg_, tilt_cloud, tilt_laser_max_range_, false, mask);//, true);
+
+ /// ---[ Perhaps unnecessary, but find out which channel contains the index
+ int c_idx = -1;
+ for (unsigned int d = 0; d < tilt_cloud.get_chan_size (); d++)
+ {
+ if (tilt_cloud.chan[d].name == "index")
+ {
+ c_idx = d;
+ break;
+ }
+ }
+ if (c_idx == -1 || tilt_cloud.chan[c_idx].vals.size () == 0) return;
+ /// ]--
+
+ // Prepare the storage for the temporary array ([] and resize are faster than push_back)
+ PointCloud tilt_full_cloud (tilt_cloud);
+ tilt_full_cloud.pts.resize (n_scan);
+ for (unsigned int d = 0; d < tilt_cloud.get_chan_size (); d++)
+ tilt_full_cloud.chan[d].vals.resize (n_scan);
+
+ // Prepare data storage for the output array ([] and resize are faster than push_back)
+ PointCloud filtered_cloud (tilt_cloud);
+ filtered_cloud.pts.resize (n_scan);
+ for (unsigned int d = 0; d < tilt_cloud.get_chan_size (); d++)
+ filtered_cloud.chan[d].vals.resize (n_scan);
+
+ // Construct a complete laser cloud resembling the original LaserScan (0..LASER_MAX measurements)
+ constructCompleteLaserScanCloud (c_idx, tilt_cloud, tilt_full_cloud);
+
+ // Filter points
+ filterShadowPoints (tilt_full_cloud, filtered_cloud);
+
+ // Set timestamp/frameid and publish
+ filtered_cloud.header = tilt_scan_msg_.header;
+ publish ("tilt_laser_cloud_filtered", filtered_cloud);
+ }
+
+} ;
+
+/* ---[ */
+int
+ main (int argc, char** argv)
+{
+ ros::init (argc, argv);
+
+ ScanShadowsFilter f;
+ f.spin ();
+
+ ros::fini ();
+
+ return (0);
+}
+/* ]--- */
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-10 23:49:12
|
Revision: 9182
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9182&view=rev
Author: rdiankov
Date: 2009-01-10 23:49:08 +0000 (Sat, 10 Jan 2009)
Log Message:
-----------
added robotlinks_filter node in laser_scan package to prune all points that are in the convex hull of the links. the node is reconfigurable to any robot and has options for padding and using individual timestamps for every point
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/3rdparty/openrave/manifest.xml
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
pkg/trunk/util/laser_scan/CMakeLists.txt
pkg/trunk/util/laser_scan/manifest.xml
Added Paths:
-----------
pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp
pkg/trunk/util/laser_scan/start_robotlinks_filter.ros.xml
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-10 19:59:58 UTC (rev 9181)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-10 23:49:08 UTC (rev 9182)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 601
+SVN_REVISION = -r 602
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/3rdparty/openrave/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/openrave/manifest.xml 2009-01-10 19:59:58 UTC (rev 9181)
+++ pkg/trunk/3rdparty/openrave/manifest.xml 2009-01-10 23:49:08 UTC (rev 9182)
@@ -4,6 +4,7 @@
For successful compilation, remove Qt3 development tools (qt3-dev-tools,libqt3-headers) to prevent conflict with Qt4.
</description>
+ <author>Rosen Diankov (rdi...@cs...)</author>
<license>LGPL</license>
<review status="3rdparty" notes=""/>
<export>
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-10 19:59:58 UTC (rev 9181)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-10 23:49:08 UTC (rev 9182)
@@ -1,5 +1,5 @@
// Software License Agreement (BSD License)
-// Copyright (c) 2008, Willow Garage, Inc.
+// Copyright (c) 2008, Rosen Diankov (rdi...@cs...)
// 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,
@@ -21,9 +21,17 @@
// 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.
-//
-// author: Rosen Diankov
+/**
+@mainpage
+
+@htmlinclude manifest.html
+
+\author Rosen Diankov (rdi...@cs...)
+
+@b SessionServer creates and manages a roscpp session that wraps various OpenRAVE services.
+ **/
+
#include "openraveros.h"
#include "rosserver.h"
Modified: pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml 2009-01-10 19:59:58 UTC (rev 9181)
+++ pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml 2009-01-10 23:49:08 UTC (rev 9182)
@@ -2,7 +2,7 @@
<launch>
<machine name="localhost-openrave" address="localhost"/>
<node machine="localhost-openrave" pkg="openraveros" type="openraveros" output="screen">
- <env name="OPENRAVE_DATA" value="$(env OPENRAVE_DATA):$(find openrave)/share/openrave:$(find openrave_robot_description):$(find ormanipulation)"/>
- <env name="OPENRAVE_PLUGINS" value="$(env OPENRAVE_PLUGINS):$(find openrave)/share/openrave/plugins:$(find orplugins)"/>
+ <env name="OPENRAVE_DATA" value="$(optenv OPENRAVE_DATA):$(find openrave)/share/openrave:$(find openrave_robot_description):$(find ormanipulation)"/>
+ <env name="OPENRAVE_PLUGINS" value="$(optenv OPENRAVE_PLUGINS):$(find openrave)/share/openrave/plugins:$(find orplugins)"/>
</node>
</launch>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-10 19:59:58 UTC (rev 9181)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-10 23:49:08 UTC (rev 9182)
@@ -52,10 +52,10 @@
assert(pnode != NULL);
Destroy();
- _srvTrajectoryStart = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryStart::request, pr2_mechanism_controllers::TrajectoryStart::response>(_strTrajectoryServiceDir+"TrajectoryStart", false);
- _srvTrajectoryCancel = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryCancel::request, pr2_mechanism_controllers::TrajectoryCancel::response>(_strTrajectoryServiceDir+"TrajectoryCancel", false);
- _srvTrajectoryWait = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryWait::request, pr2_mechanism_controllers::TrajectoryWait::response>(_strTrajectoryServiceDir+"TrajectoryWait", false);
- _srvTrajectoryQuery = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryQuery::request, pr2_mechanism_controllers::TrajectoryQuery::response>(_strTrajectoryServiceDir+"TrajectoryQuery", false);
+ _srvTrajectoryStart = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryStart::request, pr2_mechanism_controllers::TrajectoryStart::response>(_strTrajectoryServiceDir+"TrajectoryStart", true);
+ _srvTrajectoryCancel = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryCancel::request, pr2_mechanism_controllers::TrajectoryCancel::response>(_strTrajectoryServiceDir+"TrajectoryCancel", true);
+ _srvTrajectoryWait = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryWait::request, pr2_mechanism_controllers::TrajectoryWait::response>(_strTrajectoryServiceDir+"TrajectoryWait", true);
+ _srvTrajectoryQuery = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryQuery::request, pr2_mechanism_controllers::TrajectoryQuery::response>(_strTrajectoryServiceDir+"TrajectoryQuery", true);
if( !_srvTrajectoryQuery ) {
RAVELOG_ERRORA("failed to find %s service\n", (_strTrajectoryServiceDir+"TrajectoryQuery").c_str());
Modified: pkg/trunk/util/laser_scan/CMakeLists.txt
===================================================================
--- pkg/trunk/util/laser_scan/CMakeLists.txt 2009-01-10 19:59:58 UTC (rev 9181)
+++ pkg/trunk/util/laser_scan/CMakeLists.txt 2009-01-10 23:49:08 UTC (rev 9182)
@@ -7,6 +7,37 @@
rospack_add_executable(median_node src/median_filter_node.cpp)
target_link_libraries(median_node laser_median_filter)
+rospack_add_executable(robotlinks_filter_node src/robotlinks_filter_node.cpp)
+target_link_libraries (robotlinks_filter_node openrave-core qhull)
+
+# check for OpenMP
+include(CheckIncludeFile)
+include(CheckCXXCompilerFlag)
+include(CheckLibraryExists)
+
+if( WIN32 )
+ CHECK_INCLUDE_FILE(omp.h HAVE_OMP_H)
+ if( HAVE_OMP_H )
+ message(STATUS "Using OpenMP")
+ check_cxx_compiler_flag(/openmp HAVE_OPENMP)
+
+ if( HAVE_OPENMP )
+ add_definitions("/openmp")
+ endif()
+ endif()
+elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
+
+ # check if compilers supports -fopenmp
+ check_cxx_compiler_flag(-fopenmp HAVE_OPENMP)
+ check_library_exists(gomp omp_get_num_threads "" HAS_GOMP_LIB)
+
+ if( HAVE_OPENMP AND HAS_GOMP_LIB )
+ add_definitions("-fopenmp")
+ target_link_libraries(robotlinks_filter_node gomp)
+ set(OPENMP_LFLAGS "-lgomp")
+ endif()
+endif()
+
rospack_add_executable(scan_shadows_filter_node src/scan_shadows_filter.cpp)
target_link_libraries (scan_shadows_filter_node laser_scan)
Modified: pkg/trunk/util/laser_scan/manifest.xml
===================================================================
--- pkg/trunk/util/laser_scan/manifest.xml 2009-01-10 19:59:58 UTC (rev 9181)
+++ pkg/trunk/util/laser_scan/manifest.xml 2009-01-10 23:49:08 UTC (rev 9182)
@@ -4,15 +4,25 @@
Helper functions for manipulating Lasers Scans
</description>
-<author>Tully Foote</author>
+<author>Tully Foote, Rosen Diankov (rdi...@cs...)</author>
<license>BSD</license>
<review status="API cleared" notes=""/>
-<url>http://pr.willowgarage.com</url>
+<url>http://pr.willowgarage.com/wiki/laser_scan</url>
<depend package="std_msgs"/>
<depend package="roscpp"/>
<depend package="tf"/>
<depend package="filters"/>
<depend package="boost"/>
+
+<!-- openrave is necessary for getting the geometry of the links of the robot.
+ Cannot use URDF because openrave2urdf does not exist and some robots only have openrave descriptions. -->
+<depend package="openrave"/>
+<depend package="openrave_robot_description"/>
+
+<!-- qhull necessary for convex hull computation of robot links -->
+<sysdepend os="ubuntu" version="7.04-feisty" package="libqhull-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="libqhull-dev"/>
+
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -llaser_scan"/>
</export>
Added: pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp (rev 0)
+++ pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp 2009-01-10 23:49:08 UTC (rev 9182)
@@ -0,0 +1,437 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Rosen Diankov (rdi...@cs...)
+// 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.
+// * The name of the author may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+
+/**
+@mainpage
+
+@htmlinclude manifest.html
+
+\author Rosen Diankov (rdi...@cs...)
+
+@b RobotLinksFilter removes all nodes that lie on the robot. Each robot link is approximated by a convex hull.
+
+ **/
+#include <vector>
+#include <sstream>
+
+#include <ros/node.h>
+#include <std_msgs/PointCloud.h>
+#include <tf/transform_listener.h>
+
+#include <openrave-core.h>
+
+#include <cstdio>
+#include <cstdlib>
+
+extern "C"
+{
+#include <qhull/qhull.h>
+#include <qhull/mem.h>
+#include <qhull/qset.h>
+#include <qhull/geom.h>
+#include <qhull/merge.h>
+#include <qhull/poly.h>
+#include <qhull/io.h>
+#include <qhull/stat.h>
+}
+
+using namespace OpenRAVE;
+using namespace std;
+
+#define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++)
+
+boost::shared_ptr<ros::node> s_pmasternode;
+
+inline string _stdwcstombs(const wchar_t* pname)
+{
+ string s;
+ size_t len = wcstombs(NULL, pname, 0);
+ if( len != (size_t)-1 ) {
+ s.resize(len);
+ wcstombs(&s[0], pname, len);
+ }
+
+ return s;
+}
+
+class RobotLinksFilter
+{
+public:
+ struct LINK
+ {
+ string tfframe;
+ vector<Vector> vconvexhull; ///< convex hull of the link
+ Transform tstart, tend;
+ vector<Vector> vnewconvexhull;
+ };
+ struct LASERPOINT
+ {
+ LASERPOINT() {}
+ LASERPOINT(const Vector& ptnew, dReal timenew) : pt(ptnew), time(timenew),inside(0) {}
+ Vector pt;
+ dReal time; // 0-1 value specifying (stamp-stampstart)/(stampend-stampstart) where stamp is time of point,
+ // startstart and startend are start and end times of the entire scan
+ int inside; // inside robot frame
+ };
+
+ RobotLinksFilter(const string& robotname, dReal convexpadding, bool bAccurateTiming) : _robotname(robotname), _convexpadding(convexpadding), _bAccurateTiming(bAccurateTiming)
+ {
+ if( InitRobotLinksFromOpenRAVE(robotname) ) {
+ double tf_cache_time_secs;
+ s_pmasternode->param("~tf_cache_time_secs", tf_cache_time_secs, 10.0);
+ if (tf_cache_time_secs < 0)
+ ROS_ERROR("RobotLinksFilter: Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs);
+ unsigned long long tf_cache_time = tf_cache_time_secs*1000000000ULL;
+ _tf.reset(new tf::TransformListener(*s_pmasternode, true, tf_cache_time));
+ ROS_INFO("RobotLinksFilter: TF Cache Time: %f Seconds", tf_cache_time_secs);
+
+ // **** Set TF Extrapolation Limit ****
+ double tf_extrap_limit_secs ;
+ s_pmasternode->param("~tf_extrap_limit", tf_extrap_limit_secs, 0.00);
+ if (tf_extrap_limit_secs < 0.0)
+ ROS_ERROR("RobotLinksFilter: parameter tf_extrap_limit<0 (%f)", tf_extrap_limit_secs);
+
+ ros::Duration extrap_limit;
+ extrap_limit.fromSec(tf_extrap_limit_secs);
+ _tf->setExtrapolationLimit(extrap_limit);
+ ROS_INFO("RobotLinksFilter: tf extrapolation Limit: %f Seconds", tf_extrap_limit_secs);
+ }
+ else
+ ROS_ERROR("failed to init robot %s", robotname.c_str());
+
+ s_pmasternode->subscribe("tilt_laser_cloud_filtered", _pointcloudin, &RobotLinksFilter::PointCloudCallback, this, 10);
+ s_pmasternode->advertise<std_msgs::PointCloud> ("robotlinks_cloud_filtered", 10);
+ }
+ virtual ~RobotLinksFilter()
+ {
+ if( s_pmasternode != NULL ) {
+ s_pmasternode->unsubscribe("tilt_scan");
+ s_pmasternode->unadvertise("robotlinks_cloud_filtered");
+ }
+ }
+
+ bool InitRobotLinksFromOpenRAVE(const string& robotname)
+ {
+ _vLinkHulls.clear();
+
+ if( robotname.size() == 0 )
+ return false;
+
+ ROS_INFO("opening OpenRAVE robot file %s", robotname.c_str());
+
+ // create the main environment
+ boost::shared_ptr<EnvironmentBase> penv(CreateEnvironment());
+ if( !penv )
+ return false;
+
+ // load the scene
+ if( !penv->Load(robotname.c_str()) ) {
+ ROS_ERROR("RobotLinksFilter failed create robot %s", robotname.c_str());
+ return false;
+ }
+
+ // get the first robot
+ if( penv->GetRobots().size() == 0 ) {
+ ROS_ERROR("RobotLinksFilter no robots in file %s", robotname.c_str());
+ return false;
+ }
+
+ RobotBase* probot = penv->GetRobots().front();
+ ROS_INFO("generating convex hulls for robot %S, num links: %d", probot->GetName(), probot->GetLinks().size());
+
+ ros::Time starthull = ros::Time::now();
+ _vLinkHulls.resize(probot->GetLinks().size());
+ vector<LINK>::iterator ithull = _vLinkHulls.begin();
+ size_t totalplanes = 0;
+ FOREACH(itlink, probot->GetLinks()) {
+ // compute convex hull
+ if( compute_convex_hull((*itlink)->GetCollisionData().vertices, ithull->vconvexhull) ) {
+ totalplanes += ithull->vconvexhull.size();
+ ROS_INFO("link %S convex hull has %d planes", (*itlink)->GetName(), ithull->vconvexhull.size());
+ }
+ else
+ ROS_ERROR("failed to compute convex hull for link %S", (*itlink)->GetName());
+
+ ithull->tfframe = _stdwcstombs((*itlink)->GetName());
+ ++ithull;
+ }
+
+ ROS_INFO("total convex planes: %d, time: %fs", totalplanes, (ros::Time::now()-starthull).toSec());
+
+ return true;
+ }
+
+ bool InitRobotLinksFromURDF(const string& robotname)
+ {
+ ROS_ERROR("InitRobotLinksFromURDF not implemented yet!");
+ return false;
+ }
+
+private:
+
+ void PointCloudCallback()
+ {
+ if( _vLinkHulls.size() == 0 || !_tf) {
+ // just pass the data
+ s_pmasternode->publish("robotlinks_cloud_filtered",_pointcloudin);
+ return;
+ }
+
+ if( _pointcloudin.pts.size() == 0 )
+ return;
+
+ ros::Time stampprocess = ros::Time::now();
+
+ if( _bAccurateTiming )
+ PruneWithAccurateTiming(_pointcloudin, _vlaserpoints);
+ else
+ PruneWithSimpleTiming(_pointcloudin, _vlaserpoints);
+
+ int totalpoints = 0;
+ FOREACH(itpoint, _vlaserpoints)
+ totalpoints += itpoint->inside==0;
+
+ _pointcloudout.header = _pointcloudin.header;
+ _pointcloudout.set_pts_size(totalpoints);
+ _pointcloudout.set_chan_size(_pointcloudin.chan.size());
+ for(int ichan = 0; ichan < (int)_pointcloudin.chan.size(); ++ichan) {
+ _pointcloudout.chan[ichan].name = _pointcloudin.chan[ichan].name;
+ _pointcloudout.chan[ichan].set_vals_size(_pointcloudin.chan[ichan].vals.size());
+ }
+
+ for(int oldindex = 0, newindex = 0; oldindex < (int)_vlaserpoints.size(); ++oldindex) {
+ if( _vlaserpoints[oldindex].inside )
+ continue;
+
+ _pointcloudout.pts[newindex] = _pointcloudin.pts[oldindex];
+ for(int ichan = 0; ichan < (int)_pointcloudin.chan.size(); ++ichan)
+ _pointcloudout.chan[ichan].vals[newindex] = _pointcloudin.chan[ichan].vals[oldindex];
+ ++newindex;
+ }
+
+ ROS_INFO("published %d points, processing time=%fs\n", totalpoints, (ros::Time::now()-stampprocess).toSec());
+ s_pmasternode->publish("robotlinks_cloud_filtered",_pointcloudout);
+ }
+
+ /// prune all the points that are inside the convex hulls of the robot links
+ /// Uses a different timestamp for every laser point cloud
+ void PruneWithAccurateTiming(const std_msgs::PointCloud& pointcloudin, vector<LASERPOINT>& vlaserpoints)
+ {
+ ros::Time stampstart = pointcloudin.header.stamp, stampend = pointcloudin.header.stamp;
+ tf::Stamped<btTransform> bttransform;
+ vlaserpoints.resize(pointcloudin.pts.size());
+
+ // look for timestamp channel
+ int index = 0;
+ FOREACH(itp, pointcloudin.pts)
+ vlaserpoints[index++] = LASERPOINT(Vector(itp->x,itp->y,itp->z,1),0);
+
+ FOREACH(ithull, _vLinkHulls) {
+ _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, stampstart, bttransform);
+ ithull->tstart = GetTransform(bttransform);
+ _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, stampend, bttransform);
+ ithull->tend = GetTransform(bttransform);
+ }
+
+ // points are independent from each and loop can be parallelized
+ #pragma omp parallel for schedule(dynamic,128)
+ for(int i = 0; i < (int)vlaserpoints.size(); ++i) {
+ LASERPOINT& laserpoint = vlaserpoints[i];
+ FOREACH(ithull, _vLinkHulls) {
+ Transform tinv, tinvstart = ithull->tstart.inverse(), tinvend = ithull->tend.inverse();
+ tinv.rot = dQSlerp(tinvstart.rot,tinvend.rot,laserpoint.time);
+ tinv.trans = tinvstart.trans*(1-laserpoint.time) + tinvend.trans*laserpoint.time;
+
+ bool bInside = true;
+ FOREACH(itplane, ithull->vconvexhull) {
+ if( dot4(*itplane,tinv * laserpoint.pt) > 0 ) {
+ bInside = false;
+ break;
+ }
+ }
+
+ if( bInside ) {
+ laserpoint.inside = 1;
+ break;
+ }
+ }
+ }
+ }
+
+ /// prune all the points that are inside the convex hulls of the robot links
+ /// Uses the header timestamp for all laser point clouds
+ void PruneWithSimpleTiming(const std_msgs::PointCloud& pointcloudin, vector<LASERPOINT>& vlaserpoints)
+ {
+ tf::Stamped<btTransform> bttransform;
+ vlaserpoints.resize(pointcloudin.pts.size());
+
+ int index = 0;
+ FOREACH(itp, pointcloudin.pts)
+ vlaserpoints[index++] = LASERPOINT(Vector(itp->x,itp->y,itp->z,1),0);
+
+ // compute new hulls
+ FOREACH(ithull, _vLinkHulls) {
+ _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, pointcloudin.header.stamp, bttransform);
+ TransformMatrix tinvstart = GetTransform(bttransform).inverse();
+
+ ithull->vnewconvexhull.resize(ithull->vconvexhull.size());
+ vector<Vector>::iterator itnewplane = ithull->vnewconvexhull.begin();
+ FOREACH(itplane, ithull->vconvexhull) {
+ itnewplane->x = itplane->x*tinvstart.m[0]+itplane->y*tinvstart.m[4]+itplane->z*tinvstart.m[8];
+ itnewplane->y = itplane->x*tinvstart.m[1]+itplane->y*tinvstart.m[5]+itplane->z*tinvstart.m[9];
+ itnewplane->z = itplane->x*tinvstart.m[2]+itplane->y*tinvstart.m[6]+itplane->z*tinvstart.m[10];
+ itnewplane->w = itplane->x*tinvstart.trans.x+itplane->y*tinvstart.trans.y+itplane->z*tinvstart.trans.z+itplane->w;
+ ++itnewplane;
+ }
+ }
+
+ // points are independent from each and loop can be parallelized
+ #pragma omp parallel for schedule(dynamic,128)
+ for(int i = 0; i < (int)vlaserpoints.size(); ++i) {
+ LASERPOINT& laserpoint = vlaserpoints[i];
+ FOREACH(ithull, _vLinkHulls) {
+
+ bool bInside = true;
+ FOREACH(itplane, ithull->vconvexhull) {
+ if( dot4(*itplane,laserpoint.pt) > 0 ) {
+ bInside = false;
+ break;
+ }
+ }
+
+ if( bInside ) {
+ laserpoint.inside = 1;
+ break;
+ }
+ }
+ }
+ }
+
+ bool compute_convex_hull(const vector<Vector>& verts, vector<Vector>& vconvexplanes)
+ {
+ if( verts.size() <= 3 )
+ return false;
+
+ vconvexplanes.resize(0);
+
+ int dim = 3; // dimension of points
+ vector<coordT> qpoints(3*verts.size());
+ for(size_t i = 0; i < verts.size(); ++i) {
+ qpoints[3*i+0] = verts[i].x;
+ qpoints[3*i+1] = verts[i].y;
+ qpoints[3*i+2] = verts[i].z;
+ }
+
+ bool bSuccess = false;
+ boolT ismalloc = 0; // True if qhull should free points in qh_freeqhull() or reallocation
+ char flags[]= "qhull Tv"; // option flags for qhull, see qh_opt.htm
+ FILE *outfile = NULL; // output from qh_produce_output(), use NULL to skip qh_produce_output()
+ FILE *errfile = stderr; // error messages from qhull code
+
+ int exitcode= qh_new_qhull (dim, qpoints.size()/3, &qpoints[0], ismalloc, flags, outfile, errfile);
+ if (!exitcode) { // no error
+ vconvexplanes.reserve(100);
+
+ facetT *facet; // set by FORALLfacets
+ FORALLfacets { // 'qh facet_list' contains the convex hull
+ vconvexplanes.push_back(Vector(facet->normal[0], facet->normal[1], facet->normal[2], facet->offset-_convexpadding));
+ }
+
+ bSuccess = true;
+ }
+
+ qh_freeqhull(!qh_ALL);
+ int curlong, totlong; // memory remaining after qh_memfreeshort
+ qh_memfreeshort (&curlong, &totlong);
+ if (curlong || totlong)
+ ROS_ERROR("qhull internal warning (main): did not free %d bytes of long memory (%d pieces)", totlong, curlong);
+
+ return bSuccess;
+ }
+
+ Transform GetTransform(const btTransform& bt)
+ {
+ btQuaternion q = bt.getRotation();
+ btVector3 o = bt.getOrigin();
+ return Transform(Vector(q.w(),q.x(),q.y(),q.z()),Vector(o.x(),o.y(),o.z()));
+ }
+
+ vector<LINK> _vLinkHulls;
+ boost::shared_ptr<tf::TransformListener> _tf;
+ std_msgs::PointCloud _pointcloudin, _pointcloudout;
+ vector<LASERPOINT> _vlaserpoints;
+ string _robotname;
+ dReal _convexpadding;
+ bool _bAccurateTiming; ///< if true, will interpolate the convex hulls for every time stamp
+};
+
+int main(int argc, char ** argv)
+{
+ // parse the command line options
+ string robotname;
+ dReal padding = 0.01; // 0.01m padding
+ bool bAccurateTiming = true;
+ int i = 1;
+ while(i < argc) {
+ if( strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0 ) {
+ // print the arguments and exit
+ printf("robotlinks_filter_node [--robotfile openravefile] [--padding distance] [--accuratetiming enable]\n"
+ " Start a node to prune points that are on the robot surface.\n"
+ " Currently the robot file specified has to be in OpenRAVE format\n"
+ " (see openrave_robot_descirption ros package for details)\n");
+
+ return 0;
+ }
+ if( strcmp(argv[i], "--robotfile") == 0 ) {
+ robotname = argv[i+1];
+ i += 2;
+ }
+ else if( strcmp(argv[i], "--padding") == 0 ) {
+ padding = atof(argv[i+1]);
+ i += 2;
+ }
+ else if( strcmp(argv[i], "--accuratetiming") == 0 ) {
+ bAccurateTiming = atoi(argv[i+1])>0;
+ i += 2;
+ }
+ else
+ break;
+ }
+
+ ros::init(argc,argv);
+ s_pmasternode.reset(new ros::node("roobtlinks_filter"));
+
+ if( !s_pmasternode->checkMaster() )
+ return -1;
+
+ boost::shared_ptr<RobotLinksFilter> plinksfilter(new RobotLinksFilter(robotname, padding, bAccurateTiming));
+
+ s_pmasternode->spin();
+
+ plinksfilter.reset();
+ ros::fini();
+ s_pmasternode.reset();
+ return 0;
+}
Added: pkg/trunk/util/laser_scan/start_robotlinks_filter.ros.xml
===================================================================
--- pkg/trunk/util/laser_scan/start_robotlinks_filter.ros.xml (rev 0)
+++ pkg/trunk/util/laser_scan/start_robotlinks_filter.ros.xml 2009-01-10 23:49:08 UTC (rev 9182)
@@ -0,0 +1,8 @@
+<!-- start openraveros server and set all the correct paths -->
+<launch>
+ <node pkg="laser_scan" type="scan_shadows_filter_node"/>
+ <node pkg="laser_scan" type="robotlinks_filter_node" args="--robotfile robots/pr2full.robot.xml --padding 0.01 --accuratetiming 1" output="screen">
+ <env name="OPENRAVE_DATA" value="$(find openrave)/share/openrave:$(find openrave_robot_description)"/>
+ <env name="OPENRAVE_PLUGINS" value="$(find openrave)/share/openrave/plugins"/>
+ </node>
+</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-11 02:22:21
|
Revision: 9184
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9184&view=rev
Author: rdiankov
Date: 2009-01-11 01:50:44 +0000 (Sun, 11 Jan 2009)
Log Message:
-----------
fixed several bugs with the robot links pruning laser_can util, should be working perfect now
Modified Paths:
--------------
pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
pkg/trunk/util/laser_scan/mainpage.dox
pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp
Modified: pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2009-01-11 00:12:41 UTC (rev 9183)
+++ pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2009-01-11 01:50:44 UTC (rev 9184)
@@ -8,5 +8,5 @@
<node pkg="phase_space" type="phase_space_node"/>
<!-- launch octave scripts to start the openrave client -->
- <node pkg="ormanipulation" type="runperception.m" output="screen"/>
+<!-- <node pkg="ormanipulation" type="runperception.m" output="screen"/> -->
</launch>
Modified: pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml 2009-01-11 00:12:41 UTC (rev 9183)
+++ pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml 2009-01-11 01:50:44 UTC (rev 9184)
@@ -2,7 +2,7 @@
<launch>
<machine name="localhost-openrave" address="localhost"/>
<node machine="localhost-openrave" pkg="openraveros" type="openraveros" output="screen">
- <env name="OPENRAVE_DATA" value="$(optenv OPENRAVE_DATA):$(find openrave)/share/openrave:$(find openrave_robot_description):$(find ormanipulation)"/>
- <env name="OPENRAVE_PLUGINS" value="$(optenv OPENRAVE_PLUGINS):$(find openrave)/share/openrave/plugins:$(find orplugins)"/>
+ <env name="OPENRAVE_DATA" value="$(find openrave)/share/openrave:$(find openrave_robot_description):$(find ormanipulation)"/>
+ <env name="OPENRAVE_PLUGINS" value="$(find openrave)/share/openrave/plugins:$(find orplugins)"/>
</node>
</launch>
Modified: pkg/trunk/util/laser_scan/mainpage.dox
===================================================================
--- pkg/trunk/util/laser_scan/mainpage.dox 2009-01-11 00:12:41 UTC (rev 9183)
+++ pkg/trunk/util/laser_scan/mainpage.dox 2009-01-11 01:50:44 UTC (rev 9184)
@@ -21,9 +21,10 @@
\subsection mean Mean Filter
\todo Document
-\subsection self_collision Self Collision Filter
-\todo document
+\subsection robotlinks_filter Robot Links Pruning Filter
+RobotLinksFilter removes all nodes that lie on the robot. Each robot link is approximated by a convex hull that is dynamically generated based on the mesh of the robot.
+
\subsection shadows Scan Shadows Filter
\todo document
Modified: pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp 2009-01-11 00:12:41 UTC (rev 9183)
+++ pkg/trunk/util/laser_scan/src/robotlinks_filter_node.cpp 2009-01-11 01:50:44 UTC (rev 9184)
@@ -22,19 +22,11 @@
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
+/// \author Rosen Diankov (rdi...@cs...)
-/**
-@mainpage
-
-@htmlinclude manifest.html
-
-\author Rosen Diankov (rdi...@cs...)
-
-@b RobotLinksFilter removes all nodes that lie on the robot. Each robot link is approximated by a convex hull.
-
- **/
#include <vector>
#include <sstream>
+#include <cstdio>
#include <ros/node.h>
#include <std_msgs/PointCloud.h>
@@ -121,13 +113,13 @@
else
ROS_ERROR("failed to init robot %s", robotname.c_str());
- s_pmasternode->subscribe("tilt_laser_cloud_filtered", _pointcloudin, &RobotLinksFilter::PointCloudCallback, this, 10);
+ s_pmasternode->subscribe("tilt_laser_cloud_filtered", _pointcloudin, &RobotLinksFilter::PointCloudCallback, this, 2);
s_pmasternode->advertise<std_msgs::PointCloud> ("robotlinks_cloud_filtered", 10);
}
virtual ~RobotLinksFilter()
{
if( s_pmasternode != NULL ) {
- s_pmasternode->unsubscribe("tilt_scan");
+ s_pmasternode->unsubscribe("tilt_laser_cloud_filtered");
s_pmasternode->unadvertise("robotlinks_cloud_filtered");
}
}
@@ -218,7 +210,7 @@
_pointcloudout.set_chan_size(_pointcloudin.chan.size());
for(int ichan = 0; ichan < (int)_pointcloudin.chan.size(); ++ichan) {
_pointcloudout.chan[ichan].name = _pointcloudin.chan[ichan].name;
- _pointcloudout.chan[ichan].set_vals_size(_pointcloudin.chan[ichan].vals.size());
+ _pointcloudout.chan[ichan].set_vals_size(totalpoints);
}
for(int oldindex = 0, newindex = 0; oldindex < (int)_vlaserpoints.size(); ++oldindex) {
@@ -231,7 +223,7 @@
++newindex;
}
- ROS_INFO("published %d points, processing time=%fs\n", totalpoints, (ros::Time::now()-stampprocess).toSec());
+ ROS_INFO("published %d points, processing time=%fs", totalpoints, (ros::Time::now()-stampprocess).toSec());
s_pmasternode->publish("robotlinks_cloud_filtered",_pointcloudout);
}
@@ -248,13 +240,19 @@
FOREACH(itp, pointcloudin.pts)
vlaserpoints[index++] = LASERPOINT(Vector(itp->x,itp->y,itp->z,1),0);
- FOREACH(ithull, _vLinkHulls) {
- _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, stampstart, bttransform);
- ithull->tstart = GetTransform(bttransform);
- _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, stampend, bttransform);
- ithull->tend = GetTransform(bttransform);
+ try {
+ FOREACH(ithull, _vLinkHulls) {
+ _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, stampstart, bttransform);
+ ithull->tstart = GetTransform(bttransform);
+ _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, stampend, bttransform);
+ ithull->tend = GetTransform(bttransform);
+ }
}
-
+ catch(tf::TransformException& ex) {
+ ROS_WARN("failed to get tf frame");
+ return;
+ }
+
// points are independent from each and loop can be parallelized
#pragma omp parallel for schedule(dynamic,128)
for(int i = 0; i < (int)vlaserpoints.size(); ++i) {
@@ -266,7 +264,8 @@
bool bInside = true;
FOREACH(itplane, ithull->vconvexhull) {
- if( dot4(*itplane,tinv * laserpoint.pt) > 0 ) {
+ Vector v = tinv * laserpoint.pt; v.w = 1;
+ if( dot4(*itplane,v) > 0 ) {
bInside = false;
break;
}
@@ -285,17 +284,28 @@
void PruneWithSimpleTiming(const std_msgs::PointCloud& pointcloudin, vector<LASERPOINT>& vlaserpoints)
{
tf::Stamped<btTransform> bttransform;
+ vlaserpoints.resize(0);
+
+ // compute new hulls
+ try {
+ FOREACH(ithull, _vLinkHulls) {
+ _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, pointcloudin.header.stamp, bttransform);
+ ithull->tstart = GetTransform(bttransform);
+ }
+ }
+ catch(tf::TransformException& ex) {
+ ROS_WARN("failed to get tf frame");
+ return;
+ }
+
vlaserpoints.resize(pointcloudin.pts.size());
int index = 0;
FOREACH(itp, pointcloudin.pts)
vlaserpoints[index++] = LASERPOINT(Vector(itp->x,itp->y,itp->z,1),0);
- // compute new hulls
FOREACH(ithull, _vLinkHulls) {
- _tf->lookupTransform(pointcloudin.header.frame_id, ithull->tfframe, pointcloudin.header.stamp, bttransform);
- TransformMatrix tinvstart = GetTransform(bttransform).inverse();
-
+ TransformMatrix tinvstart = ithull->tstart.inverse();
ithull->vnewconvexhull.resize(ithull->vconvexhull.size());
vector<Vector>::iterator itnewplane = ithull->vnewconvexhull.begin();
FOREACH(itplane, ithull->vconvexhull) {
@@ -314,7 +324,7 @@
FOREACH(ithull, _vLinkHulls) {
bool bInside = true;
- FOREACH(itplane, ithull->vconvexhull) {
+ FOREACH(itplane, ithull->vnewconvexhull) {
if( dot4(*itplane,laserpoint.pt) > 0 ) {
bInside = false;
break;
@@ -347,8 +357,8 @@
bool bSuccess = false;
boolT ismalloc = 0; // True if qhull should free points in qh_freeqhull() or reallocation
char flags[]= "qhull Tv"; // option flags for qhull, see qh_opt.htm
- FILE *outfile = NULL; // output from qh_produce_output(), use NULL to skip qh_produce_output()
- FILE *errfile = stderr; // error messages from qhull code
+ FILE *outfile = NULL; // stdout, output from qh_produce_output(), use NULL to skip qh_produce_output()
+ FILE *errfile = tmpfile(); // stderr, error messages from qhull code
int exitcode= qh_new_qhull (dim, qpoints.size()/3, &qpoints[0], ismalloc, flags, outfile, errfile);
if (!exitcode) { // no error
@@ -421,7 +431,7 @@
}
ros::init(argc,argv);
- s_pmasternode.reset(new ros::node("roobtlinks_filter"));
+ s_pmasternode.reset(new ros::node("robobtlinks_filter"));
if( !s_pmasternode->checkMaster() )
return -1;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-11 10:18:14
|
Revision: 9200
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9200&view=rev
Author: rdiankov
Date: 2009-01-11 10:18:03 +0000 (Sun, 11 Jan 2009)
Log Message:
-----------
new openrave sensor system for displaying a collision map
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/mapping/collision_map/manifest.xml
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
pkg/trunk/util/laser_scan/CMakeLists.txt
pkg/trunk/util/laser_scan/mainpage.dox
pkg/trunk/util/robot_self_filter/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-11 10:18:03 UTC (rev 9200)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 602
+SVN_REVISION = -r 603
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/mapping/collision_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/collision_map/manifest.xml 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/mapping/collision_map/manifest.xml 2009-01-11 10:18:03 UTC (rev 9200)
@@ -12,7 +12,10 @@
<depend package="std_msgs" />
<depend package="cloud_geometry" />
<depend package="tf" />
-
+ <export>
+ <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
+ </export>
+
<sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="liblapack-dev"/>
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-11 10:18:03 UTC (rev 9200)
@@ -21,3 +21,8 @@
if( isempty(out) )
error('failed to create phasespace');
end
+
+out = orProblemSendCommand('createsystem CollisionMap collision_map',probs.task);
+if( isempty(out) )
+ error('failed to create phasespace');
+end
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-11 10:18:03 UTC (rev 9200)
@@ -14,4 +14,5 @@
<depend package="robot_msgs"/>
<depend package="pr2_mechanism_controllers"/>
<depend package="tf"/>
+ <depend package="collision_map"/>
</package>
Added: pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h (rev 0)
+++ pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-11 10:18:03 UTC (rev 9200)
@@ -0,0 +1,118 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Rosen Diankov
+// 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.
+// * The name of the author may not 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.
+//
+// \author Rosen Diankov
+#ifndef COLLISIONMAP_MOCAP_SYSTEM
+#define COLLISIONMAP_MOCAP_SYSTEM
+
+#include "rossensorsystem.h"
+#include <collision_map/CollisionMap.h>
+
+// used to update objects through a mocap system
+class CollisionMapXMLID
+{
+public:
+ static const char* GetXMLId() { return "collisionmap"; }
+};
+
+class CollisionMapSystem : public ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>
+{
+public:
+ CollisionMapSystem(EnvironmentBase* penv)
+ : ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>(penv), _nNextId(1)
+ {
+ }
+
+ virtual bool Init(istream& sinput)
+ {
+ _topic = "collision_map";
+ return ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>::Init(sinput);
+ }
+
+private:
+ void newdatacb()
+ {
+ // create the new kinbody
+ KinBody* pbody = GetEnv()->CreateKinBody();
+
+ _vaabbs.resize(_topicmsg.boxes.size());
+ vector<AABB>::iterator itab = _vaabbs.begin();
+ FOREACH(itmsgab, _topicmsg.boxes) {
+ itab->pos = Vector(itmsgab->center.x, itmsgab->center.y, itmsgab->center.z);
+ itab->extents = Vector(itmsgab->extents.x, itmsgab->extents.y, itmsgab->extents.z);
+ ++itab;
+ }
+
+ if( !pbody->InitFromBoxes(_vaabbs, true) ) {
+ RAVELOG_ERRORA("failed to create collision map\n");
+ delete pbody;
+ return;
+ }
+
+ // append an id to the body
+ stringstream ss;
+ ss << "CollisionMap" << _nNextId++;
+ pbody->SetName(ss.str().c_str());
+
+ // add the new kinbody
+ if( !GetEnv()->AddKinBody(pbody) ) {
+ RAVELOG_ERRORA("failed to add body %S\n", pbody->GetName());
+ delete pbody;
+ return;
+ }
+
+ {
+ boost::mutex::scoped_lock lock(_mutex);
+ GetEnv()->LockPhysics(true);
+
+ // remove all unlocked bodies
+ TYPEOF(_mapbodies.begin()) itbody = _mapbodies.begin();
+ while(itbody != _mapbodies.end()) {
+ if( !itbody->second->IsLocked() ) {
+ _mapbodies.erase(itbody++);
+ }
+ else
+ ++itbody;
+ }
+
+ GetEnv()->LockPhysics(false);
+ }
+
+ BODY* b = AddKinBody(pbody, NULL);
+ if( b == NULL ) {
+ RAVELOG_ERRORA("removing/destroying kinbody\n");
+ GetEnv()->RemoveKinBody(pbody, true);
+ }
+ }
+
+ Transform GetTransform(const std_msgs::Transform& pose)
+ {
+ return Transform(Vector(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z), Vector(pose.translation.x, pose.translation.y, pose.translation.z));
+ }
+
+ vector<AABB> _vaabbs;
+ int _nNextId;
+};
+
+#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2009-01-11 10:18:03 UTC (rev 9200)
@@ -22,7 +22,7 @@
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
-// author: Rosen Diankov
+// \author Rosen Diankov
#ifndef OBJECTTRANSFORM_SENSOR_SYSTEM
#define OBJECTTRANSFORM_SENSOR_SYSTEM
@@ -42,7 +42,7 @@
{
public:
ObjectTransformSystem(EnvironmentBase* penv)
- : ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>(penv), _robotid(0), nNextId(1)
+ : ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>(penv), _robotid(0), _nNextId(1)
{
}
virtual ~ObjectTransformSystem() {
@@ -227,7 +227,7 @@
// append an id to the body
wstringstream ss;
- ss << pbody->GetName() << nNextId++;
+ ss << pbody->GetName() << _nNextId++;
pbody->SetName(ss.str().c_str());
if( !GetEnv()->AddKinBody(pbody) ) {
@@ -238,7 +238,7 @@
BODY* b = AddKinBody(pbody, NULL);
if( b == NULL ) {
- delete pbody;
+ GetEnv()->RemoveKinBody(pbody, true);
continue;
}
@@ -272,7 +272,7 @@
int _robotid;
Transform _toffset; ///< offset from tf frame
dReal _fThreshSqr;
- int nNextId;
+ int _nNextId;
};
#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-01-11 10:18:03 UTC (rev 9200)
@@ -22,12 +22,13 @@
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
-// author: Rosen Diankov
+// \author Rosen Diankov
#include "plugindefs.h"
#include "rosarmik.h"
#include "phasespacesystem.h"
#include "objecttransformsystem.h"
+#include "collisionmapsystem.h"
#include "rosrobotcontroller.h"
#include "rosplanningproblem.h"
@@ -68,8 +69,10 @@
case PT_SensorSystem:
if( wcsicmp(name, L"PhaseSpace") == 0 )
return new PhaseSpaceMocapClient(penv);
- if( wcsicmp(name, L"ObjectTransform") == 0 )
+ else if( wcsicmp(name, L"ObjectTransform") == 0 )
return new ObjectTransformSystem(penv);
+ else if( wcsicmp(name, L"CollisionMap") == 0 )
+ return new CollisionMapSystem(penv);
break;
default:
break;
@@ -86,9 +89,11 @@
return false;
}
+ pinfo->controllers.push_back(L"ROSRobot");
pinfo->iksolvers.push_back(L"ROSArmIK");
pinfo->sensorsystems.push_back(L"PhaseSpace");
pinfo->sensorsystems.push_back(L"ObjectTransform");
+ pinfo->sensorsystems.push_back(L"CollisionMap");
pinfo->problems.push_back(L"ROSPlanning");
return true;
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2009-01-11 10:18:03 UTC (rev 9200)
@@ -22,7 +22,7 @@
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
-// author: Rosen Diankov
+// \author Rosen Diankov
#ifndef ROS_SENSORSYSTEM_SYSTEM
#define ROS_SENSORSYSTEM_SYSTEM
Modified: pkg/trunk/util/laser_scan/CMakeLists.txt
===================================================================
--- pkg/trunk/util/laser_scan/CMakeLists.txt 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/util/laser_scan/CMakeLists.txt 2009-01-11 10:18:03 UTC (rev 9200)
@@ -1,5 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
+set(ROS_BUILD_TYPE RelWithDebInfo)
rospack(laser_scan)
rospack_add_library(laser_scan src/laser_scan.cc )
rospack_add_library(laser_median_filter src/median_filter.cpp )
Modified: pkg/trunk/util/laser_scan/mainpage.dox
===================================================================
--- pkg/trunk/util/laser_scan/mainpage.dox 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/util/laser_scan/mainpage.dox 2009-01-11 10:18:03 UTC (rev 9200)
@@ -21,10 +21,6 @@
\subsection mean Mean Filter
\todo Document
-\subsection robotlinks_filter Robot Links Pruning Filter
-
-RobotLinksFilter removes all nodes that lie on the robot. Each robot link is approximated by a convex hull that is dynamically generated based on the mesh of the robot.
-
\subsection shadows Scan Shadows Filter
\todo document
Modified: pkg/trunk/util/robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/util/robot_self_filter/CMakeLists.txt 2009-01-11 09:57:32 UTC (rev 9199)
+++ pkg/trunk/util/robot_self_filter/CMakeLists.txt 2009-01-11 10:18:03 UTC (rev 9200)
@@ -1,6 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
-set(ROS_BUILD_TYPE RelDebInfo)
+set(ROS_BUILD_TYPE RelWithDebInfo)
rospack(robot_self_filter)
rospack_add_executable(robotlinks_filter_node src/robotlinks_filter_node.cpp)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-11 12:40:13
|
Revision: 9206
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9206&view=rev
Author: rdiankov
Date: 2009-01-11 12:40:07 +0000 (Sun, 11 Jan 2009)
Log Message:
-----------
fixed openrave bug in displaying CollisionMap message
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-11 11:01:55 UTC (rev 9205)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-11 12:40:07 UTC (rev 9206)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 603
+SVN_REVISION = -r 604
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-11 11:01:55 UTC (rev 9205)
+++ pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-11 12:40:07 UTC (rev 9206)
@@ -54,6 +54,7 @@
void newdatacb()
{
// create the new kinbody
+ GetEnv()->LockPhysics(true);
KinBody* pbody = GetEnv()->CreateKinBody();
_vaabbs.resize(_topicmsg.boxes.size());
@@ -61,6 +62,7 @@
FOREACH(itmsgab, _topicmsg.boxes) {
itab->pos = Vector(itmsgab->center.x, itmsgab->center.y, itmsgab->center.z);
itab->extents = Vector(itmsgab->extents.x, itmsgab->extents.y, itmsgab->extents.z);
+ //RAVELOG_VERBOSEA("pos%d: %f %f %f\n", (int)(itmsgab-_topicmsg.boxes.begin()), itab->pos.x, itab->pos.y, itab->pos.z);
++itab;
}
@@ -84,22 +86,27 @@
{
boost::mutex::scoped_lock lock(_mutex);
- GetEnv()->LockPhysics(true);
// remove all unlocked bodies
TYPEOF(_mapbodies.begin()) itbody = _mapbodies.begin();
while(itbody != _mapbodies.end()) {
if( !itbody->second->IsLocked() ) {
+ BODY* b = itbody->second.get();
+ KinBody::Link* plink = itbody->second->GetOffsetLink();
+ assert( plink != NULL );
+ GetEnv()->RemoveKinBody(plink->GetParent());
_mapbodies.erase(itbody++);
}
else
++itbody;
}
-
- GetEnv()->LockPhysics(false);
}
- BODY* b = AddKinBody(pbody, NULL);
+ GetEnv()->LockPhysics(false);
+
+ MocapData* pdata = new MocapData();
+ pdata->strOffsetLink = pbody->GetLinks().front()->GetName();
+ BODY* b = AddKinBody(pbody, pdata);
if( b == NULL ) {
RAVELOG_ERRORA("removing/destroying kinbody\n");
GetEnv()->RemoveKinBody(pbody, true);
Modified: pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2009-01-11 11:01:55 UTC (rev 9205)
+++ pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2009-01-11 12:40:07 UTC (rev 9206)
@@ -397,7 +397,7 @@
KinBody::Link* plink = itbody->second->GetOffsetLink();
if( plink != NULL ) {
RAVELOG_DEBUGA("object %S expired %fs\n", plink->GetParent()->GetName(), (float)(curtime-itbody->second->lastupdated).toSec());
- GetEnv()->RemoveKinBody(plink->GetParent());
+ GetEnv()->RemoveKinBody(plink->GetParent(), true);
}
GetEnv()->LockPhysics(false);
Modified: pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-01-11 11:01:55 UTC (rev 9205)
+++ pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-01-11 12:40:07 UTC (rev 9206)
@@ -223,7 +223,7 @@
++newindex;
}
- ROS_INFO("published %d points, processing time=%fs", totalpoints, (ros::Time::now()-stampprocess).toSec());
+ ROS_DEBUG("robotlinks_filter_node published %d points, processing time=%fs", totalpoints, (ros::Time::now()-stampprocess).toSec());
s_pmasternode->publish("robotlinks_cloud_filtered",_pointcloudout);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-12 03:11:41
|
Revision: 9219
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9219&view=rev
Author: rdiankov
Date: 2009-01-12 03:11:30 +0000 (Mon, 12 Jan 2009)
Log Message:
-----------
openrave manipulation demo on real robot rocks now
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml
pkg/trunk/openrave_planning/ormanipulation/data/willow_tablef.kinbody.xml
pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m
pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m
pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m
pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml
pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
Added Paths:
-----------
pkg/trunk/openrave_planning/ormanipulation/octave/RobotMoveJointValues.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-12 03:11:30 UTC (rev 9219)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 604
+SVN_REVISION = -r 606
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-01-12 03:11:30 UTC (rev 9219)
@@ -80,15 +80,16 @@
class SendProblemWorker : public ServerWorker
{
public:
- SendProblemWorker(boost::shared_ptr<ProblemInstance> prob, const string& cmd, string& out) : _prob(prob), _cmd(cmd), _out(out) {}
+ SendProblemWorker(boost::shared_ptr<ProblemInstance> prob, const string& cmd, string& out, bool& bSuccessful) : _prob(prob), _cmd(cmd), _out(out), _bSuccessful(bSuccessful) {}
virtual void work() {
- _prob->SendCommand(_cmd.c_str(),_out);
+ _bSuccessful = _prob->SendCommand(_cmd.c_str(),_out);
}
private:
boost::shared_ptr<ProblemInstance> _prob;
const string& _cmd;
string& _out;
+ bool& _bSuccessful;
};
class LoadProblemWorker : public ServerWorker
@@ -1208,8 +1209,9 @@
if( itprob == _mapproblems.end() )
return false;
- AddWorker(new SendProblemWorker(itprob->second,req.cmd,res.output), true);
- return true;
+ bool bSuccessful = false;
+ AddWorker(new SendProblemWorker(itprob->second,req.cmd,res.output,bSuccessful), true);
+ return bSuccessful;
}
bool robot_controllersend_srv(robot_controllersend::request& req, robot_controllersend::response& res)
Modified: pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml 2009-01-12 03:11:30 UTC (rev 9219)
@@ -1,7 +1,7 @@
<Kinbody name="ricebox">
<Body>
<Geom type="box">
- <extents>0.1175 0.06 0.035</extents>
+ <extents>0.13 0.06 0.035</extents>
<translation>0 0 0.03</translation>
<diffusecolor>0.1 0.6 1</diffusecolor>
</Geom>
Modified: pkg/trunk/openrave_planning/ormanipulation/data/willow_tablef.kinbody.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/willow_tablef.kinbody.xml 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/data/willow_tablef.kinbody.xml 2009-01-12 03:11:30 UTC (rev 9219)
@@ -1,7 +1,7 @@
<KinBody name="willow_table">
<Body name="base">
<Geom type="box">
- <extents>0.39 0.63 0.04</extents>
+ <extents>0.39 0.63 0.055</extents>
<translation>0 0 0.711</translation>
</Geom>
<Geom type="box">
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -81,6 +81,12 @@
curobj = targetobjs{orderedtargets(1)};
end
+if( isempty(curobj.info) )
+ display('invalid object');
+ curobj = [];
+ return;
+end
+
[dests, surfaceplane] = scenedata.GetDestsFn(); % compute a set of destinations
offsetfromtable = 0.04; %% set destination a little above the table
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -178,7 +178,12 @@
if( ~success )
error('failed to movehandstraight');
end
- success = StartTrajectory(robotid,trajdata);
+
+ %% hack for now
+ [trajvals,transvals,timevals] = ParseTrajData(trajdata);
+ trajvals(41,end) = 0.35;
+ newtrajdata = [sprintf('%d %d 13 ',size(trajvals,2), size(trajvals,1)) sprintf('%f ',[timevals;trajvals;transvals])];
+ success = StartTrajectory(robotid,newtrajdata);
if( ~success )
warning('failed to close fingers');
return;
@@ -272,16 +277,18 @@
% move the hand down until collision
display('moving hand down');
- [trajdata, success] = orProblemSendCommand(['MoveHandStraight execute 0 outputtraj direction ' sprintf('%f ', -updir) ...
- ' maxdist ' sprintf('%f ', 0.3)],probs.manip);
+ [trajdata, success] = orProblemSendCommand(['MoveHandStraight ignorefirstcollision 0 execute 0 ' ...
+ ' outputtraj direction ' sprintf('%f ', -updir) ...
+ ' maxdist ' sprintf('%f ', 0.3)],probs.manip);
if( ~success )
- error('failed to movehandstraight');
+ warning('failed to movehandstraight');
+ else
+ success = StartTrajectory(robotid,trajdata);
+ if( ~success )
+ warning('failed to move hand straight');
+ return;
+ end
end
- success = StartTrajectory(robotid,trajdata);
- if( ~success )
- warning('failed to move hand straight');
- return;
- end
display('opening hand');
@@ -296,14 +303,20 @@
if( ~success )
warning('problems releasing, releasing target first');
orProblemSendCommand('releaseall', probs.manip);
- [trajdata, success] = orProblemSendCommand(['ReleaseFingers execute 0 outputtraj target ' curobj.info.name ...
- ' movingdir ' sprintf('%f ', handrobot.releasedir)], probs.manip);
+ [trajdata, success] = orProblemSendCommand(['ReleaseFingers execute 0 outputtraj ' ...
+ ' target ' curobj.info.name ...
+ ' movingdir ' sprintf('%f ', handrobot.releasedir)], ...
+ probs.manip);
if( ~success )
- error('failed to release fingers');
+ warning('failed to release fingers, forcing open');
+ success = RobotMoveJointValues(robotid,open_config,handjoints);
+ else
+ success = StartTrajectory(robotid,trajdata,4);
end
+ else
+ success = StartTrajectory(robotid,trajdata,4);
end
-
- success = StartTrajectory(robotid,trajdata,4);
+
if( ~success )
warning('trajectory failed to release fingers');
return;
@@ -312,10 +325,9 @@
%% now release object
orProblemSendCommand('releaseall', probs.manip);
- if( squeezesuccess > 0 & putsuccess > 0 )
+ if( squeezesuccess > 0 && putsuccess > 0 )
display('success, putting down');
- orBodySetJointValues(robotid,open_config,handjoints);
% only break when succeeded
%orProblemSendCommand(['MoveHandStraight stepsize 0.003 minsteps ' sprintf('%f ', 90) ' maxsteps ' sprintf('%f ', 100) ' direction ' sprintf('%f ', updir')]);
RobotGoInitial(robot);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -27,19 +27,9 @@
end
display('moving hand');
+success = RobotMoveJointValues(robotid, home(handjoints+1),handjoints)
-prevsession = openraveros_getglobalsession();
-prevprobs = probs;
-setrealsession();
-orRobotSetDOFValues(robotid,home(handjoints+1),handjoints);
-pause(0.4); % pause a little to give a chance for controller to start
-success = WaitForRobot(robotid);
-setclonesession(prevsession);
-probs = prevprobs;
-
%orRobotSetActiveDOFs(robotid,0:(robot.dof-1));
%curvalues = orRobotGetDOFValues(robotid);
%orRobotStartActiveTrajectory(robotid,[curvalues home(:)]);
%WaitForRobot(robotid);
-
-success = 1;
Added: pkg/trunk/openrave_planning/ormanipulation/octave/RobotMoveJointValues.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RobotMoveJointValues.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RobotMoveJointValues.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -0,0 +1,11 @@
+function success = RobotMoveJointValues(robotid, joints, jointinds)
+global probs
+prevsession = openraveros_getglobalsession();
+prevprobs = probs;
+setrealsession();
+orRobotSetDOFValues(robotid,joints,jointinds);
+pause(0.4); % pause a little to give a chance for controller to start
+success = WaitForRobot(robotid);
+setclonesession(prevsession);
+probs = prevprobs;
+success = 1;
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -89,4 +89,6 @@
%destroy
openraveros_destroysession(sessionclone);
end
+
+ input('press any key to get next measurement: ');
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -134,7 +134,7 @@
enabledjoints([robot.manips{1}.armjoints; robot.manips{1}.handjoints]) = [];
jointnames_cell = transpose(robot.jointnames(enabledjoints+1));
jointnames_str = cell2mat (cellfun(@(x) [x ' '], jointnames_cell,'uniformoutput',false));
- orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /right_arm_trajectory_controller joints ' jointnames_str]);
+ orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /right_arm_trajectory_controller/ joints ' jointnames_str]);
end
%% dests is a 12xN array where every column is a 3x4 matrix
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -50,5 +50,5 @@
display('wait ended');
newjointconfig = orBodyGetJointValues(robotid);
setclonesession(prevsession);
-probs = prevprobs
+probs = prevprobs;
orBodySetJointValues(robotid,newjointconfig);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -1,6 +1,36 @@
#!/usr/bin/env octave
-global updir probs
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
+global updir probs
+
startup;
[robot, scenedata] = SetupTableScene('data/pr2table_real.env.xml',1);
+
+Tlaser = [1.00000 0.00062 0.00232 0.01577
+ -0.00066 0.99988 0.01554 -0.00208
+ -0.00231 -0.01555 0.99988 0.03934];
+Tcamera = [0.01611 0.01631 0.99974 0.02890
+ -0.99966 -0.02020 0.01644 0.03236
+ 0.02047 -0.99966 0.01598 0.09532];
+
+%% objects from camera
+out = orProblemSendCommand(['createsystem ObjectTransform topic /checkerdetector/ObjectDetection thresh 0.1 robot ' sprintf('%d ', robot.id) ' matrixoffset ' sprintf('%f ', Tcamera(1:3,1:4))],probs.task);
+if( isempty(out) )
+ error('failed to create checkerboard detector');
+end
+
+%% laser-based dynamic collision map
+out = orProblemSendCommand('createsystem CollisionMap collision_map',probs.task);
+if( isempty(out) )
+ error('failed to create collision map');
+end
+
+%% phase space system
+out = orProblemSendCommand('createsystem PhaseSpace phase_space_snapshot',probs.task);
+if( isempty(out) )
+ error('failed to create phasespace');
+end
+
+input('press any key to start grasping: ');
+RunDynamicGrasping(robot,scenedata,0,0);
+
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -11,7 +11,7 @@
-0.00231 -0.01555 0.99988 0.03934];
Tcamera = [0.01611 0.01631 0.99974 0.02890
-0.99966 -0.02020 0.01644 0.03236
- 0.02047 -0.99966 0.01598 0.06732];
+ 0.02047 -0.99966 0.01598 0.09532];
%% objects from camera
out = orProblemSendCommand(['createsystem ObjectTransform topic /checkerdetector/ObjectDetection thresh 0.1 robot ' sprintf('%d ', robot.id) ' matrixoffset ' sprintf('%f ', Tcamera(1:3,1:4))],probs.task);
@@ -22,7 +22,7 @@
%% laser-based dynamic collision map
out = orProblemSendCommand('createsystem CollisionMap collision_map',probs.task);
if( isempty(out) )
- error('failed to create phasespace');
+ error('failed to create collision map');
end
%% phase space system
Modified: pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2009-01-12 03:11:30 UTC (rev 9219)
@@ -4,9 +4,10 @@
<!-- start openraveros server and set all the correct paths -->
<include file="$(find ormanipulation)/startopenrave.ros.xml"/>
- <include file="$(find ormanipulation)/startchecker.ros.xml"/>
+<!-- <include file="$(find ormanipulation)/startchecker.ros.xml"/> -->
<node pkg="phase_space" type="phase_space_node"/>
+
<!-- launch octave scripts to start the openrave client -->
<!-- <node pkg="ormanipulation" type="runperception.m" output="screen"/> -->
</launch>
Modified: pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml 2009-01-12 03:11:30 UTC (rev 9219)
@@ -3,6 +3,7 @@
<machine name="localhost" address="localhost" default="true"/>
<include file="$(find ormanipulation)/startopenrave.ros.xml"/>
+
<include file="$(find ormanipulation)/startchecker.ros.xml"/>
<node pkg="phase_space" type="phase_space_node"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-12 03:11:30 UTC (rev 9219)
@@ -40,21 +40,125 @@
{
public:
CollisionMapSystem(EnvironmentBase* penv)
- : ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>(penv), _nNextId(1)
+ : ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>(penv), _robotid(0), _nNextId(1), _bEnableCollisions(true)
{
}
+ virtual ~CollisionMapSystem() {
+ stopsubscriptions(); // need to stop the subscriptions because the virtual destructor will not call the overridden stopsubscriptions
+ ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>::Destroy();
+ }
virtual bool Init(istream& sinput)
{
+ _robotid = 0;
_topic = "collision_map";
- return ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>::Init(sinput);
+
+ string cmd;
+ while(!sinput.eof()) {
+ sinput >> cmd;
+ if( !sinput )
+ break;
+
+ if( stricmp(cmd.c_str(), "topic") == 0 )
+ sinput >> _topic;
+ else if( stricmp(cmd.c_str(), "collisions") == 0 )
+ sinput >> _bEnableCollisions;
+ else if( stricmp(cmd.c_str(), "robot") == 0 ) {
+ int id;
+ sinput >> id;
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(id);
+ if( pbody != NULL )
+ _robotid = id;
+
+ if( _robotid == 0 )
+ RAVELOG_WARNA("failed to find robot with id %d\n", id);
+ }
+ else break;
+
+ if( !sinput ) {
+ RAVELOG_ERRORA("failed\n");
+ return false;
+ }
+ }
+
+ startsubscriptions();
+ return _bSubscribed;
}
private:
+ virtual void startsubscriptions()
+ {
+ ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>::startsubscriptions();
+
+ if( _bSubscribed ) {
+ boost::mutex::scoped_lock lock(_mutex);
+ ros::node* pnode = check_roscpp_nocreate();
+ if( pnode != NULL ) {
+ double tf_cache_time_secs;
+ pnode->param("~tf_cache_time_secs", tf_cache_time_secs, 10.0);
+ if (tf_cache_time_secs < 0)
+ RAVELOG_ERRORA("ROSSensorSystem: Parameter tf_cache_time_secs<0 (%f)\n", tf_cache_time_secs);
+ unsigned long long tf_cache_time = tf_cache_time_secs*1000000000ULL;
+ _tf.reset(new tf::TransformListener(*pnode, true, tf_cache_time));
+ RAVELOG_INFOA("ROSSensorSystem: TF Cache Time: %f Seconds\n", tf_cache_time_secs);
+
+ // **** Set TF Extrapolation Limit ****
+ double tf_extrap_limit_secs ;
+ pnode->param("~tf_extrap_limit", tf_extrap_limit_secs, 0.00);
+ if (tf_extrap_limit_secs < 0.0)
+ RAVELOG_ERRORA("ROSSensorSystem: parameter tf_extrap_limit<0 (%f)\n", tf_extrap_limit_secs);
+
+ ros::Duration extrap_limit;
+ extrap_limit.fromSec(tf_extrap_limit_secs);
+ _tf->setExtrapolationLimit(extrap_limit);
+ RAVELOG_INFOA("ROSSensorSystem: tf extrapolation Limit: %f Seconds\n", tf_extrap_limit_secs);
+ }
+ }
+ }
+
+ virtual void stopsubscriptions()
+ {
+ ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>::stopsubscriptions();
+ _tf.reset();
+ }
+
void newdatacb()
{
// create the new kinbody
GetEnv()->LockPhysics(true);
+
+ Transform tcollision;
+ string strrobotbaselink;
+ bool bHasRobotTransform = false;
+
+ if( _robotid != 0 ) {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(_robotid);
+ if( pbody != NULL ) {
+ bHasRobotTransform = true;
+ tcollision = pbody->GetTransform();
+ strrobotbaselink = _stdwcstombs(pbody->GetLinks().front()->GetName());
+ }
+ }
+
+ if( bHasRobotTransform && !!_tf ) {
+ tf::Stamped<btTransform> bttransform;
+
+ try {
+ _tf->lookupTransform(strrobotbaselink, _topicmsg.header.frame_id, _topicmsg.header.stamp, bttransform);
+ tcollision = tcollision * GetTransform(bttransform);
+ }
+ catch(tf::TransformException& ex) {
+ try {
+ _tf->lookupTransform(strrobotbaselink, _topicmsg.header.frame_id, ros::Time(), bttransform);
+ tcollision = tcollision * GetTransform(bttransform);
+ }
+ catch(tf::TransformException& ex) {
+ RAVELOG_WARNA("failed to get tf frames %s (robot link:%s)\n", _topicmsg.header.frame_id.c_str(), strrobotbaselink.c_str());
+ return;
+ }
+ }
+ }
+
KinBody* pbody = GetEnv()->CreateKinBody();
_vaabbs.resize(_topicmsg.boxes.size());
@@ -91,7 +195,6 @@
TYPEOF(_mapbodies.begin()) itbody = _mapbodies.begin();
while(itbody != _mapbodies.end()) {
if( !itbody->second->IsLocked() ) {
- BODY* b = itbody->second.get();
KinBody::Link* plink = itbody->second->GetOffsetLink();
assert( plink != NULL );
GetEnv()->RemoveKinBody(plink->GetParent());
@@ -113,13 +216,18 @@
}
}
- Transform GetTransform(const std_msgs::Transform& pose)
+ Transform GetTransform(const btTransform& bt)
{
- return Transform(Vector(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z), Vector(pose.translation.x, pose.translation.y, pose.translation.z));
+ btQuaternion q = bt.getRotation();
+ btVector3 o = bt.getOrigin();
+ return Transform(Vector(q.w(),q.x(),q.y(),q.z()),Vector(o.x(),o.y(),o.z()));
}
+ boost::shared_ptr<tf::TransformListener> _tf;
+ int _robotid;
vector<AABB> _vaabbs;
int _nNextId;
+ bool _bEnableCollisions;
};
#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-12 03:11:30 UTC (rev 9219)
@@ -65,12 +65,12 @@
pr2_mechanism_controllers::TrajectoryQuery::request req;
pr2_mechanism_controllers::TrajectoryQuery::response res;
if( !_srvTrajectoryQuery->call(req,res) ) {
- RAVELOG_ERRORA("failed to query trajectory service %s", _strTrajectoryServiceDir.c_str());
+ RAVELOG_ERRORA("failed to query trajectory service %s\n", _strTrajectoryServiceDir.c_str());
return false;
}
if( res.jointnames.size() == 0 ) {
- RAVELOG_ERRORA("no joint names for trajectory service %s", _strTrajectoryServiceDir.c_str());
+ RAVELOG_ERRORA("no joint names for trajectory service %s\n", _strTrajectoryServiceDir.c_str());
return false;
}
@@ -78,6 +78,8 @@
for(size_t i = 0; i < _probot->GetJoints().size(); ++i)
vrobotjoints[i] = _stdwcstombs(_probot->GetJoints()[i]->GetName());
+ stringstream ss;
+ ss << "roscontroller: " << _strTrajectoryServiceDir;
_vjointmap.reserve(res.jointnames.size());
FOREACH(itname, res.jointnames) {
vector<string>::iterator itindex = find(vrobotjoints.begin(), vrobotjoints.end(), *itname);
@@ -87,8 +89,12 @@
return false;
}
_vjointmap.push_back((int)(itindex-vrobotjoints.begin()));
+ ss << " " << *itname << " (" << _vjointmap.back() << ")";
}
+ ss << endl;
+ RAVELOG_DEBUGA(ss.str().c_str());
+
return true;
}
Modified: pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-01-12 03:11:30 UTC (rev 9219)
@@ -403,6 +403,7 @@
if (curlong || totlong)
ROS_ERROR("qhull internal warning (main): did not free %d bytes of long memory (%d pieces)", totlong, curlong);
+ fclose(errfile);
return bSuccess;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-01-12 22:27:57
|
Revision: 9272
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9272&view=rev
Author: tpratkanis
Date: 2009-01-12 22:10:26 +0000 (Mon, 12 Jan 2009)
Log Message:
-----------
Moving std_msgs/Planner2DState and std_msgs/Planner2DGoal to robot_msgs.
Modified Paths:
--------------
pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/BaseControllerAdapter.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/BaseControllerAdapter.hh
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
pkg/trunk/nav/wavefront_player/cli.cpp
pkg/trunk/nav/wavefront_player/manifest.xml
pkg/trunk/nav/wavefront_player/wavefront_player.cc
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/Planner2DGoal.msg
pkg/trunk/robot_msgs/msg/Planner2DState.msg
Modified: pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-01-12 21:58:09 UTC (rev 9271)
+++ pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-01-12 22:10:26 UTC (rev 9272)
@@ -38,7 +38,7 @@
rostools.update_path('executive_python')
import rospy
import random
-from std_msgs.msg import Planner2DGoal, Planner2DState
+from robot_msgs.msg import Planner2DGoal, Planner2DState
class NavigationAdapter:
def __init__(self, no_plan_limit, time_limit, state_topic, goal_topic):
@@ -83,6 +83,7 @@
goal.goal.x = goal_pts[0]
goal.goal.y = goal_pts[1]
goal.goal.th = goal_pts[2]
+ goal.timeout = 0
goal.enable = 1
self.pub.publish(goal)
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/BaseControllerAdapter.cc
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/BaseControllerAdapter.cc 2009-01-12 21:58:09 UTC (rev 9271)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/BaseControllerAdapter.cc 2009-01-12 22:10:26 UTC (rev 9272)
@@ -1,16 +1,16 @@
#include "ROSControllerAdapter.hh"
#include "IntervalDomain.hh"
#include "Token.hh"
-#include <std_msgs/Planner2DState.h>
-#include <std_msgs/Planner2DGoal.h>
+#include <robot_msgs/Planner2DState.h>
+#include <robot_msgs/Planner2DGoal.h>
namespace TREX {
- class BaseControllerAdapter: public ROSControllerAdapter<std_msgs::Planner2DState, std_msgs::Planner2DGoal> {
+ class BaseControllerAdapter: public ROSControllerAdapter<robot_msgs::Planner2DState, robot_msgs::Planner2DGoal> {
public:
BaseControllerAdapter(const LabelStr& agentName, const TiXmlElement& configData)
- : ROSControllerAdapter<std_msgs::Planner2DState, std_msgs::Planner2DGoal>(agentName, configData){
+ : ROSControllerAdapter<robot_msgs::Planner2DState, robot_msgs::Planner2DGoal>(agentName, configData){
}
virtual ~BaseControllerAdapter(){}
@@ -29,7 +29,7 @@
obs->push_back("th", new IntervalDomain(stateMsg.pos.th));
}
- void fillRequestParameters(std_msgs::Planner2DGoal& goalMsg, const TokenId& goalToken){
+ void fillRequestParameters(robot_msgs::Planner2DGoal& goalMsg, const TokenId& goalToken){
const IntervalDomain& x = goalToken->getVariable("x")->lastDomain();
const IntervalDomain& y = goalToken->getVariable("y")->lastDomain();
const IntervalDomain& th = goalToken->getVariable("th")->lastDomain();
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/BaseControllerAdapter.hh
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/BaseControllerAdapter.hh 2009-01-12 21:58:09 UTC (rev 9271)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/BaseControllerAdapter.hh 2009-01-12 22:10:26 UTC (rev 9272)
@@ -4,7 +4,7 @@
#include "ROSAdapter.hh"
// State Update messages
-#include <std_msgs/Planner2DState.h>
+#include <robot_msgs/Planner2DState.h>
namespace TREX {
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2009-01-12 21:58:09 UTC (rev 9271)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2009-01-12 22:10:26 UTC (rev 9272)
@@ -52,8 +52,8 @@
#include <plan_wrap.h>
// Message structures used
-#include <std_msgs/Planner2DState.h>
-#include <std_msgs/Planner2DGoal.h>
+#include <robot_msgs/Planner2DState.h>
+#include <robot_msgs/Planner2DGoal.h>
#include <std_msgs/LaserScan.h>
#include <std_msgs/BaseVel.h>
#include <std_msgs/RobotBase2DOdom.h>
@@ -73,7 +73,7 @@
namespace ros {
namespace highlevel_controllers {
- class MoveBase : public HighlevelController<std_msgs::Planner2DState, std_msgs::Planner2DGoal> {
+ class MoveBase : public HighlevelController<robot_msgs::Planner2DState, robot_msgs::Planner2DGoal> {
public:
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-01-12 21:58:09 UTC (rev 9271)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-01-12 22:10:26 UTC (rev 9272)
@@ -114,7 +114,7 @@
render_panel_->getViewport()->setCamera( camera_ );
- ros_node_->advertise<std_msgs::Planner2DGoal>("goal", 1);
+ ros_node_->advertise<robot_msgs::Planner2DGoal>("goal", 1);
ros_node_->advertise<std_msgs::Pose2DFloat32>("initialpose", 1);
ros_node_->subscribe("particlecloud", cloud_, &NavViewPanel::incomingParticleCloud, this, 1);
ros_node_->subscribe("gui_path", path_line_, &NavViewPanel::incomingGuiPath, this, 1);
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-01-12 21:58:09 UTC (rev 9271)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-01-12 22:10:26 UTC (rev 9272)
@@ -33,7 +33,7 @@
#include "nav_view_panel_generated.h"
#include "std_msgs/ParticleCloud2D.h"
-#include "std_msgs/Planner2DGoal.h"
+#include "robot_msgs/Planner2DGoal.h"
#include "std_msgs/Polyline2D.h"
#include "std_msgs/Pose2DFloat32.h"
#include "std_srvs/StaticMap.h"
@@ -206,7 +206,7 @@
Ogre::TexturePtr map_texture_;
std_msgs::ParticleCloud2D cloud_;
- std_msgs::Planner2DGoal goal_;
+ robot_msgs::Planner2DGoal goal_;
std_msgs::Polyline2D path_line_;
std_msgs::Polyline2D local_path_;
std_msgs::Polyline2D robot_footprint_;
Modified: pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-01-12 21:58:09 UTC (rev 9271)
+++ pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-01-12 22:10:26 UTC (rev 9272)
@@ -158,7 +158,7 @@
if ( is_goal_ )
{
- std_msgs::Planner2DGoal goal;
+ robot_msgs::Planner2DGoal goal;
goal.goal.x = pos_.x;
goal.goal.y = pos_.y;
goal.goal.th = angle;
Modified: pkg/trunk/nav/wavefront_player/cli.cpp
===================================================================
--- pkg/trunk/nav/wavefront_player/cli.cpp 2009-01-12 21:58:09 UTC (rev 9271)
+++ pkg/trunk/nav/wavefront_player/cli.cpp 2009-01-12 22:10:26 UTC (rev 9272)
@@ -1,14 +1,14 @@
#include <cstdio>
#include <stdlib.h>
#include "ros/node.h"
-#include "std_msgs/Planner2DState.h"
-#include "std_msgs/Planner2DGoal.h"
+#include "robot_msgs/Planner2DState.h"
+#include "robot_msgs/Planner2DGoal.h"
class WavefrontCLI : public ros::node
{
public:
- std_msgs::Planner2DState wf_state;
- std_msgs::Planner2DGoal wf_goal;
+ robot_msgs::Planner2DState wf_state;
+ robot_msgs::Planner2DGoal wf_goal;
enum { WF_IDLE, WF_SEEKING_GOAL, WF_DONE } state;
WavefrontCLI(double x, double y, double th)
@@ -19,7 +19,7 @@
wf_goal.goal.th = th;
wf_goal.enable = 1;
subscribe("state", wf_state, &WavefrontCLI::state_cb, 1);
- advertise<std_msgs::Planner2DGoal>("goal", 1);
+ advertise<robot_msgs::Planner2DGoal>("goal", 1);
}
void state_cb()
{
Modified: pkg/trunk/nav/wavefront_player/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront_player/manifest.xml 2009-01-12 21:58:09 UTC (rev 9271)
+++ pkg/trunk/nav/wavefront_player/manifest.xml 2009-01-12 22:10:26 UTC (rev 9272)
@@ -8,6 +8,7 @@
<depend package="player" />
<depend package="laser_scan" />
<depend package="std_srvs" />
+ <depend package="robot_msgs" />
<depend package="tf"/>
<export>
<cpp cflags="-I${prefix}/srv/cpp"/>
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-01-12 21:58:09 UTC (rev 9271)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-01-12 22:10:26 UTC (rev 9272)
@@ -114,8 +114,8 @@
#include "boost/thread/mutex.hpp"
// The messages that we'll use
-#include <std_msgs/Planner2DState.h>
-#include <std_msgs/Planner2DGoal.h>
+#include <robot_msgs/Planner2DState.h>
+#include <robot_msgs/Planner2DGoal.h>
#include <std_msgs/BaseVel.h>
#include <std_msgs/PointCloud.h>
#include <std_msgs/LaserScan.h>
@@ -202,11 +202,11 @@
double tvmin, tvmax, avmin, avmax, amin, amax;
// incoming/outgoing messages
- std_msgs::Planner2DGoal goalMsg;
+ robot_msgs::Planner2DGoal goalMsg;
//MsgRobotBase2DOdom odomMsg;
std_msgs::Polyline2D polylineMsg;
std_msgs::Polyline2D pointcloudMsg;
- std_msgs::Planner2DState pstate;
+ robot_msgs::Planner2DState pstate;
//MsgRobotBase2DOdom prevOdom;
bool firstodom;
@@ -365,7 +365,7 @@
this->firstodom = true;
- advertise<std_msgs::Planner2DState>("state",1);
+ advertise<robot_msgs::Planner2DState>("state",1);
advertise<std_msgs::Polyline2D>("gui_path",1);
advertise<std_msgs::Polyline2D>("gui_laser",1);
advertise<std_msgs::BaseVel>("cmd_vel",1);
Added: pkg/trunk/robot_msgs/msg/Planner2DGoal.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/Planner2DGoal.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/Planner2DGoal.msg 2009-01-12 22:10:26 UTC (rev 9272)
@@ -0,0 +1,6 @@
+Header header
+std_msgs/Pose2DFloat32 goal
+byte enable
+
+# Timelimit for planning. Set it to zero for unlimited.
+float64 timeout
Added: pkg/trunk/robot_msgs/msg/Planner2DState.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/Planner2DState.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/Planner2DState.msg 2009-01-12 22:10:26 UTC (rev 9272)
@@ -0,0 +1,22 @@
+Header header
+# Is the planner actively going to a goal?
+byte active
+# Did the planner find a valid path?
+byte valid
+# Have we arrived at the goal?
+byte done
+#Did the planner give up?
+byte aborted
+#Was the planner told to stop?
+byte preempted
+# Current location (m,m,rad)
+std_msgs/Pose2DFloat32 pos
+# Goal location (m,m,rad)
+std_msgs/Pose2DFloat32 goal
+# Current waypoint location (m,m,rad)
+std_msgs/Pose2DFloat32 waypoint
+# Current list of waypoints in the plan
+std_msgs/Pose2DFloat32[] waypoints
+# Current waypoint index. May be negative if there's no plan, or if
+# the plan is done
+int32 waypoint_idx
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-01-12 23:39:45
|
Revision: 9281
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9281&view=rev
Author: tpratkanis
Date: 2009-01-12 23:39:40 +0000 (Mon, 12 Jan 2009)
Log Message:
-----------
Add abort and prempt functionality to highlevel controllers.
Modified Paths:
--------------
pkg/trunk/highlevel/executive_python/src/recharge_adapter.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh
pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
pkg/trunk/highlevel/highlevel_controllers/msg/PlugInGoal.msg
pkg/trunk/highlevel/highlevel_controllers/msg/PlugInState.msg
pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg
pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
pkg/trunk/pr2_msgs/msg/MoveArmState.msg
pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg
pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
pkg/trunk/robot_msgs/msg/Planner2DGoal.msg
Modified: pkg/trunk/highlevel/executive_python/src/recharge_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/recharge_adapter.py 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/executive_python/src/recharge_adapter.py 2009-01-12 23:39:40 UTC (rev 9281)
@@ -52,6 +52,7 @@
goal = RechargeGoal()
goal.recharge_level = self.recharge_level
goal.enable = 1
+ goal.timeout = 0
goal.pose.x = pose[0]
goal.pose.y = pose[1]
goal.pose.th = pose[2]
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh 2009-01-12 23:39:40 UTC (rev 9281)
@@ -114,6 +114,7 @@
G goalMsg;
fillRequestParameters(goalMsg, goal);
goalMsg.enable = enableController;
+ goalMsg.timeout = 0;
ROS_DEBUG("[%d}Dispatching %s", getCurrentTick(), goal->toString().c_str());
m_node->publishMsg<G>(goalTopic, goalMsg);
Modified: pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2009-01-12 23:39:40 UTC (rev 9281)
@@ -69,8 +69,8 @@
* @param goalTopic The ROS topic on which controller goals are received
*/
HighlevelController(const std::string& nodeName, const std::string& _stateTopic, const std::string& _goalTopic):
- ros::node(nodeName), initialized(false), terminated(false), stateTopic(_stateTopic), goalTopic(_goalTopic),
- controllerCycleTime_(0.1), plannerCycleTime_(0.0), plannerThread_(NULL){
+ ros::node(nodeName), initialized(false), terminated(false), stateTopic(_stateTopic),
+ goalTopic(_goalTopic), controllerCycleTime_(0.1), plannerCycleTime_(0.0), plannerThread_(NULL), timeout(0, 0) {
// Obtain the control frequency for this node
double controller_frequency(10);
@@ -146,6 +146,7 @@
* it will call the planner with the given timeout.
*/
void plannerLoop(){
+ ros::Time lastPlan = ros::Time::now();
while(ok() && !isTerminated()){
ros::Time curr = ros::Time::now();
@@ -156,9 +157,16 @@
setValid(makePlan());
if(!isValid()){
// Could use a refined locking scheme but for now do not want to delegate that to a derived class
- lock();
+ lock();
+ if ((ros::Time::now() - lastPlan) < timeout && timeout.toSec() != 0.0) {
+ this->stateMsg.aborted = true;
+ ROS_ERROR("Controller timed out.");
+ }
handlePlanningFailure();
- unlock();
+ unlock();
+ } else {
+ this->stateMsg.aborted = false;
+ lastPlan = ros::Time::now();
}
}
}
@@ -187,6 +195,20 @@
}
/**
+ * @brief Access aborted state of the planner.
+ */
+ bool isAborted() {
+ return this->stateMsg.aborted;
+ }
+
+ /**
+ * @brief Access preempted state of the planner.
+ */
+ bool isPreempted() {
+ return this->stateMsg.preempted;
+ }
+
+ /**
* @brief Activation of the controller will set the state, the stateMsg but indicate that the
* goal has not yet been accomplished and that no plan has been constructed yet.
*/
@@ -197,6 +219,8 @@
this->stateMsg.active = 1;
this->stateMsg.valid = 0;
this->stateMsg.done = 0;
+ this->stateMsg.aborted = 0;
+ this->stateMsg.preempted = 0;
handleActivation();
}
@@ -207,9 +231,15 @@
void deactivate(){
ROS_INFO("Deactivating controller\n");
+ if (this->state == ACTIVE) {
+ ROS_INFO("Controller preempted.");
+ this->stateMsg.preempted = 1;
+ }
+
this->state = INACTIVE;
this->stateMsg.active = 0;
this->stateMsg.valid = 0;
+ this->stateMsg.aborted = 0;
handleDeactivation();
}
@@ -320,6 +350,13 @@
private:
void goalCallback(){
+ if (goalMsg.timeout < 0) {
+ ROS_ERROR("Controller was given negative timeout, setting to zero.");
+ timeout = ros::Duration().fromSec(0);
+ } else {
+ timeout = ros::Duration().fromSec(goalMsg.timeout);
+ }
+
// Do nothing if not initialized
if(!isInitialized() || isTerminated())
return;
@@ -432,6 +469,7 @@
boost::mutex lock_; /*!< Lock for access to class members in callbacks */
boost::thread* plannerThread_; /*!< Thread running the planner loop */
highlevel_controllers::Ping shutdownMsg_; /*!< For receiving shutdown from executive */
+ ros::Duration timeout; /*< The time limit for planning failure. */
};
#endif
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/PlugInGoal.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/PlugInGoal.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/PlugInGoal.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -3,3 +3,7 @@
byte goal
#True to submit a goal, false to recall it
byte enable
+
+# Timelimit for the controller, if a plan is not
+# found by this time, abort. Set it to zero for unlimited.
+float64 timeout
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/PlugInState.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/PlugInState.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/PlugInState.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -7,5 +7,9 @@
byte done
# Are we waiting to be plugged in or unplugged. 1 if plug in.
byte goal
+#Did the planner give up?
+byte aborted
+#Was the planner told to stop?
+byte preempted
# Are we plugged in or unplugged. 1 if plug in.
byte state
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -10,3 +10,7 @@
# True to submit a goal, false to recall it
byte enable
+
+# Timelimit for the controller, if a plan is not
+# found by this time, abort. Set it to zero for unlimited.
+float64 timeout
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -9,6 +9,12 @@
# Have we arrived at the goal?
byte done
+#Did the planner give up?
+byte aborted
+
+#Was the planner told to stop?
+byte preempted
+
# Actual recharge level
float32 recharge_level
@@ -16,4 +22,4 @@
float32 goal_recharge_level
# The goal pose for accessing the plug
-std_msgs/Pose2DFloat32 goal_pose
\ No newline at end of file
+std_msgs/Pose2DFloat32 goal_pose
Modified: pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -4,3 +4,7 @@
# Indicate if the goal is being enabled or disabled
byte enable
+
+# Total timelimit for the controller, if a plan is not
+# found by this time, abort. Set it to zero for unlimited.
+float64 timeout
Modified: pkg/trunk/pr2_msgs/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -5,6 +5,10 @@
byte valid
#Did we actually successfully arrive at the goal?
byte done
+#Did the planner give up?
+byte aborted
+#Was the planner told to stop?
+byte preempted
#Current arm configuration
robot_msgs/JointState[] configuration
#Goal arm configuration
Modified: pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -4,3 +4,7 @@
# Indicate if the goal is being enabled or disabled
byte enable
+
+# Timelimit for the controller, if a plan is not
+# found by this time, abort. Set it to zero for unlimited.
+float64 timeout
Modified: pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -5,6 +5,10 @@
byte valid
#Did we actually successfully arrive at the goal?
byte done
+#Did the planner give up?
+byte aborted
+#Was the planner told to stop?
+byte preempted
#Current arm configuration
robot_msgs/JointState[] configuration
#Goal arm configuration
Modified: pkg/trunk/robot_msgs/msg/Planner2DGoal.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/Planner2DGoal.msg 2009-01-12 23:38:19 UTC (rev 9280)
+++ pkg/trunk/robot_msgs/msg/Planner2DGoal.msg 2009-01-12 23:39:40 UTC (rev 9281)
@@ -2,5 +2,6 @@
std_msgs/Pose2DFloat32 goal
byte enable
-# Timelimit for planning. Set it to zero for unlimited.
+# Total timelimit for the controller, if a plan is not
+# found by this time, abort. Set it to zero for unlimited.
float64 timeout
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-01-13 00:04:06
|
Revision: 9286
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9286&view=rev
Author: tpratkanis
Date: 2009-01-13 00:03:55 +0000 (Tue, 13 Jan 2009)
Log Message:
-----------
Add obstacle filtering to the costmap.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/manifest.xml
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2009-01-13 00:03:55 UTC (rev 9286)
@@ -57,10 +57,12 @@
#include <std_msgs/LaserScan.h>
#include <std_msgs/BaseVel.h>
#include <std_msgs/RobotBase2DOdom.h>
+#include <std_msgs/PointCloud.h>
// For transform support
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
+#include <tf/message_notifier.h>
// Laser projection
#include "laser_scan/laser_scan.h"
@@ -70,6 +72,10 @@
#include <list>
+namespace robot_filter {
+ class RobotFilter;
+}
+
namespace ros {
namespace highlevel_controllers {
@@ -167,6 +173,7 @@
void baseScanCallback();
void tiltScanCallback();
void tiltCloudCallback();
+ void tiltCloudCallbackTransform(const tf::MessageNotifier<std_msgs::PointCloud>::MessagePtr& message);
void groundPlaneCloudCallback();
void stereoCloudCallback();
void groundPlaneCallback();
@@ -260,7 +267,12 @@
// Tolerances for determining if goal has been reached
double yaw_goal_tolerance_;
double xy_goal_tolerance_;
+
+ //Robot filter
+ robot_filter::RobotFilter* filter_;
+ tf::MessageNotifier<std_msgs::PointCloud>* tiltLaserNotifier_;
+
//ground plane extraction
ransac_ground_plane_extraction::RansacGroundPlaneExtraction ground_plane_extractor_;
pr2_msgs::PlaneStamped groundPlaneMsg_;
Modified: pkg/trunk/highlevel/highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-01-13 00:03:55 UTC (rev 9286)
@@ -27,6 +27,7 @@
<depend package="rostime"/>
<depend package="navfn"/>
<depend package="angles"/>
+ <depend package="robot_filter"/>
<export>
<cpp cflags="-I${prefix}/msg/cpp"/>
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2009-01-13 00:03:55 UTC (rev 9286)
@@ -45,6 +45,7 @@
#include <iterator>
#include <angles/angles.h>
#include <boost/thread.hpp>
+#include <robot_filter/RobotFilter.h>
namespace ros {
namespace highlevel_controllers {
@@ -141,23 +142,40 @@
param("/costmap_2d/tilt_laser_keepalive", tilt_laser_keepalive, tilt_laser_keepalive);
param("/costmap_2d/low_obstacle_keepalive", low_obstacle_keepalive, low_obstacle_keepalive);
param("/costmap_2d/stereo_keepalive", stereo_keepalive, stereo_keepalive);
+
+ //Create robot filter
+ std::string robotName = "/robotdesc/pr2";
+ double bodypartScale = 2.4;
+ bool useFilter = false;
+ param("/costmap_2d/body_part_scale", bodypartScale, bodypartScale);
+ param("/costmap_2d/robot_name", robotName, robotName);
+ param("/costmap_2d/filter_robot_points", useFilter, useFilter);
+
+ if (useFilter) {
+ filter_ = new robot_filter::RobotFilter((ros::node*)this, robotName, true, bodypartScale);
+ filter_->loadRobotDescription();
+ filter_->waitForState();
+ } else {
+ filter_ = NULL;
+ }
+
// Then allocate observation buffers
baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), tf_,
ros::Duration().fromSec(base_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
- inscribedRadius, minZ_, maxZ_);
+ inscribedRadius, minZ_, maxZ_, filter_);
tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), tf_,
ros::Duration().fromSec(tilt_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
- inscribedRadius, minZ_, maxZ_);
+ inscribedRadius, minZ_, maxZ_, filter_);
lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), tf_,
ros::Duration().fromSec(low_obstacle_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(low_obstacle_update_rate),
- inscribedRadius, -10.0, maxZ_);
+ inscribedRadius, -10.0, maxZ_, filter_);
stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), tf_,
ros::Duration().fromSec(stereo_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
- inscribedRadius, minZ_, maxZ_);
+ inscribedRadius, minZ_, maxZ_, filter_);
// get map via RPC
@@ -286,6 +304,9 @@
subscribe("base_scan", baseScanMsg_, &MoveBase::baseScanCallback, 1);
//subscribe("tilt_scan", tiltScanMsg_, &MoveBase::tiltScanCallback, 1);
subscribe("tilt_laser_cloud_filtered", tiltCloudMsg_, &MoveBase::tiltCloudCallback, 1);
+ tiltLaserNotifier_ = new tf::MessageNotifier<std_msgs::PointCloud>(&tf_, this,
+ boost::bind(&MoveBase::tiltCloudCallbackTransform, this, _1),
+ "tilt_laser_cloud_filtered", "map", 1);
subscribe("dcam/cloud", stereoCloudMsg_, &MoveBase::stereoCloudCallback, 1);
subscribe("ground_plane", groundPlaneMsg_, &MoveBase::groundPlaneCallback, 1);
subscribe("obstacle_cloud", groundPlaneCloudMsg_, &MoveBase::groundPlaneCloudCallback, 1);
@@ -318,6 +339,8 @@
delete lowObstacleBuffer_;
delete tiltScanBuffer_;
delete stereoCloudBuffer_;
+ delete filter_;
+ delete tiltLaserNotifier_;
}
void MoveBase::updateGlobalPose(){
@@ -425,8 +448,12 @@
void MoveBase::tiltCloudCallback()
{
+ }
+
+ void MoveBase::tiltCloudCallbackTransform(const tf::MessageNotifier<std_msgs::PointCloud>::MessagePtr& message)
+ {
lock();
- tiltScanBuffer_->buffer_cloud(tiltCloudMsg_);
+ tiltScanBuffer_->buffer_cloud(*message);
unlock();
}
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h 2009-01-13 00:03:55 UTC (rev 9286)
@@ -34,15 +34,19 @@
#include <costmap_2d/observation_buffer.h>
+namespace robot_filter {
+ class RobotFilter;
+}
+
namespace costmap_2d {
/**
* @brief Extend base class to handle buffering until a transform is available, and to support locking for mult-threaded
* access
*/
- class BasicObservationBuffer: public ObservationBuffer {
+ class BasicObservationBuffer : public ObservationBuffer {
public:
- BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,double robotRadius, double minZ, double maxZ);
+ BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter = NULL);
virtual void buffer_cloud(const std_msgs::PointCloud& local_cloud);
@@ -64,6 +68,7 @@
std::deque<std_msgs::PointCloud> point_clouds_; /**< Buffer point clouds until a transform is available */
ros::thread::mutex buffer_mutex_;
const double robotRadius_, minZ_, maxZ_; /**< Constraints for filtering points */
+ robot_filter::RobotFilter* filter_;
};
}
#endif
Modified: pkg/trunk/world_models/costmap_2d/manifest.xml
===================================================================
--- pkg/trunk/world_models/costmap_2d/manifest.xml 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/world_models/costmap_2d/manifest.xml 2009-01-13 00:03:55 UTC (rev 9286)
@@ -8,6 +8,7 @@
<depend package="std_msgs" />
<depend package="pr2_msgs" />
<depend package="tf" />
+<depend package="robot_filter" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
</export>
Modified: pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-12 23:59:40 UTC (rev 9285)
+++ pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-13 00:03:55 UTC (rev 9286)
@@ -1,10 +1,11 @@
#include <costmap_2d/basic_observation_buffer.h>
+#include <robot_filter/RobotFilter.h>
namespace costmap_2d {
BasicObservationBuffer::BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,
- double robotRadius, double minZ, double maxZ)
- : ObservationBuffer(frame_id, keepAlive, refresh_interval), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ){}
+ double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter)
+ : ObservationBuffer(frame_id, keepAlive, refresh_interval), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ), filter_(filter) {}
void BasicObservationBuffer::buffer_cloud(const std_msgs::PointCloud& local_cloud)
{
@@ -72,12 +73,34 @@
newData = NULL;
}
+ if (filter_) {
+ newData = filter_->filter(*map_cloud);
+
+ if (map_cloud != NULL){
+ delete map_cloud;
+ map_cloud = NULL;
+ }
+ map_cloud = newData;
+ newData = NULL;
+ }
+
// Get the point from whihc we ray trace
std_msgs::Point o;
o.x = map_origin.getX();
o.y = map_origin.getY();
o.z = map_origin.getZ();
+ if (!map_cloud) {
+ ROS_ERROR("Cloud is null.");
+ continue;
+ }
+
+ if (map_cloud->pts.empty()) {
+ ROS_ERROR("Cloud is empty.");
+ delete map_cloud;
+ continue;
+ }
+
// Allocate and buffer the observation
Observation obs(o, map_cloud);
buffer_observation(obs);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-01-13 07:24:20
|
Revision: 9324
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9324&view=rev
Author: jfaustwg
Date: 2009-01-13 07:24:01 +0000 (Tue, 13 Jan 2009)
Log Message:
-----------
Merge from josh branch... compatibility with roscpp sessions merge and cmake 2.4
Modified Paths:
--------------
pkg/trunk/3rdparty/ANN/Makefile
pkg/trunk/3rdparty/FL-ANN/Makefile
pkg/trunk/3rdparty/bfl/Makefile
pkg/trunk/3rdparty/bullet/Makefile
pkg/trunk/3rdparty/convex_decomposition/Makefile
pkg/trunk/3rdparty/eigen/Makefile
pkg/trunk/3rdparty/eml/Makefile
pkg/trunk/3rdparty/estar/Makefile
pkg/trunk/3rdparty/fast_detector/CMakeLists.txt
pkg/trunk/3rdparty/freeimage/Makefile
pkg/trunk/3rdparty/glc/elfhacks/Makefile
pkg/trunk/3rdparty/glc/glc/Makefile
pkg/trunk/3rdparty/glc/packetstream/Makefile
pkg/trunk/3rdparty/gmapping/Makefile
pkg/trunk/3rdparty/ivcon/CMakeLists.txt
pkg/trunk/3rdparty/kdl/Makefile
pkg/trunk/3rdparty/libbk_maxflow/CMakeLists.txt
pkg/trunk/3rdparty/libdc1394v2/Makefile
pkg/trunk/3rdparty/libfz_ht_superpixels/CMakeLists.txt
pkg/trunk/3rdparty/libsunflower/Makefile
pkg/trunk/3rdparty/opencv_latest/Makefile
pkg/trunk/3rdparty/opende/Makefile
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/3rdparty/player/Makefile
pkg/trunk/3rdparty/plplot/Makefile
pkg/trunk/3rdparty/sicktoolbox/Makefile
pkg/trunk/3rdparty/sicktoolbox/stdlib_include.patch
pkg/trunk/3rdparty/soqt/Makefile
pkg/trunk/3rdparty/spacenav/Makefile
pkg/trunk/3rdparty/stage/Makefile
pkg/trunk/3rdparty/toro/Makefile
pkg/trunk/3rdparty/trex/Makefile
pkg/trunk/3rdparty/velodyne-driver/Makefile
pkg/trunk/3rdparty/xenomai/manifest.xml
pkg/trunk/audio/audio_capture/CMakeLists.txt
pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt
pkg/trunk/calibration/wheel_odometry_calibration/CMakeLists.txt
pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/controllers/control_toolbox/CMakeLists.txt
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/CMakeLists.txt
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/demos/2dnav_gazebo/CMakeLists.txt
pkg/trunk/deprecated/libTF/CMakeLists.txt
pkg/trunk/deprecated/plan_kinematic_path/CMakeLists.txt
pkg/trunk/drivers/cam/axis_cam/CMakeLists.txt
pkg/trunk/drivers/cam/bumblebee_bridge/CMakeLists.txt
pkg/trunk/drivers/cam/dc1394_cam/CMakeLists.txt
pkg/trunk/drivers/cam/dc1394_cam_server/CMakeLists.txt
pkg/trunk/drivers/cam/dcam/CMakeLists.txt
pkg/trunk/drivers/cam/videre_cam/CMakeLists.txt
pkg/trunk/drivers/imu/3dmgx2_driver/CMakeLists.txt
pkg/trunk/drivers/imu/imu_node/CMakeLists.txt
pkg/trunk/drivers/input/joy/CMakeLists.txt
pkg/trunk/drivers/input/joy_node/CMakeLists.txt
pkg/trunk/drivers/laser/hokuyo_driver/CMakeLists.txt
pkg/trunk/drivers/laser/hokuyo_node/CMakeLists.txt
pkg/trunk/drivers/laser/laser_scan_annotator/CMakeLists.txt
pkg/trunk/drivers/laser/laser_view/CMakeLists.txt
pkg/trunk/drivers/laser/sicktoolbox_wrapper/CMakeLists.txt
pkg/trunk/drivers/motor/ethercat_hardware/CMakeLists.txt
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/network/wifi_ddwrt/CMakeLists.txt
pkg/trunk/drivers/phase_space/CMakeLists.txt
pkg/trunk/drivers/robot/erratic_player/CMakeLists.txt
pkg/trunk/drivers/robot/irobot_create/CMakeLists.txt
pkg/trunk/drivers/robot/katana/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/pr2_phase_space/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/pr2_power_board/CMakeLists.txt
pkg/trunk/drivers/robot/segway_apox/CMakeLists.txt
pkg/trunk/drivers/robot/segway_apox/manifest.xml
pkg/trunk/drivers/robot/segway_apox/segway.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/estimation/robot_pose_ekf/CMakeLists.txt
pkg/trunk/grasp_module/grasp_learner/CMakeLists.txt
pkg/trunk/grasp_module/grasp_module/CMakeLists.txt
pkg/trunk/grasp_module/grasp_module_node/CMakeLists.txt
pkg/trunk/grasp_module/grasp_planner/CMakeLists.txt
pkg/trunk/grasp_module/object_detector/CMakeLists.txt
pkg/trunk/hardware_test/qualification/CMakeLists.txt
pkg/trunk/hardware_test/qualification/manifest.xml
pkg/trunk/hardware_test/runtime_monitor/CMakeLists.txt
pkg/trunk/hardware_test/self_test/CMakeLists.txt
pkg/trunk/highlevel/executive_python/CMakeLists.txt
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/CMakeLists.txt
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/Executive.hh
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/Executive.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/WatchDog.cc
pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
pkg/trunk/highlevel/highlevel_controllers/src/ControlCli.cpp
pkg/trunk/highlevel/highlevel_controllers/src/joy_batt_sender.cpp
pkg/trunk/highlevel/test_executive_trex_pr2/CMakeLists.txt
pkg/trunk/highlevel/test_highlevel_controllers/CMakeLists.txt
pkg/trunk/image_msgs/CMakeLists.txt
pkg/trunk/manip/arm_trajectory/arm_trajectory.cc
pkg/trunk/manip/spacenav_node/CMakeLists.txt
pkg/trunk/manip/teleop_arm_keyboard/CMakeLists.txt
pkg/trunk/mapping/cloud_geometry/CMakeLists.txt
pkg/trunk/mapping/cloud_io/CMakeLists.txt
pkg/trunk/mapping/cloud_kdtree/CMakeLists.txt
pkg/trunk/mapping/cloud_octree/CMakeLists.txt
pkg/trunk/mapping/collision_map/CMakeLists.txt
pkg/trunk/mapping/normal_estimation/CMakeLists.txt
pkg/trunk/mapping/planar_patch_map/CMakeLists.txt
pkg/trunk/mapping/point_cloud_downsampler/CMakeLists.txt
pkg/trunk/mapping/sample_consensus/CMakeLists.txt
pkg/trunk/mapping/semantic_point_annotator/CMakeLists.txt
pkg/trunk/mapping/stereo_convex_patch_histogram/CMakeLists.txt
pkg/trunk/mechanism/hardware_interface/CMakeLists.txt
pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt
pkg/trunk/motion_planning/mp_benchmarks/CMakeLists.txt
pkg/trunk/motion_planning/mpbench/CMakeLists.txt
pkg/trunk/motion_planning/mpglue/CMakeLists.txt
pkg/trunk/motion_planning/navfn/CMakeLists.txt
pkg/trunk/motion_planning/ompl/CMakeLists.txt
pkg/trunk/motion_planning/planning_research/dynamic_planning/CMakeLists.txt
pkg/trunk/motion_planning/planning_research/robarm3d/CMakeLists.txt
pkg/trunk/motion_planning/sbpl/CMakeLists.txt
pkg/trunk/motion_planning/sbpl_util/CMakeLists.txt
pkg/trunk/motion_planning/test_collision_space/CMakeLists.txt
pkg/trunk/motion_planning/test_collision_space/src/test_cs.cpp
pkg/trunk/nav/amcl_player/CMakeLists.txt
pkg/trunk/nav/deadreckon/CMakeLists.txt
pkg/trunk/nav/fake_localization/CMakeLists.txt
pkg/trunk/nav/nav_view/CMakeLists.txt
pkg/trunk/nav/nav_view/manifest.xml
pkg/trunk/nav/nav_view_sdl/CMakeLists.txt
pkg/trunk/nav/sbpl_2dnav/CMakeLists.txt
pkg/trunk/nav/slam_gmapping/CMakeLists.txt
pkg/trunk/nav/teleop_base/CMakeLists.txt
pkg/trunk/nav/teleop_base_keyboard/CMakeLists.txt
pkg/trunk/nav/wavefront_player/CMakeLists.txt
pkg/trunk/nav/wavefront_player/cli.cpp
pkg/trunk/openrave_planning/openraveros/CMakeLists.txt
pkg/trunk/openrave_planning/orcollision/CMakeLists.txt
pkg/trunk/openrave_planning/orplugins/CMakeLists.txt
pkg/trunk/openrave_planning/orrosplanning/CMakeLists.txt
pkg/trunk/pr2_msgs/CMakeLists.txt
pkg/trunk/pr2_srvs/CMakeLists.txt
pkg/trunk/robot_control_loops/pr2_etherCAT/CMakeLists.txt
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt
pkg/trunk/robot_msgs/CMakeLists.txt
pkg/trunk/robot_srvs/CMakeLists.txt
pkg/trunk/sandbox/axis_cam2/CMakeLists.txt
pkg/trunk/simulators/gazebo_map_extruder/CMakeLists.txt
pkg/trunk/simulators/nepumuk/Makefile
pkg/trunk/simulators/rosstage/CMakeLists.txt
pkg/trunk/trajectory_rollout/CMakeLists.txt
pkg/trunk/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/trajectory_rollout/manifest.xml
pkg/trunk/util/angles/CMakeLists.txt
pkg/trunk/util/filter_coefficient_server/CMakeLists.txt
pkg/trunk/util/filter_demo/CMakeLists.txt
pkg/trunk/util/filters/CMakeLists.txt
pkg/trunk/util/kinematics/libKinematics/CMakeLists.txt
pkg/trunk/util/kinematics/robot_kinematics/CMakeLists.txt
pkg/trunk/util/laser_scan/CMakeLists.txt
pkg/trunk/util/logsetta/CMakeLists.txt
pkg/trunk/util/math_expr/CMakeLists.txt
pkg/trunk/util/message_sequencing/CMakeLists.txt
pkg/trunk/util/message_sequencing/example/testdelay.cpp
pkg/trunk/util/misc_utils/CMakeLists.txt
pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
pkg/trunk/util/misc_utils/manifest.xml
pkg/trunk/util/mux/CMakeLists.txt
pkg/trunk/util/point_cloud_utils/CMakeLists.txt
pkg/trunk/util/point_cloud_utils/include/point_cloud_utils/timed_scan_assembler.h
pkg/trunk/util/profiling_utils/CMakeLists.txt
pkg/trunk/util/random_utils/CMakeLists.txt
pkg/trunk/util/resource_locator/CMakeLists.txt
pkg/trunk/util/robot_self_filter/CMakeLists.txt
pkg/trunk/util/robot_self_filter/manifest.xml
pkg/trunk/util/stl_utils/CMakeLists.txt
pkg/trunk/util/string_utils/CMakeLists.txt
pkg/trunk/util/tf/CMakeLists.txt
pkg/trunk/util/trajectory/CMakeLists.txt
pkg/trunk/vision/borg/CMakeLists.txt
pkg/trunk/vision/calib_converter/CMakeLists.txt
pkg/trunk/vision/calonder_descriptor/CMakeLists.txt
pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/CMakeLists.txt
pkg/trunk/vision/checkerboard_detector/CMakeLists.txt
pkg/trunk/vision/color_calib/CMakeLists.txt
pkg/trunk/vision/cv_cam_calib/CMakeLists.txt
pkg/trunk/vision/cv_view/CMakeLists.txt
pkg/trunk/vision/cv_view/src/cv_view.cpp
pkg/trunk/vision/cv_view/src/cv_view_array.cpp
pkg/trunk/vision/cv_view/src/cv_view_array_overlay.cpp
pkg/trunk/vision/dorylus/CMakeLists.txt
pkg/trunk/vision/gmmseg/CMakeLists.txt
pkg/trunk/vision/laser_processor/CMakeLists.txt
pkg/trunk/vision/laser_processor/laser_processor.cpp
pkg/trunk/vision/mech_turk/CMakeLists.txt
pkg/trunk/vision/mrf_image_classifier/CMakeLists.txt
pkg/trunk/vision/outlet_detection/CMakeLists.txt
pkg/trunk/vision/people/CMakeLists.txt
pkg/trunk/vision/people/src/calc_leg_features.cpp
pkg/trunk/vision/people/src/leg_detector.cpp
pkg/trunk/vision/place_recognition/CMakeLists.txt
pkg/trunk/vision/render/CMakeLists.txt
pkg/trunk/vision/scan_utils/CMakeLists.txt
pkg/trunk/vision/spacetime_stereo/CMakeLists.txt
pkg/trunk/vision/star_detector/CMakeLists.txt
pkg/trunk/vision/stereo_blob_tracker/CMakeLists.txt
pkg/trunk/vision/stereo_calib/CMakeLists.txt
pkg/trunk/vision/stereo_capture/CMakeLists.txt
pkg/trunk/vision/stereo_image_proc/CMakeLists.txt
pkg/trunk/vision/stereo_utils/CMakeLists.txt
pkg/trunk/vision/stereo_view/CMakeLists.txt
pkg/trunk/vision/vision_utils/CMakeLists.txt
pkg/trunk/vision/visual_odometry/CMakeLists.txt
pkg/trunk/vision/vslam/CMakeLists.txt
pkg/trunk/visualization/log_gui/CMakeLists.txt
pkg/trunk/visualization/ogre_tools/CMakeLists.txt
pkg/trunk/visualization/ogre_tools/manifest.xml
pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt
pkg/trunk/visualization/ogre_visualizer/manifest.xml
pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp
pkg/trunk/visualization/scene_labeler/CMakeLists.txt
pkg/trunk/visualization/wx_camera_panel/CMakeLists.txt
pkg/trunk/visualization/wx_rosout/CMakeLists.txt
pkg/trunk/visualization/wx_topic_display/CMakeLists.txt
pkg/trunk/world_models/costmap_2d/CMakeLists.txt
pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/fake_world_3d_map/CMakeLists.txt
pkg/trunk/world_models/map_saver/CMakeLists.txt
pkg/trunk/world_models/map_server/CMakeLists.txt
pkg/trunk/world_models/map_server/src/main.cpp
pkg/trunk/world_models/map_server/test/rtest.cpp
pkg/trunk/world_models/ransac_ground_plane_extraction/CMakeLists.txt
pkg/trunk/world_models/robot_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
pkg/trunk/world_models/robot_models/robot_filter/CMakeLists.txt
pkg/trunk/world_models/robot_models/robot_model/CMakeLists.txt
pkg/trunk/world_models/static_map_server/CMakeLists.txt
pkg/trunk/world_models/topological_map/CMakeLists.txt
pkg/trunk/world_models/voxel_grid/CMakeLists.txt
pkg/trunk/world_models/world_3d_map/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/3rdparty/opencv_latest/cmake_2.4.6.patch
pkg/trunk/3rdparty/player/cmake-2.4-ros.patch
pkg/trunk/util/realtime_thread/
pkg/trunk/util/realtime_thread/CMakeLists.txt
pkg/trunk/util/realtime_thread/Makefile
pkg/trunk/util/realtime_thread/include/
pkg/trunk/util/realtime_thread/include/asm/
pkg/trunk/util/realtime_thread/include/asm/arith.h
pkg/trunk/util/realtime_thread/include/asm/arith_32.h
pkg/trunk/util/realtime_thread/include/asm/arith_64.h
pkg/trunk/util/realtime_thread/include/asm/atomic.h
pkg/trunk/util/realtime_thread/include/asm/atomic_32.h
pkg/trunk/util/realtime_thread/include/asm/atomic_64.h
pkg/trunk/util/realtime_thread/include/asm/bits/
pkg/trunk/util/realtime_thread/include/asm/bits/heap.h
pkg/trunk/util/realtime_thread/include/asm/bits/init.h
pkg/trunk/util/realtime_thread/include/asm/bits/init_32.h
pkg/trunk/util/realtime_thread/include/asm/bits/init_64.h
pkg/trunk/util/realtime_thread/include/asm/bits/intr.h
pkg/trunk/util/realtime_thread/include/asm/bits/pod.h
pkg/trunk/util/realtime_thread/include/asm/bits/pod_32.h
pkg/trunk/util/realtime_thread/include/asm/bits/pod_64.h
pkg/trunk/util/realtime_thread/include/asm/bits/shadow.h
pkg/trunk/util/realtime_thread/include/asm/bits/shadow_32.h
pkg/trunk/util/realtime_thread/include/asm/bits/shadow_64.h
pkg/trunk/util/realtime_thread/include/asm/bits/thread.h
pkg/trunk/util/realtime_thread/include/asm/bits/thread_32.h
pkg/trunk/util/realtime_thread/include/asm/bits/thread_64.h
pkg/trunk/util/realtime_thread/include/asm/bits/timer.h
pkg/trunk/util/realtime_thread/include/asm/calibration.h
pkg/trunk/util/realtime_thread/include/asm/features.h
pkg/trunk/util/realtime_thread/include/asm/features_32.h
pkg/trunk/util/realtime_thread/include/asm/features_64.h
pkg/trunk/util/realtime_thread/include/asm/fptest.h
pkg/trunk/util/realtime_thread/include/asm/hal.h
pkg/trunk/util/realtime_thread/include/asm/hal_32.h
pkg/trunk/util/realtime_thread/include/asm/hal_64.h
pkg/trunk/util/realtime_thread/include/asm/smi.h
pkg/trunk/util/realtime_thread/include/asm/switch.h
pkg/trunk/util/realtime_thread/include/asm/switch_32.h
pkg/trunk/util/realtime_thread/include/asm/switch_64.h
pkg/trunk/util/realtime_thread/include/asm/syscall.h
pkg/trunk/util/realtime_thread/include/asm/syscall_32.h
pkg/trunk/util/realtime_thread/include/asm/syscall_64.h
pkg/trunk/util/realtime_thread/include/asm/system.h
pkg/trunk/util/realtime_thread/include/asm/system_32.h
pkg/trunk/util/realtime_thread/include/asm/system_64.h
pkg/trunk/util/realtime_thread/include/asm/wrappers.h
pkg/trunk/util/realtime_thread/include/asm/wrappers_32.h
pkg/trunk/util/realtime_thread/include/asm/wrappers_64.h
pkg/trunk/util/realtime_thread/include/asm/xenomai
pkg/trunk/util/realtime_thread/include/asm-generic/
pkg/trunk/util/realtime_thread/include/asm-generic/arith.h
pkg/trunk/util/realtime_thread/include/asm-generic/bits/
pkg/trunk/util/realtime_thread/include/asm-generic/bits/bind.h
pkg/trunk/util/realtime_thread/include/asm-generic/bits/heap.h
pkg/trunk/util/realtime_thread/include/asm-generic/bits/intr.h
pkg/trunk/util/realtime_thread/include/asm-generic/bits/mlock_alert.h
pkg/trunk/util/realtime_thread/include/asm-generic/bits/pod.h
pkg/trunk/util/realtime_thread/include/asm-generic/features.h
pkg/trunk/util/realtime_thread/include/asm-generic/hal.h
pkg/trunk/util/realtime_thread/include/asm-generic/syscall.h
pkg/trunk/util/realtime_thread/include/asm-generic/system.h
pkg/trunk/util/realtime_thread/include/asm-generic/wrappers.h
pkg/trunk/util/realtime_thread/include/asm-generic/xenomai
pkg/trunk/util/realtime_thread/include/native/
pkg/trunk/util/realtime_thread/include/native/alarm.h
pkg/trunk/util/realtime_thread/include/native/cond.h
pkg/trunk/util/realtime_thread/include/native/event.h
pkg/trunk/util/realtime_thread/include/native/heap.h
pkg/trunk/util/realtime_thread/include/native/intr.h
pkg/trunk/util/realtime_thread/include/native/misc.h
pkg/trunk/util/realtime_thread/include/native/mutex.h
pkg/trunk/util/realtime_thread/include/native/pipe.h
pkg/trunk/util/realtime_thread/include/native/ppd.h
pkg/trunk/util/realtime_thread/include/native/queue.h
pkg/trunk/util/realtime_thread/include/native/sem.h
pkg/trunk/util/realtime_thread/include/native/syscall.h
pkg/trunk/util/realtime_thread/include/native/task.h
pkg/trunk/util/realtime_thread/include/native/timer.h
pkg/trunk/util/realtime_thread/include/native/types.h
pkg/trunk/util/realtime_thread/include/nucleus/
pkg/trunk/util/realtime_thread/include/nucleus/assert.h
pkg/trunk/util/realtime_thread/include/nucleus/bheap.h
pkg/trunk/util/realtime_thread/include/nucleus/compiler.h
pkg/trunk/util/realtime_thread/include/nucleus/core.h
pkg/trunk/util/realtime_thread/include/nucleus/heap.h
pkg/trunk/util/realtime_thread/include/nucleus/intr.h
pkg/trunk/util/realtime_thread/include/nucleus/jhash.h
pkg/trunk/util/realtime_thread/include/nucleus/map.h
pkg/trunk/util/realtime_thread/include/nucleus/module.h
pkg/trunk/util/realtime_thread/include/nucleus/pipe.h
pkg/trunk/util/realtime_thread/include/nucleus/pod.h
pkg/trunk/util/realtime_thread/include/nucleus/ppd.h
pkg/trunk/util/realtime_thread/include/nucleus/queue.h
pkg/trunk/util/realtime_thread/include/nucleus/registry.h
pkg/trunk/util/realtime_thread/include/nucleus/select.h
pkg/trunk/util/realtime_thread/include/nucleus/shadow.h
pkg/trunk/util/realtime_thread/include/nucleus/stat.h
pkg/trunk/util/realtime_thread/include/nucleus/synch.h
pkg/trunk/util/realtime_thread/include/nucleus/system.h
pkg/trunk/util/realtime_thread/include/nucleus/thread.h
pkg/trunk/util/realtime_thread/include/nucleus/timebase.h
pkg/trunk/util/realtime_thread/include/nucleus/timer.h
pkg/trunk/util/realtime_thread/include/nucleus/trace.h
pkg/trunk/util/realtime_thread/include/nucleus/types.h
pkg/trunk/util/realtime_thread/include/nucleus/version.h
pkg/trunk/util/realtime_thread/include/nucleus/xenomai.h
pkg/trunk/util/realtime_thread/include/realtime_thread/
pkg/trunk/util/realtime_thread/include/realtime_thread/realtime_thread.h
pkg/trunk/util/realtime_thread/include/xeno_config.h
pkg/trunk/util/realtime_thread/manifest.xml
pkg/trunk/util/realtime_thread/src/
pkg/trunk/util/realtime_thread/src/realtime_thread.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:6549
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:9316
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/3rdparty/ANN/Makefile
===================================================================
--- pkg/trunk/3rdparty/ANN/Makefile 2009-01-13 07:07:00 UTC (rev 9323)
+++ pkg/trunk/3rdparty/ANN/Makefile 2009-01-13 07:24:01 UTC (rev 9324)
@@ -1,4 +1,4 @@
-all: ann
+all: installed
TARBALL = build/ann_1.1.1.tar.gz
TARBALL_URL = http://www.cs.umd.edu/~mount/ANN/Files/1.1.1/ann_1.1.1.tar.gz
@@ -8,8 +8,9 @@
include $(shell rospack find mk)/download_unpack_build.mk
-ann: $(SOURCE_DIR)
+installed: $(SOURCE_DIR) Makefile
echo "Building FL-ANN..."
cd build/ann_1.1.1 && make linux-shared-g++
+ touch installed
wipe:
- rm -rf build
+ rm -rf build installed
Modified: pkg/trunk/3rdparty/FL-ANN/Makefile
===================================================================
--- pkg/trunk/3rdparty/FL-ANN/Makefile 2009-01-13 07:07:00 UTC (rev 9323)
+++ pkg/trunk/3rdparty/FL-ANN/Makefile 2009-01-13 07:24:01 UTC (rev 9324)
@@ -1,4 +1,4 @@
-all: fl-ann
+all: installed
TARBALL = build/flann-1.11-src.zip
TARBALL_URL = http://pr.willowgarage.com/downloads/flann-1.11-src.zip
@@ -8,9 +8,14 @@
include $(shell rospack find mk)/download_unpack_build.mk
-fl-ann: $(SOURCE_DIR)
+installed: wiped $(SOURCE_DIR)
echo "Building FL-ANN..."
mkdir -p build/build && cd build/build && cmake ../flann-1.11-src/src && make
+ touch installed
+wiped: Makefile
+ make wipe
+ touch wiped
+
wipe:
- rm -rf build
+ rm -rf build installed
Modified: pkg/trunk/3rdparty/bfl/Makefile
===================================================================
--- pkg/trunk/3rdparty/bfl/Makefile 2009-01-13 07:07:00 UTC (rev 9323)
+++ pkg/trunk/3rdparty/bfl/Makefile 2009-01-13 07:24:01 UTC (rev 9324)
@@ -1,36 +1,26 @@
-.PHONY: $(SVN_DIR) bfl
+all: installed
-all: bfl
-
SVN_REVISION= -r 29808
SVN_DIR = bfl-svn
SVN_URL = http://svn.mech.kuleuven.be:/repos/orocos/trunk/bfl
INSTALL_DIR = bfl-boost
-CMAKE = `rospack find cmake`/cmake/bin/cmake
+CMAKE = cmake
BOOST_ROOT =$(shell rospack find boost)/boost
CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=$(PWD)/$(INSTALL_DIR)/ \
-DMATRIX_LIB=boost -DRNG_LIB=boost -DBOOST_FOUND=`rospack find boost`/boost/include/
include $(shell rospack find mk)/svn_checkout.mk
-
-
-build: wiped bfl
-
-
-configured: SVN_UP Makefile
+installed: $(SVN_DIR) patched
mkdir -p $(SVN_DIR)/build-boost
cd $(SVN_DIR)/build-boost && $(CMAKE) $(CMAKE_ARGS) ..
- touch configured
-
-
-bfl: configured
cd $(SVN_DIR)/build-boost && make $(PARALLEL_JOBS) && make install
+ touch installed
clean:
rm -rf $(INSTALL_DIR)
rm -rf $(SVN_DIR)
- rm -rf configured
+ rm -rf installed
#test:
-# cd $(SVN_DIR)/build-boost && make check
\ No newline at end of file
+# cd $(SVN_DIR)/build-boost && make check
Modified: pkg/trunk/3rdparty/bullet/Makefile
===================================================================
--- pkg/trunk/3rdparty/bullet/Makefile 2009-01-13 07:07:00 UTC (rev 9323)
+++ pkg/trunk/3rdparty/bullet/Makefile 2009-01-13 07:24:01 UTC (rev 9324)
@@ -1,4 +1,4 @@
-all: build
+all: installed
SVN_DIR = bullet_svn
SVN_URL = http://bullet.googlecode.com/svn/trunk/
@@ -10,15 +10,42 @@
.PHONY: build
-build: SVN_UP_REVERT_PATCH
- cd $(SVN_DIR) && $(shell rospack find cmake)/cmake/bin/cmake -DCMAKE_INSTALL_PREFIX=$(PWD) .
- cd $(SVN_DIR) && make $(PARALLEL_JOBS) && make $(PARALLEL_JOBS) install
- touch bullet
+# Poor man's installation procedure setup
+BULLET_LIBS = $(SVN_DIR)/src/BulletCollision/libBulletCollision.a \
+ $(SVN_DIR)/src/BulletDynamics/libBulletDynamics.a \
+ $(SVN_DIR)/src/BulletSoftBody/libBulletSoftBody.a \
+ $(SVN_DIR)/src/LinearMath/libLinearMath.a
+BULLET_INC_DIRS = BulletSoftBody \
+ BulletCollision/CollisionShapes \
+ BulletCollision/BroadphaseCollision \
+ BulletCollision/NarrowPhaseCollision \
+ BulletCollision/CollisionDispatch \
+ BulletCollision/Gimpact \
+ BulletDynamics/ConstraintSolver \
+ BulletDynamics/Vehicle \
+ BulletDynamics/Dynamics \
+ BulletDynamics/Character \
+ LinearMath \
+
+installed: $(SVN_DIR) patched
+ cd $(SVN_DIR) && cmake -DCMAKE_INSTALL_PREFIX=$(PWD) .
+ # Bullet appears not be parallel-make safe
+ #cd $(SVN_DIR) && make $(PARALLEL_JOBS)
+ cd $(SVN_DIR) && make
+ # The 'install' target only works with cmake 2.6 for some reason
+ #cd $(SVN_DIR) && make $(PARALLEL_JOBS) install
+ mkdir -p lib
+ cp $(BULLET_LIBS) lib
+ mkdir -p include
+ $(foreach d,$(BULLET_INC_DIRS), mkdir -p include/$(d) && cp $(SVN_DIR)/src/$(d)/*.h include/$(d);)
+ touch installed
+
clean:
+ rm -f installed
-make -C $(SVN_DIR) clean
-wipe:
- -rm -f bullet patched
+wipe: clean
+ -rm -f patched
-rm -rf bullet_svn
-rm -rf build
Modified: pkg/trunk/3rdparty/convex_decomposition/Makefile
===================================================================
--- pkg/trunk/3rdparty/convex_decomposition/Makefile 2009-01-13 07:07:00 UTC (rev 9323)
+++ pkg/trunk/3rdparty/convex_decomposition/Makefile 2009-01-13 07:24:01 UTC (rev 9324)
@@ -1,31 +1,33 @@
-all: convex_decomposition
+all: installed
TARBALL = build/ConvexDecomposition.zip
TARBALL_URL = http://www.amillionpixels.us/ConvexDecomposition.zip
SOURCE_DIR = build/convex_decomposition
INITIAL_DIR = build/ConvexDecomposition
UNPACK_CMD = unzip
+TARBALL_PATCH=convex_decomposition.patch
include $(shell rospack find mk)/download_unpack_build.mk
-PATCH=convex_decomposition.patch
ROOT = $(shell rospack find convex_decomposition)/convex_decomposition
-convex_decomposition: $(SOURCE_DIR)
+installed: wiped $(SOURCE_DIR)
@echo "making it"
@echo "ROOT is: $(ROOT)"
-mkdir -p $(ROOT)
-mkdir -p $(ROOT)/bin
- -cd $(SOURCE_DIR) && rm -f Makefile #remove if exist, to avoid appending Makefile twice
- cd $(SOURCE_DIR) ; patch -p0 -N < ../../convex_decomposition.patch || echo
cd $(SOURCE_DIR) ; make $(PARALLEL_JOBS); make install
+ touch installed
+wiped: Makefile
+ make wipe
+ touch wiped
+
clean:
-cd $(SOURCE_DIR) && make clean
- rm -rf $(ROOT)
+ rm -rf $(ROOT) installed
-wipe:
+wipe: clean
rm -rf build
- rm -rf $(ROOT)
.PHONY : clean download wipe
Modified: pkg/trunk/3rdparty/eigen/Makefile
===================================================================
--- pkg/trunk/3rdparty/eigen/Makefile 2009-01-13 07:07:00 UTC (rev 9323)
+++ pkg/trunk/3rdparty/eigen/Makefile 2009-01-13 07:24:01 UTC (rev 9324)
@@ -8,3 +8,9 @@
include $(shell rospack find mk)/download_unpack_build.mk
eigen: $(SOURCE_DIR)
+
+clean:
+ rm -rf $(SOURCE_DIR)
+
+wipe: clean
+ rm -rf build
\ No newline at end of file
Modified: pkg/trunk/3rdparty/eml/Makefile
===================================================================
--- pkg/trunk/3rdparty/eml/Makefile 2009-01-13 07:07:00 UTC (rev 9323)
+++ pkg/trunk/3rdparty/eml/Makefile 2009-01-13 07:24:01 UTC (rev 9324)
@@ -1,4 +1,4 @@
-all: build
+all: installed
TARBALL = build/eml-r36-patched.tar.bz2
TARBALL_URL = http://pr.willowgarage.com/downloads/eml-r36-patched.tar.bz2
@@ -6,26 +6,21 @@
UNPACK_CMD = tar xjf
include $(shell rospack find mk)/download_unpack_build.mk
-build: wiped eml
-
-configured: $(SOURCE_DIR) Makefile
+installed: wiped $(SOURCE_DIR) Makefile
cd $(SOURCE_DIR) && \
mkdir -p build && \
cd build && \
- `rospack find cmake`/cmake/bin/cmake -D RTNET_INSTALL_PATH=`rospack find rtnet`/rtnet -D XENOMAI_INSTALL_PATH=`rospack find xenomai`/xenomai -D CMAKE_INSTALL_PREFIX=`rospack find eml`/eml .. && \
- touch configured
+ cmake -D RTNET_INSTALL_PATH=`rospack find rtnet`/rtnet -D XENOMAI_INSTALL_PATH=`rospack find xenomai`/xenomai -D CMAKE_INSTALL_PREFIX=`rospack find eml`/eml .. && \
+ make install
+ touch installed
wiped: Makefile
make wipe
touch wiped
-.PHONY: eml
-eml: configured
- make -C $(SOURCE_DIR)/build install
-
clean:
-make -C $(SOURCE_DIR)/build clean
- rm -rf eml configured
+ rm -rf eml installed
wipe: clean
rm -rf build
Modified: pkg/trunk/3rdparty/estar/Makefile
===================================================================
--- pkg/trunk/3rdparty/estar/Makefile 2009-01-13 07:07:00 UTC (rev 9323)
+++ pkg/trunk/3rdparty/estar/Makefile 2009-01-13 07:24:01 UTC (rev 9324)
@@ -14,7 +14,8 @@
# $ make svn-all
##all: tarball-all
-all: svn-all
+#all: svn-all
+all: installed
VERSION= r267
@@ -27,11 +28,20 @@
BOOST_DIR= `rospack cflags-only-I boost`
SVN_DIR= $(PWD)/build/estar-svn
-SVN_REV= HEAD
+SVN_REVISION= -r 276
SVN_URL= https://estar.svn.sourceforge.net/svnroot/estar/trunk/estar
-include $(shell rospack find mk)/download_unpack_build.mk
+#include $(shell rospack find mk)/download_unpack_build.mk
+include $(shell rospack find mk)/svn_checkout.mk
+installed: $(SVN_DIR) patched
+ cd $(SVN_DIR) && ./bootstrap-buildsystem.sh
+ test -d $(BUILD_DIR) || mkdir -p $(BUILD_DIR)
+ cd $(BUILD_DIR) && $(SVN_DIR)/configur...
[truncated message content] |
Thanks for helping keep SourceForge clean.
X