openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
sensorsystem.h
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2010 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/>.
22 #ifndef OPENRAVE_SENSORSYSTEM_H
23 #define OPENRAVE_SENSORSYSTEM_H
24 
25 namespace OpenRAVE {
26 
31 {
32 public:
33  SensorSystemBase(EnvironmentBasePtr penv) : InterfaceBase(PT_SensorSystem, penv) {
34  }
35  virtual ~SensorSystemBase() {
36  }
37 
39  static inline InterfaceType GetInterfaceTypeStatic() {
40  return PT_SensorSystem;
41  }
42 
44  virtual void Reset() = 0;
45 
47  virtual void AddRegisteredBodies(const std::vector<KinBodyPtr>& vbodies) = 0;
48 
51  virtual KinBody::ManageDataPtr AddKinBody(KinBodyPtr pbody, XMLReadableConstPtr pdata) = 0;
53  virtual bool RemoveKinBody(KinBodyPtr pbody) = 0;
55  virtual bool IsBodyPresent(KinBodyPtr pbody) = 0;
57  virtual bool EnableBody(KinBodyPtr pbody, bool bEnable) = 0;
58 
62  virtual bool SwitchBody(KinBodyPtr pbody1, KinBodyPtr pbody2) = 0;
63 
64 protected:
65  virtual void SetManageData(KinBodyPtr pbody, KinBody::ManageDataPtr data) {
66  pbody->SetManageData(data);
67  }
68 
69 private:
70  virtual const char* GetHash() const {
72  }
73 };
74 
77 {
78 public:
79  class OPENRAVE_API XMLData : public XMLReadable {
80 public:
81  XMLData(const std::string& xmlid) : XMLReadable(xmlid), id(0) {
82  }
83  virtual void copy(boost::shared_ptr<XMLData const> pdata) {
84  *this = *pdata;
85  }
86 
87  std::string sid;
88  int id;
89  std::string strOffsetLink;
90  Transform transOffset,transPreOffset; // final offset = transOffset * transReturnedFromVision * transPreOffset
91 
92  friend class SimpleSensorSystem;
93  };
94 
96 public:
97  BodyData(SensorSystemBasePtr psensorsystem, KinBodyPtr pbody, boost::shared_ptr<XMLData> initdata) : KinBody::ManageData(psensorsystem), _initdata(initdata), bPresent(false), bEnabled(true), bLock(false)
98  {
99  SetBody(pbody);
100  }
101 
102  virtual XMLReadableConstPtr GetData() const {
103  return _initdata;
104  }
105  virtual KinBody::LinkPtr GetOffsetLink() const {
106  return KinBody::LinkPtr(_plink);
107  }
108 
109  virtual bool IsPresent() const {
110  return bPresent;
111  }
112  virtual bool IsEnabled() const {
113  return bEnabled;
114  }
115  virtual bool IsLocked() const {
116  return bLock;
117  }
118  virtual bool Lock(bool bDoLock) {
119  bLock = bDoLock; return true;
120  }
121 
122  virtual int GetId() {
123  return _initdata->id;
124  }
125  virtual const std::string& GetSid() {
126  return _initdata->sid;
127  }
128  virtual const Transform& GetRecentTransform() {
129  return tnew;
130  }
131 
132 protected:
133  virtual void SetBody(KinBodyPtr pbody)
134  {
135  KinBody::LinkPtr plink;
136  if( _initdata->strOffsetLink.size() > 0 )
137  plink = pbody->GetLink(_initdata->strOffsetLink);
138  if( !plink )
139  plink = pbody->GetLinks().front();
140  _plink = plink;
141  }
142 
143  boost::shared_ptr<XMLData> _initdata;
144  uint64_t lastupdated;
146 
147  bool bPresent;
148  bool bEnabled;
149  bool bLock;
150 
152  friend class SimpleSensorSystem;
153  };
154 
156  {
157 public:
158  SimpleXMLReader(boost::shared_ptr<XMLData>);
160  return _pdata;
161  }
162  virtual ProcessElement startElement(const std::string& name, const AttributesList& atts);
163  virtual bool endElement(const std::string& name);
164  virtual void characters(const std::string& ch);
165 
166 protected:
167  boost::shared_ptr<XMLData> _pdata;
168  std::stringstream ss;
169  };
170 
172  static UserDataPtr RegisterXMLReaderId(EnvironmentBasePtr penv, const std::string& xmlid);
173 
174  SimpleSensorSystem(const std::string& xmlid, EnvironmentBasePtr penv);
175  virtual ~SimpleSensorSystem();
176 
177  virtual void Reset();
178 
179  virtual void AddRegisteredBodies(const std::vector<KinBodyPtr>& vbodies);
180  virtual KinBody::ManageDataPtr AddKinBody(KinBodyPtr pbody, XMLReadableConstPtr pdata);
181 
182  virtual bool RemoveKinBody(KinBodyPtr pbody);
183  virtual bool IsBodyPresent(KinBodyPtr pbody);
184  virtual bool EnableBody(KinBodyPtr pbody, bool bEnable);
185  virtual bool SwitchBody(KinBodyPtr pbody1, KinBodyPtr pbody2);
186 
187 protected:
188  typedef std::pair<boost::shared_ptr<BodyData>, Transform > SNAPSHOT;
189  typedef std::map<int,boost::shared_ptr<BodyData> > BODIES;
190  virtual boost::shared_ptr<BodyData> CreateBodyData(KinBodyPtr pbody, boost::shared_ptr<XMLData const> pdata);
191  virtual void _UpdateBodies(std::list<SNAPSHOT>& listbodies);
192  virtual void _UpdateBodiesThread();
193 
194  virtual void SetRecentTransform(boost::shared_ptr<BodyData> pdata, const Transform& t) {
195  pdata->tnew = t;
196  }
197 
199  static BaseXMLReaderPtr CreateXMLReaderId(const std::string& xmlid, InterfaceBasePtr ptr, const AttributesList& atts);
200 
201  std::string _xmlid;
203  boost::mutex _mutex;
204  uint64_t _expirationtime;
206  boost::thread _threadUpdate;
207 };
208 
209 } // end namespace OpenRAVE
210 
211 #endif