hpp-corbaserver  4.15.1
Corba server for Humanoid Path Planner applications
distances.hh
Go to the documentation of this file.
1 // Copyright (C) 2019 by Joseph Mirabel, LAAS-CNRS.
2 //
3 
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are
6 // met:
7 //
8 // 1. Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 //
11 // 2. Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
18 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
21 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
22 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
23 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26 // DAMAGE.
27 //
28 // This software is provided "as is" without warranty of any kind,
29 // either expressed or implied, including but not limited to the
30 // implied warranties of fitness for a particular purpose.
31 //
32 // See the COPYING file for more information.
33 
34 #ifndef HPP_CORE_IDL_DISTANCES_HH
35 #define HPP_CORE_IDL_DISTANCES_HH
36 
37 #include <stdlib.h>
38 
40 #include <hpp/corbaserver/fwd.hh>
41 #include <vector>
42 
44 #include "hpp/core/distance.hh"
45 #include "hpp/core/weighed-distance.hh"
47 
48 namespace hpp {
49 namespace corbaServer {
50 namespace core_idl {
51 template <typename D>
52 class DistanceStorage : public AbstractStorage<D, core::Distance> {
53  public:
55  using parent_t::element;
56  using typename parent_t::ptr_t;
57 
59  DistanceStorage(const core::DevicePtr_t& _r, const ptr_t& _d)
60  : parent_t(_d), r(_r) {}
61 
62  template <typename T>
64  return DistanceStorage<T>(r, HPP_DYNAMIC_PTR_CAST(T, element.lock()));
65  }
66 };
67 
68 template <typename _Base, typename _Storage>
69 class DistanceServant : public ServantBase<core::Distance, _Storage>,
70  public virtual _Base {
72 
73  public:
74  DistanceServant(Server* server, const Storage& s) : _ServantBase(server, s) {}
75 
76  virtual ~DistanceServant() {}
77 
78  CORBA::Double value(const floatSeq& q1, const floatSeq& q2) {
79  Configuration_t qq1(floatSeqToConfig(getS().r, q1, true)),
80  qq2(floatSeqToConfig(getS().r, q2, true));
81  return (*get())(qq1, qq2);
82  }
83 };
84 
88 
89 template <typename _Base, typename _Storage>
90 class WeighedDistanceServant : public DistanceServant<_Base, _Storage> {
92 
93  public:
95 
97  : Parent(server, s) {}
98 
100 
101  floatSeq* getWeights() { return vectorToFloatSeq(getT()->weights()); }
102 
103  void setWeights(const floatSeq& weights) {
104  try {
105  return getT()->weights(floatSeqToVector(weights));
106  } catch (const std::exception& e) {
107  throw Error(e.what());
108  }
109  }
110 };
111 
115 } // namespace core_idl
116 } // end of namespace corbaServer.
117 } // end of namespace hpp.
118 
119 #endif // HPP_CORE_IDL_DISTANCES_HH
Definition: servant-base.hh:120
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:74
CORBA::Double value(const floatSeq &q1, const floatSeq &q2)
Definition: distances.hh:78
Implement CORBA interface ``Obstacle&#39;&#39;.
Definition: client.hh:46
#define SERVANT_BASE_TYPEDEFS(idlObj, hppObj)
Definition: servant-base.hh:73
ptr_t element
Definition: servant-base.hh:222
WeighedDistanceServant(Server *server, const Storage &s)
Definition: distances.hh:96
void setWeights(const floatSeq &weights)
Definition: distances.hh:103
vector_t floatSeqToVector(const floatSeq &dofArray, const size_type expectedSize=-1)
Definition: distances.idl:19
Corba exception travelling through the Corba channel.
Definition: common.idl:26
Definition: servant-base.hh:218
DistanceStorage(const core::DevicePtr_t &_r, const ptr_t &_d)
Definition: distances.hh:59
DistanceServant< POA_hpp::core_idl::Distance, DistanceStorage< core::Distance > > Distance
Definition: distances.hh:87
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:66
virtual ~DistanceServant()
Definition: distances.hh:76
To define and solve a path planning problem.
Definition: distances.idl:26
DistanceStorage< T > cast() const
Definition: distances.hh:63
DistanceServant(Server *server, const Storage &s)
Definition: distances.hh:74
weak_ptr< D > ptr_t
Definition: servant-base.hh:220
Implementation of Hpp module Corba server.
Definition: server.hh:77
DistanceServant< Base, Storage > Parent
Definition: distances.hh:94
floatSeq * getWeights()
Definition: distances.hh:101
~WeighedDistanceServant()
Definition: distances.hh:99
core::DevicePtr_t r
Definition: distances.hh:58
floatSeq * vectorToFloatSeq(core::vectorIn_t input)
weak_ptr< T > ptr_t
Definition: servant-base.hh:220
_Storage Storage
Definition: servant-base.hh:122
Definition: common-idl.hh:689
Configuration_t floatSeqToConfig(const DevicePtr_t &robot, const floatSeq &dofArray, bool throwIfNotNormalized)
AbstractStorage< D, core::Distance > parent_t
Definition: distances.hh:54
WeighedDistanceServant< POA_hpp::core_idl::WeighedDistance, DistanceStorage< core::WeighedDistance > > WeighedDistance
Definition: distances.hh:114