openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
sensorsystem.cpp
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2011 Rosen Diankov (rosen.diankov@gmail.com)
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 #include "libopenrave.h"
18 
19 namespace OpenRAVE {
20 
21 SimpleSensorSystem::SimpleXMLReader::SimpleXMLReader(boost::shared_ptr<XMLData> p) : _pdata(p)
22 {
23 }
24 
26 {
27  ss.str("");
28  if((name != _pdata->GetXMLId())&&(name != "offsetlink")&&(name != "id")&&(name != "sid")&&(name != "translation")&&(name != "rotationmat")&&(name != "rotationaxis")&&(name != "quat")&&(name != "pretranslation")&&(name != "prerotation")&&(name != "prerotationaxis")&&(name != "prequat")) {
29  return PE_Pass;
30  }
31  return PE_Support;
32 }
33 
35 {
36  if( name == "offsetlink" ) {
37  ss >> _pdata->strOffsetLink;
38  }
39  else if( name == "id" ) {
40  ss >> _pdata->id;
41  }
42  else if( name == "sid" ) {
43  ss >> _pdata->sid;
44  }
45  else if( name == "translation" ) {
46  ss >> _pdata->transOffset.trans.x >> _pdata->transOffset.trans.y >> _pdata->transOffset.trans.z;
47  }
48  else if( name == "rotationmat" ) {
50  ss >> m.m[0] >> m.m[1] >> m.m[2] >> m.m[4] >> m.m[5] >> m.m[6] >> m.m[8] >> m.m[9] >> m.m[10];
51  _pdata->transOffset.rot = Transform(m).rot;
52  }
53  else if( name == "rotationaxis" ) {
54  Vector axis; dReal fang;
55  ss >> axis.x >> axis.y >> axis.z >> fang;
56  _pdata->transOffset.rot = quatFromAxisAngle(axis,fang*dReal(PI/180.0));
57  }
58  else if( name == "quat" ) {
59  ss >> _pdata->transOffset.rot;
60  }
61  else if( name == "pretranslation") {
62  ss >> _pdata->transPreOffset.trans.x >> _pdata->transPreOffset.trans.y >> _pdata->transPreOffset.trans.z;
63  }
64  else if( name == "prerotationmat") {
66  ss >> m.m[0] >> m.m[1] >> m.m[2] >> m.m[4] >> m.m[5] >> m.m[6] >> m.m[8] >> m.m[9] >> m.m[10];
67  _pdata->transPreOffset.rot = Transform(m).rot;
68  }
69  else if( name == "prerotationaxis") {
70  Vector axis; dReal fang;
71  ss >> axis.x >> axis.y >> axis.z >> fang;
72  _pdata->transPreOffset.rot = quatFromAxisAngle(axis,fang*dReal(PI/180.0));
73  }
74  else if( name == "prequat") {
75  ss >> _pdata->transPreOffset.rot;
76  }
77  else if( name == utils::ConvertToLowerCase(_pdata->GetXMLId()) ) {
78  return true;
79  }
80  if( !ss ) {
81  RAVELOG_WARN(str(boost::format("error parsing %s\n")%name));
82  }
83  return false;
84 }
85 
87 {
88  ss.clear();
89  ss << ch;
90 }
91 
93 {
94  return BaseXMLReaderPtr(new SimpleXMLReader(boost::shared_ptr<XMLData>(new XMLData(xmlid))));
95 }
96 
98 {
99  return RaveRegisterXMLReader(PT_KinBody,xmlid, boost::bind(&SimpleSensorSystem::CreateXMLReaderId,xmlid, _1,_2));
100 }
101 
103 {
104  _xmlid = xmlid;
105  std::transform(_xmlid.begin(), _xmlid.end(), _xmlid.begin(), ::tolower);
106 }
107 
109 {
110  Reset();
111  _bShutdown = true;
112  _threadUpdate.join();
113 }
114 
116 {
117  boost::mutex::scoped_lock lock(_mutex);
118  _mapbodies.clear();
119 }
120 
121 void SimpleSensorSystem::AddRegisteredBodies(const std::vector<KinBodyPtr>& vbodies)
122 {
123  // go through all bodies in the environment and check for mocap data
124  FOREACHC(itbody, vbodies) {
125  boost::shared_ptr<XMLData> pmocapdata = boost::dynamic_pointer_cast<XMLData>((*itbody)->GetReadableInterface(_xmlid));
126  if( !!pmocapdata ) {
127  KinBody::ManageDataPtr p = AddKinBody(*itbody, pmocapdata);
128  if( !!p ) {
129  p->Lock(true);
130  }
131  }
132  }
133 }
134 
136 {
137  BOOST_ASSERT(pbody->GetEnv()==GetEnv());
138  boost::shared_ptr<XMLData const> pdata = boost::static_pointer_cast<XMLData const>(_pdata);
139  if( !pdata ) {
140  pdata = boost::dynamic_pointer_cast<XMLData const>(pbody->GetReadableInterface(_xmlid));
141  if( !pdata ) {
142  RAVELOG_VERBOSE(str(boost::format("failed to find manage data for body %s\n")%pbody->GetName()));
143  return KinBody::ManageDataPtr();
144  }
145  }
146 
147  boost::mutex::scoped_lock lock(_mutex);
148  if( _mapbodies.find(pbody->GetEnvironmentId()) != _mapbodies.end() ) {
149  RAVELOG_WARN(str(boost::format("body %s already added\n")%pbody->GetName()));
150  return KinBody::ManageDataPtr();
151  }
152 
153  boost::shared_ptr<BodyData> b = CreateBodyData(pbody, pdata);
154  b->lastupdated = utils::GetMicroTime();
155  _mapbodies[pbody->GetEnvironmentId()] = b;
156  RAVELOG_VERBOSE(str(boost::format("system adding body %s (%s), total: %d\n")%pbody->GetName()%pbody->GetURI()%_mapbodies.size()));
157  SetManageData(pbody,b);
158  return b;
159 }
160 
162 {
163  boost::mutex::scoped_lock lock(_mutex);
164  bool bSuccess = _mapbodies.erase(pbody->GetEnvironmentId())>0;
165  RAVELOG_VERBOSE(str(boost::format("system removing body %s %s\n")%pbody->GetName()%(bSuccess ? "succeeded" : "failed")));
166  return bSuccess;
167 }
168 
170 {
171  boost::mutex::scoped_lock lock(_mutex);
172  return _mapbodies.find(pbody->GetEnvironmentId()) != _mapbodies.end();
173 }
174 
176 {
177  boost::mutex::scoped_lock lock(_mutex);
178  BODIES::iterator it = _mapbodies.find(pbody->GetEnvironmentId());
179  if( it == _mapbodies.end() ) {
180  RAVELOG_WARN("trying to %s body %s that is not in system\n", bEnable ? "enable" : "disable", pbody->GetName().c_str());
181  return false;
182  }
183 
184  it->second->bEnabled = bEnable;
185  return true;
186 }
187 
189 {
190  //boost::mutex::scoped_lock lock(_mutex);
191  BODIES::iterator it = _mapbodies.find(pbody1->GetEnvironmentId());
192  boost::shared_ptr<BodyData> pb1,pb2;
193  if( it != _mapbodies.end() ) {
194  pb1 = it->second;
195  }
196  it = _mapbodies.find(pbody2->GetEnvironmentId());
197  if( it != _mapbodies.end() ) {
198  pb2 = it->second;
199  }
200  if( !pb1 || !pb2 ) {
201  return false;
202  }
203  if( !!pb1 ) {
204  pb1->SetBody(pbody2);
205  }
206  if( !!pb2 ) {
207  pb2->SetBody(pbody1);
208  }
209  return true;
210 }
211 
212 boost::shared_ptr<SimpleSensorSystem::BodyData> SimpleSensorSystem::CreateBodyData(KinBodyPtr pbody, boost::shared_ptr<XMLData const> pdata)
213 {
214  boost::shared_ptr<XMLData> pnewdata(new XMLData(_xmlid));
215  pnewdata->copy(pdata);
216  return boost::shared_ptr<BodyData>(new BodyData(RaveInterfaceCast<SimpleSensorSystem>(shared_from_this()),pbody, pnewdata));
217 }
218 
219 void SimpleSensorSystem::_UpdateBodies(list<SimpleSensorSystem::SNAPSHOT>& listbodies)
220 {
221  EnvironmentMutex::scoped_lock lockenv(GetEnv()->GetMutex()); // always lock environment to preserve mutex order
222  uint64_t curtime = utils::GetMicroTime();
223  if( listbodies.size() > 0 ) {
224 
225  FOREACH(it, listbodies) {
226  BOOST_ASSERT( it->first->IsEnabled() );
227 
228  KinBody::LinkPtr plink = it->first->GetOffsetLink();
229  if( !plink ) {
230  continue;
231  }
232  // transform with respect to offset link
233  TransformMatrix tlink = plink->GetTransform();
234  TransformMatrix tbase = plink->GetParent()->GetTransform();
235  TransformMatrix toffset = tbase * tlink.inverse() * it->first->_initdata->transOffset;
236  TransformMatrix tfinal = toffset * it->second*it->first->_initdata->transPreOffset;
237 
238  plink->GetParent()->SetTransform(tfinal);
239  it->first->lastupdated = curtime;
240  it->first->tnew = it->second;
241 
242  if( !it->first->IsPresent() ) {
243  RAVELOG_VERBOSE(str(boost::format("updating body %s\n")%plink->GetParent()->GetName()));
244  }
245  it->first->bPresent = true;
246  }
247  }
248 
249  boost::mutex::scoped_lock lock(_mutex);
250  BODIES::iterator itbody = _mapbodies.begin();
251  while(itbody != _mapbodies.end()) {
252  KinBody::LinkPtr plink = itbody->second->GetOffsetLink();
253  if( !!plink &&(plink->GetParent()->GetEnvironmentId()==0)) {
254  _mapbodies.erase(itbody++);
255  continue;
256  }
257  else if( curtime-itbody->second->lastupdated > _expirationtime ) {
258  if( !itbody->second->IsLocked() ) {
259  if( !!plink ) {
260  //RAVELOG_VERBOSE(str(boost::format("object %s expired %fs\n")%plink->GetParent()->GetName()*((curtime-itbody->second->lastupdated)*1e-6f)));
261  GetEnv()->Remove(plink->GetParent());
262  }
263  _mapbodies.erase(itbody++);
264  continue;
265  }
266 
267  if( itbody->second->IsPresent() && !!plink ) {
268  RAVELOG_VERBOSE(str(boost::format("body %s not present\n")%plink->GetParent()->GetName()));
269  }
270  itbody->second->bPresent = false;
271  }
272 
273  ++itbody;
274  }
275 }
276 
278 {
279  list< SNAPSHOT > listbodies;
280 
281  while(!_bShutdown) {
282  {
283  _UpdateBodies(listbodies);
284  }
285  usleep(10000); // 10ms
286  }
287 }
288 
289 }