Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8092] pkg/trunk
|
From: <rdi...@us...> - 2008-12-15 18:02:45
|
Revision: 8092
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8092&view=rev
Author: rdiankov
Date: 2008-12-15 18:02:38 +0000 (Mon, 15 Dec 2008)
Log Message:
-----------
fixed many bugs in openraveros server, viewers can be freely attached, environments can be cloned
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST
pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg
pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m
pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
pkg/trunk/openrave_planning/openraveros/src/openraveros.h
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/openraveros/srv/env_set.srv
pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
pkg/trunk/openrave_planning/openraveros/test/testsessioncreation.m
Added Paths:
-----------
pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m
pkg/trunk/openrave_planning/openraveros/test/testhanoi.m
Removed Paths:
-------------
pkg/trunk/openrave_planning/openraveros/srv/body_getlinks.srv
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-15 18:02:38 UTC (rev 8092)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 549
+SVN_REVISION = -r 554
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,2 +1,2 @@
-depends on roscpp sessions, which can be found in
-https://ros.svn.sourceforge.net/svnroot/ros/branches/sessions-rosen/core/roscpp
+depends on new roscpp protocol, which can be found in this branch:
+https://ros.svn.sourceforge.net/svnroot/ros/branches/sessions-dec2008/
Modified: pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg 2008-12-15 18:02:38 UTC (rev 8092)
@@ -33,4 +33,4 @@
uint16 Req_Links=2
uint16 Req_LinkNames=4
uint16 Req_JointLimits=8
-uint16 Req_Names=16 # if set, fills filename, name, and type
\ No newline at end of file
+uint16 Req_Names=16 # if set, fills filename, name, and type
Modified: pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-15 18:02:38 UTC (rev 8092)
@@ -10,7 +10,12 @@
# information about the active dofs
ActiveDOFs active
+# joint limits
+float32[] activelowerlimit
+float32[] activeupperlimit
+
# upper 8 bits
uint16 Req_Manipulators=256
uint16 Req_Sensors=512
uint16 Req_ActiveDOFs=1024
+uint16 Req_ActiveLimits=2048
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -14,7 +14,7 @@
body.T = reshape(cell2mat(bodyinfo.transform.m),[3 4]);
body.jointvalues = cell2mat(bodyinfo.jointvalues);
-body.links = zeros(1,length(bodyinfo.links));
+body.links = zeros(12,length(bodyinfo.links));
for i = 1:length(bodyinfo.links)
body.links(:,i) = cell2mat(bodyinfo.links{i}.m);
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -30,3 +30,7 @@
robot.affinedof = robotinfo.active.affine;
robot.activejoints = cell2mat(robotinfo.active.joints);
robot.rotationaxis = cell2mat(robotinfo.active.rotationaxis);
+
+robot.activelowerlimit = cell2mat(bodyinfo.activelowerlimit);
+robot.activeupperlimit = cell2mat(bodyinfo.activeupperlimit);
+
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,4 +1,4 @@
-%% openraveros_restart(sessionserver)
+%% openraveros_restart(sessionserver, viewer)
%%
%% restars an openraveros session if the current one is invalid
@@ -27,7 +27,7 @@
%% POSSIBILITY OF SUCH DAMAGE.
%%
%% author: Rosen Diankov
-function openraveros_restart(sessionserver)
+function openraveros_restart(sessionserver,viewer)
global openraveros_globalsession
openraveros_startup();
@@ -45,3 +45,15 @@
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;
+ resset = rosoct_session_call(openraveros_globalsession.id,'env_set',reqset);
+end
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,4 +1,6 @@
-% R = openraveros_rotfromquat(quat)
+%% R = openraveros_rotfromquat(quat)
+%%
+%% quat - the format is [cos(angle/2) axis*sin(angle/2)]
function R = openraveros_rotfromquat(quat)
R = zeros(3,3);
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,4 +1,4 @@
-%% openraveros_startup(sessionserver)
+%% openraveros_startup(sessionserver, createsession, viewer)
%% adds all the necessary paths for the openraveros octave client
%% Software License Agreement (BSD License)
@@ -26,7 +26,7 @@
%% POSSIBILITY OF SUCH DAMAGE.
%%
%% author: Rosen Diankov
-function openraveros_startup(sessionserver,createsession)
+function openraveros_startup(sessionserver,createsession, veiwer)
global openraveros_globalsession
persistent openraveros_initialized
@@ -50,8 +50,13 @@
sessionserver = 'openrave_session';
end
+if( ~exist('viewer','var') )
+ viewer = 'qtcoin';
+end
+
if( createsession && isempty(openraveros_globalsession) )
req = openraveros_openrave_session();
+ req.viewer = viewer; % default viewer
[localid,res] = rosoct_create_session(sessionserver,req);
if( ~isempty(localid) && ~isempty(res) )
@@ -61,6 +66,6 @@
openraveros_globalsession.id = localid;
openraveros_globalsession.server = sessionserver;
openraveros_globalsession.uuid = res.sessionid;
- display(sprintf('created openraveros session uuid %d',res.sessionid));
+ %display(sprintf('created openraveros session uuid %d',res.sessionid));
end
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -11,7 +11,7 @@
res = rosoct_session_call(session.id,'body_getaabb',req);
if(~isempty(res))
- aabb = [res.center res.extents];
+ aabb = [cell2mat(res.box.center) cell2mat(res.box.extents)];
else
aabb = [];
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,22 +1,24 @@
-% values = orBodyGetLinks(bodyid)
+% links = orBodyGetLinks(bodyid)
%
% Returns the transformations of all the body's links in a 12 x L matrix. Where L
% is the number of links and each column is a 3x4 transformation
% (use T=reshape(., [3 4]) to recover).
% T * [X;1] = Xnew
-function values = orBodyGetLinks(bodyid)
+function links = orBodyGetLinks(bodyid)
+
+%% get some body info
session = openraveros_getglobalsession();
-req = openraveros_body_getlinks();
+req = openraveros_env_getbodies();
req.bodyid = bodyid;
-res = rosoct_session_call(session.id,'body_getlinks',req);
+req.options = openraveros_BodyInfo().Req_Links();
+res = rosoct_session_call(session.id,'env_getbodies',req);
+if( ~isempty(res) )
-if(~isempty(res))
- values = zeros(1,length(res.links));
- for i = 1:length(res.links)
- values(:,i) = cell2mat(res.links{i}.m);
+ links = zeros(12,length(res.bodies{1}.links));
+ for i = 1:size(links,2)
+ links(:,i) = cell2mat(res.bodies{1}.links{i}.m);
end
else
- values = [];
+ links = [];
end
-
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -61,7 +61,7 @@
req.publishanytime = str2num(rem);
case 'debug'
req.setmask = req.Set_DebugLevel();
- req.debuglevel = str2num(rem);
+ req.debuglevel = strtrim(rem);
otherwise
display('unknown command');
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -7,17 +7,24 @@
%% get some robot info
session = openraveros_getglobalsession();
-req = openraveros_env_getbodies();
+req = openraveros_env_getrobots();
req.bodyid = robotid;
-res = rosoct_session_call(session.id,'env_getbodies',req);
-if( ~isempty(resinfo) )
- if( length(res.lowerlimit) ~= length(res.upperlimit) )
+req.options = openraveros_RobotInfo().Req_ActiveLimits();
+res = rosoct_session_call(session.id,'env_getrobots',req);
+if( ~isempty(res) )
+ if( res.robots{1}.id ~= robotid )
+ error('wrong robot id');
+ end
+
+ lowerlimit = res.robots{1}.activelowerlimit;
+ upperlimit = res.robots{1}.activeupperlimit;
+ if( length(lowerlimit) ~= length(upperlimit) )
error('limits not same size');
end
- values = zeros(length(res.lowerlimit),2);
- values(:,1) = cell2mat(res.lowerlimit);
- values(:,2) = cell2mat(res.upperlimit);
+ values = zeros(length(lowerlimit),2);
+ values(:,1) = cell2mat(lowerlimit);
+ values(:,2) = cell2mat(upperlimit);
else
values = [];
end
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp 2008-12-15 18:02:38 UTC (rev 8092)
@@ -25,22 +25,36 @@
// author: Rosen Diankov
#include "openraveros.h"
#include "session.h"
+#include <signal.h>
+void sigint_handler(int);
+
+boost::shared_ptr<ros::node> s_pmasternode;
+boost::shared_ptr<SessionServer> s_sessionserver;
+
int main(int argc, char ** argv)
{
+ signal(SIGINT,sigint_handler); // control C
+
ros::init(argc,argv);
- ros::node masternode("openraveserver");
+ s_pmasternode.reset(new ros::node("openraveserver", ros::node::DONT_HANDLE_SIGINT));
- if( !masternode.checkMaster() )
+ if( !s_pmasternode->checkMaster() )
return -1;
- boost::shared_ptr<SessionServer> sessionserver(new SessionServer());
- if( !sessionserver->Init() )
+ s_sessionserver.reset(new SessionServer());
+ if( !s_sessionserver->Init() )
return -1;
- masternode.spin();
-
- sessionserver.reset();
+ s_sessionserver->spin();
+ s_sessionserver.reset();
ros::fini();
+ s_pmasternode.reset();
return 0;
}
+
+void sigint_handler(int)
+{
+ s_sessionserver->selfDestruct();
+ s_pmasternode->selfDestruct();
+}
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2008-12-15 18:02:38 UTC (rev 8092)
@@ -99,6 +99,7 @@
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
#include <boost/static_assert.hpp>
#include <boost/bind.hpp>
@@ -109,7 +110,6 @@
#include <openraveros/body_getaabbs.h>
#include <openraveros/body_getdof.h>
#include <openraveros/body_getjointvalues.h>
-#include <openraveros/body_getlinks.h>
#include <openraveros/body_setjointvalues.h>
#include <openraveros/body_settransform.h>
#include <openraveros/env_checkcollision.h>
@@ -145,4 +145,10 @@
using namespace std;
using namespace openraveros;
+class SetViewerFunc
+{
+public:
+ virtual bool SetViewer(EnvironmentBase* penv, const string& viewername) = 0;
+};
+
#endif
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-15 18:02:38 UTC (rev 8092)
@@ -49,12 +49,17 @@
};
public:
- ROSServer(EnvironmentBase* penv) : RaveServerBase(penv), _nNextFigureId(1), _nNextPlannerId(1), _nNextProblemId(1) {
+ ROSServer(SetViewerFunc* psetviewer, EnvironmentBase* penv, const string& physicsengine, const string& collisionchecker, const string& viewer)
+ : RaveServerBase(penv), _nNextFigureId(1), _nNextPlannerId(1), _nNextProblemId(1) {
+ _psetviewer.reset(psetviewer);
_bThreadDestroyed = false;
_bCloseClient = false;
_fSimulationTimestep = 0.01;
_vgravity = Vector(0,0,-9.8f);
- GetEnv()->SetDebugLevel(1);
+
+ SetPhysicsEngine(physicsengine);
+ SetCollisionChecker(collisionchecker);
+ SetViewer(viewer);
}
virtual ~ROSServer() {
Destroy();
@@ -72,13 +77,8 @@
// have to maintain a certain destruction order
_pphysics.reset();
_pcolchecker.reset();
+ _threadviewer.join();
- if( !!_pviewer ) {
- _pviewer->quitmainloop();
- _threadviewer.join();
- _pviewer.reset();
- }
-
_bCloseClient = false;
}
@@ -111,6 +111,80 @@
return _bCloseClient;
}
+ /// viewer thread assuming you can create different viewers in their own therads
+ virtual void ViewerThread(const string& strviewer)
+ {
+ {
+ boost::mutex::scoped_lock lock(_mutexViewer);
+ _pviewer.reset(GetEnv()->CreateViewer(strviewer.c_str()));
+ if( !!_pviewer ) {
+ GetEnv()->AttachViewer(_pviewer.get());
+ _pviewer->ViewerSetSize(1024,768);
+ }
+ _conditionViewer.notify_all();
+ }
+
+ if( !_pviewer )
+ return;
+
+ _pviewer->main(); // spin until quitfrommainloop is called
+ RAVELOG_DEBUGA("destroying viewer\n");
+ _pviewer.reset();
+ }
+
+ bool SetPhysicsEngine(const string& physicsengine)
+ {
+ if( physicsengine.size() > 0 ) {
+ _pphysics.reset(GetEnv()->CreatePhysicsEngine(physicsengine.c_str()));
+ GetEnv()->SetPhysicsEngine(_pphysics.get());
+ if( !_pphysics )
+ return false;
+ _pphysics->SetGravity(_vgravity);
+ }
+
+ return true;
+ }
+
+ bool SetCollisionChecker(const string& collisionchecker)
+ {
+ if( collisionchecker.size() > 0 ) {
+ _pcolchecker.reset(GetEnv()->CreateCollisionChecker(collisionchecker.c_str()));
+ if( !_pcolchecker )
+ return false;
+ GetEnv()->SetCollisionChecker(_pcolchecker.get());
+ }
+
+ return true;
+ }
+
+ bool SetViewer(const string& viewer)
+ {
+ if( !!_psetviewer )
+ return _psetviewer->SetViewer(GetEnv(),viewer);
+
+ _threadviewer.join(); // wait for the viewer
+
+ if( viewer.size() > 0 ) {
+ boost::mutex::scoped_lock lock(_mutexViewer);
+ _threadviewer = boost::thread(boost::bind(&ROSServer::ViewerThread, this, viewer));
+ _conditionViewer.wait(lock);
+
+ if( !_pviewer ) {
+ RAVELOG_WARNA("failed to create viewer %s\n", viewer.c_str());
+ _threadviewer.join();
+ return false;
+ }
+ else
+ RAVELOG_INFOA("viewer %s successfully attached\n", viewer.c_str());
+ }
+
+ return true;
+ }
+
+ //////////////
+ // services //
+ //////////////
+
bool body_destroy_srv(body_destroy::request& req, body_destroy::response& res)
{
KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
@@ -200,22 +274,6 @@
return true;
}
- bool body_getlinks_srv(body_getlinks::request& req, body_getlinks::response& res)
- {
- KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
- if( pbody == NULL )
- return false;
-
- LockEnvironment envlock(this);
- vector<Transform> vtrans; pbody->GetBodyTransformations(vtrans);
-
- res.links.resize(vtrans.size()); int index = 0;
- FOREACH(ittrans, vtrans)
- res.links[index++] = GetAffineTransform(*ittrans);
-
- return true;
- }
-
bool body_setjointvalues_srv(body_setjointvalues::request& req, body_setjointvalues::response& res)
{
KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
@@ -423,8 +481,10 @@
}
}
- if( !GetEnv()->LoadProblem(pproblem.get(), req.args.c_str()) )
+ if( GetEnv()->LoadProblem(pproblem.get(), req.args.c_str()) != 0 ) {
+ RAVELOG_WARNA("failed to load problem %s with args %s\n", req.problemtype.c_str(), req.args.c_str());
return false;
+ }
_mapproblems[++_nNextProblemId] = pproblem;
res.problemid = _nNextProblemId;
@@ -600,6 +660,17 @@
FillActiveDOFs(probot, info.active);
info.activedof = probot->GetActiveDOF();
}
+ if( options & BodyInfo::Req_JointLimits ) {
+ vector<dReal> vlower, vupper;
+ probot->GetActiveDOFLimits(vlower,vupper);
+ ROS_ASSERT( vlower.size() == vupper.size() );
+ info.activelowerlimit.resize(vlower.size());
+ info.activeupperlimit.resize(vupper.size());
+ for(size_t i = 0; i < vlower.size(); ++i) {
+ info.activelowerlimit[i] = vlower[i];
+ info.activeupperlimit[i] = vupper[i];
+ }
+ }
}
bool env_getrobots_srv(env_getrobots::request& req, env_getrobots::response& res)
@@ -781,16 +852,12 @@
}
}
if( req.setmask & env_set::request::Set_PhysicsEngine ) {
- _pphysics.reset(GetEnv()->CreatePhysicsEngine(req.physicsengine.c_str()));
- GetEnv()->SetPhysicsEngine(_pphysics.get());
- if( !!_pphysics )
- _pphysics->SetGravity(_vgravity);
+ SetPhysicsEngine(req.physicsengine);
}
if( req.setmask & env_set::request::Set_CollisionChecker ) {
int options = GetEnv()->GetCollisionOptions();
- _pcolchecker.reset(GetEnv()->CreateCollisionChecker(req.collisionchecker.c_str()));
- GetEnv()->SetCollisionChecker(_pcolchecker.get());
- GetEnv()->SetCollisionOptions(options);
+ if( SetCollisionChecker(req.collisionchecker) )
+ GetEnv()->SetCollisionOptions(options);
}
if( req.setmask & env_set::request::Set_Gravity ) {
_vgravity = Vector(req.gravity[0],req.gravity[1],req.gravity[2]);
@@ -801,19 +868,27 @@
GetEnv()->SetPublishBodiesAnytime(req.publishanytime>0);
}
if( req.setmask & env_set::request::Set_DebugLevel ) {
- GetEnv()->SetDebugLevel(req.debuglevel);
+ map<string,DebugLevel> mlevels;
+ mlevels["fatal"] = Level_Fatal;
+ mlevels["error"] = Level_Error;
+ mlevels["info"] = Level_Info;
+ mlevels["warn"] = Level_Warn;
+ mlevels["debug"] = Level_Debug;
+ DebugLevel level = GetEnv()->GetDebugLevel();
+ if( mlevels.find(req.debuglevel) != mlevels.end() )
+ level = mlevels[req.debuglevel];
+ else {
+ stringstream ss(req.debuglevel);
+ int nlevel;
+ ss >> nlevel;
+ if( !!ss )
+ level = (DebugLevel)nlevel;
+ }
+ GetEnv()->SetDebugLevel(level);
}
if( req.setmask & env_set::request::Set_Viewer ) {
- if( !!_pviewer ) {
- _pviewer->quitmainloop();
- // no need to wait for joins
- //_threadviewer.join();
- }
-
- _pviewer.reset(GetEnv()->CreateViewer(req.viewer.c_str()));
- GetEnv()->AttachViewer(_pviewer.get());
- if( !!_pviewer )
- _threadviewer = boost::thread(boost::bind(&RaveViewerBase::main, _pviewer.get()));
+ GetEnv()->AttachViewer(NULL);
+ SetViewer(req.viewer);
}
return true;
@@ -1287,15 +1362,20 @@
return am;
}
+ boost::shared_ptr<SetViewerFunc> _psetviewer;
boost::shared_ptr<PhysicsEngineBase> _pphysics;
boost::shared_ptr<CollisionCheckerBase> _pcolchecker;
- boost::shared_ptr<RaveViewerBase> _pviewer;
map<int, boost::shared_ptr<PlannerBase> > _mapplanners;
map<int, boost::shared_ptr<ProblemInstance> > _mapproblems;
map<int, boost::shared_ptr<FIGURE> > _mapFigureIds;
int _nNextFigureId, _nNextPlannerId, _nNextProblemId;
- boost::thread _threadviewer;
float _fSimulationTimestep;
Vector _vgravity;
bool _bThreadDestroyed, _bCloseClient;
+
+ /// viewer control variables
+ boost::shared_ptr<RaveViewerBase> _pviewer;
+ boost::thread _threadviewer;
+ boost::mutex _mutexViewer;
+ boost::condition _conditionViewer;
};
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-15 18:02:38 UTC (rev 8092)
@@ -34,10 +34,11 @@
#define REFLECT_SERVICE(srvname) \
bool srvname##_srv(srvname::request& req, srvname::response& res) \
{ \
- /* need separate copy in order to guarantee thread safety */ \
- SessionState state = getstate(req); \
- if( !state._pserver ) \
+ SessionState state = getstate(req); /* need separate copy in order to guarantee thread safety */ \
+ if( !state._pserver ) { \
+ ROS_INFO("failed to find session for service %s\n", #srvname); \
return false; \
+ } \
return state._pserver->srvname##_srv(req,res); \
}
@@ -57,20 +58,33 @@
boost::shared_ptr<EnvironmentBase> _penv;
};
+ class SessionSetViewerFunc : public SetViewerFunc
+ {
+ public:
+ SessionSetViewerFunc(SessionServer* pserv) : _pserv(pserv) {}
+ virtual bool SetViewer(EnvironmentBase* penv, const string& viewername) {
+ return _pserv->SetViewer(penv,viewername);
+ }
+
+ private:
+ SessionServer* _pserv;
+ };
+
string _sessionname;
public:
- SessionServer() {
+ SessionServer() : _penvViewer(NULL), _ok(true) {
_pParentEnvironment.reset(CreateEnvironment());
}
virtual ~SessionServer() {
Destroy();
}
- bool Init() {
+ bool Init()
+ {
node* pnode = node::instance();
if( pnode == NULL )
return false;
-
+
if( !pnode->advertiseService("openrave_session",&SessionServer::session_callback,this, 1) )
return false;
_sessionname = pnode->mapName("openrave_session");
@@ -88,8 +102,6 @@
return false;
if( !pnode->advertiseService("body_getjointvalues",&SessionServer::body_getjointvalues_srv,this,-1) )
return false;
- if( !pnode->advertiseService("body_getlinks",&SessionServer::body_getlinks_srv,this,-1) )
- return false;
if( !pnode->advertiseService("body_setjointvalues",&SessionServer::body_setjointvalues_srv,this,-1) )
return false;
if( !pnode->advertiseService("body_settransform",&SessionServer::body_settransform_srv,this,-1) )
@@ -149,11 +161,16 @@
if( !pnode->advertiseService("robot_setactivevalues",&SessionServer::robot_setactivevalues_srv,this,-1) )
return false;
+ _ok = true;
+ _threadviewer = boost::thread(boost::bind(&SessionServer::ViewerThread, this));
+
return true;
}
void Destroy()
{
+ selfDestruct();
+
node* pnode = node::instance();
if( pnode == NULL )
return;
@@ -165,7 +182,6 @@
pnode->unadvertiseService("body_getaabbs");
pnode->unadvertiseService("body_getdof");
pnode->unadvertiseService("body_getjointvalues");
- pnode->unadvertiseService("body_getlinks");
pnode->unadvertiseService("body_setjointvalues");
pnode->unadvertiseService("body_settransform");
pnode->unadvertiseService("env_checkcollision");
@@ -195,13 +211,97 @@
pnode->unadvertiseService("robot_sensorsend");
pnode->unadvertiseService("robot_setactivedofs");
pnode->unadvertiseService("robot_setactivevalues");
+
+ _threadviewer.join();
}
+
+ virtual void spin()
+ {
+ while (_ok) {
+ usleep(1000);
+ };
+ }
+ virtual void selfDestruct()
+ {
+ _ok = false;
+ boost::mutex::scoped_lock lockcreate(_mutexViewer);
+ if( !!_penvViewer )
+ _penvViewer->AttachViewer(NULL);
+ }
+
+ bool SetViewer(EnvironmentBase* penv, const string& viewer)
+ {
+ boost::mutex::scoped_lock lock(_mutexViewer);
+
+ // destroy the old viewer
+ if( !!_penvViewer ) {
+ _penvViewer->AttachViewer(NULL);
+
+ _conditionViewer.wait(lock);
+ }
+
+ ROS_ASSERT(!_penvViewer);
+
+ _strviewer = viewer;
+ _penvViewer = penv;
+
+ if( viewer.size() == 0 || !_penvViewer )
+ return true;
+
+ _conditionViewer.wait(lock);
+ return !!_pviewer;
+ }
+
private:
map<int,SessionState> _mapsessions;
boost::mutex _mutexsession;
boost::shared_ptr<EnvironmentBase> _pParentEnvironment;
+ // persistent viewer
+ boost::shared_ptr<RaveViewerBase> _pviewer;
+ boost::thread _threadviewer; ///< persistent thread (qtcoin openrave plugin needs this)
+ boost::mutex _mutexViewer;
+ boost::condition _conditionViewer;
+ EnvironmentBase* _penvViewer;
+ string _strviewer;
+
+ bool _ok;
+
+ virtual void ViewerThread()
+ {
+ while(_ok) {
+
+ {
+ boost::mutex::scoped_lock lockcreate(_mutexViewer);
+ if( _strviewer.size() == 0 || !_penvViewer ) {
+ usleep(1000);
+ continue;
+ }
+
+ _pviewer.reset(_penvViewer->CreateViewer(_strviewer.c_str()));
+ if( !!_pviewer ) {
+ _penvViewer->AttachViewer(_pviewer.get());
+ _pviewer->ViewerSetSize(1024,768);
+ }
+ _conditionViewer.notify_all();
+
+ if( !_pviewer )
+ continue;
+ }
+
+ _pviewer->main(); // spin until quitfrommainloop is called
+
+ boost::mutex::scoped_lock lockcreate(_mutexViewer);
+ RAVELOG_DEBUGA("destroying viewer\n");
+ ROS_ASSERT(_penvViewer == _pviewer->GetEnv());
+ _penvViewer->AttachViewer(NULL);
+ _pviewer.reset();
+ _penvViewer = NULL;
+ _conditionViewer.notify_all();
+ }
+ }
+
template <class MReq>
SessionState getstate(const MReq& req)
{
@@ -242,16 +342,16 @@
if( req.clone_sessionid ) {
// clone the environment from clone_sessionid
- SessionState state;
+ SessionState clonestate;
{
boost::mutex::scoped_lock lock(_mutexsession);
- state = _mapsessions[req.clone_sessionid];
+ clonestate = _mapsessions[req.clone_sessionid];
}
- if( !state._penv )
+ if( !clonestate._penv )
ROS_INFO("failed to find session %d", req.clone_sessionid);
else
- state._penv.reset(state._penv->CloneSelf(req.clone_options));
+ state._penv.reset(clonestate._penv->CloneSelf(req.clone_options));
}
if( !state._penv ) {
@@ -260,7 +360,7 @@
state._penv.reset(_pParentEnvironment->CloneSelf(0));
}
- state._pserver.reset(new ROSServer(state._penv.get()));
+ state._pserver.reset(new ROSServer(new SessionSetViewerFunc(this), state._penv.get(), req.physicsengine, req.collisionchecker, req.viewer));
_mapsessions[id] = state;
res.sessionid = id;
@@ -275,7 +375,6 @@
REFLECT_SERVICE(body_getaabbs)
REFLECT_SERVICE(body_getdof)
REFLECT_SERVICE(body_getjointvalues)
- REFLECT_SERVICE(body_getlinks)
REFLECT_SERVICE(body_setjointvalues)
REFLECT_SERVICE(body_settransform)
REFLECT_SERVICE(env_checkcollision)
Deleted: pkg/trunk/openrave_planning/openraveros/srv/body_getlinks.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/body_getlinks.srv 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/srv/body_getlinks.srv 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,4 +0,0 @@
-# Returns the transformations of all the body's links in the global coordinate system
-int32 bodyid
----
-AffineTransformMatrix[] links
Modified: pkg/trunk/openrave_planning/openraveros/srv/env_set.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/env_set.srv 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/srv/env_set.srv 2008-12-15 18:02:38 UTC (rev 8092)
@@ -21,5 +21,5 @@
string viewer
float32[3] gravity
uint8 publishanytime
-uint8 debuglevel
+string debuglevel # one of fatal,error,warn,info,debug. If numeric, sets the level directly
---
Modified: pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv 2008-12-15 18:02:38 UTC (rev 8092)
@@ -18,5 +18,11 @@
uint8 CloneSimulation=4 # copy simulation settings
uint8 CloneRealControllers=8 # copy real robot controllers
+# if not empty, the interfaces to set for the new environment
+# otherwise appropriate defaults are created
+string physicsengine
+string collisionchecker
+string viewer
+
---
int32 sessionid
Added: pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -0,0 +1,21 @@
+%% addexamplesdir(subdir)
+%%
+%% Adds the openrave examples directory
+function addexamplesdir(subdir)
+
+basepath = [];
+if( isunix() )
+ %% try getting from openrave-config
+ [status,basepath] = system('openrave-config --prefix');
+ basepath = strtrim(basepath);
+end
+
+if( isempty(basepath) )
+ basepath = 'C:\Program Files\openrave';
+end
+
+if( ~exist('subdir','var') )
+ subdir = [];
+end
+
+addpath(fullfile(basepath,'share','openrave','examples',subdir));
Added: pkg/trunk/openrave_planning/openraveros/test/testhanoi.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/test/testhanoi.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/test/testhanoi.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -0,0 +1,5 @@
+%% Runs the openrave hanoi example
+addpath(fullfile(pwd,'..','octave')); % in case launched from test folder
+openraveros_restart();
+addexamplesdir('hanoi_puzzle');
+hanoi
Modified: pkg/trunk/openrave_planning/openraveros/test/testsessioncreation.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/test/testsessioncreation.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/test/testsessioncreation.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -46,3 +46,11 @@
sessions = {};
openraveros_restart();
+% for i = 1:4
+% orEnvSetOptions('viewer qtcoin');
+% orEnvLoadScene('data/lab1.env.xml');
+% end
+
+orEnvLoadScene('data/lab1.env.xml');
+orEnvSetOptions('viewer qtcoin');
+orEnvLoadScene('robots/barrettwam.robot.xml',1);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
