00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef COLLISIONMAP_MOCAP_SYSTEM
00027 #define COLLISIONMAP_MOCAP_SYSTEM
00028
00029 #include "rossensorsystem.h"
00030 #include <arm_navigation_msgs/CollisionMap.h>
00031
00032 class CollisionMapSystem : public ROSSensorSystem<arm_navigation_msgs::CollisionMap>
00033 {
00034 public:
00035 static boost::shared_ptr<void> RegisterXMLReader(EnvironmentBasePtr penv)
00036 {
00037 return RegisterXMLReaderId(penv,"collisionmap");
00038 }
00039
00040 CollisionMapSystem(EnvironmentBasePtr penv)
00041 : ROSSensorSystem<arm_navigation_msgs::CollisionMap>("collisionmap",penv), _fPrunePadding(0), _nNextId(1), _bPruneCollisions(false)
00042 {
00043 __description = ":Interface Author: Rosen Diankov\n\nListens to ROS arm_navigation_msgs/CollisionMap and dynamically updates an environment collision map.";
00044 RegisterCommand("collisionstream",boost::bind(&CollisionMapSystem::collisionstream,this,_1,_2),
00045 "ROS stream");
00046 RegisterCommand("GetTimeStamp",boost::bind(&CollisionMapSystem::GetTimeStamp,this,_1,_2),
00047 "Gets the time-stamp of the current collision map ");
00048 }
00049 virtual ~CollisionMapSystem() {
00050 }
00051
00052 virtual bool Init(istream& sinput)
00053 {
00054 _bCollisionStream = true;
00055 _bHasCollisionMap = false;
00056 _listtopics.clear();
00057 string cmd;
00058 while(!sinput.eof()) {
00059 sinput >> cmd;
00060 if( !sinput ) {
00061 break;
00062 }
00063 std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);
00064 if( cmd == "topic" ) {
00065 string topic;
00066 sinput >> topic;
00067 _listtopics.push_back(topic);
00068 }
00069 else if( cmd == "prunecollisions")
00070 sinput >> _bPruneCollisions >> _fPrunePadding;
00071 else if( cmd == "robot" || cmd == "bodyoffset" ) {
00072 string name;
00073 sinput >> name;
00074 _pbodyoffset = GetEnv()->GetKinBody(name);
00075 if( !_pbodyoffset )
00076 RAVELOG_WARN(str(boost::format("failed to get %s body\n")%name));
00077 }
00078 else if( cmd == "expirationtime") {
00079 double expirationtime;
00080 sinput >> expirationtime;
00081 if( !!sinput )
00082 _expirationtime = (uint64_t)(expirationtime*1000000);
00083 }
00084 else
00085 break;
00086
00087 if( !sinput ) {
00088 RAVELOG_ERROR("failed\n");
00089 return false;
00090 }
00091 }
00092
00093 if( _listtopics.size() == 0 )
00094 _listtopics.push_back("collision_map");
00095
00096 return startsubscriptions(1);
00097 }
00098
00099 virtual bool collisionstream(std::ostream& os, std::istream& is)
00100 {
00101 is >> _bCollisionStream;
00102 RAVELOG_INFO("collisionstream: %d\n",(int)_bCollisionStream);
00103 return true;
00104 }
00105
00106 virtual bool GetTimeStamp(std::ostream& os, std::istream& is)
00107 {
00108 EnvironmentMutex::scoped_lock envlock(GetEnv()->GetMutex());
00109 if( !_bHasCollisionMap ) {
00110 return false;
00111 }
00112 os << _collisionstamp.toNSec();
00113 return true;
00114 }
00115
00116 private:
00117 virtual bool startsubscriptions(int queuesize)
00118 {
00119 if( !ROSSensorSystem<arm_navigation_msgs::CollisionMap>::startsubscriptions(queuesize) ) {
00120 return false;
00121 }
00122 _tflistener.reset(new tf::TransformListener(*_ros));
00123 return true;
00124 }
00125
00126 virtual void Destroy()
00127 {
00128 ROSSensorSystem<arm_navigation_msgs::CollisionMap>::Destroy();
00129 _tflistener.reset();
00130 }
00131
00132 virtual void newdatacb(const boost::shared_ptr<arm_navigation_msgs::CollisionMap const>& topicmsg)
00133 {
00134 if( !_bCollisionStream ) {
00135 return;
00136 }
00137 KinBodyPtr pbody;
00138 {
00139 EnvironmentMutex::scoped_lock envlock(GetEnv()->GetMutex());
00140
00141 Transform tcollision;
00142 string strbodybaselink;
00143 bool bHasBodyTransform = false;
00144
00145 if( !!_pbodyoffset ) {
00146 bHasBodyTransform = true;
00147 tcollision = _pbodyoffset->GetTransform();
00148 strbodybaselink = _pbodyoffset->GetLinks().front()->GetName();
00149 }
00150
00151 if( bHasBodyTransform && !!_tflistener ) {
00152 tf::StampedTransform bttransform;
00153
00154 try {
00155 _tflistener->lookupTransform(strbodybaselink, topicmsg->header.frame_id, topicmsg->header.stamp, bttransform);
00156 tcollision = tcollision * GetTransform(bttransform);
00157 }
00158 catch(tf::TransformException& ex) {
00159 try {
00160 _tflistener->lookupTransform(strbodybaselink, topicmsg->header.frame_id, ros::Time(), bttransform);
00161 tcollision = tcollision * GetTransform(bttransform);
00162 }
00163 catch(tf::TransformException& ex) {
00164 RAVELOG_WARNA("failed to get tf frames %s (body link:%s)\n", topicmsg->header.frame_id.c_str(), strbodybaselink.c_str());
00165 return;
00166 }
00167 }
00168 }
00169
00170 list<KinBodyPtr> listcheckbodies;
00171 if( _bPruneCollisions ) {
00172 vector<KinBodyPtr> vallbodies;
00173 GetEnv()->GetBodies(vallbodies);
00174 FOREACH(itbody,vallbodies) {
00175 if( !!(*itbody)->GetManageData() && (*itbody)->GetManageData()->GetSystem() != shared_system() )
00176 listcheckbodies.push_back(*itbody);
00177 }
00178
00179 vector<RobotBasePtr> vallrobots;
00180 GetEnv()->GetRobots(vallrobots);
00181 FOREACH(itrobot,vallrobots) {
00182 (*itrobot)->GetGrabbed(vallbodies);
00183 FOREACH(itbody,vallbodies) {
00184 if( find(listcheckbodies.begin(),listcheckbodies.end(),*itbody) == listcheckbodies.end() )
00185 listcheckbodies.push_back(*itbody);
00186 }
00187 }
00188 }
00189
00190 pbody = RaveCreateKinBody(GetEnv());
00191
00192 _vobbs.resize(0); _vobbs.resize(topicmsg->boxes.size());
00193 vector<OBB>::iterator itobb = _vobbs.begin();
00194 TransformMatrix tm;
00195 FOREACH(itmsgab, topicmsg->boxes) {
00196 if( _bPruneCollisions && listcheckbodies.size() > 0 ) {
00197 if( !_pbodytestbox ) {
00198 vector<AABB> vabs(1);
00199 vabs[0].extents = Vector(itmsgab->extents.x+_fPrunePadding, itmsgab->extents.y+_fPrunePadding, itmsgab->extents.z+_fPrunePadding);
00200 _pbodytestbox = RaveCreateKinBody(GetEnv());
00201 _pbodytestbox->InitFromBoxes(vabs,false);
00202 _pbodytestbox->SetName(str(boost::format("testbox%d")%RaveRandomInt()));
00203 GetEnv()->AddKinBody(_pbodytestbox);
00204 }
00205 Transform t;
00206 t.rot = quatFromAxisAngle(Vector(itmsgab->axis.x,itmsgab->axis.y,itmsgab->axis.z),(dReal)itmsgab->angle);
00207 t.trans = Vector(itmsgab->center.x, itmsgab->center.y, itmsgab->center.z);
00208 _pbodytestbox->SetTransform(t);
00209 bool bPrune=false;
00210 FOREACH(itcolbody,listcheckbodies) {
00211 if( GetEnv()->CheckCollision(KinBodyConstPtr(_pbodytestbox),KinBodyConstPtr(*itcolbody)) ) {
00212 bPrune = true;
00213 break;
00214 }
00215 }
00216 if( bPrune )
00217 continue;
00218 }
00219 itobb->pos = Vector(itmsgab->center.x, itmsgab->center.y, itmsgab->center.z);
00220 itobb->extents = Vector(itmsgab->extents.x, itmsgab->extents.y, itmsgab->extents.z);
00221 tm = matrixFromAxisAngle(Vector(itmsgab->axis.x,itmsgab->axis.y,itmsgab->axis.z),(dReal)itmsgab->angle);
00222 itobb->right = Vector(tm.m[0],tm.m[4],tm.m[8]);
00223 itobb->up = Vector(tm.m[1],tm.m[5],tm.m[9]);
00224 itobb->dir = Vector(tm.m[2],tm.m[6],tm.m[10]);
00225 ++itobb;
00226 }
00227
00228 if( !!_pbodytestbox ) {
00229 GetEnv()->Remove(_pbodytestbox);
00230 _pbodytestbox.reset();
00231 }
00232
00233 RAVELOG_INFO("number of boxes %d\n",(int)_vobbs.size());
00234 if( !pbody->InitFromBoxes(_vobbs, true) ) {
00235 RAVELOG_ERRORA("failed to create collision map\n");
00236 return;
00237 }
00238
00239
00240 pbody->SetName(str(boost::format("CollisionMap%d")%_nNextId++));
00241
00242
00243 GetEnv()->AddKinBody(pbody, true);
00244 pbody->SetTransform(tcollision);
00245 _collisionstamp = topicmsg->header.stamp;
00246 _bHasCollisionMap = true;
00247 }
00248
00249 {
00250 EnvironmentMutex::scoped_lock envlock(GetEnv()->GetMutex());
00251 boost::mutex::scoped_lock lock(_mutex);
00252
00253
00254 BODIES::iterator itbody = _mapbodies.begin();
00255 while(itbody != _mapbodies.end()) {
00256 if( !itbody->second->IsLocked() ) {
00257 KinBody::LinkPtr plink = itbody->second->GetOffsetLink();
00258 GetEnv()->Remove(plink->GetParent());
00259 _mapbodies.erase(itbody++);
00260 }
00261 else
00262 ++itbody;
00263 }
00264 }
00265
00266 boost::shared_ptr<XMLData> pdata(new XMLData(_xmlid));
00267 pdata->strOffsetLink = pbody->GetLinks().front()->GetName();
00268 boost::shared_ptr<BodyData> b = boost::static_pointer_cast<BodyData>(AddKinBody(pbody, pdata));
00269 if( !b ) {
00270 RAVELOG_ERRORA("removing/destroying kinbody\n");
00271 GetEnv()->Remove(pbody);
00272 }
00273 }
00274
00275 Transform GetTransform(const tf::Transform& bt)
00276 {
00277 tf::Quaternion q = bt.getRotation();
00278 tf::Vector3 o = bt.getOrigin();
00279 return Transform(Vector(q.w(),q.x(),q.y(),q.z()),Vector(o.x(),o.y(),o.z()));
00280 }
00281
00282 boost::shared_ptr<tf::TransformListener> _tflistener;
00283 KinBodyPtr _pbodyoffset, _pbodytestbox;
00284 dReal _fPrunePadding;
00285 vector<OBB> _vobbs;
00286 ros::Time _collisionstamp;
00287 int _nNextId;
00288 bool _bPruneCollisions;
00289 bool _bCollisionStream;
00290 bool _bHasCollisionMap;
00291 };
00292
00293 #endif
