hpp-corbaserver  4.12.0
Corba server for Humanoid Path Planner applications
path_planners.hh
Go to the documentation of this file.
1 #ifndef hpp_core_idl__path__planners_hxx__
2 #define hpp_core_idl__path__planners_hxx__
3 
4 //
5 // Implemention of IDL interfaces in file /local/robotpkg/var/tmp/robotpkg/path/py-hpp-corbaserver/work/hpp-corbaserver-4.12.0/idl/hpp/core_idl/path_planners.idl
6 //
7 
9 
10 #include <sstream>
11 
12 #include <hpp/corbaserver/fwd.hh>
15 
16 
17 
18 //
19 // Implementational code for IDL interface hpp::core_idl::Roadmap
20 //
21 namespace hpp {
22 
23 namespace core_impl {
24 template <typename _Base, typename _Storage>
26  : hpp::corbaServer::ServantBase<hpp::core::Roadmap, _Storage> (server, s)
27 {
28  // add extra constructor code here
29 }
30 template <typename _Base, typename _Storage>
32 {
33  // add extra destructor code here
34 }
35 
36 // Methods corresponding to IDL attributes and operations
37 
38 template <typename _Base, typename _Storage>
40 {
41  try {
42  // automatically generated code.
43  _ServantBase::deleteThis();
44  } catch (const std::exception& e) {
45  throw ::hpp::Error (e.what());
46  }
47 }
48 
49 template <typename _Base, typename _Storage>
51 {
52  try {
53  // automatically generated code.
54 
55  (getT()->clear ());
56 
57 
58  } catch (const std::exception& e) {
59  throw ::hpp::Error (e.what());
60  }
61 }
62 
63 template <typename _Base, typename _Storage>
65 {
66  try {
67  // automatically generated code.
69  (getT()->addNode (_config));
70 
71 
72  } catch (const std::exception& e) {
73  throw ::hpp::Error (e.what());
74  }
75 }
76 
77 template <typename _Base, typename _Storage>
79 {
80  try {
81  // generated from /local/robotpkg/var/tmp/robotpkg/path/py-hpp-corbaserver/work/hpp-corbaserver-4.12.0/idl/hpp/core_idl/path_planners.idl:22
82  core::PathPtr_t path (corbaServer::reference_to_servant_base<core::Path>(server_, path_)->get());
83  getT()->addEdge(getT()->addNode(corbaServer::floatSeqToVector(cfgfrom)),
84  getT()->addNode(corbaServer::floatSeqToVector(cfgto )), path);
85 
86  } catch (const std::exception& e) {
87  throw ::hpp::Error (e.what());
88  }
89 }
90 
91 template <typename _Base, typename _Storage>
93 {
94  try {
95  // generated from /local/robotpkg/var/tmp/robotpkg/path/py-hpp-corbaserver/work/hpp-corbaserver-4.12.0/idl/hpp/core_idl/path_planners.idl:22
96  core::PathPtr_t path (corbaServer::reference_to_servant_base<core::Path>(server_, path_)->get());
97  getT()->addEdges(getT()->addNode(corbaServer::floatSeqToVector(cfgfrom)),
98  getT()->addNode(corbaServer::floatSeqToVector(cfgto )), path);
99 
100  } catch (const std::exception& e) {
101  throw ::hpp::Error (e.what());
102  }
103 }
104 
105 template <typename _Base, typename _Storage>
107 {
108  try {
109  // generated from /local/robotpkg/var/tmp/robotpkg/path/py-hpp-corbaserver/work/hpp-corbaserver-4.12.0/idl/hpp/core_idl/path_planners.idl:22
111  corbaServer::floatSeqToVector(config), distance, reverse)->configuration()));
112 
113  } catch (const std::exception& e) {
114  throw ::hpp::Error (e.what());
115  }
116 }
117 
118 template <typename _Base, typename _Storage>
120 {
121  try {
122  // generated from /local/robotpkg/var/tmp/robotpkg/path/py-hpp-corbaserver/work/hpp-corbaserver-4.12.0/idl/hpp/core_idl/path_planners.idl:22
123  hpp::core::Nodes_t nodes = getT()->nearestNodes(
124  corbaServer::floatSeqToVector(config), k);
125  hpp::core::matrix_t configs (nodes.size(), config.length());
126  size_type i = 0;
127  for (hpp::core::Nodes_t::const_iterator _node = nodes.begin(); _node != nodes.end(); ++_node)
128  configs.row(i++) = *((*_node)->configuration());
129  return corbaServer::matrixToFloatSeqSeq(configs);
130 
131  } catch (const std::exception& e) {
132  throw ::hpp::Error (e.what());
133  }
134 }
135 
136 template <typename _Base, typename _Storage>
138 {
139  try {
140  // generated from /local/robotpkg/var/tmp/robotpkg/path/py-hpp-corbaserver/work/hpp-corbaserver-4.12.0/idl/hpp/core_idl/path_planners.idl:22
141  return getT()->nodes().size();
142 
143  } catch (const std::exception& e) {
144  throw ::hpp::Error (e.what());
145  }
146 }
147 
148 template <typename _Base, typename _Storage>
150 {
151  try {
152  // generated from /local/robotpkg/var/tmp/robotpkg/path/py-hpp-corbaserver/work/hpp-corbaserver-4.12.0/idl/hpp/core_idl/path_planners.idl:22
153  return corbaServer::vectorToFloatSeq(*((*std::next(getT()->nodes().begin(), i))->configuration()));
154 
155  } catch (const std::exception& e) {
156  throw ::hpp::Error (e.what());
157  }
158 }
159 
160 template <typename _Base, typename _Storage>
162 {
163  try {
164  // generated from /local/robotpkg/var/tmp/robotpkg/path/py-hpp-corbaserver/work/hpp-corbaserver-4.12.0/idl/hpp/core_idl/path_planners.idl:22
165  return getT()->edges().size();
166 
167  } catch (const std::exception& e) {
168  throw ::hpp::Error (e.what());
169  }
170 }
171 
172 template <typename _Base, typename _Storage>
174 {
175  try {
176  // generated from /local/robotpkg/var/tmp/robotpkg/path/py-hpp-corbaserver/work/hpp-corbaserver-4.12.0/idl/hpp/core_idl/path_planners.idl:22
177  return corbaServer::makeServant<hpp::core_idl::Path_ptr> (server_,
178  new Path (server_, (*std::next(getT()->edges().begin(),i))->path()));
179 
180  } catch (const std::exception& e) {
181  throw ::hpp::Error (e.what());
182  }
183 }
184 
185 // End of implementational code
186 } // namespace core_impl
187 
188 } // namespace hpp
189 
190 //
191 // Implementational code for IDL interface hpp::core_idl::PathPlanner
192 //
193 namespace hpp {
194 
195 namespace core_impl {
196 template <typename _Base, typename _Storage>
198  : hpp::corbaServer::ServantBase<hpp::core::PathPlanner, _Storage> (server, s)
199 {
200  // add extra constructor code here
201 }
202 template <typename _Base, typename _Storage>
204 {
205  // add extra destructor code here
206 }
207 
208 // Methods corresponding to IDL attributes and operations
209 
210 template <typename _Base, typename _Storage>
212 {
213  try {
214  // automatically generated code.
215  _ServantBase::deleteThis();
216  } catch (const std::exception& e) {
217  throw ::hpp::Error (e.what());
218  }
219 }
220 
221 template <typename _Base, typename _Storage>
223 {
224  try {
225  // automatically generated code.
226 
227  hpp::core::PathVectorPtr_t __return__ (getT()->solve ());
228 
229  return ::hpp::corbaServer::makeServantDownCast<hpp::core_impl::Path,hpp::core_impl::PathVector>(server_, __return__)._retn();
230  } catch (const std::exception& e) {
231  throw ::hpp::Error (e.what());
232  }
233 }
234 
235 template <typename _Base, typename _Storage>
237 {
238  try {
239  // automatically generated code.
240 
241  (getT()->startSolve ());
242 
243 
244  } catch (const std::exception& e) {
245  throw ::hpp::Error (e.what());
246  }
247 }
248 
249 template <typename _Base, typename _Storage>
251 {
252  try {
253  // automatically generated code.
254 
255  (getT()->tryConnectInitAndGoals ());
256 
257 
258  } catch (const std::exception& e) {
259  throw ::hpp::Error (e.what());
260  }
261 }
262 
263 template <typename _Base, typename _Storage>
265 {
266  try {
267  // automatically generated code.
268 
269  (getT()->oneStep ());
270 
271 
272  } catch (const std::exception& e) {
273  throw ::hpp::Error (e.what());
274  }
275 }
276 
277 template <typename _Base, typename _Storage>
279 {
280  try {
281  // automatically generated code.
282 
283  hpp::core::PathVectorPtr_t __return__ (getT()->computePath ());
284 
285  return ::hpp::corbaServer::makeServantDownCast<hpp::core_impl::Path,hpp::core_impl::PathVector>(server_, __return__)._retn();
286  } catch (const std::exception& e) {
287  throw ::hpp::Error (e.what());
288  }
289 }
290 
291 template <typename _Base, typename _Storage>
293 {
294  try {
295  // automatically generated code.
296  hpp::core::PathVectorPtr_t _path = ::hpp::corbaServer::reference_to_object<hpp::core::PathVector>(server_, path);
297  hpp::core::PathVectorPtr_t __return__ (getT()->finishSolve (_path));
298 
299  return ::hpp::corbaServer::makeServantDownCast<hpp::core_impl::Path,hpp::core_impl::PathVector>(server_, __return__)._retn();
300  } catch (const std::exception& e) {
301  throw ::hpp::Error (e.what());
302  }
303 }
304 
305 template <typename _Base, typename _Storage>
307 {
308  try {
309  // automatically generated code.
310 
311  (getT()->interrupt ());
312 
313 
314  } catch (const std::exception& e) {
315  throw ::hpp::Error (e.what());
316  }
317 }
318 
319 template <typename _Base, typename _Storage>
321 {
322  try {
323  // automatically generated code.
324 
325  (getT()->maxIterations (n));
326 
327 
328  } catch (const std::exception& e) {
329  throw ::hpp::Error (e.what());
330  }
331 }
332 
333 template <typename _Base, typename _Storage>
335 {
336  try {
337  // automatically generated code.
338 
339  (getT()->timeOut (seconds));
340 
341 
342  } catch (const std::exception& e) {
343  throw ::hpp::Error (e.what());
344  }
345 }
346 
347 template <typename _Base, typename _Storage>
349 {
350  try {
351  // automatically generated code.
352 
353  hpp::core::RoadmapPtr_t __return__ (getT()->roadmap ());
354 
355  return ::hpp::corbaServer::makeServantDownCast<hpp::core_impl::Roadmap,hpp::core_impl::Roadmap>(server_, __return__)._retn();
356  } catch (const std::exception& e) {
357  throw ::hpp::Error (e.what());
358  }
359 }
360 
361 template <typename _Base, typename _Storage>
363 {
364  try {
365  // automatically generated code.
366 
367  (getT()->stopWhenProblemIsSolved (enable));
368 
369 
370  } catch (const std::exception& e) {
371  throw ::hpp::Error (e.what());
372  }
373 }
374 
375 // End of implementational code
376 } // namespace core_impl
377 
378 } // namespace hpp
379 
380 //
381 // Implementational code for IDL interface hpp::core_idl::PathOptimizer
382 //
383 namespace hpp {
384 
385 namespace core_impl {
386 template <typename _Base, typename _Storage>
388  : hpp::corbaServer::ServantBase<hpp::core::PathOptimizer, _Storage> (server, s)
389 {
390  // add extra constructor code here
391 }
392 template <typename _Base, typename _Storage>
394 {
395  // add extra destructor code here
396 }
397 
398 // Methods corresponding to IDL attributes and operations
399 
400 template <typename _Base, typename _Storage>
402 {
403  try {
404  // automatically generated code.
405  _ServantBase::deleteThis();
406  } catch (const std::exception& e) {
407  throw ::hpp::Error (e.what());
408  }
409 }
410 
411 template <typename _Base, typename _Storage>
413 {
414  try {
415  // automatically generated code.
416  hpp::core::PathVectorPtr_t _path = ::hpp::corbaServer::reference_to_object<hpp::core::PathVector>(server_, path);
417  hpp::core::PathVectorPtr_t __return__ (getT()->optimize (_path));
418 
419  return ::hpp::corbaServer::makeServantDownCast<hpp::core_impl::Path,hpp::core_impl::PathVector>(server_, __return__)._retn();
420  } catch (const std::exception& e) {
421  throw ::hpp::Error (e.what());
422  }
423 }
424 
425 template <typename _Base, typename _Storage>
427 {
428  try {
429  // automatically generated code.
430 
431  (getT()->interrupt ());
432 
433 
434  } catch (const std::exception& e) {
435  throw ::hpp::Error (e.what());
436  }
437 }
438 
439 template <typename _Base, typename _Storage>
441 {
442  try {
443  // automatically generated code.
444 
445  (getT()->maxIterations (n));
446 
447 
448  } catch (const std::exception& e) {
449  throw ::hpp::Error (e.what());
450  }
451 }
452 
453 template <typename _Base, typename _Storage>
455 {
456  try {
457  // automatically generated code.
458 
459  (getT()->timeOut (seconds));
460 
461 
462  } catch (const std::exception& e) {
463  throw ::hpp::Error (e.what());
464  }
465 }
466 
467 // End of implementational code
468 } // namespace core_impl
469 
470 } // namespace hpp
471 
472 
473 
474 
475 
476 #endif // hpp_core_idl__path__planners_hxx__
477 
void timeOut(hpp::value_type seconds)
Definition: path_planners.hh:334
void clear()
Definition: path_planners.hh:50
Definition: path_planners-fwd.hh:31
void stopWhenProblemIsSolved(::CORBA::Boolean enable)
Definition: path_planners.hh:362
void deleteThis()
Definition: path_planners.hh:401
hpp::core_idl::Roadmap_ptr getRoadmap()
Definition: path_planners.hh:348
hpp::core_idl::Path_ptr getEdge(hpp::size_type i)
Definition: path_planners.hh:173
Implement CORBA interface ``Obstacle&#39;&#39;.
Definition: basic-server.hh:27
RoadmapServant(::hpp::corbaServer::Server *server, const _Storage &s)
Definition: path_planners.hh:25
double value_type
Definition: common.idl:18
void deleteThis()
Definition: path_planners.hh:39
sequence< floatSeq > floatSeqSeq
Definition: common.idl:34
hpp::floatSeqSeq * nearestNodes(const hpp::floatSeq &config, hpp::size_type &k)
Definition: path_planners.hh:119
hpp::core_idl::PathVector_ptr optimize(hpp::core_idl::PathVector_ptr path)
Definition: path_planners.hh:412
_objref_Roadmap * Roadmap_ptr
Definition: path_planners-idl.hh:109
virtual ~PathPlannerServant()
Definition: path_planners.hh:203
vector_t floatSeqToVector(const floatSeq &dofArray, const size_type expectedSize=-1)
virtual ~PathOptimizerServant()
Definition: path_planners.hh:393
Server * server_
Definition: servant-base.hh:97
hpp::floatSeq * getNode(hpp::size_type i)
Definition: path_planners.hh:149
hpp::core_idl::PathVector_ptr computePath()
Definition: path_planners.hh:278
Definition: path_planners-fwd.hh:170
void addNode(const hpp::floatSeq &config)
Definition: path_planners.hh:64
virtual ~RoadmapServant()
Definition: path_planners.hh:31
pinocchio::vector_t vector_t
Definition: fwd.hh:89
hpp::size_type getNbEdges()
Definition: path_planners.hh:161
void oneStep()
Definition: path_planners.hh:264
hpp::size_type getNbNodes()
Definition: path_planners.hh:137
ServantBase(Server *server, const Storage &_s)
Definition: servant-base.hh:165
void startSolve()
Definition: path_planners.hh:236
PathServant< POA_hpp::core_idl::Path, hpp::weak_ptr< hpp::core::Path > > Path
Definition: paths-fwd.hh:84
void interrupt()
Definition: path_planners.hh:306
hpp::floatSeq * nearestNode(const hpp::floatSeq &config, hpp::value_type &distance, ::CORBA::Boolean reverse)
Definition: path_planners.hh:106
void interrupt()
Definition: path_planners.hh:426
void timeOut(hpp::value_type seconds)
Definition: path_planners.hh:454
_objref_PathVector * PathVector_ptr
Definition: paths-idl.hh:82
void maxIterations(hpp::size_type n)
Definition: path_planners.hh:320
void maxIterations(hpp::size_type n)
Definition: path_planners.hh:440
core::PathVectorPtr_t PathVectorPtr_t
Definition: fwd.hh:77
Implementation of Hpp module Corba server.
Definition: server.hh:54
pinocchio::matrix_t matrix_t
Definition: fwd.hh:87
void addNodeAndEdge(const hpp::floatSeq &cfgfrom, const hpp::floatSeq &cfgto, hpp::core_idl::Path_ptr path_)
Definition: path_planners.hh:78
Definition: path_planners-fwd.hh:99
floatSeqSeq * matrixToFloatSeqSeq(core::matrixIn_t input)
Returns a sequence of the rows of the input matrix.
hpp::core_idl::PathVector_ptr finishSolve(hpp::core_idl::PathVector_ptr path)
Definition: path_planners.hh:292
floatSeq * vectorToFloatSeq(core::vectorIn_t input)
StorageElementShPtr_t getT() const
Definition: servant-base.hh:121
void deleteThis()
Definition: path_planners.hh:211
_objref_Path * Path_ptr
Definition: path_planners-idl.hh:83
sequence< double > floatSeq
Robot configuration is defined by a sequence of dof value.
Definition: common.idl:33
hpp::core_idl::PathVector_ptr solve()
Definition: path_planners.hh:222
core::Nodes_t Nodes_t
Definition: fwd.hh:69
core::PathPtr_t PathPtr_t
Definition: fwd.hh:74
void addNodeAndEdges(const hpp::floatSeq &cfgfrom, const hpp::floatSeq &cfgto, hpp::core_idl::Path_ptr path_)
Definition: path_planners.hh:92
void tryConnectInitAndGoals()
Definition: path_planners.hh:250
PathPlannerServant(::hpp::corbaServer::Server *server, const _Storage &s)
Definition: path_planners.hh:197
PathOptimizerServant(::hpp::corbaServer::Server *server, const _Storage &s)
Definition: path_planners.hh:387
long long size_type
Definition: common.idl:19