1 #ifndef hpp_core_idl__path__planners_hxx__ 2 #define hpp_core_idl__path__planners_hxx__ 24 template <
typename _Base,
typename _Storage>
31 template <
typename _Base,
typename _Storage>
39 template <
typename _Base,
typename _Storage>
44 _ServantBase::deleteThis();
45 }
catch (
const std::exception& e) {
46 throw ::hpp::Error (e.what());
50 template <
typename _Base,
typename _Storage>
57 nodes[0]->configuration()->size());
59 for (hpp::core::NodeVector_t::const_iterator _node = nodes.begin();
60 _node != nodes.end(); ++_node)
61 configs.row(i++) = *((*_node)->configuration());
64 }
catch (
const std::exception& e) {
65 throw ::hpp::Error (e.what());
80 template <
typename _Base,
typename _Storage>
83 :
hpp::corbaServer::ServantBase<
hpp::core::
Roadmap, _Storage> (server, s)
87 template <
typename _Base,
typename _Storage>
95 template <
typename _Base,
typename _Storage>
100 _ServantBase::deleteThis();
101 }
catch (
const std::exception& e) {
102 throw ::hpp::Error (e.what());
106 template <
typename _Base,
typename _Storage>
115 }
catch (
const std::exception& e) {
116 throw ::hpp::Error (e.what());
120 template <
typename _Base,
typename _Storage>
126 (getT()->addNode (_config));
129 }
catch (
const std::exception& e) {
130 throw ::hpp::Error (e.what());
134 template <
typename _Base,
typename _Storage>
139 core::PathPtr_t path (corbaServer::reference_to_servant_base<core::Path>(server_, path_)->
get());
143 }
catch (
const std::exception& e) {
144 throw ::hpp::Error (e.what());
148 template <
typename _Base,
typename _Storage>
153 core::PathPtr_t path (corbaServer::reference_to_servant_base<core::Path>(server_, path_)->
get());
157 }
catch (
const std::exception& e) {
158 throw ::hpp::Error (e.what());
162 template <
typename _Base,
typename _Storage>
170 }
catch (
const std::exception& e) {
171 throw ::hpp::Error (e.what());
175 template <
typename _Base,
typename _Storage>
184 for (hpp::core::Nodes_t::const_iterator _node = nodes.begin(); _node != nodes.end(); ++_node)
185 configs.row(i++) = *((*_node)->configuration());
188 }
catch (
const std::exception& e) {
189 throw ::hpp::Error (e.what());
193 template <
typename _Base,
typename _Storage>
198 return getT()->nodes().size();
200 }
catch (
const std::exception& e) {
201 throw ::hpp::Error (e.what());
205 template <
typename _Base,
typename _Storage>
212 }
catch (
const std::exception& e) {
213 throw ::hpp::Error (e.what());
217 template <
typename _Base,
typename _Storage>
222 return getT()->edges().size();
224 }
catch (
const std::exception& e) {
225 throw ::hpp::Error (e.what());
229 template <
typename _Base,
typename _Storage>
234 return corbaServer::makeServant<hpp::core_idl::Path_ptr> (server_,
235 new Path (server_, (*std::next(getT()->edges().begin(),i))->path()));
237 }
catch (
const std::exception& e) {
238 throw ::hpp::Error (e.what());
242 template <
typename _Base,
typename _Storage>
251 }
catch (
const std::exception& e) {
252 throw ::hpp::Error (e.what());
266 namespace core_impl {
267 template <
typename _Base,
typename _Storage>
274 template <
typename _Base,
typename _Storage>
282 template <
typename _Base,
typename _Storage>
287 _ServantBase::deleteThis();
288 }
catch (
const std::exception& e) {
289 throw ::hpp::Error (e.what());
293 template <
typename _Base,
typename _Storage>
301 return ::hpp::corbaServer::makeServantDownCast<hpp::core_impl::Path,hpp::core_impl::PathVector>(server_, __return__)._retn();
302 }
catch (
const std::exception& e) {
303 throw ::hpp::Error (e.what());
307 template <
typename _Base,
typename _Storage>
313 (getT()->startSolve ());
316 }
catch (
const std::exception& e) {
317 throw ::hpp::Error (e.what());
321 template <
typename _Base,
typename _Storage>
327 (getT()->tryConnectInitAndGoals ());
330 }
catch (
const std::exception& e) {
331 throw ::hpp::Error (e.what());
335 template <
typename _Base,
typename _Storage>
341 (getT()->oneStep ());
344 }
catch (
const std::exception& e) {
345 throw ::hpp::Error (e.what());
349 template <
typename _Base,
typename _Storage>
357 return ::hpp::corbaServer::makeServantDownCast<hpp::core_impl::Path,hpp::core_impl::PathVector>(server_, __return__)._retn();
358 }
catch (
const std::exception& e) {
359 throw ::hpp::Error (e.what());
363 template <
typename _Base,
typename _Storage>
371 return ::hpp::corbaServer::makeServantDownCast<hpp::core_impl::Path,hpp::core_impl::PathVector>(server_, __return__)._retn();
372 }
catch (
const std::exception& e) {
373 throw ::hpp::Error (e.what());
377 template <
typename _Base,
typename _Storage>
383 (getT()->interrupt ());
386 }
catch (
const std::exception& e) {
387 throw ::hpp::Error (e.what());
391 template <
typename _Base,
typename _Storage>
397 (getT()->maxIterations (n));
400 }
catch (
const std::exception& e) {
401 throw ::hpp::Error (e.what());
405 template <
typename _Base,
typename _Storage>
411 (getT()->timeOut (seconds));
414 }
catch (
const std::exception& e) {
415 throw ::hpp::Error (e.what());
419 template <
typename _Base,
typename _Storage>
425 hpp::core::RoadmapPtr_t __return__ (getT()->roadmap ());
427 return ::hpp::corbaServer::makeServantDownCast<hpp::core_impl::Roadmap,hpp::core_impl::Roadmap>(server_, __return__)._retn();
428 }
catch (
const std::exception& e) {
429 throw ::hpp::Error (e.what());
433 template <
typename _Base,
typename _Storage>
439 (getT()->stopWhenProblemIsSolved (enable));
442 }
catch (
const std::exception& e) {
443 throw ::hpp::Error (e.what());
457 namespace core_impl {
458 template <
typename _Base,
typename _Storage>
465 template <
typename _Base,
typename _Storage>
473 template <
typename _Base,
typename _Storage>
478 _ServantBase::deleteThis();
479 }
catch (
const std::exception& e) {
480 throw ::hpp::Error (e.what());
484 template <
typename _Base,
typename _Storage>
492 return ::hpp::corbaServer::makeServantDownCast<hpp::core_impl::Path,hpp::core_impl::PathVector>(server_, __return__)._retn();
493 }
catch (
const std::exception& e) {
494 throw ::hpp::Error (e.what());
498 template <
typename _Base,
typename _Storage>
504 (getT()->interrupt ());
507 }
catch (
const std::exception& e) {
508 throw ::hpp::Error (e.what());
512 template <
typename _Base,
typename _Storage>
518 (getT()->maxIterations (n));
521 }
catch (
const std::exception& e) {
522 throw ::hpp::Error (e.what());
526 template <
typename _Base,
typename _Storage>
532 (getT()->timeOut (seconds));
535 }
catch (
const std::exception& e) {
536 throw ::hpp::Error (e.what());
549 #endif // hpp_core_idl__path__planners_hxx__ void timeOut(hpp::value_type seconds)
Definition: path_planners.hh:406
core::NodeVector_t NodeVector_t
Definition: fwd.hh:93
void clear()
Definition: path_planners.hh:107
Definition: path_planners-fwd.hh:74
void stopWhenProblemIsSolved(::CORBA::Boolean enable)
Definition: path_planners.hh:434
void deleteThis()
Definition: path_planners.hh:474
hpp::core_idl::Roadmap_ptr getRoadmap()
Definition: path_planners.hh:420
hpp::core_idl::Path_ptr getEdge(hpp::size_type i)
Definition: path_planners.hh:230
Implement CORBA interface `‘Obstacle’'.
Definition: basic-server.hh:35
ConnectedComponentServant(::hpp::corbaServer::Server *server, const _Storage &s)
Definition: path_planners.hh:25
RoadmapServant(::hpp::corbaServer::Server *server, const _Storage &s)
Definition: path_planners.hh:81
double value_type
Definition: common.idl:18
void deleteThis()
Definition: path_planners.hh:96
hpp::core_idl::ConnectedComponentSeq * getConnectedComponents()
Definition: path_planners.hh:243
sequence< floatSeq > floatSeqSeq
Definition: common.idl:35
hpp::floatSeqSeq * nearestNodes(const hpp::floatSeq &config, hpp::size_type &k)
Definition: path_planners.hh:176
hpp::core_idl::PathVector_ptr optimize(hpp::core_idl::PathVector_ptr path)
Definition: path_planners.hh:485
_objref_Roadmap * Roadmap_ptr
Definition: path_planners-idl.hh:338
virtual ~PathPlannerServant()
Definition: path_planners.hh:275
void deleteThis()
Definition: path_planners.hh:40
vector_t floatSeqToVector(const floatSeq &dofArray, const size_type expectedSize=-1)
virtual ~PathOptimizerServant()
Definition: path_planners.hh:466
hpp::floatSeqSeq * nodes()
Definition: path_planners.hh:51
hpp::floatSeq * getNode(hpp::size_type i)
Definition: path_planners.hh:206
hpp::core_idl::PathVector_ptr computePath()
Definition: path_planners.hh:350
Definition: path_planners-fwd.hh:218
void addNode(const hpp::floatSeq &config)
Definition: path_planners.hh:121
virtual ~RoadmapServant()
Definition: path_planners.hh:88
pinocchio::vector_t vector_t
Definition: fwd.hh:112
hpp::size_type getNbEdges()
Definition: path_planners.hh:218
void oneStep()
Definition: path_planners.hh:336
hpp::size_type getNbNodes()
Definition: path_planners.hh:194
void startSolve()
Definition: path_planners.hh:308
PathServant< POA_hpp::core_idl::Path, hpp::weak_ptr< hpp::core::Path > > Path
Definition: paths-fwd.hh:85
void interrupt()
Definition: path_planners.hh:378
hpp::floatSeq * nearestNode(const hpp::floatSeq &config, hpp::value_type &distance, ::CORBA::Boolean reverse)
Definition: path_planners.hh:163
void interrupt()
Definition: path_planners.hh:499
void timeOut(hpp::value_type seconds)
Definition: path_planners.hh:527
_objref_PathVector * PathVector_ptr
Definition: paths-idl.hh:82
Definition: servant-base.hh:436
void maxIterations(hpp::size_type n)
Definition: path_planners.hh:392
void maxIterations(hpp::size_type n)
Definition: path_planners.hh:513
core::PathVectorPtr_t PathVectorPtr_t
Definition: fwd.hh:100
Implementation of Hpp module Corba server.
Definition: server.hh:77
pinocchio::matrix_t matrix_t
Definition: fwd.hh:110
void addNodeAndEdge(const hpp::floatSeq &cfgfrom, const hpp::floatSeq &cfgto, hpp::core_idl::Path_ptr path_)
Definition: path_planners.hh:135
Definition: path_planners-fwd.hh:146
sequence< ConnectedComponent > ConnectedComponentSeq
Definition: path_planners.idl:33
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:364
floatSeq * vectorToFloatSeq(core::vectorIn_t input)
virtual ~ConnectedComponentServant()
Definition: path_planners.hh:32
void deleteThis()
Definition: path_planners.hh:283
Definition: path_planners-fwd.hh:32
_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:34
hpp::core_idl::PathVector_ptr solve()
Definition: path_planners.hh:294
core::Nodes_t Nodes_t
Definition: fwd.hh:92
core::PathPtr_t PathPtr_t
Definition: fwd.hh:97
void addNodeAndEdges(const hpp::floatSeq &cfgfrom, const hpp::floatSeq &cfgto, hpp::core_idl::Path_ptr path_)
Definition: path_planners.hh:149
void tryConnectInitAndGoals()
Definition: path_planners.hh:322
PathPlannerServant(::hpp::corbaServer::Server *server, const _Storage &s)
Definition: path_planners.hh:268
PathOptimizerServant(::hpp::corbaServer::Server *server, const _Storage &s)
Definition: path_planners.hh:459
long long size_type
Definition: common.idl:19