Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7748] pkg/trunk
|
From: <rdi...@us...> - 2008-12-07 09:37:27
|
Revision: 7748
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7748&view=rev
Author: rdiankov
Date: 2008-12-07 09:37:19 +0000 (Sun, 07 Dec 2008)
Log Message:
-----------
openrave session server finished (not fully tested with a client yet, but all planned functionality is there)
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/msg/ActiveDOFs.msg
pkg/trunk/openrave_planning/openraveros/msg/AffineTransformMatrix.msg
pkg/trunk/openrave_planning/openraveros/msg/AttachedSensor.msg
pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg
pkg/trunk/openrave_planning/openraveros/msg/Manipulator.msg
pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
pkg/trunk/openrave_planning/openraveros/msg/Trajectory.msg
pkg/trunk/openrave_planning/openraveros/msg/TrajectoryPoint.msg
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_destroy.srv
pkg/trunk/openrave_planning/openraveros/srv/body_enable.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getaabb.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getaabbs.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getdof.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getlinks.srv
pkg/trunk/openrave_planning/openraveros/srv/body_setjointvalues.srv
pkg/trunk/openrave_planning/openraveros/srv/body_settransform.srv
pkg/trunk/openrave_planning/openraveros/srv/env_checkcollision.srv
pkg/trunk/openrave_planning/openraveros/srv/env_closefigures.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createbody.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createrobot.srv
pkg/trunk/openrave_planning/openraveros/srv/env_getbodies.srv
pkg/trunk/openrave_planning/openraveros/srv/env_getbody.srv
pkg/trunk/openrave_planning/openraveros/srv/env_getrobots.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_triangulate.srv
pkg/trunk/openrave_planning/openraveros/srv/env_wait.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_getactivevalues.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_sensorgetdata.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_sensorsend.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_setactivedofs.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_setactivevalues.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_starttrajectory.srv
Added Paths:
-----------
pkg/trunk/openrave_planning/openraveros/srv/body_getjointvalues.srv
Removed Paths:
-------------
pkg/trunk/openrave_planning/openraveros/srv/robot_getactivedof.srv
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-07 09:37:19 UTC (rev 7748)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 532
+SVN_REVISION = -r 536
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/msg/ActiveDOFs.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/ActiveDOFs.msg 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/openrave_planning/openraveros/msg/ActiveDOFs.msg 2008-12-07 09:37:19 UTC (rev 7748)
@@ -1,11 +1,17 @@
# specifies the active degrees of freedom of the robot
-byte[] activedofs
-byte affinedofs
+# mask of active base degrees of freedom
+uint8 affine
+
+# active joints
+uint8[] joints
+
+float32[3] rotationaxis
+
# mask for affine dofs
-byte DOF_X = 1
-byte DOF_Y = 2
-byte DOF_Z = 4
-byte DOF_RotationAxis = 8
-byte DOF_Rotation3D = 16
-byte DOF_RotationQuat = 32
+uint8 DOF_X = 1
+uint8 DOF_Y = 2
+uint8 DOF_Z = 4
+uint8 DOF_RotationAxis = 8
+uint8 DOF_Rotation3D = 16
+uint8 DOF_RotationQuat = 32
Modified: pkg/trunk/openrave_planning/openraveros/msg/AffineTransformMatrix.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/AffineTransformMatrix.msg 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/openrave_planning/openraveros/msg/AffineTransformMatrix.msg 2008-12-07 09:37:19 UTC (rev 7748)
@@ -3,4 +3,4 @@
# [0] [3] [6] [9]
# [1] [4] [7] [10]
# [2] [5] [8] [11]
-float32[12] transform
\ No newline at end of file
+float32[12] m
Modified: pkg/trunk/openrave_planning/openraveros/msg/AttachedSensor.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/AttachedSensor.msg 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/openrave_planning/openraveros/msg/AttachedSensor.msg 2008-12-07 09:37:19 UTC (rev 7748)
@@ -4,10 +4,13 @@
string name
# zero-based index of link sensor is attached to
-byte attachedlink
+int8 attachedlink
# 3x4 matrix of the relative transform of the sensor frame with respect to the link frame
AffineTransformMatrix trelative
+# 3x4 matrix of the global transform of the sensor (ie, with link transform applied)
+AffineTransformMatrix tglobal
+
# type of sensor
string type
Modified: pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg 2008-12-07 09:37:19 UTC (rev 7748)
@@ -1,6 +1,16 @@
# information about a body
-uint32 bodyid
+int32 bodyid
+# current transformation
+AffineTransformMatrix transform
+
+# degrees of freedom (number of joints)
+uint8 dof
+
+# enabled status
+uint8 enabled
+
+
# filename used to create body geometry
string filename
@@ -10,25 +20,17 @@
# type of body
string type
-# current transformation
-AffineTransformMatrix transform
-
-# degrees of freedom (number of joints)
-byte dof
-
-# enabled status
-byte enabled
-
float32[] jointvalues
AffineTransformMatrix[] links
string[] linknames
-# request information bitmasks, also holds robot specific request information
-uint16 Req_Transform=1
-uint16 Req_JointValues=2
-uint16 Req_Links=4
-uint16 Req_LinkNames=8
-uint16 Req_JointLimits=16
-uint16 Req_RobotManipulators=32
-uint16 Req_RobotSensors=64
-uint16 Req_RobotActiveDOFs=128
+# joint limits
+float32[] lowerlimit
+float32[] upperlimit
+
+# request information bitmasks, also holds robot specific request information (lower 8 bits)
+uint16 Req_JointValues=1
+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
Modified: pkg/trunk/openrave_planning/openraveros/msg/Manipulator.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/Manipulator.msg 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/openrave_planning/openraveros/msg/Manipulator.msg 2008-12-07 09:37:19 UTC (rev 7748)
@@ -1,19 +1,19 @@
# information specific to a manipulator for a robot
# zero-based index of base link that manipulator is attached to
-byte baselink
+int8 baselink
# zero-based index of link defining the end-effector
-byte eelink
+int8 eelink
# 3x4 matrix of the grasp frame relative to the end-effector link
AffineTransformMatrix tgrasp
# zero-based hand joint indices
-byte[] handjoints
+uint8[] handjoints
# zero-based arm joints indices (from base to end-effector)
-byte[] armjoints
+uint8[] armjoints
# name of ik solver using
string iksolvername
Modified: pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-07 09:37:19 UTC (rev 7748)
@@ -3,4 +3,14 @@
BodyInfo bodyinfo
Manipulator[] manips
AttachedSensor[] sensors
-ActiveDOFs dofs
+
+# total active dof
+uint8 activedof
+
+# information about the active dofs
+ActiveDOFs active
+
+# upper 8 bits
+uint16 Req_Manipulators=256
+uint16 Req_Sensors=512
+uint16 Req_ActiveDOFs=1024
Modified: pkg/trunk/openrave_planning/openraveros/msg/Trajectory.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/Trajectory.msg 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/openrave_planning/openraveros/msg/Trajectory.msg 2008-12-07 09:37:19 UTC (rev 7748)
@@ -1,2 +1,2 @@
TrajectoryPoint[] points
-ActiveDOFs dofs
+ActiveDOFs active
Modified: pkg/trunk/openrave_planning/openraveros/msg/TrajectoryPoint.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/TrajectoryPoint.msg 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/openrave_planning/openraveros/msg/TrajectoryPoint.msg 2008-12-07 09:37:19 UTC (rev 7748)
@@ -2,6 +2,6 @@
float32[] position
# optional
-float32[] velocities
+float32[] velocity
AffineTransformMatrix transform
float32 timestamp
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2008-12-07 09:37:19 UTC (rev 7748)
@@ -74,6 +74,26 @@
#define stricmp strcasecmp
#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;
+}
+
+inline std::string _stdwcstombs(const wchar_t* pname)
+{
+ std::string s;
+ size_t len = wcstombs(NULL, pname, 0);
+ if( len != (size_t)-1 ) {
+ s.resize(len);
+ wcstombs(&s[0], pname, len);
+ }
+
+ return s;
+}
+
#include <openrave-core.h>
#include <ros/node.h>
@@ -88,6 +108,7 @@
#include <openraveros/body_getaabb.h>
#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>
@@ -113,7 +134,6 @@
#include <openraveros/problem_sendcommand.h>
#include <openraveros/robot_controllersend.h>
#include <openraveros/robot_controllerset.h>
-#include <openraveros/robot_getactivedof.h>
#include <openraveros/robot_getactivevalues.h>
#include <openraveros/robot_sensorgetdata.h>
#include <openraveros/robot_sensorsend.h>
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-07 08:15:41 UTC (rev 7747)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-07 09:37:19 UTC (rev 7748)
@@ -31,17 +31,27 @@
class LockEnvironment
{
public:
- LockEnvironment(ROSServer* pserv) : _pserv(pserv) { _pserv->_penv->LockPhysics(true); }
- ~LockEnvironment() { _pserv->_penv->LockPhysics(false); }
+ LockEnvironment(ROSServer* pserv) : _pserv(pserv) { _pserv->GetEnv()->LockPhysics(true); }
+ ~LockEnvironment() { _pserv->GetEnv()->LockPhysics(false); }
private:
ROSServer* _pserv;
};
+ class FIGURE
+ {
+ public:
+ FIGURE(EnvironmentBase* penv, void* handle) : _handle(handle) {}
+ ~FIGURE() { _penv->closegraph(_handle); }
+ private:
+ EnvironmentBase* _penv;
+ void* _handle;
+ };
+
public:
- ROSServer(boost::shared_ptr<EnvironmentBase> penv) : RaveServerBase(penv.get()), _nNextFigureId(1), _nNextPlannerId(1) {
- _penv = penv;
+ ROSServer(EnvironmentBase* penv) : RaveServerBase(penv), _nNextFigureId(1), _nNextPlannerId(1), _nNextProblemId(1) {
_bThreadDestroyed = false;
+ _bCloseClient = false;
_fSimulationTimestep = 0.01;
_vgravity = Vector(0,0,-9.8f);
}
@@ -51,11 +61,12 @@
virtual void Destroy()
{
+ _bCloseClient = true;
Reset();
- _penv->SetCollisionChecker(NULL);
- _penv->SetPhysicsEngine(NULL);
- _penv->AttachViewer(NULL);
+ GetEnv()->SetCollisionChecker(NULL);
+ GetEnv()->SetPhysicsEngine(NULL);
+ GetEnv()->AttachViewer(NULL);
// have to maintain a certain destruction order
_pphysics.reset();
@@ -66,12 +77,12 @@
_threadviewer.join();
_pviewer.reset();
}
+
+ _bCloseClient = false;
}
virtual void Reset()
{
- FOREACH(it, _mapFigureIds)
- _penv->closegraph(it->second);
_mapFigureIds.clear();
// destroy environment specific state: problems, planners, figures
@@ -96,22 +107,22 @@
virtual bool IsClosing()
{
- return false;
+ return _bCloseClient;
}
bool body_destroy_srv(body_destroy::request& req, body_destroy::response& res)
{
- KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
if( pbody == NULL )
return false;
LockEnvironment envlock(this);
- return _penv->RemoveKinBody(pbody,true);
+ return GetEnv()->RemoveKinBody(pbody,true);
}
bool body_enable_srv(body_enable::request& req, body_enable::response& res)
{
- KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
if( pbody == NULL )
return false;
@@ -122,7 +133,7 @@
bool body_getaabb_srv(body_getaabb::request& req, body_getaabb::response& res)
{
- KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
if( pbody == NULL )
return false;
@@ -135,12 +146,12 @@
bool body_getaabbs_srv(body_getaabbs::request& req, body_getaabbs::response& res)
{
- KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
if( pbody == NULL )
return false;
LockEnvironment envlock(this);
- res.set_boxes_size(pbody->GetLinks().size()); int index = 0;
+ res.boxes.resize(pbody->GetLinks().size()); int index = 0;
FOREACHC(itlink, pbody->GetLinks()) {
OpenRAVE::AABB ab = (*itlink)->ComputeAABB();
openraveros::AABB& resbox = res.boxes[index++];
@@ -151,77 +162,631 @@
bool body_getdof_srv(body_getdof::request& req, body_getdof::response& res)
{
- KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
if( pbody == NULL )
return false;
- LockEnvironment envlock(this);
res.dof = pbody->GetDOF();
return true;
}
+ bool body_getjointvalues_srv(body_getjointvalues::request& req, body_getjointvalues::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL )
+ return false;
+ LockEnvironment envlock(this);
+
+ if( req.indices.size() > 0 ) {
+ vector<dReal> vtemp;
+ pbody->GetJointValues(vtemp);
+
+ res.values.resize(req.indices.size());
+ for(size_t i = 0; i < req.indices.size(); ++i) {
+ ROS_ASSERT( req.indices[i] < vtemp.size() );
+ res.values[i] = vtemp[req.indices[i]];
+ }
+ }
+ else {
+ vector<dReal> vtemp;
+ pbody->GetJointValues(vtemp);
+
+ res.values.resize(vtemp.size());
+ for(size_t i = 0; i < req.indices.size(); ++i)
+ res.values[i] = vtemp[i];
+ }
+
+ return true;
+ }
+
bool body_getlinks_srv(body_getlinks::request& req, body_getlinks::response& res)
{
- KinBody* pbody = _penv->GetBodyFromNetworkId(req.bodyid);
+ KinBody* pbody = GetEnv()->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;
+ 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);
+ if( pbody == NULL )
+ return false;
+
+ if( pbody->GetDOF() == 0 )
+ return false;
+
+ LockEnvironment envlock(this);
+ vector<dReal> vvalues;
+
+ if( req.indices.size() > 0 ) {
+ if( req.indices.size() != req.jointvalues.size() ) {
+ RAVELOG_WARNA("indices (%d) different size than joint values (%d)\n", req.indices.size(), req.jointvalues.size());
+ return false;
+ }
+
+ pbody->GetJointValues(vvalues);
+ for(uint32_t i = 0; i < req.indices.size(); ++i)
+ vvalues[req.indices[i]] = req.jointvalues[i];
}
+ else {
+ if( pbody->GetDOF() != (int)req.jointvalues.size() ) {
+ RAVELOG_WARNA("body dof (%d) not equal to jointvalues (%d)", pbody->GetDOF(), req.jointvalues.size());
+ return false;
+ }
+
+ vvalues.reserve(req.jointvalues.size());
+ FOREACHC(it,req.jointvalues)
+ vvalues.push_back(*it);
+ }
+
+ pbody->SetJointValues(NULL, NULL, &vvalues[0], true);
+
+ if( pbody->IsRobot() ) {
+ // if robot, should turn off any trajectory following
+ RobotBase* probot = (RobotBase*)pbody;
+ if( probot->GetController() != NULL ) {
+ probot->GetJointValues(vvalues); // reget the values since they'll go through the joint limits
+ probot->GetController()->SetDesired(&vvalues[0]);
+ }
+ }
+
+ return true;
}
- 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);
- bool env_closefigures_srv(env_closefigures::request& req, env_closefigures::response& res);
- bool env_createbody_srv(env_createbody::request& req, env_createbody::response& res);
- bool env_createplanner_srv(env_createplanner::request& req, env_createplanner::response& res);
- bool env_createproblem_srv(env_createproblem::request& req, env_createproblem::response& res);
- bool env_createrobot_srv(env_createrobot::request& req, env_createrobot::response& res);
- bool env_destroyproblem_srv(env_destroyproblem::request& req, env_destroyproblem::response& res);
- bool env_getbodies_srv(env_getbodies::request& req, env_getbodies::response& res);
- bool env_getbody_srv(env_getbody::request& req, env_getbody::response& res);
- bool env_getrobots_srv(env_getrobots::request& req, env_getrobots::response& res);
- bool env_loadplugin_srv(env_loadplugin::request& req, env_loadplugin::response& res);
- bool env_loadscene_srv(env_loadscene::request& req, env_loadscene::response& res);
- bool env_plot_srv(env_plot::request& req, env_plot::response& res);
- bool env_raycollision_srv(env_raycollision::request& req, env_raycollision::response& res);
+ bool body_settransform_srv(body_settransform::request& req, body_settransform::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL )
+ return false;
+
+ TransformMatrix tm;
+ tm.m[0] = req.transform.m[0]; tm.m[1] = req.transform.m[3]; tm.m[2] = req.transform.m[6];
+ tm.m[4] = req.transform.m[1]; tm.m[5] = req.transform.m[4]; tm.m[6] = req.transform.m[7];
+ tm.m[8] = req.transform.m[2]; tm.m[9] = req.transform.m[5]; tm.m[10] = req.transform.m[8];
+ tm.trans.x = req.transform.m[9]; tm.trans.y = req.transform.m[10]; tm.trans.z = req.transform.m[11];
+
+ Transform t = tm;
+ LockEnvironment envlock(this);
+ pbody->SetTransform(tm);
+
+ if( pbody->IsRobot() ) {
+ RobotBase* probot = (RobotBase*)pbody;
+ if( probot->GetController() != NULL )
+ probot->GetController()->SetPath(NULL);
+ }
+
+ return true;
+ }
+
+ bool env_checkcollision_srv(env_checkcollision::request& req, env_checkcollision::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL )
+ return false;
+
+ set<KinBody*> setignore;
+ FOREACH(it, req.excludedbodyids) {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL )
+ return false;
+ setignore.insert(pbody);
+ }
+
+ COLLISIONREPORT report;
+ {
+ LockEnvironment envlock(this);
+ int oldopts = GetEnv()->GetCollisionOptions();
+ if( !GetEnv()->SetCollisionOptions(req.options) )
+ RAVELOG_WARNA("failed to set collision options\n");
+
+ set<KinBody::Link*> empty;
+ if( req.linkid < 0 )
+ res.collision = GetEnv()->CheckCollision(pbody, setignore, empty, &report);
+ else {
+ if( req.linkid >= (int)pbody->GetLinks().size() )
+ return false;
+ res.collision = GetEnv()->CheckCollision(pbody->GetLinks()[req.linkid], setignore, empty, &report);
+ }
+
+ GetEnv()->SetCollisionOptions(oldopts);
+ }
+
+ res.collidingbodyid = 0;
+
+ if( res.collision ) {
+ KinBody::Link* plinkcol = report.plink1;
+ if( report.plink2 && report.plink2->GetParent() != pbody ) {
+ ROS_ASSERT(report.plink1->GetParent() == pbody);
+ plinkcol = report.plink2;
+ }
+
+ if( plinkcol != NULL ) {
+ res.collidingbodyid = plinkcol->GetParent()->GetNetworkId();
+ res.collidinglink = plinkcol->GetIndex();
+ }
+
+ RAVELOG_INFOA("collision %S:%S with %S:%S\n",
+ report.plink1?report.plink1->GetParent()->GetName():L"(NULL",
+ report.plink1?report.plink1->GetName():L"(NULL)",
+ report.plink2?report.plink2->GetParent()->GetName():L"(NULL)",
+ report.plink2?report.plink2->GetName():L"(NULL)");
+ }
+
+ if( req.options & CO_Distance )
+ res.mindist = report.minDistance;
+ if( req.options & CO_Contacts ) {
+ res.contacts.resize(report.contacts.size()); int index = 0;
+ FOREACHC(itc, report.contacts) {
+ openraveros::Contact& c = res.contacts[index++];
+ c.position[0] = itc->pos.x; c.position[1] = itc->pos.y; c.position[2] = itc->pos.z;
+ c.normal[0] = itc->norm.x; c.normal[1] = itc->norm.y; c.normal[2] = itc->norm.z;
+ }
+ }
+
+ return true;
+ }
+
+ bool env_closefigures_srv(env_closefigures::request& req, env_closefigures::response& res)
+ {
+ bool bSuccess = true;
+
+ if( req.figureids.size() > 0 ) {
+ FOREACH(itid, req.figureids) {
+ if( !_mapFigureIds.erase(*itid) )
+ bSuccess = false;
+ }
+ }
+ else // destroy everything
+ _mapFigureIds.clear();
+
+ return bSuccess;
+ }
+
+ bool env_createbody_srv(env_createbody::request& req, env_createbody::response& res)
+ {
+ LockEnvironment envlock(this);
+ KinBody* pbody = GetEnv()->CreateKinBody();
+
+ if( req.file.size() > 0 ) {
+ if( !pbody->Init(req.file.c_str(), NULL) )
+ return false;
+ }
+
+ pbody->SetName(req.name.c_str());
+
+ if( !GetEnv()->AddKinBody(pbody) )
+ return false;
+
+ res.bodyid = pbody->GetNetworkId();
+ return true;
+ }
+
+ bool env_createplanner_srv(env_createplanner::request& req, env_createplanner::response& res)
+ {
+ boost::shared_ptr<PlannerBase> pplanner(GetEnv()->CreatePlanner(req.plannertype.c_str()));
+ if( !pplanner )
+ return false;
+
+ _mapplanners[++_nNextPlannerId] = pplanner;
+ res.plannerid = _nNextPlannerId;
+ return true;
+ }
+
+ bool env_createproblem_srv(env_createproblem::request& req, env_createproblem::response& res)
+ {
+ boost::shared_ptr<ProblemInstance> pproblem(GetEnv()->CreateProblem(req.problemtype.c_str()));
+ if( !pproblem )
+ return false;
+
+ if( req.destroyduplicates ) {
+ map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.begin();
+ while(itprob != _mapproblems.end()) {
+ if( stricmp(itprob->second->GetXMLId(), req.problemtype.c_str()) == 0 ) {
+ RAVELOG_INFOA("deleting duplicate problem %s\n", req.problemtype.c_str());
+ if( !GetEnv()->RemoveProblem(itprob->second.get()) )
+ RAVELOG_WARNA("failed to remove problem %s\n", itprob->second->GetXMLId());
+
+ _mapproblems.erase(itprob++);
+ }
+ else
+ ++itprob;
+ }
+ }
+
+ if( !GetEnv()->LoadProblem(pproblem.get(), req.args.c_str()) )
+ return false;
+
+ _mapproblems[++_nNextProblemId] = pproblem;
+ res.problemid = _nNextProblemId;
+ return true;
+ }
+
+ bool env_createrobot_srv(env_createrobot::request& req, env_createrobot::response& res)
+ {
+ RobotBase* probot = GetEnv()->CreateRobot(req.type.c_str());
+ if( !probot )
+ return false;
+
+ if( req.file.size() > 0 ) {
+ if( !probot->Init(req.file.c_str(), NULL) )
+ return false;
+ }
+
+ probot->SetName(req.name.c_str());
+
+ if( !GetEnv()->AddRobot(probot) )
+ return false;
+
+ res.bodyid = probot->GetNetworkId();
+ return true;
+ }
+
+ bool env_destroyproblem_srv(env_destroyproblem::request& req, env_destroyproblem::response& res)
+ {
+ map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.find(req.problemid);
+ if( itprob == _mapproblems.end() )
+ return false;
+
+ if( !GetEnv()->RemoveProblem(itprob->second.get()) )
+ RAVELOG_WARNA("failed to remove problem\n");
+
+ _mapproblems.erase(itprob);
+ return true;
+ }
+
+ void FillKinBodyInfo(KinBody* pbody, BodyInfo& info, uint32_t options)
+ {
+ info.bodyid = pbody->GetDOF();
+ info.transform = GetAffineTransform(pbody->GetTransform());
+ info.enabled = pbody->IsEnabled();
+
+ if( options & BodyInfo::Req_JointValues ) {
+ vector<dReal> vvalues; pbody->GetJointValues(vvalues);
+ info.jointvalues.resize(vvalues.size());
+ for(size_t i = 0; i < vvalues.size(); ++i)
+ info.jointvalues[i] = vvalues[i];
+ }
+ if( options & BodyInfo::Req_Links ) {
+ vector<Transform> vlinks; pbody->GetBodyTransformations(vlinks);
+ info.links.resize(vlinks.size());
+ for(size_t i = 0; i < vlinks.size(); ++i)
+ info.links[i] = GetAffineTransform(vlinks[i]);
+ }
+ if( options & BodyInfo::Req_LinkNames ) {
+ info.linknames.resize(pbody->GetLinks().size());
+ for(size_t i = 0; i < info.linknames.size(); ++i)
+ info.linknames[i] = _stdwcstombs(pbody->GetLinks()[i]->GetName());
+ }
+ if( options & BodyInfo::Req_JointLimits ) {
+ vector<dReal> vlower, vupper;
+ pbody->GetJointLimits(vlower,vupper);
+ ROS_ASSERT( vlower.size() == vupper.size() );
+ info.lowerlimit.resize(vlower.size());
+ info.upperlimit.resize(vupper.size());
+ for(size_t i = 0; i < vlower.size(); ++i) {
+ info.lowerlimit[i] = vlower[i];
+ info.upperlimit[i] = vupper[i];
+ }
+ }
+ if( options & BodyInfo::Req_Names ) {
+ info.filename = pbody->GetXMLFilename();
+ info.name = _stdwcstombs(pbody->GetName());
+ info.type = pbody->GetXMLId();
+ }
+ }
+
+ bool env_getbodies_srv(env_getbodies::request& req, env_getbodies::response& res)
+ {
+ vector<KinBody*> vbodies;
+ boost::shared_ptr<EnvironmentBase::EnvLock> lock(GetEnv()->GetLockedBodies(vbodies));
+
+ if( req.bodyid != 0 ) {
+ KinBody* pfound = NULL;
+ FOREACH(it, vbodies) {
+ if( (*it)->GetNetworkId() == req.bodyid ) {
+ pfound = *it;
+ break;
+ }
+ }
+
+ if( pfound == NULL )
+ return false;
+
+ // add only one body
+ vbodies.resize(0); vbodies.push_back(pfound);
+ }
+
+ res.bodies.resize(vbodies.size()); int index = 0;
+ FOREACH(itbody, vbodies) {
+ openraveros::BodyInfo& info = res.bodies[index++];
+ FillKinBodyInfo(*itbody, info, req.options);
+ }
+
+ return true;
+ }
+
+ bool env_getbody_srv(env_getbody::request& req, env_getbody::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetKinBody(_ravembstowcs(req.name.c_str()).c_str());
+ if( pbody == NULL )
+ return false;
+ res.bodyid = pbody->GetNetworkId();
+ return true;
+ }
+
+ void FillActiveDOFs(RobotBase* probot, openraveros::ActiveDOFs& active)
+ {
+ active.affine = probot->GetAffineDOF();
+ active.joints.resize(probot->GetActiveJointIndices().size());
+ for(size_t i = 0; i < probot->GetActiveJointIndices().size(); ++i)
+ active.joints[i] = probot->GetActiveJointIndices()[i];
+ active.rotationaxis[0] = probot->GetAffineRotationAxis().x;
+ active.rotationaxis[1] = probot->GetAffineRotationAxis().y;
+ active.rotationaxis[2] = probot->GetAffineRotationAxis().z;
+ }
+
+ void FillRobotInfo(RobotBase* probot, RobotInfo& info, uint32_t options)
+ {
+ FillKinBodyInfo(probot,info.bodyinfo,options);
+
+ if( options & RobotInfo::Req_Manipulators ) {
+ info.manips.resize(probot->GetManipulators().size()); int index = 0;
+ FOREACHC(itmanip, probot->GetManipulators()) {
+ openraveros::Manipulator& rosmanip = info.manips[index++];
+ rosmanip.baselink = itmanip->pBase != NULL ? itmanip->pBase->GetIndex() : -1;
+ rosmanip.eelink = itmanip->pEndEffector != NULL ? itmanip->pEndEffector->GetIndex() : -1;
+ rosmanip.tgrasp = GetAffineTransform(itmanip->tGrasp);
+
+ rosmanip.handjoints.resize(itmanip->_vecjoints.size());
+ for(size_t i = 0; i < itmanip->_vecjoints.size(); ++i)
+ rosmanip.handjoints[i] = itmanip->_vecjoints[i];
+
+ rosmanip.armjoints.resize(itmanip->_vecarmjoints.size());
+ for(size_t i = 0; i < itmanip->_vecarmjoints.size(); ++i)
+ rosmanip.armjoints[i] = itmanip->_vecarmjoints[i];
+
+ rosmanip.iksolvername = _stdwcstombs(itmanip->GetIKSolverName().c_str());
+ }
+ }
+ if( options & RobotInfo::Req_Sensors ) {
+ info.sensors.resize(probot->GetSensors().size()); int index = 0;
+ FOREACHC(its, probot->GetSensors()) {
+ openraveros::AttachedSensor& rossensor = info.sensors[index++];
+ rossensor.name = _stdwcstombs(its->GetName());
+ rossensor.attachedlink = its->GetAttachingLink()->GetIndex();
+ rossensor.trelative = GetAffineTransform(its->GetRelativeTransform());
+
+ if( its->GetSensor() != NULL ) {
+ rossensor.tglobal = GetAffineTransform(its->GetSensor()->GetTransform());
+ rossensor.type = its->GetSensor()->GetXMLId();
+ }
+ else {
+ rossensor.tglobal = GetAffineIdentity();
+ rossensor.type = "";
+ }
+ }
+ }
+ if( options & RobotInfo::Req_ActiveDOFs ) {
+ FillActiveDOFs(probot, info.active);
+ info.activedof = probot->GetActiveDOF();
+ }
+ }
+
+ bool env_getrobots_srv(env_getrobots::request& req, env_getrobots::response& res)
+ {
+ vector<RobotBase*> vrobots;
+ boost::shared_ptr<EnvironmentBase::EnvLock> lock(GetEnv()->GetLockedRobots(vrobots));
+
+ if( req.bodyid != 0 ) {
+ RobotBase* pfound = NULL;
+ FOREACH(it, vrobots) {
+ if( (*it)->GetNetworkId() == req.bodyid ) {
+ pfound = *it;
+ break;
+ }
+ }
+
+ if( pfound == NULL )
+ return false;
+
+ // add only one body
+ vrobots.resize(0); vrobots.push_back(pfound);
+ }
+
+ res.robots.resize(vrobots.size()); int index = 0;
+ FOREACH(itrobot, vrobots) {
+ ROS_ASSERT( (*itrobot)->IsRobot() );
+ openraveros::RobotInfo& info = res.robots[index++];
+ FillRobotInfo(*itrobot, info, req.options);
+ }
+
+ return true;
+ }
+
+ bool env_loadplugin_srv(env_loadplugin::request& req, env_loadplugin::response& res)
+ {
+ LockEnvironment envlock(this);
+ return GetEnv()->LoadPlugin(req.filename.c_str());
+ }
+
+ bool env_loadscene_srv(env_loadscene::request& req, env_loadscene::response& res)
+ {
+ if( req.resetscene ) {
+ RAVELOG_INFOA("resetting scene\n");
+ GetEnv()->Reset();
+ }
+
+ if( req.filename.size() > 0 ) {
+ LockEnvironment envlock(this);
+ return GetEnv()->Load(req.filename.c_str());
+ }
+
+ return true;
+ }
+
+ bool env_plot_srv(env_plot::request& req, env_plot::response& res)
+ {
+ bool bOneColor = req.colors.size() != req.points.size();
+ float falpha = max(0.0f, 1.0f-req.transparency);
+ falpha = min(1.0f,falpha);
+ RaveVector<float> vOneColor(1.0f,0.5f,0.5f,falpha);
+
+ void* figure = NULL;
+ switch(req.drawtype) {
+ case env_plot::request::Draw_Point:
+ if( bOneColor )
+ figure = GetEnv()->plot3(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,vOneColor, 0);
+ else
+ figure = GetEnv()->plot3(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,&req.colors[0], 0);
+ break;
+ case env_plot::request::Draw_LineStrip:
+ if( bOneColor )
+ figure = GetEnv()->drawlinestrip(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,vOneColor);
+ else
+ figure = GetEnv()->drawlinestrip(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,&req.colors[0]);
+ break;
+ case env_plot::request::Draw_LineList:
+ if( bOneColor )
+ figure = GetEnv()->drawlinelist(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,vOneColor);
+ else
+ figure = GetEnv()->drawlinelist(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,&req.colors[0]);
+ break;
+ case env_plot::request::Draw_TriList:
+ //if( bOneColor )
+ figure = GetEnv()->drawtrimesh(&req.points[0],3*sizeof(req.points[0]), NULL, req.points.size()/3, vOneColor);
+ //else
+ //figure = GetEnv()->plot3(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,&req.colors[0], 0);
+ break;
+ case env_plot::request::Draw_Sphere:
+ if( bOneColor )
+ figure = GetEnv()->plot3(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,vOneColor, 1);
+ else
+ figure = GetEnv()->plot3(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,&req.colors[0], 1);
+ break;
+ default:
+ return false;
+ }
+
+ if( figure == NULL )
+ return false;
+
+ _mapFigureIds[++_nNextFigureId].reset(new FIGURE(GetEnv(), figure));
+ res.figureid = _nNextFigureId;
+ return true;
+ }
+
+ bool env_raycollision_srv(env_raycollision::request& req, env_raycollision::response& res)
+ {
+ KinBody* pbody = req.bodyid != 0 ? GetEnv()->GetBodyFromNetworkId(req.bodyid) : NULL;
+
+ res.collision.reserve(req.rays.size()); res.collision.resize(0);
+
+ if( req.request_contacts ) {
+ res.contacts.reserve(req.rays.size());
+ res.contacts.resize(0);
+ }
+
+ if( req.request_bodies ) {
+ res.hitbodies.reserve(req.rays.size());
+ res.hitbodies.resize(0);
+ }
+
+ COLLISIONREPORT report;
+ {
+ LockEnvironment envlock(this);
+ int oldopts = GetEnv()->GetCollisionOptions();
+ if( !GetEnv()->SetCollisionOptions(req.request_contacts ? CO_Contacts : 0) )
+ RAVELOG_WARNA("failed to set collision options\n");
+
+ FOREACHC(itray, req.rays) {
+ RAY r;
+ r.pos = Vector(itray->position[0], itray->position[1], itray->position[2]);
+ r.dir = Vector(itray->direction[0], itray->direction[1], itray->direction[2]);
+
+ uint8_t bCollision;
+ if( pbody != NULL )
+ bCollision = GetEnv()->CheckCollision(r,pbody,&report);
+ else
+ bCollision = GetEnv()->CheckCollision(r,&report);
+
+ res.collision.push_back(bCollision);
+ if( req.request_contacts ) {
+ openraveros::Contact rosc;
+ if( report.contacts.size() > 0 ) {
+ COLLISIONREPORT::CONTACT& c = report.contacts.front();
+ rosc.position[0] = c.pos.x; rosc.position[1] = c.pos.y; rosc.position[2] = c.pos.z;
+ rosc.normal[0] = c.norm.x; rosc.normal[1] = c.norm.y; rosc.normal[2] = c.norm.z;
+ }
+ res.contacts.push_back(rosc);
+ }
+
+ if( req.request_bodies ) {
+ KinBody::Link* plink = report.plink1 != NULL ? report.plink1 : report.plink2;
+ res.hitbodies.push_back(plink != NULL ? plink->GetParent()->GetNetworkId() : 0);
+ }
+ }
+
+ GetEnv()->SetCollisionOptions(oldopts);
+ }
+
+ return true;
+ }
+
bool env_set_srv(env_set::request& req, env_set::response& res)
{
if( req.setmask & env_set::request::Set_Simulation ) {
switch(req.sim_action) {
case env_set::request::SimAction_Start:
- _penv->StartSimulation(_fSimulationTimestep);
+ GetEnv()->StartSimulation(_fSimulationTimestep);
break;
case env_set::request::SimAction_Stop:
- _penv->StopSimulation();
+ GetEnv()->StopSimulation();
break;
case env_set::request::SimAction_Timestep:
_fSimulationTimestep = req.sim_timestep;
- _penv->StartSimulation(_fSimulationTimestep);
+ GetEnv()->StartSimulation(_fSimulationTimestep);
break;
}
}
if( req.setmask & env_set::request::Set_PhysicsEngine ) {
- _pphysics.reset(_penv->CreatePhysicsEngine(req.physicsengine.c_str()));
- _penv->SetPhysicsEngine(_pphysics.get());
+ _pphysics.reset(GetEnv()->CreatePhysicsEngine(req.physicsengine.c_str()));
+ GetEnv()->SetPhysicsEngine(_pphysics.get());
if( !!_pphysics )
_pphysics->SetGravity(_vgravity);
}
if( req.setmask & env_set::request::Set_CollisionChecker ) {
- int options = _penv->GetCollisionOptions();
- _pcolchecker.reset(_penv->CreateCollisionChecker(req.collisionchecker.c_str()));
- _penv->SetCollisionChecker(_pcolchecker.get());
- _penv->SetCollisionOptions(options);
+ int options = GetEnv()->GetCollisionOptions();
+ _pcolchecker.reset(GetEnv()->CreateCollisionChecker(req.collisionchecker.c_str()));
+ GetEnv()->SetCollisionChecker(_pcolchecker.get());
+ GetEnv()->SetCollisionOptions(options);
}
if( req.setmask & env_set::request::Set_Gravity ) {
_vgravity = Vector(req.gravity[0],req.gravity[1],req.gravity[2]);
@@ -229,10 +794,10 @@
_pphysics->SetGravity(_vgravity);
}
if( req.setmask & env_set::request::Set_PublishAnytime ) {
- _penv->SetPublishBodiesAnytime(req.publishanytime>0);
+ GetEnv()->SetPublishBodiesAnytime(req.publishanytime>0);
}
if( req.setmask & env_set::request::Set_DebugLevel ) {
- _penv->SetDebugLevel(req.debuglevel);
+ GetEnv()->SetDebugLevel(req.debuglevel);
}
if( req.setmask & env_set::request::Set_Viewer ) {
if( !!_pviewer ) {
@@ -241,8 +806,8 @@
//_threadviewer.join();
}
- _pviewer.reset(_penv->CreateViewer(req.viewer.c_str()));
- _penv->AttachViewer(_pviewer.get());
+ _pviewer.reset(GetEnv()->CreateViewer(req.viewer.c_str()));
+ GetEnv()->AttachViewer(_pviewer.get());
if( !!_pviewer )
_threadviewer = boost::thread(boost::bind(&RaveViewerBase::main, _pviewer.get()));
}
@@ -250,32 +815,483 @@
return true;
}
- bool env_triangulate_srv(env_triangulate::request& req, env_triangulate::response& res);
- bool env_wait_srv(env_wait::request& req, env_wait::response& res);
- bool planner_init_srv(planner_init::request& req, planner_init::response& res);
- bool planner_plan_srv(planner_plan::request& req, planner_plan::response& res);
- bool problem_sendcommand_srv(problem_sendcommand::request& req, problem_sendcommand::response& res);
- bool robot_controllersend_srv(robot_controllersend::request& req, robot_controllersend::response& res);
- bool robot_controllerset_srv(robot_controllerset::request& req, robot_controllerset::response& res);
- bool robot_getactivedof_srv(robot_getactivedof::request& req, robot_getactivedof::response& res);
- bool robot_getactivevalues_srv(robot_getactivevalues::request& req, robot_getactivevalues::response& res);
- bool robot_sensorgetdata_srv(robot_sensorgetdata::request& req, robot_sensorgetdata::response& res);
- bool robot_sensorsend_srv(robot_sensorsend::request& req, robot_sensorsend::response& res);
- bool robot_setactivedofs_srv(robot_setactivedofs::request& req, robot_setactivedofs::response& res);
- bool robot_setactivevalues_srv(robot_setactivevalues::request& req, robot_setactivevalues::response& res);
- bool robot_starttrajectory_srv(robot_starttrajectory::request& req, robot_starttrajectory::response& res);
+ bool env_triangulate_srv(env_triangulate::request& req, env_triangulate::response& res)
+ {
+ set<int> setobjids;
+ FOREACH(it, req.bodyids)
+ setobjids.insert(*it);
+
+ KinBody::Link::TRIMESH trimesh;
+ {
+ LockEnvironment envlock(this);
+
+ vector<KinBody*> vbodies;
+ boost::shared_ptr<EnvironmentBase::EnvLock> lock(GetEnv()->GetLockedBodies(vbodies));
+
+ FOREACH(itbody, vbodies) {
+ if( (setobjids.find((*itbody)->GetNetworkId()) == setobjids.end()) ^ !req.inclusive )
+ continue;
+ GetEnv()->Triangulate(trimesh, *itbody);
+ }
+ }
+
+ res.points.resize(3*trimesh.vertices.size());
+ for(size_t i = 0; i < trimesh.vertices.size(); ++i) {
+ Vector& v = trimesh.vertices[i];
+ res.points[3*i+0] = v.x;
+ res.points[3*i+1] = v.y;
+ res.points[3*i+2] = v.z;
+ }
+
+ res.indices.resize(trimesh.indices.size());
+ for(size_t i = 0; i < trimesh.indices.size(); ++i)
+ res.indices[i] = (uint32_t)trimesh.indices[i];
+
+ return true;
+ }
+
+ bool env_wait_srv(env_wait::request& req, env_wait::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL || !pbody->IsRobot() )
+ return false;
+
+ RobotBase* probot = (RobotBase*)pbody;
+
+ if( probot->GetController() == NULL )
+ return false;
+
+ uint32_t timeout = (uint32_t)(req.timeout*1000.0f);
+
+ while( !probot->GetController()->IsDone() ) {
+ usleep(400);
+ if( timeout > 0 ) {
+ if( --timeout == 0 )
+ break;
+ }
+ if( IsClosing() )
+ return false;
+ }
+
+ res.isdone = probot->GetController()->IsDone();
+ return true;
+ }
+
+ bool planner_init_srv(planner_init::request& req, planner_init::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.robotid);
+ if( pbody == NULL || !pbody->IsRobot() )
+ return false;
+
+ RobotBase* probot = (RobotBase*)pbody;
+
+ map<int, boost::shared_ptr<PlannerBase> >::iterator itplanner = _mapplanners.find(req.plannerid);
+ if( itplanner == _mapplanners.end() )
+ return false;
+
+ PlannerBase::PlannerParameters params;
+ // fill with request
+ RAVELOG_ERRORA("need to fill with params!\n");
+
+ return itplanner->second->InitPlan(probot, ¶ms);
+ }
+
+ bool planner_plan_srv(planner_plan::request& req, planner_plan::response& res)
+ {
+ map<int, boost::shared_ptr<PlannerBase> >::iterator itplanner = _mapplanners.find(req.plannerid);
+ if( itplanner == _mapplanners.end() )
+ return false;
+
+ ROS_ASSERT( itplanner->second->GetRobot() != NULL );
+
+ boost::shared_ptr<OpenRAVE::Trajectory> traj(GetEnv()->CreateTrajectory(itplanner->second->GetRobot()->GetActiveDOF()));
+
+ RAVELOG_INFOA("starting to plan");
+ if( !itplanner->second->PlanPath(traj.get(), NULL) ) {
+ RAVELOG_INFOA("plan failed\n");
+ return false;
+ }
+
+ RAVELOG_INFOA("plan succeeded\n");
+
+ FillActiveDOFs(itplanner->second->GetRobot(), res.trajectory.active);
+ res.trajectory.points.resize(traj->GetPoints().size()); int index = 0;
+ FOREACHC(itpoint, traj->GetPoints()) {
+ openraveros::TrajectoryPoint& rospt = res.trajectory.points[index++];
+ rospt.timestamp = itpoint->time;
+ rospt.position.resize(itpoint->q.size());
+ for(size_t i = 0; i < itpoint->q.size(); ++i)
+ rospt.position[i] = itpoint->q[i];
+ rospt.velocity.resize(itpoint->qdot.size());
+ for(size_t i = 0; i < itpoint->qdot.size(); ++i)
+ rospt.velocity[i] = itpoint->qdot[i];
+ rospt.transform = GetAffineTransform(itpoint->trans);
+ }
+
+ return true;
+ }
+
+ bool problem_sendcommand_srv(problem_sendcommand::request& req, problem_sendcommand::response& res)
+ {
+ map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.find(req.problemid);
+ if( itprob == _mapproblems.end() )
+ return false;
+
+ itprob->second->SendCommand(req.cmd.c_str(),res.output);
+ return true;
+ }
+
+ bool robot_controllersend_srv(robot_controllersend::request& req, robot_controllersend::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL || !pbody->IsRobot() )
+ return false;
+
+ RobotBase* probot = (RobotBase*)pbody;
+
+ if( probot->GetController() == NULL || !probot->GetController()->SupportsCmd(req.cmd.c_str()) )
+ return false;
+
+ return probot->GetController()->SendCmd(req.cmd.c_str());
+ }
+
+ bool robot_controllerset_srv(robot_controllerset::request& req, robot_controllerset::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL || !pbody->IsRobot() )
+ return false;
+
+ RobotBase* probot = (RobotBase*)pbody;
+ return probot->SetController(_ravembstowcs(req.controllername.c_str()).c_str(), req.controllerargs.c_str(), true);
+ }
+
+ bool robot_getactivevalues_srv(robot_getactivevalues::request& req, robot_getactivevalues::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL || !pbody->IsRobot() )
+ return false;
+
+ RobotBase* probot = (RobotBase*)pbody;
+
+ LockEnvironment envlock(this);
+
+ if( req.indices.size() > 0 ) {
+ vector<dReal> vtemp;
+ probot->GetActiveDOFValues(vtemp);
+
+ res.values.resize(req.indices.size());
+ for(size_t i = 0; i < req.indices.size(); ++i) {
+ ROS_ASSERT( req.indices[i] < vtemp.size() );
+ res.values[i] = vtemp[req.indices[i]];
+ }
+ }
+ else {
+ vector<dReal> vtemp;
+ probot->GetActiveDOFValues(vtemp);
+
+ res.values.resize(vtemp.size());
+ for(size_t i = 0; i < req.indices.size(); ++i)
+ res.values[i] = vtemp[i];
+ }
+
+ return true;
+ }
+
+ bool robot_sensorgetdata_srv(robot_sensorgetdata::request& req, robot_sensorgetdata::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL || !pbody->IsRobot() )
+ return false;
+
+ RobotBase* probot = (RobotBase*)pbody;
+
+ if( req.sensorindex >= probot->GetSensors().size() )
+ return false;
+
+
+ if( probot->GetSensors()[req.sensorindex].GetSensor() == NULL )
+ return false;
+
+ SensorBase* psensor = probot->GetSensors()[req.sensorindex].GetSensor();
+
+ boost::shared_ptr<SensorBase::SensorData> pdata(psensor->CreateSensorData());
+
+ if( !pdata ) {
+ RAVELOG_ERRORA("Robot %S, failed to create sensor %S data\n", probot->GetName(), probot->GetSensors()[req.sensorindex].GetName());
+ return false;
+ }
+
+ if( !psensor->GetSensorData(pdata.get()) ) {
+ RAVELOG_ERRORA("Robot %S, failed to get sensor %S data\n", probot->GetName(), probot->GetSensors()[req.sensorindex].GetName());
+ return false;
+ }
+
+ // serialize the data
+
+ switch(pdata->GetType()) {
+ case SensorBase::ST_Laser: {
+ res.type = "laser";
+ SensorBase::LaserSensorData* plaserdata = (SensorBase::LaserSensorData*)pdata.get();
+
+ int index;
+
+ res.laserrange.resize(3*plaserdata->ranges.size()); index = 0;
+ FOREACH(itpos, plaserdata->ranges) {
+ res.laserrange[3*index+0] = itpos->x;
+ res.laserrange[3*index+1] = itpos->y;
+ res.laserrange[3*index+2] = itpos->z;
+ ++index;
+ }
+
+ res.laserpos.resize(3*plaserdata->positions.size()); index = 0;
+ FOREACH(itpos, plaserdata->positions) {
+ res.laserpos[3*index+0] = itpos->x;
+ res.laserpos[3*index+1] = itpos->y;
+ res.laserpos[3*index+2] = itpos->z;
+ ++index;
+ }
+
+ res.laserint.resize(plaserdata->intensity.size()); index = 0;
+ FOREACH(itint, plaserdata->intensity)
+ res.laserint[index++] = *itint;
+
+ break;
+ }
+ case SensorBase::ST_Camera:
+ res.type = "camera";
+ RAVELOG_ERRORA("camera data serialization not implemented yet!\n");
+ ROS_ASSERT(0);
+ break;
+// SensorBase::CameraSensorData* pcameradata = (SensorBase::CameraSensorData*)pdata.get();
+//
+// if( psensor->GetSensorGeometry()->GetType() != SensorBase::ST_Camera ) {
+// RAVELOG(L"sensor geometry not a camera type\n");
+// return false;
+// }
+//
+// SensorBase::CameraGeomData* pgeom = (SensorBase::CameraGeomData*)psensor->GetSensorGeometry();
+//
+// if( (int)pcameradata->vimagedata.size() != pgeom->width*pgeom->height*3 ) {
+// RAVELOG(L"image data wrong size %d != %d\n", pcameradata->vimagedata.size(), pgeom->width*pgeom->height*3);
+// return false;
+// }
+//
+// sout << pgeom->width << " " << pgeom->height << " ";
+// for(int i = 0; i < 4; ++i)
+// sout << pgeom->KK[i] << " ";
+// sout << TransformMatrix(pcameradata->t) << " ";
+// //FOREACH(it, pcameradata->vimagedata) sout << (int)*it << " ";
+//
+// // RLE encoding (about 3x faster than sending raw images)
+// int curvalue = 0, lastdiff = 0, lastvalue = 0xffffff&*(int*)&pcameradata->vimagedata[0];
+// list<int> difs, values;
+// for(int i = 1; i < (int)pcameradata->vimagedata.size()/3; ++i) {
+// curvalue = 0xffffff&*(int*)&pcameradata->vimagedata[3*i];
+// if( curvalue != lastvalue ) {
+// values.push_back(lastvalue);
+// difs.push_back(i-lastdiff);
+// lastdiff = i;
+// lastvalue = curvalue;
+// }
+// }
+// difs.push_back(pcameradata->vimagedata.size()/3-lastdiff);
+// values.push_back(curvalue);
+//
+// sout << values.size() << " ";
+// FOREACH(it, values)
+// sout << *it << " ";
+// sout << difs.size() << " ";
+// FOREACH(it, difs)
+// sout << *it << " ";
+ }
+
+ return true;
+ }
+
+ bool robot_sensorsend_srv(robot_sensorsend::request& req, robot_sensorsend::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL || !pbody->IsRobot() )
+ return false;
+
+ RobotBase* probot = (RobotBase*)pbody;
+
+ if( req.sensorindex >= probot->GetSensors().size() )
+ return false;
+
+ RobotBase::AttachedSensor& sensor = probot->GetSensors()[req.sensorindex];
+
+ if( sensor.GetSensor() == NULL )
+ return false;
+
+ if( !sensor.GetSensor()->SupportsCmd(req.cmd.c_str()) ) {
+ RAVELOG_ERRORA("Robot %S sensor %d doesn't support command: \"%s\"\n", probot->GetName(), req.sensorindex, req.cmd.c_str());
+ return false;
+ }
+
+ stringstream ss(req.args);
+ stringstream sout;
+ if( !sensor.GetSensor()->SendCmd(ss,sout) )
+ return false;
+
+ res.out = sout.str();
+ return true;
+ }
+
+ void RobotSetActiveDOFs(RobotBase* probot, openraveros::ActiveDOFs& active)
+ {
+ vector<int> vjointindices(active.joints.size());
+ for(size_t i = 0; i < active.joints.size(); ++i)
+ vjointindices[i] = active.joints[i];
+
+ Vector vaxis(active.rotationaxis[0], active.rotationaxis[1], active.rotationaxis[2]);
+ probot->SetActiveDOFs(vjointindices, active.affine, (active.affine&RobotBase::DOF_RotationAxis)?&vaxis:NULL);
+ }
+
+ bool robot_setactivedofs_srv(robot_setactivedofs::request& req, robot_setactivedofs::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL || !pbody->IsRobot() )
+ return false;
+
+ RobotBase* probot = (RobotBase*)pbody;
+
+ LockEnvironment envlock(this);
+ RobotSetActiveDOFs(probot, req.active);
+ return true;
+ }
+
+ bool robot_setactivevalues_srv(robot_setactivevalues::request& req, robot_setactivevalues::response& res)
+ {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
+ if( pbody == NULL || !pbody->IsRobot() )
+ return false;
+
+ RobotBase* probot = (RobotBase*)pbody;
+
+ if( probot->GetActiveDOF() == 0 )
+ return false;
+
+ LockEnvironment envlock(this);
+ vector<dReal> vvalues;
+
+ if( req.indices.size() > 0 ) {
+ if( req.indices.size() != req.values.size() ) {
+ RAVELOG_WARNA("indices (%d) different size than joint values (%d)\n", req.indices.size(), req.values.size());
+ return false;
+ }
+
+ probot->GetActiveDOFValues(vvalues);
+ for(uint32_t i = 0; i < req.indices.size(); ++i)
+ vvalues[req.indices[i]] = req.values[i];
+ }
+ else {
+ if( probot->GetActiveDOF() != (int)req.values.size() ) {
+ RAVELOG_WARNA("body dof (%d) not equal to values (%d)", probot->GetDOF(), req.values.size());
+ return false;
+ }
+
+ vvalues.reserve(req.values.size());
+ FOREACHC(it,req.values)
+ vvalues.push_back(*it);
+ }
+
+ probot->SetActiveDOFValues(NULL, &vvalues[0], true);
+
+ if( probot->GetController() != NULL ) {
+ probot->GetJointValues(vvalues); // reget the values since they'll go through the joint limits
+ probot->GetController()->SetDesired(&vvalues[0]);
+ }
+
+ return true;
+ }
+
+ bool robot_starttrajectory_srv(robot_starttrajectory::request& req, robot_starttrajectory::response& res)
+ {
+ KinBody* pbod...
[truncated message content] |
