|
From: <rdi...@us...> - 2008-12-06 18:39:21
|
Revision: 7733
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7733&view=rev
Author: rdiankov
Date: 2008-12-06 18:39:11 +0000 (Sat, 06 Dec 2008)
Log Message:
-----------
progress towards openrave session server
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
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/body_enable.srv
pkg/trunk/openrave_planning/openraveros/srv/body_setjointvalues.srv
pkg/trunk/openrave_planning/openraveros/srv/env_checkcollision.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createproblem.srv
pkg/trunk/openrave_planning/openraveros/srv/env_loadplugin.srv
pkg/trunk/openrave_planning/openraveros/srv/env_loadscene.srv
pkg/trunk/openrave_planning/openraveros/srv/env_plot.srv
pkg/trunk/openrave_planning/openraveros/srv/env_raycollision.srv
pkg/trunk/openrave_planning/openraveros/srv/env_set.srv
pkg/trunk/openrave_planning/openraveros/srv/env_triangulate.srv
pkg/trunk/openrave_planning/openraveros/srv/env_wait.srv
pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
pkg/trunk/openrave_planning/openraveros/srv/planner_init.srv
pkg/trunk/openrave_planning/openraveros/srv/problem_sendcommand.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_controllersend.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_controllerset.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_sensorsend.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_setactivevalues.srv
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-06 18:39:11 UTC (rev 7733)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 527
+SVN_REVISION = -r 532
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2008-12-06 18:39:11 UTC (rev 7733)
@@ -39,6 +39,7 @@
#include <boost/typeof/std/vector.hpp>
#include <boost/typeof/std/list.hpp>
#include <boost/typeof/std/map.hpp>
+#include <boost/typeof/std/set.hpp>
#include <boost/typeof/std/string.hpp>
#define FOREACH(it, v) for(BOOST_TYPEOF(v)::iterator it = (v).begin(); it != (v).end(); (it)++)
@@ -50,6 +51,7 @@
#include <vector>
#include <list>
#include <map>
+#include <set>
#include <string>
#define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++)
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-06 18:39:11 UTC (rev 7733)
@@ -28,6 +28,16 @@
class ROSServer : public RaveServerBase
{
+ class LockEnvironment
+ {
+ public:
+ LockEnvironment(ROSServer* pserv) : _pserv(pserv) { _pserv->_penv->LockPhysics(true); }
+ ~LockEnvironment() { _pserv->_penv->LockPhysics(false); }
+
+ private:
+ ROSServer* _pserv;
+ };
+
public:
ROSServer(boost::shared_ptr<EnvironmentBase> penv) : RaveServerBase(penv.get()), _nNextFigureId(1), _nNextPlannerId(1) {
_penv = penv;
@@ -60,15 +70,9 @@
virtual void Reset()
{
-
-// pthread_mutex_lock(&_mutexWorker);
-// listWorkers.clear();
-// pthread_mutex_unlock(&_mutexWorker);
-// pthread_cond_signal(&_condWorker);
-//
-// FOREACH(it, s_mapFigureIds)
-// g_Environ.closegraph(it->second);
-// s_mapFigureIds.clear();
+ FOREACH(it, _mapFigureIds)
+ _penv->closegraph(it->second);
+ _mapFigureIds.clear();
// destroy environment specific state: problems, planners, figures
_mapplanners.clear();
@@ -95,17 +99,86 @@
return false;
}
- // called from threads other than the main worker to wait until
- virtual void SyncWithWorkerThread()
+ bool body_destroy_srv(body_destroy::request& req, body_destroy::response& res)
{
+ KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL )
+ return false;
+
+ LockEnvironment envlock(this);
+ return _penv->RemoveKinBody(pbody,true);
}
- bool body_destroy_srv(body_destroy::request& req, body_destroy::response& res);
- bool body_enable_srv(body_enable::request& req, body_enable::response& res);
- bool body_getaabb_srv(body_getaabb::request& req, body_getaabb::response& res);
- bool body_getaabbs_srv(body_getaabbs::request& req, body_getaabbs::response& res);
- bool body_getdof_srv(body_getdof::request& req, body_getdof::response& res);
- bool body_getlinks_srv(body_getlinks::request& req, body_getlinks::response& res);
+ bool body_enable_srv(body_enable::request& req, body_enable::response& res)
+ {
+ KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL )
+ return false;
+
+ LockEnvironment envlock(this);
+ pbody->Enable(req.enable);
+ return true;
+ }
+
+ bool body_getaabb_srv(body_getaabb::request& req, body_getaabb::response& res)
+ {
+ KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL )
+ return false;
+
+ LockEnvironment envlock(this);
+ OpenRAVE::AABB ab = pbody->ComputeAABB();
+ res.box.center[0] = ab.pos.x; res.box.center[1] = ab.pos.y; res.box.center[2] = ab.pos.z;
+ res.box.extents[0] = ab.extents.x; res.box.extents[1] = ab.extents.y; res.box.extents[2] = ab.extents.z;
+ return true;
+ }
+
+ bool body_getaabbs_srv(body_getaabbs::request& req, body_getaabbs::response& res)
+ {
+ KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL )
+ return false;
+
+ LockEnvironment envlock(this);
+ res.set_boxes_size(pbody->GetLinks().size()); int index = 0;
+ FOREACHC(itlink, pbody->GetLinks()) {
+ OpenRAVE::AABB ab = (*itlink)->ComputeAABB();
+ openraveros::AABB& resbox = res.boxes[index++];
+ resbox.center[0] = ab.pos.x; resbox.center[1] = ab.pos.y; resbox.center[2] = ab.pos.z;
+ resbox.extents[0] = ab.extents.x; resbox.extents[1] = ab.extents.y; resbox.extents[2] = ab.extents.z;
+ }
+ }
+
+ bool body_getdof_srv(body_getdof::request& req, body_getdof::response& res)
+ {
+ KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL )
+ return false;
+ LockEnvironment envlock(this);
+ res.dof = pbody->GetDOF();
+ return true;
+ }
+
+ bool body_getlinks_srv(body_getlinks::request& req, body_getlinks::response& res)
+ {
+ KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL )
+ return false;
+
+ LockEnvironment envlock(this);
+ vector<Transform> vtrans; pbody->GetBodyTransformations(vtrans);
+
+ res.set_links_size(vtrans.size()); int index = 0;
+ FOREACH(ittrans, vtrans) {
+ TransformMatrix tm = TransformMatrix(*ittrans);
+ AffineTransformMatrix& am = res.links[index++];
+ am.transform[0] = tm.m[0]; am.transform[3] = tm.m[1]; am.transform[6] = tm.m[2];
+ am.transform[1] = tm.m[4]; am.transform[4] = tm.m[5]; am.transform[7] = tm.m[6];
+ am.transform[2] = tm.m[8]; am.transform[5] = tm.m[9]; am.transform[8] = tm.m[10];
+ am.transform[9] = tm.trans.x; am.transform[10] = tm.trans.y; am.transform[11] = tm.trans.z;
+ }
+ }
+
bool body_setjointvalues_srv(body_setjointvalues::request& req, body_setjointvalues::response& res);
bool body_settransform_srv(body_settransform::request& req, body_settransform::response& res);
bool env_checkcollision_srv(env_checkcollision::request& req, env_checkcollision::response& res);
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-06 18:39:11 UTC (rev 7733)
@@ -56,6 +56,7 @@
public:
SessionServer() {
+ _pParentEnvironment.reset(CreateEnvironment());
}
virtual ~SessionServer() {
Destroy();
@@ -194,6 +195,7 @@
private:
map<int,SessionState> _mapsessions;
boost::mutex _mutexsession;
+ boost::shared_ptr<EnvironmentBase> _pParentEnvironment;
SessionState getstate(int sessionid)
{
@@ -222,21 +224,23 @@
id = rand();
SessionState state;
- state._penv.reset(CreateEnvironment());
state._pserver.reset(new ROSServer(state._penv));
if( req.clone_sessionid ) {
// clone the environment from clone_sessionid
SessionState state = getstate(req.clone_sessionid);
- if( !state._penv ) {
- RAVEPRINT(L"failed to find session %d\n", req.clone_sessionid);
- }
- else {
- // clone
-
- }
+ if( !state._penv )
+ ROS_INFO("failed to find session %d\n", req.clone_sessionid);
+ else
+ state._penv.reset(state._penv->CloneSelf(req.clone_options));
}
+ if( !state._penv ) {
+ // cloning from parent
+ ROS_DEBUG("cloning from parent\n");
+ state._penv.reset(_pParentEnvironment->CloneSelf(0));
+ }
+
_mapsessions[id] = state;
res.sessionid = id;
@@ -284,6 +288,11 @@
};
// check that message constants match OpenRAVE constants
+BOOST_STATIC_ASSERT(EnvironmentBase::Clone_Bodies==openrave_session::request::CloneBodies);
+BOOST_STATIC_ASSERT(EnvironmentBase::Clone_Viewer==openrave_session::request::CloneViewer);
+BOOST_STATIC_ASSERT(EnvironmentBase::Clone_Simulation==openrave_session::request::CloneSimulation);
+BOOST_STATIC_ASSERT(EnvironmentBase::Clone_RealControllers==openrave_session::request::CloneRealControllers);
+
BOOST_STATIC_ASSERT(ActiveDOFs::DOF_X==RobotBase::DOF_X);
BOOST_STATIC_ASSERT(ActiveDOFs::DOF_Y==RobotBase::DOF_Y);
BOOST_STATIC_ASSERT(ActiveDOFs::DOF_Z==RobotBase::DOF_Z);
Modified: pkg/trunk/openrave_planning/openraveros/srv/body_enable.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/body_enable.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/body_enable.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -3,5 +3,5 @@
string sessionname
int32 sessionid
uint32 bodyid
-byte enable
+uint8 enable
---
Modified: pkg/trunk/openrave_planning/openraveros/srv/body_setjointvalues.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/body_setjointvalues.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/body_setjointvalues.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -5,5 +5,5 @@
uint32 bodyid
float32[] jointvalues
# optional array specifying the indices to set
-byte[] indices
+uint8[] indices
---
Modified: pkg/trunk/openrave_planning/openraveros/srv/env_checkcollision.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/env_checkcollision.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/env_checkcollision.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -5,7 +5,7 @@
uint32 bodyid
# the link of the body to check collision again, if -1 check all links
-byte linkid
+uint8 linkid
uint32[] excludedbodyids
@@ -13,15 +13,15 @@
float32 tolerance
# optional collision checker options, affects what information is sent back
-byte options
+uint8 options
-byte CO_Distance=1
-byte CO_UseTolerance=2
-byte CO_Contacts=4
-byte CO_RayAnyHit=8
+uint8 CO_Distance=1
+uint8 CO_UseTolerance=2
+uint8 CO_Contacts=4
+uint8 CO_RayAnyHit=8
---
-byte collision
+uint8 collision
uint32 collidingbodyid
Contact[] contacts
Modified: pkg/trunk/openrave_planning/openraveros/srv/env_createproblem.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/env_createproblem.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/env_createproblem.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -7,6 +7,6 @@
string args
# if true, will destroy any problems with similar types (default is 1)
-byte destroyduplicates
+uint8 destroyduplicates
---
uint32 problemid
Modified: pkg/trunk/openrave_planning/openraveros/srv/env_loadplugin.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/env_loadplugin.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/env_loadplugin.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -5,4 +5,4 @@
# absolute path of plugin recommended
string filename
---
-byte success
+uint8 success
Modified: pkg/trunk/openrave_planning/openraveros/srv/env_loadscene.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/env_loadscene.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/env_loadscene.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -5,4 +5,4 @@
# absolute path of scene recommended, specifying an empty string resets the entire scene
string filename
---
-byte success
+uint8 success
Modified: pkg/trunk/openrave_planning/openraveros/srv/env_plot.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/env_plot.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/env_plot.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -14,10 +14,10 @@
# [0,1] transparency of object (0 is opaque)
float32 transparency
-byte drawtype
+uint8 drawtype
-byte Draw_Point=0 # default
-byte Draw_LineStrip=1
-byte Draw_LineList=2
-byte Draw_TriList=3
-byte Draw_Sphere=4
+uint8 Draw_Point=0 # default
+uint8 Draw_LineStrip=1
+uint8 Draw_LineList=2
+uint8 Draw_TriList=3
+uint8 Draw_Sphere=4
Modified: pkg/trunk/openrave_planning/openraveros/srv/env_raycollision.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/env_raycollision.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/env_raycollision.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -8,9 +8,9 @@
uint32 bodyid
# if 1 will also return the contacts points of each of the rays (default is 0)
-byte request_contacts
+uint8 request_contacts
---
# collision - N dim vector that is 1 for colliding rays and 0 otherwise
-byte[] collision
+uint8[] collision
Contact[] contacts
Modified: pkg/trunk/openrave_planning/openraveros/srv/env_set.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/env_set.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/env_set.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -12,16 +12,16 @@
uint32 Set_Viewer=64
# validity depending on setmask
-byte sim_action
-byte SimAction_Start=1
-byte SimAction_Stop=2
-byte SimAction_Timestep=3
+uint8 sim_action
+uint8 SimAction_Start=1
+uint8 SimAction_Stop=2
+uint8 SimAction_Timestep=3
float32 sim_timestep
string physicsengine
string collisionchecker
string viewer
float32[3] gravity
-byte publishanytime
-byte debuglevel
+uint8 publishanytime
+uint8 debuglevel
---
Modified: pkg/trunk/openrave_planning/openraveros/srv/env_triangulate.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/env_triangulate.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/env_triangulate.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -5,7 +5,7 @@
# if 1, will only triangulate the bodies pointed to by ids
# if 0, will triangulate all objects except the bodies pointed to by ids (default)
# To triangulate everything, just set inclusive to 0 and have empty bodyids
-byte inclusive
+uint8 inclusive
uint32[] bodyids
---
Modified: pkg/trunk/openrave_planning/openraveros/srv/env_wait.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/env_wait.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/env_wait.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -13,4 +13,4 @@
---
# will return with success set to 0 if robot did not finish its commands by the timeout.
-byte success
+uint8 success
Modified: pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -9,9 +9,12 @@
int32 clone_sessionid
# mask of Clone* options
-byte clone_options
+uint8 clone_options
-byte CloneBodies=1 # copy all bodies
+uint8 CloneBodies=1 # copy all bodies
+uint8 CloneViewer=2 # copy current viewer
+uint8 CloneSimulation=4 # copy simulation settings
+uint8 CloneRealControllers=8 # copy real robot controllers
---
int32 sessionid
Modified: pkg/trunk/openrave_planning/openraveros/srv/planner_init.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/planner_init.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/planner_init.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -6,4 +6,4 @@
uint32 robotid
PlannerParameters params
---
-byte success
+uint8 success
Modified: pkg/trunk/openrave_planning/openraveros/srv/problem_sendcommand.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/problem_sendcommand.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/problem_sendcommand.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -10,7 +10,7 @@
# If 1, the SendCommand is called in the main thread, in sync
# with the rest of the primitives. If 0, called in a different thread. (default is 1)
-byte sync
+uint8 sync
---
# the concatenated output of all the problems that the command is sent to
string output
Modified: pkg/trunk/openrave_planning/openraveros/srv/robot_controllersend.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/robot_controllersend.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/robot_controllersend.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -8,4 +8,4 @@
string cmd
---
# 1 if command was accepted, 0 if not
-byte success
+uint8 success
Modified: pkg/trunk/openrave_planning/openraveros/srv/robot_controllerset.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/robot_controllerset.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/robot_controllerset.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -10,4 +10,4 @@
# the arguments to ControllerBase::Init
string controllerargs
---
-byte success
+uint8 success
Modified: pkg/trunk/openrave_planning/openraveros/srv/robot_sensorsend.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/robot_sensorsend.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/robot_sensorsend.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -10,7 +10,7 @@
uint32 sensorindex
string cmd
---
-byte success
+uint8 success
# output of the command
string out
Modified: pkg/trunk/openrave_planning/openraveros/srv/robot_setactivevalues.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/robot_setactivevalues.srv 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/openraveros/srv/robot_setactivevalues.srv 2008-12-06 18:39:11 UTC (rev 7733)
@@ -6,5 +6,5 @@
float32[] values
# indices into the active degree of freedom table
-byte[] indices
+uint8[] indices
---
Modified: pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2008-12-06 08:48:48 UTC (rev 7732)
+++ pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2008-12-06 18:39:11 UTC (rev 7733)
@@ -73,4 +73,8 @@
return true;
}
+void DECL_STDCALL(DestroyPlugin, ())
+{
}
+
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|