Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8328] pkg/trunk
|
From: <rdi...@us...> - 2008-12-18 16:56:55
|
Revision: 8328
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8328&view=rev
Author: rdiankov
Date: 2008-12-18 16:56:50 +0000 (Thu, 18 Dec 2008)
Log Message:
-----------
openrave interface to process phasespace system messages
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
pkg/trunk/openrave_planning/ormanipulation/data/willow_table.kinbody.xml
pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
Added Paths:
-----------
pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
pkg/trunk/openrave_planning/orrosplanning/src/rosplanningproblem.h
pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-18 16:56:50 UTC (rev 8328)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 562
+SVN_REVISION = -r 573
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
@@ -11,7 +11,7 @@
build: SVN_UP openrave
openrave: SVN_UP
- @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && export BOOST_ROOT=$(shell rospack find boost)/boost && svn up && mkdir -p build && cd build && $(shell rospack find cmake)/cmake/bin/cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=Release .. && make install
+ @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && export BOOST_ROOT=$(shell rospack find boost)/boost && mkdir -p build && cd build && $(shell rospack find cmake)/cmake/bin/cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=Release .. && make install
clean:
make -C $(SVN_DIR) clean
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-18 16:56:50 UTC (rev 8328)
@@ -14,9 +14,8 @@
% move. This is useful when visualizing internal C++ states. When off, the GUI
% will only reflect the state of robots after all calls to stepsimulation and
% server send messages have been done. The default is off.
-% - debug [debug level] - toggles debugging messages by RAVELOG.
-% 0 - only RAVEPRINT statements show
-% 1+ - RAVELOG statements with various debug levels show
+% - debug [debuglevel] - toggles debugging messages by RAVELOG_X. Can be one of
+%% fatal, error, warn, info, debug, verbose
% - quit - closes the openrave instance
function success = orEnvSetOptions(options)
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-18 16:56:50 UTC (rev 8328)
@@ -1005,6 +1005,7 @@
mlevels["info"] = Level_Info;
mlevels["warn"] = Level_Warn;
mlevels["debug"] = Level_Debug;
+ mlevels["verbose"] = Level_Verbose;
DebugLevel level = GetEnv()->GetDebugLevel();
if( mlevels.find(req.debuglevel) != mlevels.end() )
level = mlevels[req.debuglevel];
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2008-12-18 16:56:50 UTC (rev 8328)
@@ -4,7 +4,7 @@
<camrotaxis>-0.092050 -0.490650 -0.866481 211</camrotaxis>
<Robot file="robots/pr2full.robot.xml">
- <translation>-0.708 0.005 0.012</translation>
+ <translation>-0.708 0.005 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.000000 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>
</Robot>
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2008-12-18 16:56:50 UTC (rev 8328)
@@ -1,12 +1,17 @@
<Environment>
- <bkgndcol>0.3 0.7 0.8</bkgndcol>
- <camtrans>0.792115 1.544696 2.088982</camtrans>
- <camrotaxis>-0.092050 -0.490650 -0.866481 211</camrotaxis>
+ <bkgndcolor>1 1 1</bkgndcolor>
+ <camtrans> -0.285786 -4.231486 0.296317</camtrans>
+ <camrotaxis>-0.998283 0.046406 0.035754 265</camrotaxis>
+
+<!-- <camtrans>0.792115 1.544696 2.088982</camtrans>
+ <camrotaxis>-0.092050 -0.490650 -0.866481 211</camrotaxis> -->
<Robot file="robots/pr2full.robot.xml">
<translation>-0.708 0.005 0.0</translation>
</Robot>
-
+
+ <Kinbody file="willow_table.kinbody.xml"/>
+
<Kinbody name="floor">
<Body>
<Geom type="box">
Modified: pkg/trunk/openrave_planning/ormanipulation/data/willow_table.kinbody.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/willow_table.kinbody.xml 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/openrave_planning/ormanipulation/data/willow_table.kinbody.xml 2008-12-18 16:56:50 UTC (rev 8328)
@@ -1,5 +1,5 @@
<KinBody name="willow_table">
- <Body>
+ <Body name="base">
<Geom type="box">
<extents>0.38 0.62 0.035</extents>
<translation>0 0 0.711</translation>
@@ -13,4 +13,10 @@
<translation>0 0.555 0.35</translation>
</Geom>
</Body>
+ <phasespace>
+ <offsetlink>base</offsetlink>
+ <id>1</id>
+ <pretranslation>0.27 0.3425 -0.75</pretranslation>
+ <prerotationaxis>0 0 1 -90</prerotationaxis>
+ </phasespace>
</KinBody>
Modified: pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml 2008-12-18 16:56:50 UTC (rev 8328)
@@ -1,6 +1,6 @@
<launch>
<!-- start openraveros server and set all the correct paths -->
- <node pkg="openraveros" type="openraveros">
+ <node 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)"/>
</node>
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2008-12-18 16:56:50 UTC (rev 8328)
@@ -6,6 +6,7 @@
<license>BSD</license>
<depend package="roscpp"/>
<depend package="openrave"/>
+ <depend package="phase_space"/>
<depend package="openrave_robot_description"/>
<depend package="libKinematics"/>
<depend package="boost"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2008-12-18 16:56:50 UTC (rev 8328)
@@ -26,6 +26,8 @@
#include "plugindefs.h"
#include "rosarmik.h"
+#include "phasespacesystem.h"
+#include "rosplanningproblem.h"
// declaring variables with stdcall can be a little complex
#ifdef _MSC_VER
@@ -53,7 +55,12 @@
if( wcsicmp(name, L"ROSArmIK") == 0 )
return new ROSArmIK(penv);
break;
-
+ case PT_ProblemInstance:
+ if( wcsicmp(name, L"ROSPlanningProblem") == 0 )
+ return new ROSPlanningProblem(penv);
+ case PT_SensorSystem:
+ if( wcsicmp(name, L"PhaseSpace") == 0 )
+ return new PhaseSpaceMocapClient(penv);
default:
break;
}
@@ -70,6 +77,9 @@
}
pinfo->iksolvers.push_back(L"ROSArmIK");
+ pinfo->sensorsystems.push_back(L"PhaseSpace");
+ pinfo->problems.push_back(L"ROSPlanningProblem");
+
return true;
}
Added: pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h (rev 0)
+++ pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h 2008-12-18 16:56:50 UTC (rev 8328)
@@ -0,0 +1,177 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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 PHASESPACE_MOCAP_SYSTEM
+#define PHASESPACE_MOCAP_SYSTEM
+
+#include "simplesensorsystem.h"
+#include "phase_space/PhaseSpaceSnapshot.h"
+
+using namespace ros;
+
+// used to update objects through a mocap system
+class PhaseSpaceMocapClient : public SimpleSensorSystem
+{
+public:
+ PhaseSpaceMocapClient(EnvironmentBase* penv) : SimpleSensorSystem(penv, "phasespace"), _bSubscribed(false)
+ {
+ RegisterXMLReader(GetEnv()); // just in case, register again
+ }
+ virtual ~PhaseSpaceMocapClient() {
+ Destroy();
+ }
+
+ virtual bool Init(istream& sinput)
+ {
+ if( !SimpleSensorSystem::Init(sinput) )
+ return false;
+
+ _phasespacetopic = "phase_space_snapshot";
+ sinput >> _phasespacetopic;
+ startsubscriptions();
+ return _bSubscribed;
+ }
+
+ virtual void Destroy()
+ {
+ stopsubscriptions();
+ SimpleSensorSystem::Destroy();
+ }
+
+ static void RegisterXMLReader(EnvironmentBase* penv)
+ {
+ if( penv != NULL )
+ penv->RegisterXMLReader("phasespace", PhaseSpaceMocapClient::CreateMocapReader);
+ }
+
+ static BaseXMLReader* CreateMocapReader(KinBody* parent, const char **atts)
+ {
+ return new MocapXMLReader("phasespace", NULL, atts);
+ }
+
+private:
+ node* check_roscpp()
+ {
+ // start roscpp
+ ros::node* pnode = ros::node::instance();
+
+ if( pnode && !pnode->checkMaster() ) {
+ ros::fini();
+ delete pnode;
+ return NULL;
+ }
+
+ if (!pnode) {
+ int argc = 0;
+ char strname[256] = "nohost";
+ gethostname(strname, sizeof(strname));
+ strcat(strname,"_rosoct");
+
+ ros::init(argc,NULL);
+
+ pnode = new ros::node(strname, ros::node::DONT_HANDLE_SIGINT|ros::node::ANONYMOUS_NAME|ros::node::DONT_ADD_ROSOUT_APPENDER);
+
+ bool bCheckMaster = pnode->checkMaster();
+ ros::fini();
+ delete pnode;
+
+ if( !bCheckMaster ) {
+ RAVELOG_ERRORA("ros not present");
+ return NULL;
+ }
+
+ ros::init(argc,NULL);
+ pnode = new ros::node(strname, ros::node::DONT_HANDLE_SIGINT|ros::node::ANONYMOUS_NAME);
+ RAVELOG_DEBUGA("new roscpp node started");
+ }
+
+ return pnode;
+ }
+
+ void startsubscriptions()
+ {
+ // check if thread launched
+ _bSubscribed = false;
+ ros::node* pnode = check_roscpp();
+ if( pnode != NULL ) {
+ _bSubscribed = pnode->subscribe(_phasespacetopic, _snapshot, &PhaseSpaceMocapClient::newdatacb, this, 1);
+ if( _bSubscribed )
+ RAVELOG_DEBUGA("subscribed to %s\n", _phasespacetopic.c_str());
+ else
+ RAVELOG_ERRORA("failed to subscribe to %s\n", _phasespacetopic.c_str());
+ }
+ }
+
+ void stopsubscriptions()
+ {
+ if( _bSubscribed ) {
+ ros::node* pnode = ros::node::instance();
+ if( pnode && pnode->checkMaster() ) {
+ pnode->unsubscribe(_phasespacetopic.c_str());
+ RAVELOG_DEBUGA("unsubscribe from %s\n", _phasespacetopic.c_str());
+ }
+ _bSubscribed = false;
+ }
+ }
+
+ void newdatacb()
+ {
+ list< SNAPSHOT > listbodies;
+ boost::mutex::scoped_lock lock(_mutex);
+ RAVELOG_VERBOSEA("cb\n");
+
+ for (unsigned int i=0; i<_snapshot.get_bodies_size(); i++) {
+ const phase_space::PhaseSpaceBody& psbody = _snapshot.bodies[i];
+
+ boost::shared_ptr<BODY> b;
+ FOREACH(it, _mapbodies) {
+ if( it->second->_initdata.id == psbody.id ) {
+ b = it->second;
+ break;
+ }
+ }
+
+ if( !b || !b->IsEnabled() )
+ continue;
+
+ listbodies.push_back(SNAPSHOT(b, GetTransform(psbody.pose)));
+ }
+
+ UpdateBodies(listbodies);
+
+ // try to add the left-over objects
+ }
+
+ 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));
+ }
+
+ phase_space::PhaseSpaceSnapshot _snapshot;
+ string _phasespacetopic;
+ bool _bSubscribed;
+};
+
+#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h 2008-12-18 16:56:50 UTC (rev 8328)
@@ -22,6 +22,7 @@
#include <cstdio>
#include <cmath>
#include <cstdlib>
+#include <cstring>
#ifdef _MSC_VER
#include <boost/typeof/std/string.hpp>
@@ -141,7 +142,21 @@
#endif
+inline std::wstring _ravembstowcs(const char* pstr)
+{
+ size_t len = mbstowcs(NULL, pstr, 0);
+ std::wstring w; w.resize(len);
+ mbstowcs(&w[0], pstr, len);
+ return w;
+}
+
#include <rave/rave.h>
using namespace OpenRAVE;
+#include <ros/node.h>
+#include <ros/time.h>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/mutex.hpp>
+
#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h 2008-12-18 16:56:50 UTC (rev 8328)
@@ -27,7 +27,6 @@
#define OPENRAVE_ROSARMIK_H
#include <libKinematics/pr2_ik.h>
-#include <boost/shared_ptr.hpp>
class ROSArmIK : public IkSolverBase
{
Added: pkg/trunk/openrave_planning/orrosplanning/src/rosplanningproblem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosplanningproblem.h (rev 0)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosplanningproblem.h 2008-12-18 16:56:50 UTC (rev 8328)
@@ -0,0 +1,50 @@
+#ifndef OPENRAVE_ROS_PLANNING_PROBLEM
+#define OPENRAVE_ROS_PLANNING_PROBLEM
+
+class ROSPlanningProblem : public CmdProblemInstance
+{
+public:
+ ROSPlanningProblem(EnvironmentBase* penv) : CmdProblemInstance(penv)
+ {
+ PhaseSpaceMocapClient::RegisterXMLReader(GetEnv());
+
+ RegisterCommand("createsystem",(CommandFn)&ROSPlanningProblem::CreateSystem, "creates a sensor system and initializes it with the current bodies");
+ }
+
+ virtual void Destroy()
+ {
+ listsystems.clear();
+ }
+
+ int main(const char* cmd)
+ {
+ return 0;
+ }
+
+ bool CreateSystem(ostream& sout, istream& sinput)
+ {
+ string systemname;
+ sinput >> systemname;
+ if( !sinput )
+ return false;
+
+ boost::shared_ptr<SensorSystemBase> psystem(GetEnv()->CreateSensorSystem(systemname.c_str()));
+ if( !psystem )
+ return false;
+
+ if( !psystem->Init(sinput) )
+ return false;
+
+ psystem->AddRegisteredBodies(GetEnv()->GetBodies());
+ listsystems.push_back(psystem);
+
+ RAVELOG_DEBUGA("added %s system\n", systemname.c_str());
+ sout << 1; // signal success
+ return true;
+ }
+
+protected:
+ list<boost::shared_ptr<SensorSystemBase> > listsystems;
+};
+
+#endif
Added: pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h (rev 0)
+++ pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2008-12-18 16:56:50 UTC (rev 8328)
@@ -0,0 +1,321 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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 OPENRAVE_SIMPLE_SENSOR_SYSTEM
+#define OPENRAVE_SIMPLE_SENSOR_SYSTEM
+
+using namespace std;
+
+// used to update objects through a mocap system
+class SimpleSensorSystem : public SensorSystemBase
+{
+ public:
+ class MocapData : public XMLReadable
+ {
+ public:
+ MocapData(const string& xmlid) : _xmlid(xmlid) {}
+ virtual const char* GetXMLId() { return _xmlid.c_str(); }
+
+ string sid; ///< global id for the system id
+ int id;
+ std::wstring strOffsetLink; ///< the link where the markers are attached (if any)
+ Transform transOffset,transPreOffset; // final offset = transOffset * transReturnedFromVision * transPreOffset
+
+ private:
+ string _xmlid;
+ };
+
+ class BODY : public BODYBASE
+ {
+ public:
+ BODY(KinBody* pbody, const MocapData& initdata, const string& xmlid) : BODYBASE(), _initdata(xmlid)
+ {
+ assert( pbody != NULL );
+ _initdata = initdata;
+ pOffsetLink = pbody->GetLink(_initdata.strOffsetLink.c_str());
+ if( pOffsetLink == NULL )
+ pOffsetLink = pbody->GetLinks().front();
+
+ bPresent = false;
+ bEnabled = true;
+ bLock = false;
+ }
+
+ virtual void SetEnable(bool bNewEnable) { bEnabled = bNewEnable; }
+ virtual void SetPresent(bool bNewPresent) { bPresent = bNewPresent; }
+
+ virtual void* GetInitData(int* psize) { if( psize ) *psize = sizeof(_initdata); return &_initdata; }
+ ros::Time lastupdated;
+ MocapData _initdata;
+ };
+
+ class MocapXMLReader : public BaseXMLReader
+ {
+ public:
+ MocapXMLReader(const string& xmlid, MocapData* pMocap, const char **atts) : _xmlid(xmlid) {
+ _pMocap = pMocap;
+ if( _pMocap == NULL )
+ _pMocap = new MocapData(_xmlid);
+ }
+ virtual ~MocapXMLReader() { delete _pMocap; }
+
+ void* Release() { MocapData* temp = _pMocap; _pMocap = NULL; return temp; }
+
+ virtual void startElement(void *ctx, const char *name, const char **atts) {}
+ virtual bool endElement(void *ctx, const char *name)
+ {
+ if( stricmp((const char*)name, _xmlid.c_str()) == 0 )
+ return true;
+
+ if( stricmp((const char*)name, "offsetlink") == 0 ) {
+ string linkname;
+ ss >> linkname;
+ _pMocap->strOffsetLink = _ravembstowcs(linkname.c_str());
+ }
+ else if( stricmp((const char*)name, "id") == 0 )
+ ss >> _pMocap->id;
+ else if( stricmp((const char*)name, "sid") == 0 )
+ ss >> _pMocap->sid;
+ else if( stricmp((const char*)name, "translation") == 0 )
+ ss >> _pMocap->transOffset.trans.x >> _pMocap->transOffset.trans.y >> _pMocap->transOffset.trans.z;
+ else if( stricmp((const char*)name, "rotationmat") == 0 ) {
+ TransformMatrix m;
+ ss >> m.m[0] >> m.m[1] >> m.m[2] >> m.m[4] >> m.m[5] >> m.m[6] >> m.m[8] >> m.m[9] >> m.m[10];
+ _pMocap->transOffset.rot = Transform(m).rot;
+ }
+ else if( stricmp((const char*)name, "rotationaxis") == 0 ) {
+ Vector axis; dReal fang;
+ ss >> axis.x >> axis.y >> axis.z >> fang;
+ _pMocap->transOffset.rotfromaxisangle(axis,fang*(PI/180.0f));
+ }
+ else if( stricmp((const char*)name, "quat") == 0 )
+ ss >> _pMocap->transOffset.rot;
+ else if( stricmp((const char*)name, "pretranslation") == 0 )
+ ss >> _pMocap->transPreOffset.trans.x >> _pMocap->transPreOffset.trans.y >> _pMocap->transPreOffset.trans.z;
+ else if( stricmp((const char*)name, "prerotationmat") == 0 ) {
+ TransformMatrix m;
+ ss >> m.m[0] >> m.m[1] >> m.m[2] >> m.m[4] >> m.m[5] >> m.m[6] >> m.m[8] >> m.m[9] >> m.m[10];
+ _pMocap->transPreOffset.rot = Transform(m).rot;
+ }
+ else if( stricmp((const char*)name, "prerotationaxis") == 0 ) {
+ Vector axis; dReal fang;
+ ss >> axis.x >> axis.y >> axis.z >> fang;
+ _pMocap->transPreOffset.rotfromaxisangle(axis,fang*(PI/180.0f));
+ }
+ else if( stricmp((const char*)name, "prequat") == 0 )
+ ss >> _pMocap->transPreOffset.rot;
+ else
+ RAVELOG_ERRORA("unknown field %s\n", name);
+
+ if( !ss )
+ RAVELOG_ERRORA("MocapXMLReader error parsing %s\n", name);
+
+ return false;
+ }
+
+ virtual void characters(void *ctx, const char *ch, int len)
+ {
+ if( len > 0 ) {
+ ss.clear();
+ ss.str(string(ch, len));
+ }
+ else
+ ss.str(""); // reset
+ }
+
+ protected:
+ MocapData* _pMocap;
+ stringstream ss;
+ string _xmlid;
+ };
+
+ SimpleSensorSystem(EnvironmentBase* penv, const string& xmlid) : SensorSystemBase(penv), _expirationtime(2,0), _xmlid(xmlid)
+ {
+ }
+ virtual ~SimpleSensorSystem() {
+ Destroy();
+ }
+
+ virtual bool Init(istream& sinput)
+ {
+ return true;
+ }
+
+ virtual void Destroy()
+ {
+ boost::mutex::scoped_lock lock(_mutex);
+ _mapbodies.clear();
+ }
+
+ virtual void AddRegisteredBodies(const std::vector<KinBody*>& vbodies)
+ {
+ // go through all bodies in the environment and check for mocap data
+ FOREACHC(itbody, vbodies) {
+ MocapData* pmocapdata = (MocapData*)((*itbody)->GetExtraInterface(GetXMLId()));
+ if( pmocapdata != NULL ) {
+ BODY* p = AddKinBody(*itbody, pmocapdata);
+ if( p != NULL ) {
+ assert( p->GetOffsetLink() != NULL );
+ p->Lock(true);
+ }
+ }
+ }
+ }
+
+ virtual BODY* AddKinBody(KinBody* pbody, const void* _pdata)
+ {
+ if( _pdata == NULL || pbody == NULL )
+ return false;
+ assert( pbody->GetEnv() == GetEnv() );
+
+ const MocapData* pdata = (const MocapData*)_pdata;
+
+ boost::mutex::scoped_lock lock(_mutex);
+ if( _mapbodies.find(pbody->GetNetworkId()) != _mapbodies.end() ) {
+ RAVELOG_WARNA("body %S already added\n", pbody->GetName());
+ return NULL;
+ }
+
+ BODY* b = new BODY(pbody, *pdata, _xmlid);
+ _mapbodies[pbody->GetNetworkId()].reset(b);
+ RAVELOG_DEBUGA("system adding body %S\n", pbody->GetName());
+ return b;
+ }
+
+ virtual bool RemoveKinBody(KinBody* pbody)
+ {
+ if( pbody == NULL )
+ return false;
+ assert( pbody->GetEnv() == GetEnv() );
+
+ boost::mutex::scoped_lock lock(_mutex);
+ bool bSuccess = _mapbodies.erase(pbody->GetNetworkId());
+ RAVELOG_DEBUGA("system removing body %S %s\n", pbody->GetName(), bSuccess?"succeeded":"failed");
+ return bSuccess;
+ }
+
+ virtual bool IsBodyPresent(KinBody* pbody)
+ {
+ if( pbody == NULL )
+ return false;
+ assert( pbody->GetEnv() == GetEnv() );
+
+ boost::mutex::scoped_lock lock(_mutex);
+ return _mapbodies.find(pbody->GetNetworkId()) != _mapbodies.end();
+ }
+
+ virtual bool EnableBody(KinBody* pbody, bool bEnable)
+ {
+ boost::mutex::scoped_lock lock(_mutex);
+ if( pbody == NULL )
+ return false;
+ assert( pbody->GetEnv() == GetEnv() );
+
+ map<int,boost::shared_ptr<BODY> >::iterator it = _mapbodies.find(pbody->GetNetworkId());
+ if( it == _mapbodies.end() ) {
+ RAVELOG_WARNA("trying to %s body %S that is not in system\n", bEnable?"enable":"disable", pbody->GetName());
+ return false;
+ }
+
+ it->second->SetEnable(bEnable);
+ return true;
+ }
+
+ virtual BODY* GetBody(KinBody* pbody)
+ {
+ if( pbody == NULL )
+ return false;
+ assert( pbody->GetEnv() == GetEnv() );
+
+ boost::mutex::scoped_lock lock(_mutex);
+ map<int,boost::shared_ptr<BODY> >::iterator it = _mapbodies.find(pbody->GetNetworkId());
+ return it != _mapbodies.end() ? it->second.get() : NULL;
+ }
+
+ virtual const char* GetXMLId() { return _xmlid.c_str(); }
+
+protected:
+
+ typedef pair<boost::shared_ptr<BODY>, Transform > SNAPSHOT;
+ virtual void UpdateBodies(list<SNAPSHOT>& listbodies)
+ {
+ // assume mutex is already locked
+ if( listbodies.size() == 0 )
+ return;
+
+ ros::Time curtime = ros::Time::now();
+ GetEnv()->LockPhysics(true);
+
+ FOREACH(it, listbodies) {
+ assert( it->first->IsEnabled() && it->first->GetOffsetLink() != NULL );
+
+ // transform with respect to offset link
+ TransformMatrix tlink = it->first->GetOffsetLink()->GetTransform();
+ TransformMatrix tbase = it->first->GetOffsetLink()->GetParent()->GetTransform();
+ TransformMatrix toffset = tbase * tlink.inverse() * it->first->_initdata.transOffset;
+ TransformMatrix tfinal = toffset * it->second*it->first->_initdata.transPreOffset;
+
+ it->first->GetOffsetLink()->GetParent()->SetTransform(tfinal);
+ it->first->lastupdated = curtime;
+
+ if( !it->first->IsPresent() )
+ RAVELOG_VERBOSEA("updating body %S\n", it->first->GetOffsetLink()->GetParent()->GetName());
+ it->first->SetPresent(true);
+ }
+
+ GetEnv()->LockPhysics(false);
+
+ map<int,boost::shared_ptr<BODY> >::iterator itbody = _mapbodies.begin();
+ while(itbody != _mapbodies.end()) {
+ if( curtime-itbody->second->lastupdated > _expirationtime ) {
+ if( !itbody->second->IsLocked() ) {
+ GetEnv()->RemoveKinBody(itbody->second->GetOffsetLink()->GetParent());
+ _mapbodies.erase(itbody++);
+ continue;
+ }
+
+ if( itbody->second->IsPresent() )
+ RAVELOG_VERBOSEA("body %S not present\n", itbody->second->GetOffsetLink()->GetParent()->GetName());
+ itbody->second->SetPresent(false);
+ }
+
+ ++itbody;
+ }
+ }
+
+ map<int,boost::shared_ptr<BODY> > _mapbodies;
+ boost::mutex _mutex;
+ ros::Duration _expirationtime;
+ string _xmlid;
+};
+
+#ifdef RAVE_REGISTER_BOOST
+#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()
+BOOST_TYPEOF_REGISTER_TYPE(SimpleSensorSystem::MocapData)
+BOOST_TYPEOF_REGISTER_TYPE(SimpleSensorSystem::BODY)
+BOOST_TYPEOF_REGISTER_TYPE(SimpleSensorSystem::MocapXMLReader)
+#endif
+
+#endif
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2008-12-18 09:42:07 UTC (rev 8327)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2008-12-18 16:56:50 UTC (rev 8328)
@@ -95,7 +95,7 @@
// set geometry transform
addKeyValue(elem, "translation", values2str(3, cpos));
- addKeyValue(elem, "quaternion", values2str(4, cquat));
+ addKeyValue(elem, "quat", values2str(4, cquat));
}
void copyOpenraveMap(const robot_desc::URDF::Map& data, TiXmlElement *elem, const vector<string> *tags = NULL)
@@ -272,8 +272,8 @@
addKeyValue(joint, "offsetfrom", link->name);
if (fixed) {
- addKeyValue(joint, "lowStop", "0");
- addKeyValue(joint, "highStop", "0");
+ addKeyValue(joint, "lostop", "0");
+ addKeyValue(joint, "histop", "0");
addKeyValue(joint, "axis", "1 0 0");
}
else {
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
