1 #ifndef hpp_core_idl__path__planners_hxx__ 2 #define hpp_core_idl__path__planners_hxx__ 24 template <
typename _Base,
typename _Storage>
26 :
hpp::corbaServer::ServantBase<
hpp::core::
Roadmap, _Storage> (server, s)
30 template <
typename _Base,
typename _Storage>
38 template <
typename _Base,
typename _Storage>
43 _ServantBase::deleteThis();
44 }
catch (
const std::exception& e) {
45 throw ::hpp::Error (e.what());
49 template <
typename _Base,
typename _Storage>
58 }
catch (
const std::exception& e) {
59 throw ::hpp::Error (e.what());
63 template <
typename _Base,
typename _Storage>
69 (
getT()->addNode (_config));
72 }
catch (
const std::exception& e) {
73 throw ::hpp::Error (e.what());
77 template <
typename _Base,
typename _Storage>
86 }
catch (
const std::exception& e) {
87 throw ::hpp::Error (e.what());
91 template <
typename _Base,
typename _Storage>
100 }
catch (
const std::exception& e) {
101 throw ::hpp::Error (e.what());
105 template <
typename _Base,
typename _Storage>
113 }
catch (
const std::exception& e) {
114 throw ::hpp::Error (e.what());
118 template <
typename _Base,
typename _Storage>
127 for (hpp::core::Nodes_t::const_iterator _node = nodes.begin(); _node != nodes.end(); ++_node)
128 configs.row(i++) = *((*_node)->configuration());
131 }
catch (
const std::exception& e) {
132 throw ::hpp::Error (e.what());
136 template <
typename _Base,
typename _Storage>
141 return getT()->nodes().size();
143 }
catch (
const std::exception& e) {
144 throw ::hpp::Error (e.what());
148 template <
typename _Base,
typename _Storage>
155 }
catch (
const std::exception& e) {
156 throw ::hpp::Error (e.what());
160 template <
typename _Base,
typename _Storage>
165 return getT()->edges().size();
167 }
catch (
const std::exception& e) {
168 throw ::hpp::Error (e.what());
172 template <
typename _Base,
typename _Storage>
177 return corbaServer::makeServant<hpp::core_idl::Path_ptr> (
server_,
178 new Path (server_, (*std::next(
getT()->edges().begin(),i))->path()));
180 }
catch (
const std::exception& e) {
181 throw ::hpp::Error (e.what());
195 namespace core_impl {
196 template <
typename _Base,
typename _Storage>
202 template <
typename _Base,
typename _Storage>
210 template <
typename _Base,
typename _Storage>
215 _ServantBase::deleteThis();
216 }
catch (
const std::exception& e) {
217 throw ::hpp::Error (e.what());
221 template <
typename _Base,
typename _Storage>
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());
235 template <
typename _Base,
typename _Storage>
241 (
getT()->startSolve ());
244 }
catch (
const std::exception& e) {
245 throw ::hpp::Error (e.what());
249 template <
typename _Base,
typename _Storage>
255 (
getT()->tryConnectInitAndGoals ());
258 }
catch (
const std::exception& e) {
259 throw ::hpp::Error (e.what());
263 template <
typename _Base,
typename _Storage>
269 (
getT()->oneStep ());
272 }
catch (
const std::exception& e) {
273 throw ::hpp::Error (e.what());
277 template <
typename _Base,
typename _Storage>
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());
291 template <
typename _Base,
typename _Storage>
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());
305 template <
typename _Base,
typename _Storage>
311 (
getT()->interrupt ());
314 }
catch (
const std::exception& e) {
315 throw ::hpp::Error (e.what());
319 template <
typename _Base,
typename _Storage>
325 (
getT()->maxIterations (n));
328 }
catch (
const std::exception& e) {
329 throw ::hpp::Error (e.what());
333 template <
typename _Base,
typename _Storage>
339 (
getT()->timeOut (seconds));
342 }
catch (
const std::exception& e) {
343 throw ::hpp::Error (e.what());
347 template <
typename _Base,
typename _Storage>
353 hpp::core::RoadmapPtr_t __return__ (
getT()->roadmap ());
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());
361 template <
typename _Base,
typename _Storage>
367 (
getT()->stopWhenProblemIsSolved (enable));
370 }
catch (
const std::exception& e) {
371 throw ::hpp::Error (e.what());
385 namespace core_impl {
386 template <
typename _Base,
typename _Storage>
392 template <
typename _Base,
typename _Storage>
400 template <
typename _Base,
typename _Storage>
405 _ServantBase::deleteThis();
406 }
catch (
const std::exception& e) {
407 throw ::hpp::Error (e.what());
411 template <
typename _Base,
typename _Storage>
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());
425 template <
typename _Base,
typename _Storage>
431 (
getT()->interrupt ());
434 }
catch (
const std::exception& e) {
435 throw ::hpp::Error (e.what());
439 template <
typename _Base,
typename _Storage>
445 (
getT()->maxIterations (n));
448 }
catch (
const std::exception& e) {
449 throw ::hpp::Error (e.what());
453 template <
typename _Base,
typename _Storage>
459 (
getT()->timeOut (seconds));
462 }
catch (
const std::exception& e) {
463 throw ::hpp::Error (e.what());
476 #endif // hpp_core_idl__path__planners_hxx__ 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''.
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