Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9219] pkg/trunk
|
From: <rdi...@us...> - 2009-01-12 03:11:41
|
Revision: 9219
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9219&view=rev
Author: rdiankov
Date: 2009-01-12 03:11:30 +0000 (Mon, 12 Jan 2009)
Log Message:
-----------
openrave manipulation demo on real robot rocks now
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml
pkg/trunk/openrave_planning/ormanipulation/data/willow_tablef.kinbody.xml
pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m
pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m
pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m
pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml
pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
Added Paths:
-----------
pkg/trunk/openrave_planning/ormanipulation/octave/RobotMoveJointValues.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-12 03:11:30 UTC (rev 9219)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 604
+SVN_REVISION = -r 606
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-01-12 03:11:30 UTC (rev 9219)
@@ -80,15 +80,16 @@
class SendProblemWorker : public ServerWorker
{
public:
- SendProblemWorker(boost::shared_ptr<ProblemInstance> prob, const string& cmd, string& out) : _prob(prob), _cmd(cmd), _out(out) {}
+ SendProblemWorker(boost::shared_ptr<ProblemInstance> prob, const string& cmd, string& out, bool& bSuccessful) : _prob(prob), _cmd(cmd), _out(out), _bSuccessful(bSuccessful) {}
virtual void work() {
- _prob->SendCommand(_cmd.c_str(),_out);
+ _bSuccessful = _prob->SendCommand(_cmd.c_str(),_out);
}
private:
boost::shared_ptr<ProblemInstance> _prob;
const string& _cmd;
string& _out;
+ bool& _bSuccessful;
};
class LoadProblemWorker : public ServerWorker
@@ -1208,8 +1209,9 @@
if( itprob == _mapproblems.end() )
return false;
- AddWorker(new SendProblemWorker(itprob->second,req.cmd,res.output), true);
- return true;
+ bool bSuccessful = false;
+ AddWorker(new SendProblemWorker(itprob->second,req.cmd,res.output,bSuccessful), true);
+ return bSuccessful;
}
bool robot_controllersend_srv(robot_controllersend::request& req, robot_controllersend::response& res)
Modified: pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml 2009-01-12 03:11:30 UTC (rev 9219)
@@ -1,7 +1,7 @@
<Kinbody name="ricebox">
<Body>
<Geom type="box">
- <extents>0.1175 0.06 0.035</extents>
+ <extents>0.13 0.06 0.035</extents>
<translation>0 0 0.03</translation>
<diffusecolor>0.1 0.6 1</diffusecolor>
</Geom>
Modified: pkg/trunk/openrave_planning/ormanipulation/data/willow_tablef.kinbody.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/willow_tablef.kinbody.xml 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/data/willow_tablef.kinbody.xml 2009-01-12 03:11:30 UTC (rev 9219)
@@ -1,7 +1,7 @@
<KinBody name="willow_table">
<Body name="base">
<Geom type="box">
- <extents>0.39 0.63 0.04</extents>
+ <extents>0.39 0.63 0.055</extents>
<translation>0 0 0.711</translation>
</Geom>
<Geom type="box">
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -81,6 +81,12 @@
curobj = targetobjs{orderedtargets(1)};
end
+if( isempty(curobj.info) )
+ display('invalid object');
+ curobj = [];
+ return;
+end
+
[dests, surfaceplane] = scenedata.GetDestsFn(); % compute a set of destinations
offsetfromtable = 0.04; %% set destination a little above the table
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -178,7 +178,12 @@
if( ~success )
error('failed to movehandstraight');
end
- success = StartTrajectory(robotid,trajdata);
+
+ %% hack for now
+ [trajvals,transvals,timevals] = ParseTrajData(trajdata);
+ trajvals(41,end) = 0.35;
+ newtrajdata = [sprintf('%d %d 13 ',size(trajvals,2), size(trajvals,1)) sprintf('%f ',[timevals;trajvals;transvals])];
+ success = StartTrajectory(robotid,newtrajdata);
if( ~success )
warning('failed to close fingers');
return;
@@ -272,16 +277,18 @@
% move the hand down until collision
display('moving hand down');
- [trajdata, success] = orProblemSendCommand(['MoveHandStraight execute 0 outputtraj direction ' sprintf('%f ', -updir) ...
- ' maxdist ' sprintf('%f ', 0.3)],probs.manip);
+ [trajdata, success] = orProblemSendCommand(['MoveHandStraight ignorefirstcollision 0 execute 0 ' ...
+ ' outputtraj direction ' sprintf('%f ', -updir) ...
+ ' maxdist ' sprintf('%f ', 0.3)],probs.manip);
if( ~success )
- error('failed to movehandstraight');
+ warning('failed to movehandstraight');
+ else
+ success = StartTrajectory(robotid,trajdata);
+ if( ~success )
+ warning('failed to move hand straight');
+ return;
+ end
end
- success = StartTrajectory(robotid,trajdata);
- if( ~success )
- warning('failed to move hand straight');
- return;
- end
display('opening hand');
@@ -296,14 +303,20 @@
if( ~success )
warning('problems releasing, releasing target first');
orProblemSendCommand('releaseall', probs.manip);
- [trajdata, success] = orProblemSendCommand(['ReleaseFingers execute 0 outputtraj target ' curobj.info.name ...
- ' movingdir ' sprintf('%f ', handrobot.releasedir)], probs.manip);
+ [trajdata, success] = orProblemSendCommand(['ReleaseFingers execute 0 outputtraj ' ...
+ ' target ' curobj.info.name ...
+ ' movingdir ' sprintf('%f ', handrobot.releasedir)], ...
+ probs.manip);
if( ~success )
- error('failed to release fingers');
+ warning('failed to release fingers, forcing open');
+ success = RobotMoveJointValues(robotid,open_config,handjoints);
+ else
+ success = StartTrajectory(robotid,trajdata,4);
end
+ else
+ success = StartTrajectory(robotid,trajdata,4);
end
-
- success = StartTrajectory(robotid,trajdata,4);
+
if( ~success )
warning('trajectory failed to release fingers');
return;
@@ -312,10 +325,9 @@
%% now release object
orProblemSendCommand('releaseall', probs.manip);
- if( squeezesuccess > 0 & putsuccess > 0 )
+ if( squeezesuccess > 0 && putsuccess > 0 )
display('success, putting down');
- orBodySetJointValues(robotid,open_config,handjoints);
% only break when succeeded
%orProblemSendCommand(['MoveHandStraight stepsize 0.003 minsteps ' sprintf('%f ', 90) ' maxsteps ' sprintf('%f ', 100) ' direction ' sprintf('%f ', updir')]);
RobotGoInitial(robot);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -27,19 +27,9 @@
end
display('moving hand');
+success = RobotMoveJointValues(robotid, home(handjoints+1),handjoints)
-prevsession = openraveros_getglobalsession();
-prevprobs = probs;
-setrealsession();
-orRobotSetDOFValues(robotid,home(handjoints+1),handjoints);
-pause(0.4); % pause a little to give a chance for controller to start
-success = WaitForRobot(robotid);
-setclonesession(prevsession);
-probs = prevprobs;
-
%orRobotSetActiveDOFs(robotid,0:(robot.dof-1));
%curvalues = orRobotGetDOFValues(robotid);
%orRobotStartActiveTrajectory(robotid,[curvalues home(:)]);
%WaitForRobot(robotid);
-
-success = 1;
Added: pkg/trunk/openrave_planning/ormanipulation/octave/RobotMoveJointValues.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RobotMoveJointValues.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RobotMoveJointValues.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -0,0 +1,11 @@
+function success = RobotMoveJointValues(robotid, joints, jointinds)
+global probs
+prevsession = openraveros_getglobalsession();
+prevprobs = probs;
+setrealsession();
+orRobotSetDOFValues(robotid,joints,jointinds);
+pause(0.4); % pause a little to give a chance for controller to start
+success = WaitForRobot(robotid);
+setclonesession(prevsession);
+probs = prevprobs;
+success = 1;
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -89,4 +89,6 @@
%destroy
openraveros_destroysession(sessionclone);
end
+
+ input('press any key to get next measurement: ');
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -134,7 +134,7 @@
enabledjoints([robot.manips{1}.armjoints; robot.manips{1}.handjoints]) = [];
jointnames_cell = transpose(robot.jointnames(enabledjoints+1));
jointnames_str = cell2mat (cellfun(@(x) [x ' '], jointnames_cell,'uniformoutput',false));
- orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /right_arm_trajectory_controller joints ' jointnames_str]);
+ orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /right_arm_trajectory_controller/ joints ' jointnames_str]);
end
%% dests is a 12xN array where every column is a 3x4 matrix
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -50,5 +50,5 @@
display('wait ended');
newjointconfig = orBodyGetJointValues(robotid);
setclonesession(prevsession);
-probs = prevprobs
+probs = prevprobs;
orBodySetJointValues(robotid,newjointconfig);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -1,6 +1,36 @@
#!/usr/bin/env octave
-global updir probs
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
+global updir probs
+
startup;
[robot, scenedata] = SetupTableScene('data/pr2table_real.env.xml',1);
+
+Tlaser = [1.00000 0.00062 0.00232 0.01577
+ -0.00066 0.99988 0.01554 -0.00208
+ -0.00231 -0.01555 0.99988 0.03934];
+Tcamera = [0.01611 0.01631 0.99974 0.02890
+ -0.99966 -0.02020 0.01644 0.03236
+ 0.02047 -0.99966 0.01598 0.09532];
+
+%% objects from camera
+out = orProblemSendCommand(['createsystem ObjectTransform topic /checkerdetector/ObjectDetection thresh 0.1 robot ' sprintf('%d ', robot.id) ' matrixoffset ' sprintf('%f ', Tcamera(1:3,1:4))],probs.task);
+if( isempty(out) )
+ error('failed to create checkerboard detector');
+end
+
+%% laser-based dynamic collision map
+out = orProblemSendCommand('createsystem CollisionMap collision_map',probs.task);
+if( isempty(out) )
+ error('failed to create collision map');
+end
+
+%% phase space system
+out = orProblemSendCommand('createsystem PhaseSpace phase_space_snapshot',probs.task);
+if( isempty(out) )
+ error('failed to create phasespace');
+end
+
+input('press any key to start grasping: ');
+RunDynamicGrasping(robot,scenedata,0,0);
+
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-12 03:11:30 UTC (rev 9219)
@@ -11,7 +11,7 @@
-0.00231 -0.01555 0.99988 0.03934];
Tcamera = [0.01611 0.01631 0.99974 0.02890
-0.99966 -0.02020 0.01644 0.03236
- 0.02047 -0.99966 0.01598 0.06732];
+ 0.02047 -0.99966 0.01598 0.09532];
%% objects from camera
out = orProblemSendCommand(['createsystem ObjectTransform topic /checkerdetector/ObjectDetection thresh 0.1 robot ' sprintf('%d ', robot.id) ' matrixoffset ' sprintf('%f ', Tcamera(1:3,1:4))],probs.task);
@@ -22,7 +22,7 @@
%% laser-based dynamic collision map
out = orProblemSendCommand('createsystem CollisionMap collision_map',probs.task);
if( isempty(out) )
- error('failed to create phasespace');
+ error('failed to create collision map');
end
%% phase space system
Modified: pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2009-01-12 03:11:30 UTC (rev 9219)
@@ -4,9 +4,10 @@
<!-- start openraveros server and set all the correct paths -->
<include file="$(find ormanipulation)/startopenrave.ros.xml"/>
- <include file="$(find ormanipulation)/startchecker.ros.xml"/>
+<!-- <include file="$(find ormanipulation)/startchecker.ros.xml"/> -->
<node pkg="phase_space" type="phase_space_node"/>
+
<!-- launch octave scripts to start the openrave client -->
<!-- <node pkg="ormanipulation" type="runperception.m" output="screen"/> -->
</launch>
Modified: pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml 2009-01-12 03:11:30 UTC (rev 9219)
@@ -3,6 +3,7 @@
<machine name="localhost" address="localhost" default="true"/>
<include file="$(find ormanipulation)/startopenrave.ros.xml"/>
+
<include file="$(find ormanipulation)/startchecker.ros.xml"/>
<node pkg="phase_space" type="phase_space_node"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-12 03:11:30 UTC (rev 9219)
@@ -40,21 +40,125 @@
{
public:
CollisionMapSystem(EnvironmentBase* penv)
- : ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>(penv), _nNextId(1)
+ : ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>(penv), _robotid(0), _nNextId(1), _bEnableCollisions(true)
{
}
+ virtual ~CollisionMapSystem() {
+ stopsubscriptions(); // need to stop the subscriptions because the virtual destructor will not call the overridden stopsubscriptions
+ ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>::Destroy();
+ }
virtual bool Init(istream& sinput)
{
+ _robotid = 0;
_topic = "collision_map";
- return ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>::Init(sinput);
+
+ string cmd;
+ while(!sinput.eof()) {
+ sinput >> cmd;
+ if( !sinput )
+ break;
+
+ if( stricmp(cmd.c_str(), "topic") == 0 )
+ sinput >> _topic;
+ else if( stricmp(cmd.c_str(), "collisions") == 0 )
+ sinput >> _bEnableCollisions;
+ else if( stricmp(cmd.c_str(), "robot") == 0 ) {
+ int id;
+ sinput >> id;
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(id);
+ if( pbody != NULL )
+ _robotid = id;
+
+ if( _robotid == 0 )
+ RAVELOG_WARNA("failed to find robot with id %d\n", id);
+ }
+ else break;
+
+ if( !sinput ) {
+ RAVELOG_ERRORA("failed\n");
+ return false;
+ }
+ }
+
+ startsubscriptions();
+ return _bSubscribed;
}
private:
+ virtual void startsubscriptions()
+ {
+ ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>::startsubscriptions();
+
+ if( _bSubscribed ) {
+ boost::mutex::scoped_lock lock(_mutex);
+ ros::node* pnode = check_roscpp_nocreate();
+ if( pnode != NULL ) {
+ double tf_cache_time_secs;
+ pnode->param("~tf_cache_time_secs", tf_cache_time_secs, 10.0);
+ if (tf_cache_time_secs < 0)
+ RAVELOG_ERRORA("ROSSensorSystem: Parameter tf_cache_time_secs<0 (%f)\n", tf_cache_time_secs);
+ unsigned long long tf_cache_time = tf_cache_time_secs*1000000000ULL;
+ _tf.reset(new tf::TransformListener(*pnode, true, tf_cache_time));
+ RAVELOG_INFOA("ROSSensorSystem: TF Cache Time: %f Seconds\n", tf_cache_time_secs);
+
+ // **** Set TF Extrapolation Limit ****
+ double tf_extrap_limit_secs ;
+ pnode->param("~tf_extrap_limit", tf_extrap_limit_secs, 0.00);
+ if (tf_extrap_limit_secs < 0.0)
+ RAVELOG_ERRORA("ROSSensorSystem: parameter tf_extrap_limit<0 (%f)\n", tf_extrap_limit_secs);
+
+ ros::Duration extrap_limit;
+ extrap_limit.fromSec(tf_extrap_limit_secs);
+ _tf->setExtrapolationLimit(extrap_limit);
+ RAVELOG_INFOA("ROSSensorSystem: tf extrapolation Limit: %f Seconds\n", tf_extrap_limit_secs);
+ }
+ }
+ }
+
+ virtual void stopsubscriptions()
+ {
+ ROSSensorSystem<collision_map::CollisionMap, CollisionMapXMLID>::stopsubscriptions();
+ _tf.reset();
+ }
+
void newdatacb()
{
// create the new kinbody
GetEnv()->LockPhysics(true);
+
+ Transform tcollision;
+ string strrobotbaselink;
+ bool bHasRobotTransform = false;
+
+ if( _robotid != 0 ) {
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(_robotid);
+ if( pbody != NULL ) {
+ bHasRobotTransform = true;
+ tcollision = pbody->GetTransform();
+ strrobotbaselink = _stdwcstombs(pbody->GetLinks().front()->GetName());
+ }
+ }
+
+ if( bHasRobotTransform && !!_tf ) {
+ tf::Stamped<btTransform> bttransform;
+
+ try {
+ _tf->lookupTransform(strrobotbaselink, _topicmsg.header.frame_id, _topicmsg.header.stamp, bttransform);
+ tcollision = tcollision * GetTransform(bttransform);
+ }
+ catch(tf::TransformException& ex) {
+ try {
+ _tf->lookupTransform(strrobotbaselink, _topicmsg.header.frame_id, ros::Time(), bttransform);
+ tcollision = tcollision * GetTransform(bttransform);
+ }
+ catch(tf::TransformException& ex) {
+ RAVELOG_WARNA("failed to get tf frames %s (robot link:%s)\n", _topicmsg.header.frame_id.c_str(), strrobotbaselink.c_str());
+ return;
+ }
+ }
+ }
+
KinBody* pbody = GetEnv()->CreateKinBody();
_vaabbs.resize(_topicmsg.boxes.size());
@@ -91,7 +195,6 @@
TYPEOF(_mapbodies.begin()) itbody = _mapbodies.begin();
while(itbody != _mapbodies.end()) {
if( !itbody->second->IsLocked() ) {
- BODY* b = itbody->second.get();
KinBody::Link* plink = itbody->second->GetOffsetLink();
assert( plink != NULL );
GetEnv()->RemoveKinBody(plink->GetParent());
@@ -113,13 +216,18 @@
}
}
- Transform GetTransform(const std_msgs::Transform& pose)
+ Transform GetTransform(const btTransform& bt)
{
- return Transform(Vector(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z), Vector(pose.translation.x, pose.translation.y, pose.translation.z));
+ btQuaternion q = bt.getRotation();
+ btVector3 o = bt.getOrigin();
+ return Transform(Vector(q.w(),q.x(),q.y(),q.z()),Vector(o.x(),o.y(),o.z()));
}
+ boost::shared_ptr<tf::TransformListener> _tf;
+ int _robotid;
vector<AABB> _vaabbs;
int _nNextId;
+ bool _bEnableCollisions;
};
#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-12 03:11:30 UTC (rev 9219)
@@ -65,12 +65,12 @@
pr2_mechanism_controllers::TrajectoryQuery::request req;
pr2_mechanism_controllers::TrajectoryQuery::response res;
if( !_srvTrajectoryQuery->call(req,res) ) {
- RAVELOG_ERRORA("failed to query trajectory service %s", _strTrajectoryServiceDir.c_str());
+ RAVELOG_ERRORA("failed to query trajectory service %s\n", _strTrajectoryServiceDir.c_str());
return false;
}
if( res.jointnames.size() == 0 ) {
- RAVELOG_ERRORA("no joint names for trajectory service %s", _strTrajectoryServiceDir.c_str());
+ RAVELOG_ERRORA("no joint names for trajectory service %s\n", _strTrajectoryServiceDir.c_str());
return false;
}
@@ -78,6 +78,8 @@
for(size_t i = 0; i < _probot->GetJoints().size(); ++i)
vrobotjoints[i] = _stdwcstombs(_probot->GetJoints()[i]->GetName());
+ stringstream ss;
+ ss << "roscontroller: " << _strTrajectoryServiceDir;
_vjointmap.reserve(res.jointnames.size());
FOREACH(itname, res.jointnames) {
vector<string>::iterator itindex = find(vrobotjoints.begin(), vrobotjoints.end(), *itname);
@@ -87,8 +89,12 @@
return false;
}
_vjointmap.push_back((int)(itindex-vrobotjoints.begin()));
+ ss << " " << *itname << " (" << _vjointmap.back() << ")";
}
+ ss << endl;
+ RAVELOG_DEBUGA(ss.str().c_str());
+
return true;
}
Modified: pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-01-12 02:42:33 UTC (rev 9218)
+++ pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-01-12 03:11:30 UTC (rev 9219)
@@ -403,6 +403,7 @@
if (curlong || totlong)
ROS_ERROR("qhull internal warning (main): did not free %d bytes of long memory (%d pieces)", totlong, curlong);
+ fclose(errfile);
return bSuccess;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
