ScopedState.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Rice University
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Rice University nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_BASE_SCOPED_STATE_
38 #define OMPL_BASE_SCOPED_STATE_
39 
40 #include "ompl/base/SpaceInformation.h"
41 #include <boost/concept_check.hpp>
42 #include <iostream>
43 #include <utility>
44 
45 namespace ompl
46 {
47  namespace base
48  {
55  template <class T = StateSpace>
56  class ScopedState
57  {
59  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
60 
62  BOOST_CONCEPT_ASSERT((boost::Convertible<typename T::StateType *, State *>));
63 
64  public:
66  using StateType = typename T::StateType;
67 
71  explicit ScopedState(const SpaceInformationPtr &si) : space_(si->getStateSpace())
72  {
73  State *s = space_->allocState();
74 
75  // ideally, this should be a dynamic_cast and we
76  // should throw an exception in case of
77  // failure. However, RTTI may not be available across
78  // shared library boundaries, so we do not use it
79  state_ = static_cast<StateType *>(s);
80  }
81 
84  explicit ScopedState(StateSpacePtr space) : space_(std::move(space))
85  {
86  State *s = space_->allocState();
87 
88  // ideally, this should be a dynamic_cast and we
89  // should throw an exception in case of
90  // failure. However, RTTI may not be available across
91  // shared library boundaries, so we do not use it
92  state_ = static_cast<StateType *>(s);
93  }
94 
96  ScopedState(const ScopedState<T> &other) : space_(other.getSpace())
97  {
98  State *s = space_->allocState();
99  state_ = static_cast<StateType *>(s);
100  space_->copyState(s, static_cast<const State *>(other.get()));
101  }
102 
104  template <class O>
105  ScopedState(const ScopedState<O> &other) : space_(other.getSpace())
106  {
107  BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
108  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
109 
110  // ideally, we should use a dynamic_cast and throw an
111  // exception in case other.get() does not cast to
112  // const StateType*. However, RTTI may not be
113  // available across shared library boundaries, so we
114  // do not use it
115 
116  State *s = space_->allocState();
117  state_ = static_cast<StateType *>(s);
118  space_->copyState(s, static_cast<const State *>(other.get()));
119  }
120 
123  ScopedState(StateSpacePtr space, const State *state) : space_(std::move(space))
124  {
125  State *s = space_->allocState();
126  space_->copyState(s, state);
127 
128  // ideally, this should be a dynamic_cast and we
129  // should throw an exception in case of
130  // failure. However, RTTI may not be available across
131  // shared library boundaries, so we do not use it
132  state_ = static_cast<StateType *>(s);
133  }
134 
136  ~ScopedState()
137  {
138  space_->freeState(state_);
139  }
140 
142  const StateSpacePtr &getSpace() const
143  {
144  return space_;
145  }
146 
148  ScopedState<T> &operator=(const ScopedState<T> &other)
149  {
150  if (&other != this)
151  {
152  space_->freeState(state_);
153  space_ = other.getSpace();
154 
155  State *s = space_->allocState();
156  state_ = static_cast<StateType *>(s);
157  space_->copyState(s, static_cast<const State *>(other.get()));
158  }
159  return *this;
160  }
161 
163  ScopedState<T> &operator=(const State *other)
164  {
165  if (other != static_cast<State *>(state_))
166  {
167  // ideally, we should use a dynamic_cast and throw an
168  // exception in case other does not cast to
169  // const StateType*. However, RTTI may not be
170  // available across shared library boundaries, so we
171  // do not use it
172 
173  space_->copyState(static_cast<State *>(state_), other);
174  }
175  return *this;
176  }
177 
179  ScopedState<T> &operator=(const State &other)
180  {
181  if (&other != static_cast<State *>(state_))
182  {
183  // ideally, we should use a dynamic_cast and throw an
184  // exception in case &other does not cast to
185  // const StateType*. However, RTTI may not be
186  // available across shared library boundaries, so we
187  // do not use it
188 
189  space_->copyState(static_cast<State *>(state_), &other);
190  }
191  return *this;
192  }
193 
195  template <class O>
197  {
198  BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
199  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
200 
201  // ideally, we should use a dynamic_cast and throw an
202  // exception in case other.get() does not cast to
203  // const StateType*. However, RTTI may not be
204  // available across shared library boundaries, so we
205  // do not use it
206 
207  if (reinterpret_cast<const void *>(&other) != reinterpret_cast<const void *>(this))
208  {
209  space_->freeState(state_);
210  space_ = other.getSpace();
211 
212  State *s = space_->allocState();
213  state_ = static_cast<StateType *>(s);
214  space_->copyState(s, static_cast<const State *>(other.get()));
215  }
216  return *this;
217  }
218 
220  ScopedState<T> &operator=(const std::vector<double> &reals)
221  {
222  for (unsigned int i = 0; i < reals.size(); ++i)
223  if (double *va = space_->getValueAddressAtIndex(state_, i))
224  *va = reals[i];
225  else
226  break;
227  return *this;
228  }
229 
231  ScopedState<T> &operator=(const double value)
232  {
233  unsigned int index = 0;
234  while (double *va = space_->getValueAddressAtIndex(state_, index++))
235  *va = value;
236  return *this;
237  }
238 
240  template <class O>
241  bool operator==(const ScopedState<O> &other) const
242  {
243  BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
244  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
245 
246  // ideally, we should use a dynamic_cast and throw an
247  // exception in case other.get() does not cast to
248  // const StateType*. However, RTTI may not be
249  // available across shared library boundaries, so we
250  // do not use it
251 
252  return space_->equalStates(static_cast<const State *>(state_), static_cast<const State *>(other.get()));
253  }
254 
256  template <class O>
257  bool operator!=(const ScopedState<O> &other) const
258  {
259  return !(*this == other);
260  }
261 
267  const ScopedState<> operator[](const StateSpacePtr &s) const;
268 
270  double &operator[](const unsigned int index)
271  {
272  double *val = space_->getValueAddressAtIndex(state_, index);
273  if (val == nullptr)
274  throw Exception("Index out of bounds");
275  return *val;
276  }
277 
279  double operator[](const unsigned int index) const
280  {
281  const double *val = space_->getValueAddressAtIndex(state_, index);
282  if (val == nullptr)
283  throw Exception("Index out of bounds");
284  return *val;
285  }
286 
288  double &operator[](const std::string &name)
289  {
290  const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
291  auto it = vm.find(name);
292  if (it != vm.end())
293  {
294  double *val = space_->getValueAddressAtLocation(state_, it->second);
295  if (val != nullptr)
296  return *val;
297  }
298  throw Exception("Name '" + name + "' not known");
299  }
300 
302  double operator[](const std::string &name) const
303  {
304  const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
305  auto it = vm.find(name);
306  if (it != vm.end())
307  {
308  const double *val = space_->getValueAddressAtLocation(state_, it->second);
309  if (val != nullptr)
310  return *val;
311  }
312  throw Exception("Name '" + name + "' not known");
313  }
314 
316  template <class O>
317  double distance(const ScopedState<O> &other) const
318  {
319  BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
320  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
321  return distance(other.get());
322  }
323 
325  double distance(const State *state) const
326  {
327  return space_->distance(static_cast<const State *>(state_), state);
328  }
329 
331  void random()
332  {
333  if (!sampler_)
334  sampler_ = space_->allocStateSampler();
335  sampler_->sampleUniform(state_);
336  }
337 
339  void enforceBounds()
340  {
341  space_->enforceBounds(state_);
342  }
343 
345  bool satisfiesBounds() const
346  {
347  return space_->satisfiesBounds(state_);
348  }
349 
353  std::vector<double> reals() const
354  {
355  std::vector<double> r;
356  unsigned int index = 0;
357  while (double *va = space_->getValueAddressAtIndex(state_, index++))
358  r.push_back(*va);
359  return r;
360  }
361 
363  void print(std::ostream &out = std::cout) const
364  {
365  space_->printState(state_, out);
366  }
367 
370  {
371  return *state_;
372  }
373 
375  const StateType &operator*() const
376  {
377  return *state_;
378  }
379 
382  {
383  return state_;
384  }
385 
387  const StateType *operator->() const
388  {
389  return state_;
390  }
391 
393  StateType *get()
394  {
395  return state_;
396  }
397 
399  const StateType *get() const
400  {
401  return state_;
402  }
403 
405  StateType *operator()() const
406  {
407  return state_;
408  }
409 
410  private:
411  StateSpacePtr space_;
412  StateSamplerPtr sampler_;
413  StateType *state_;
414  };
415 
491  template <class T>
492  inline std::ostream &operator<<(std::ostream &out, const ScopedState<T> &state)
493  {
494  state.print(out);
495  return out;
496  }
497 
506  template <class T, class Y>
507  inline ScopedState<T> &operator<<(ScopedState<T> &to, const ScopedState<Y> &from)
508  {
509  copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
510  return to;
511  }
512 
521  template <class T, class Y>
522  inline const ScopedState<T> &operator>>(const ScopedState<T> &from, ScopedState<Y> &to)
523  {
524  copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
525  return from;
526  }
527 
532  template <class T, class Y>
533  inline const ScopedState<> operator^(const ScopedState<T> &a, const ScopedState<Y> &b)
534  {
535  ScopedState<> r(a.getSpace() + b.getSpace());
536  return r << a << b;
537  }
538 
541  template <class T>
542  const ScopedState<> ScopedState<T>::operator[](const StateSpacePtr &s) const
543  {
544  ScopedState<> r(s);
545  return r << *this;
546  }
547 
549  using ScopedStatePtr = std::shared_ptr<ScopedState<>>;
550  } // namespace base
551 } // namespace ompl
552 
553 #endif
bool satisfiesBounds() const
Check if the maintained state satisfies bounds.
Definition: ScopedState.h:441
std::ostream & operator<<(std::ostream &stream, Cost c)
Output operator for Cost.
Definition: Cost.cpp:39
const StateSpacePtr & getSpace() const
Get the state space that the state corresponds to.
Definition: ScopedState.h:238
A shared pointer wrapper for ompl::base::SpaceInformation.
const ScopedState operator^(const ScopedState< T > &a, const ScopedState< Y > &b)
Given state a from state space A and state b from state space B, construct a state from state space A...
Definition: ScopedState.h:597
void print(std::ostream &out=std::cout) const
Print this state to a stream.
Definition: ScopedState.h:459
StateType * operator->()
Returns a pointer to the contained state.
Definition: ScopedState.h:477
StateType * operator()() const
Returns a pointer to the contained state (used for Python bindings)
Definition: ScopedState.h:501
AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
Copy data from source (state from space sourceS) to dest (state from space destS) on a component by c...
Definition of an abstract state.
Definition: State.h:113
void random()
Set this state to a random value (uniform)
Definition: ScopedState.h:427
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:489
ScopedState< T > & operator=(const ScopedState< T > &other)
Assignment operator.
Definition: ScopedState.h:244
std::vector< double > reals() const
Return the real values corresponding to this state. If a conversion is not possible,...
Definition: ScopedState.h:449
void enforceBounds()
Enforce the bounds on the maintained state.
Definition: ScopedState.h:435
const ScopedState operator[](const StateSpacePtr &s) const
Extract a state that corresponds to the components in state space s. Those components will have the s...
Definition: ScopedState.h:606
const ScopedState< T > & operator>>(const ScopedState< T > &from, ScopedState< Y > &to)
This is a fancy version of the assignment operator. It is a partial assignment, in some sense....
Definition: ScopedState.h:586
bool operator!=(const ScopedState< O > &other) const
Checks equality of two states.
Definition: ScopedState.h:353
StateType & operator*()
De-references to the contained state.
Definition: ScopedState.h:465
ScopedState(const SpaceInformationPtr &si)
Given the space that we are working with, allocate a state from the corresponding state space.
Definition: ScopedState.h:167
double distance(const ScopedState< O > &other) const
Compute the distance to another state.
Definition: ScopedState.h:413
bool operator==(const ScopedState< O > &other) const
Checks equality of two states.
Definition: ScopedState.h:337
~ScopedState()
Free the memory of the internally allocated state.
Definition: ScopedState.h:232
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
std::shared_ptr< ScopedState<> > ScopedStatePtr
Shared pointer to a ScopedState<>
Definition: ScopedState.h:613
Definition of a scoped state.
Definition: ScopedState.h:120
typename T::StateType StateType
The type of the contained state.
Definition: ScopedState.h:162
The exception type for ompl.
Definition: Exception.h:78
Main namespace. Contains everything in this library.
Definition: AppBase.h:21