openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
customreader.cpp
Go to the documentation of this file.
1 
8 #include <openrave/openrave.h>
9 #include <openrave/plugin.h>
10 
11 using namespace std;
12 using namespace OpenRAVE;
13 
14 namespace cppexamples {
15 
17 {
18 public:
19  class XMLData : public XMLReadable
20  {
21 public:
22  XMLData() : XMLReadable("piddata") {
23  }
24  vector<dReal> pgains,igains;
25  };
26 
27  class PIDXMLReader : public BaseXMLReader
28  {
29 public:
30  PIDXMLReader(boost::shared_ptr<XMLData> piddata, const AttributesList& atts) {
31  _piddata = piddata;
32  if( !_piddata )
33  _piddata.reset(new XMLData());
34  RAVELOG_INFO("the attributes piddata is created with are:\n");
35  for(AttributesList::const_iterator itatt = atts.begin(); itatt != atts.end(); ++itatt)
36  RAVELOG_INFO("%s=%s\n",itatt->first.c_str(),itatt->second.c_str());
37  }
38 
40  return _piddata;
41  }
42 
43  virtual ProcessElement startElement(const std::string& name, const AttributesList& atts) {
44  _ss.str("");
45  return (name == "pgains" || name=="igains") ? PE_Support : PE_Pass;
46  }
47 
48  virtual bool endElement(const std::string& name)
49  {
50  if( name == "piddata" )
51  return true;
52  else if( name == "pgains" )
53  // read all the float values into a vector
54  _piddata->pgains = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
55  else if( name == "igains" )
56  // read all the float values into a vector
57  _piddata->igains = vector<dReal>((istream_iterator<dReal>(_ss)), istream_iterator<dReal>());
58  else
59  RAVELOG_ERROR("unknown field %s\n", name.c_str());
60 
61  return false;
62  }
63 
64  virtual void characters(const std::string& ch)
65  {
66  _ss.clear();
67  _ss << ch;
68  }
69 
70 protected:
71  boost::shared_ptr<XMLData> _piddata;
72  stringstream _ss;
73  };
74 
75  static BaseXMLReaderPtr CreateXMLReader(InterfaceBasePtr ptr, const AttributesList& atts)
76  {
77  // ptr is the robot interface that this reader is being created for
78  return BaseXMLReaderPtr(new PIDXMLReader(boost::shared_ptr<XMLData>(),atts));
79  }
80 
82  {
83  }
84  virtual ~CustomController() {
85  }
86 
87  virtual bool Init(RobotBasePtr robot, const std::vector<int>& dofindices, int nControlTransformation)
88  {
89  _probot = robot;
90  _dofindices = dofindices;
91  _nControlTransformation = nControlTransformation;
92 
93  // read the gains from the XML
94  boost::shared_ptr<XMLData> piddata = boost::dynamic_pointer_cast<XMLData>(GetReadableInterface("piddata"));
95  if( !!piddata ) {
96  stringstream ss;
97  ss << "piddata from custom XML reader is" << endl << "pgains: ";
98  for(vector<dReal>::iterator it = piddata->pgains.begin(); it != piddata->pgains.end(); ++it)
99  ss << *it << " ";
100  ss << endl << "igains: ";
101  for(vector<dReal>::iterator it = piddata->igains.begin(); it != piddata->igains.end(); ++it)
102  ss << *it << " ";
103  ss << endl;
104  RAVELOG_INFOA(ss.str());
105  }
106  else
107  RAVELOG_WARN("failed to find piddata\n");
108  return true;
109  }
110 
111  virtual const std::vector<int>& GetControlDOFIndices() const {
112  return _dofindices;
113  }
114  virtual int IsControlTransformation() const {
115  return _nControlTransformation;
116  }
117 
118  virtual void Reset(int options) {
119  }
120  virtual bool SetDesired(const std::vector<dReal>& values, TransformConstPtr trans) {
121  return false;
122  }
123  virtual bool SetPath(TrajectoryBaseConstPtr ptraj) {
124  return false;
125  }
126  virtual void SimulationStep(dReal fTimeElapsed) {
127  }
128  virtual bool IsDone() {
129  return false;
130  }
131  virtual dReal GetTime() const {
132  return 0;
133  }
134  virtual RobotBasePtr GetRobot() const {
135  return _probot;
136  }
137 
138 protected:
140  std::vector<int> _dofindices;
142 };
143 
144 } // end namespace cppexamples
145 
146 static boost::shared_ptr<void> s_RegisteredReader;
147 
148 InterfaceBasePtr CreateInterfaceValidated(InterfaceType type, const std::string& interfacename, std::istream& sinput, EnvironmentBasePtr penv)
149 {
150  if( !s_RegisteredReader ) {
153  }
154  switch(type) {
155  case PT_Controller:
156  if( interfacename == "customcontroller")
158  break;
159  default:
160  break;
161  }
162  return InterfaceBasePtr();
163 }
164 
166 {
167  info.interfacenames[PT_Controller].push_back("CustomController");
168 }
169 
171 {
172  s_RegisteredReader.reset(); // unregister the reader
173 }