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>
57  {
59  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
60 
62  BOOST_CONCEPT_ASSERT((boost::Convertible<typename T::StateType *, State *>));
63 
64  public:
66  typedef typename T::StateType 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>
106  : space_(other.getSpace())
107  {
108  BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
109  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
110 
111  // ideally, we should use a dynamic_cast and throw an
112  // exception in case other.get() does not cast to
113  // const StateType*. However, RTTI may not be
114  // available across shared library boundaries, so we
115  // do not use it
116 
117  State *s = space_->allocState();
118  state_ = static_cast<StateType *>(s);
119  space_->copyState(s, static_cast<const State *>(other.get()));
120  }
121 
124  ScopedState(StateSpacePtr space, const State *state) : space_(std::move(space))
125  {
126  State *s = space_->allocState();
127  space_->copyState(s, state);
128 
129  // ideally, this should be a dynamic_cast and we
130  // should throw an exception in case of
131  // failure. However, RTTI may not be available across
132  // shared library boundaries, so we do not use it
133  state_ = static_cast<StateType *>(s);
134  }
135 
138  {
139  space_->freeState(state_);
140  }
141 
143  const StateSpacePtr &getSpace() const
144  {
145  return space_;
146  }
147 
150  {
151  if (&other != this)
152  {
153  space_->freeState(state_);
154  space_ = other.getSpace();
155 
156  State *s = space_->allocState();
157  state_ = static_cast<StateType *>(s);
158  space_->copyState(s, static_cast<const State *>(other.get()));
159  }
160  return *this;
161  }
162 
165  {
166  if (other != static_cast<State *>(state_))
167  {
168  // ideally, we should use a dynamic_cast and throw an
169  // exception in case other does not cast to
170  // const StateType*. However, RTTI may not be
171  // available across shared library boundaries, so we
172  // do not use it
173 
174  space_->copyState(static_cast<State *>(state_), other);
175  }
176  return *this;
177  }
178 
181  {
182  if (&other != static_cast<State *>(state_))
183  {
184  // ideally, we should use a dynamic_cast and throw an
185  // exception in case &other does not cast to
186  // const StateType*. However, RTTI may not be
187  // available across shared library boundaries, so we
188  // do not use it
189 
190  space_->copyState(static_cast<State *>(state_), &other);
191  }
192  return *this;
193  }
194 
196  template <class O>
198  {
199  BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
200  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
201 
202  // ideally, we should use a dynamic_cast and throw an
203  // exception in case other.get() does not cast to
204  // const StateType*. However, RTTI may not be
205  // available across shared library boundaries, so we
206  // do not use it
207 
208  if (reinterpret_cast<const void *>(&other) != reinterpret_cast<const void *>(this))
209  {
210  space_->freeState(state_);
211  space_ = other.getSpace();
212 
213  State *s = space_->allocState();
214  state_ = static_cast<StateType *>(s);
215  space_->copyState(s, static_cast<const State *>(other.get()));
216  }
217  return *this;
218  }
219 
221  ScopedState<T> &operator=(const std::vector<double> &reals)
222  {
223  for (unsigned int i = 0; i < reals.size(); ++i)
224  if (double *va = space_->getValueAddressAtIndex(state_, i))
225  *va = reals[i];
226  else
227  break;
228  return *this;
229  }
230 
232  ScopedState<T> &operator=(const double value)
233  {
234  unsigned int index = 0;
235  while (double *va = space_->getValueAddressAtIndex(state_, index++))
236  *va = value;
237  return *this;
238  }
239 
241  template <class O>
242  bool operator==(const ScopedState<O> &other) const
243  {
244  BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
245  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
246 
247  // ideally, we should use a dynamic_cast and throw an
248  // exception in case other.get() does not cast to
249  // const StateType*. However, RTTI may not be
250  // available across shared library boundaries, so we
251  // do not use it
252 
253  return space_->equalStates(static_cast<const State *>(state_), static_cast<const State *>(other.get()));
254  }
255 
257  template <class O>
258  bool operator!=(const ScopedState<O> &other) const
259  {
260  return !(*this == other);
261  }
262 
268  const ScopedState<> operator[](const StateSpacePtr &s) const;
269 
271  double &operator[](const unsigned int index)
272  {
273  double *val = space_->getValueAddressAtIndex(state_, index);
274  if (!val)
275  throw Exception("Index out of bounds");
276  return *val;
277  }
278 
280  double operator[](const unsigned int index) const
281  {
282  const double *val = space_->getValueAddressAtIndex(state_, index);
283  if (!val)
284  throw Exception("Index out of bounds");
285  return *val;
286  }
287 
289  double &operator[](const std::string &name)
290  {
291  const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
292  auto it = vm.find(name);
293  if (it != vm.end())
294  {
295  double *val = space_->getValueAddressAtLocation(state_, it->second);
296  if (val)
297  return *val;
298  }
299  throw Exception("Name '" + name + "' not known");
300  }
301 
303  double operator[](const std::string &name) const
304  {
305  const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
306  auto it = vm.find(name);
307  if (it != vm.end())
308  {
309  const double *val = space_->getValueAddressAtLocation(state_, it->second);
310  if (val)
311  return *val;
312  }
313  throw Exception("Name '" + name + "' not known");
314  }
315 
317  template <class O>
318  double distance(const ScopedState<O> &other) const
319  {
320  BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
321  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
322  return distance(other.get());
323  }
324 
326  double distance(const State *state) const
327  {
328  return space_->distance(static_cast<const State *>(state_), state);
329  }
330 
332  void random()
333  {
334  if (!sampler_)
335  sampler_ = space_->allocStateSampler();
336  sampler_->sampleUniform(state_);
337  }
338 
341  {
342  space_->enforceBounds(state_);
343  }
344 
346  bool satisfiesBounds() const
347  {
348  return space_->satisfiesBounds(state_);
349  }
350 
354  std::vector<double> reals() const
355  {
356  std::vector<double> r;
357  unsigned int index = 0;
358  while (double *va = space_->getValueAddressAtIndex(state_, index++))
359  r.push_back(*va);
360  return r;
361  }
362 
364  void print(std::ostream &out = std::cout) const
365  {
366  space_->printState(state_, out);
367  }
368 
370  StateType &operator*()
371  {
372  return *state_;
373  }
374 
376  const StateType &operator*() const
377  {
378  return *state_;
379  }
380 
382  StateType *operator->()
383  {
384  return state_;
385  }
386 
388  const StateType *operator->() const
389  {
390  return state_;
391  }
392 
394  StateType *get()
395  {
396  return state_;
397  }
398 
400  const StateType *get() const
401  {
402  return state_;
403  }
404 
406  StateType *operator()() const
407  {
408  return state_;
409  }
410 
411  private:
412  StateSpacePtr space_;
413  StateSamplerPtr sampler_;
414  StateType *state_;
415  };
416 
492  template <class T>
493  inline std::ostream &operator<<(std::ostream &out, const ScopedState<T> &state)
494  {
495  state.print(out);
496  return out;
497  }
498 
507  template <class T, class Y>
508  inline ScopedState<T> &operator<<(ScopedState<T> &to, const ScopedState<Y> &from)
509  {
510  copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
511  return to;
512  }
513 
522  template <class T, class Y>
523  inline const ScopedState<T> &operator>>(const ScopedState<T> &from, ScopedState<Y> &to)
524  {
525  copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
526  return from;
527  }
528 
533  template <class T, class Y>
534  inline const ScopedState<> operator^(const ScopedState<T> &a, const ScopedState<Y> &b)
535  {
536  ScopedState<> r(a.getSpace() + b.getSpace());
537  return r << a << b;
538  }
539 
542  template <class T>
544  {
545  ScopedState<> r(s);
546  return r << *this;
547  }
548 
550  typedef std::shared_ptr<ScopedState<>> ScopedStatePtr;
551  }
552 }
553 
554 #endif
const StateSpacePtr & getSpace() const
Get the state space that the state corresponds to.
Definition: ScopedState.h:143
double operator[](const std::string &name) const
Access a double value from this state contains using its name.
Definition: ScopedState.h:303
bool operator==(const ScopedState< O > &other) const
Checks equality of two states.
Definition: ScopedState.h:242
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:534
ScopedState(StateSpacePtr space)
Given the state space that we are working with, allocate a state.
Definition: ScopedState.h:84
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:543
ScopedState< T > & operator=(const ScopedState< O > &other)
Assignment operator that allows conversion of states.
Definition: ScopedState.h:197
Definition of a scoped state.
Definition: ScopedState.h:56
A shared pointer wrapper for ompl::base::StateSpace.
const StateType & operator*() const
De-references to the contained state.
Definition: ScopedState.h:376
double distance(const ScopedState< O > &other) const
Compute the distance to another state.
Definition: ScopedState.h:318
ScopedState< T > & operator=(const double value)
Partial assignment operator. Only sets the double values of the state to a fixed value.
Definition: ScopedState.h:232
A shared pointer wrapper for ompl::base::StateSampler.
double operator[](const unsigned int index) const
Access the indexth double value this state contains.
Definition: ScopedState.h:280
double & operator[](const unsigned int index)
Access the indexth double value this state contains.
Definition: ScopedState.h:271
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...
double & operator[](const std::string &name)
Access a double value from this state contains using its name.
Definition: ScopedState.h:289
STL namespace.
bool operator!=(const ScopedState< O > &other) const
Checks equality of two states.
Definition: ScopedState.h:258
ScopedState(const SpaceInformationPtr &si)
Given the space that we are working with, allocate a state from the corresponding state space...
Definition: ScopedState.h:71
ScopedState< T > & operator=(const State &other)
Assignment operator.
Definition: ScopedState.h:180
ScopedState< T > & operator=(const ScopedState< T > &other)
Assignment operator.
Definition: ScopedState.h:149
double distance(const State *state) const
Compute the distance to another state.
Definition: ScopedState.h:326
StateType & operator*()
De-references to the contained state.
Definition: ScopedState.h:370
void enforceBounds()
Enforce the bounds on the maintained state.
Definition: ScopedState.h:340
ScopedState(StateSpacePtr space, const State *state)
Given the state space that we are working with, allocate a state and fill that state with a given val...
Definition: ScopedState.h:124
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:394
void random()
Set this state to a random value (uniform)
Definition: ScopedState.h:332
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:523
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
ScopedState(const ScopedState< T > &other)
Copy constructor.
Definition: ScopedState.h:96
StateType * operator->()
Returns a pointer to the contained state.
Definition: ScopedState.h:382
bool satisfiesBounds() const
Check if the maintained state satisfies bounds.
Definition: ScopedState.h:346
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
T::StateType StateType
The type of the contained state.
Definition: ScopedState.h:66
The exception type for ompl.
Definition: Exception.h:46
ScopedState< T > & operator=(const State *other)
Assignment operator.
Definition: ScopedState.h:164
std::vector< double > reals() const
Return the real values corresponding to this state. If a conversion is not possible, an exception is thrown.
Definition: ScopedState.h:354
StateType * operator()() const
Returns a pointer to the contained state (used for Python bindings)
Definition: ScopedState.h:406
void print(std::ostream &out=std::cout) const
Print this state to a stream.
Definition: ScopedState.h:364
~ScopedState()
Free the memory of the internally allocated state.
Definition: ScopedState.h:137
const StateType * operator->() const
Returns a pointer to the contained state.
Definition: ScopedState.h:388
std::shared_ptr< ScopedState<> > ScopedStatePtr
Shared pointer to a ScopedState<>
Definition: ScopedState.h:550
ScopedState(const ScopedState< O > &other)
Copy constructor that allows instantiation from states of other type.
Definition: ScopedState.h:105
ScopedState< T > & operator=(const std::vector< double > &reals)
Partial assignment operator. Only sets the double values of the state to specified real values...
Definition: ScopedState.h:221