All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 
44 namespace ompl
45 {
46  namespace base
47  {
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:
65 
67  typedef typename T::StateType StateType;
68 
72  explicit
73  ScopedState(const SpaceInformationPtr &si) : space_(si->getStateSpace())
74  {
75  State *s = space_->allocState();
76 
77  // ideally, this should be a dynamic_cast and we
78  // should throw an exception in case of
79  // failure. However, RTTI may not be available across
80  // shared library boundaries, so we do not use it
81  state_ = static_cast<StateType*>(s);
82  }
83 
86  explicit
87  ScopedState(const StateSpacePtr &space) : space_(space)
88  {
89  State *s = space_->allocState();
90 
91  // ideally, this should be a dynamic_cast and we
92  // should throw an exception in case of
93  // failure. However, RTTI may not be available across
94  // shared library boundaries, so we do not use it
95  state_ = static_cast<StateType*>(s);
96  }
97 
99  ScopedState(const ScopedState<T> &other) : space_(other.getSpace())
100  {
101  State *s = space_->allocState();
102  state_ = static_cast<StateType*>(s);
103  space_->copyState(s, static_cast<const State*>(other.get()));
104  }
105 
107  template<class O>
108  ScopedState(const ScopedState<O> &other) : space_(other.getSpace())
109  {
110  BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
111  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
112 
113  // ideally, we should use a dynamic_cast and throw an
114  // exception in case other.get() does not cast to
115  // const StateType*. However, RTTI may not be
116  // available across shared library boundaries, so we
117  // do not use it
118 
119  State *s = space_->allocState();
120  state_ = static_cast<StateType*>(s);
121  space_->copyState(s, static_cast<const State*>(other.get()));
122  }
123 
126  ScopedState(const StateSpacePtr &space, const State *state) : space_(space)
127  {
128  State *s = space_->allocState();
129  space_->copyState(s, state);
130 
131  // ideally, this should be a dynamic_cast and we
132  // should throw an exception in case of
133  // failure. However, RTTI may not be available across
134  // shared library boundaries, so we do not use it
135  state_ = static_cast<StateType*>(s);
136  }
137 
140  {
141  space_->freeState(state_);
142  }
143 
145  const StateSpacePtr& getSpace(void) const
146  {
147  return space_;
148  }
149 
152  {
153  if (&other != this)
154  {
155  space_->freeState(state_);
156  space_ = other.getSpace();
157 
158  State *s = space_->allocState();
159  state_ = static_cast<StateType*>(s);
160  space_->copyState(s, static_cast<const State*>(other.get()));
161  }
162  return *this;
163  }
164 
167  {
168  if (other != static_cast<State*>(state_))
169  {
170  // ideally, we should use a dynamic_cast and throw an
171  // exception in case other does not cast to
172  // const StateType*. However, RTTI may not be
173  // available across shared library boundaries, so we
174  // do not use it
175 
176  space_->copyState(static_cast<State*>(state_), other);
177  }
178  return *this;
179  }
180 
183  {
184  if (&other != static_cast<State*>(state_))
185  {
186  // ideally, we should use a dynamic_cast and throw an
187  // exception in case &other does not cast to
188  // const StateType*. However, RTTI may not be
189  // available across shared library boundaries, so we
190  // do not use it
191 
192  space_->copyState(static_cast<State*>(state_), &other);
193  }
194  return *this;
195  }
196 
198  template<class O>
200  {
201  BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
202  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
203 
204  // ideally, we should use a dynamic_cast and throw an
205  // exception in case other.get() does not cast to
206  // const StateType*. However, RTTI may not be
207  // available across shared library boundaries, so we
208  // do not use it
209 
210  if (reinterpret_cast<const void*>(&other) != reinterpret_cast<const void*>(this))
211  {
212  space_->freeState(state_);
213  space_ = other.getSpace();
214 
215  State *s = space_->allocState();
216  state_ = static_cast<StateType*>(s);
217  space_->copyState(s, static_cast<const State*>(other.get()));
218  }
219  return *this;
220  }
221 
223  ScopedState<T>& operator=(const std::vector<double> &reals)
224  {
225  for (unsigned int i = 0 ; i < reals.size() ; ++i)
226  if (double *va = space_->getValueAddressAtIndex(state_, i))
227  *va = reals[i];
228  else
229  break;
230  return *this;
231  }
232 
234  ScopedState<T>& operator=(const double value)
235  {
236  unsigned int index = 0;
237  while (double *va = space_->getValueAddressAtIndex(state_, index++))
238  *va = value;
239  return *this;
240  }
241 
243  template<class O>
244  bool operator==(const ScopedState<O> &other) const
245  {
246  BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
247  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
248 
249  // ideally, we should use a dynamic_cast and throw an
250  // exception in case other.get() does not cast to
251  // const StateType*. However, RTTI may not be
252  // available across shared library boundaries, so we
253  // do not use it
254 
255  return space_->equalStates(static_cast<const State*>(state_), static_cast<const State*>(other.get()));
256  }
257 
259  template<class O>
260  bool operator!=(const ScopedState<O> &other) const
261  {
262  return !(*this == other);
263  }
264 
270  const ScopedState<> operator[](const StateSpacePtr &s) const;
271 
273  double& operator[](const unsigned int index)
274  {
275  double *val = space_->getValueAddressAtIndex(state_, index);
276  if (!val)
277  throw Exception("Index out of bounds");
278  return *val;
279  }
280 
282  double operator[](const unsigned int index) const
283  {
284  const double *val = space_->getValueAddressAtIndex(state_, index);
285  if (!val)
286  throw Exception("Index out of bounds");
287  return *val;
288  }
289 
291  double& operator[](const std::string &name)
292  {
293  const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
294  std::map<std::string, StateSpace::ValueLocation>::const_iterator it = vm.find(name);
295  if (it != vm.end())
296  {
297  double *val = space_->getValueAddressAtLocation(state_, it->second);
298  if (val)
299  return *val;
300  }
301  throw Exception("Name '" + name + "' not known");
302  }
303 
305  double operator[](const std::string &name) const
306  {
307  const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
308  std::map<std::string, StateSpace::ValueLocation>::const_iterator it = vm.find(name);
309  if (it != vm.end())
310  {
311  const double *val = space_->getValueAddressAtLocation(state_, it->second);
312  if (val)
313  return *val;
314  }
315  throw Exception("Name '" + name + "' not known");
316  }
317 
319  template<class O>
320  double distance(const ScopedState<O> &other) const
321  {
322  BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
323  BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
324  return distance(other.get());
325  }
326 
328  double distance(const State *state) const
329  {
330  return space_->distance(static_cast<const State*>(state_), state);
331  }
332 
334  void random(void)
335  {
336  if (!sampler_)
337  sampler_ = space_->allocStateSampler();
338  sampler_->sampleUniform(state_);
339  }
340 
342  void enforceBounds(void)
343  {
344  space_->enforceBounds(state_);
345  }
346 
348  bool satisfiesBounds(void) const
349  {
350  return space_->satisfiesBounds(state_);
351  }
352 
356  std::vector<double> reals(void) const
357  {
358  std::vector<double> r;
359  unsigned int index = 0;
360  while (double *va = space_->getValueAddressAtIndex(state_, index++))
361  r.push_back(*va);
362  return r;
363  }
364 
366  void print(std::ostream &out = std::cout) const
367  {
368  space_->printState(state_, out);
369  }
370 
373  {
374  return *state_;
375  }
376 
378  const StateType& operator*(void) const
379  {
380  return *state_;
381  }
382 
385  {
386  return state_;
387  }
388 
390  const StateType* operator->(void) const
391  {
392  return state_;
393  }
394 
396  StateType* get(void)
397  {
398  return state_;
399  }
400 
402  const StateType* get(void) const
403  {
404  return state_;
405  }
406 
408  StateType* operator()(void) const
409  {
410  return state_;
411  }
412 
413  private:
414 
415  StateSpacePtr space_;
416  StateSamplerPtr sampler_;
417  StateType *state_;
418  };
419 
495  template<class T>
496  inline
497  std::ostream& operator<<(std::ostream &out, const ScopedState<T> &state)
498  {
499  state.print(out);
500  return out;
501  }
502 
511  template<class T, class Y>
512  inline
513  ScopedState<T>& operator<<(ScopedState<T> &to, const ScopedState<Y> &from)
514  {
515  copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
516  return to;
517  }
518 
527  template<class T, class Y>
528  inline
530  {
531  copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
532  return from;
533  }
534 
539  template<class T, class Y>
540  inline
542  {
543  ScopedState<> r(a.getSpace() + b.getSpace());
544  return r << a << b;
545  }
546 
549  template<class T>
551  {
552  ScopedState<> r(s);
553  return r << *this;
554  }
555 
557  typedef boost::shared_ptr< ScopedState<> > ScopedStatePtr;
558  }
559 }
560 
561 #endif
double distance(const ScopedState< O > &other) const
Compute the distance to another state.
Definition: ScopedState.h:320
void print(std::ostream &out=std::cout) const
Print this state to a stream.
Definition: ScopedState.h:366
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:541
StateType * operator()(void) const
Returns a pointer to the contained state (used for Python bindings)
Definition: ScopedState.h:408
ScopedState(const StateSpacePtr &space)
Given the state space that we are working with, allocate a state.
Definition: ScopedState.h:87
ScopedState< T > & operator=(const ScopedState< O > &other)
Assignment operator that allows conversion of states.
Definition: ScopedState.h:199
Definition of a scoped state.
Definition: ScopedState.h:56
A boost shared pointer wrapper for ompl::base::StateSpace.
ScopedState< T > & operator=(const double value)
Partial assignment operator. Only sets the double values of the state to a fixed value.
Definition: ScopedState.h:234
A boost shared pointer wrapper for ompl::base::StateSampler.
boost::shared_ptr< ScopedState<> > ScopedStatePtr
Shared pointer to a ScopedState<>
Definition: ScopedState.h:557
double & operator[](const unsigned int index)
Access the indexth double value this state contains.
Definition: ScopedState.h:273
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:291
double operator[](const std::string &name) const
Access a double value from this state contains using its name.
Definition: ScopedState.h:305
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:550
ScopedState(const SpaceInformationPtr &si)
Given the space that we are working with, allocate a state from the corresponding state space...
Definition: ScopedState.h:73
ScopedState< T > & operator=(const State &other)
Assignment operator.
Definition: ScopedState.h:182
ScopedState< T > & operator=(const ScopedState< T > &other)
Assignment operator.
Definition: ScopedState.h:151
double operator[](const unsigned int index) const
Access the indexth double value this state contains.
Definition: ScopedState.h:282
bool satisfiesBounds(void) const
Check if the maintained state satisfies bounds.
Definition: ScopedState.h:348
bool operator==(const ScopedState< O > &other) const
Checks equality of two states.
Definition: ScopedState.h:244
StateType * operator->(void)
Returns a pointer to the contained state.
Definition: ScopedState.h:384
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:529
ScopedState(const ScopedState< T > &other)
Copy constructor.
Definition: ScopedState.h:99
void random(void)
Set this state to a random value (uniform)
Definition: ScopedState.h:334
StateType * get(void)
Returns a pointer to the contained state.
Definition: ScopedState.h:396
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
double distance(const State *state) const
Compute the distance to another state.
Definition: ScopedState.h:328
T::StateType StateType
The type of the contained state.
Definition: ScopedState.h:67
The exception type for ompl.
Definition: Exception.h:47
bool operator!=(const ScopedState< O > &other) const
Checks equality of two states.
Definition: ScopedState.h:260
const StateType * operator->(void) const
Returns a pointer to the contained state.
Definition: ScopedState.h:390
StateType & operator*(void)
De-references to the contained state.
Definition: ScopedState.h:372
ScopedState< T > & operator=(const State *other)
Assignment operator.
Definition: ScopedState.h:166
~ScopedState(void)
Free the memory of the internally allocated state.
Definition: ScopedState.h:139
const StateType & operator*(void) const
De-references to the contained state.
Definition: ScopedState.h:378
const StateSpacePtr & getSpace(void) const
Get the state space that the state corresponds to.
Definition: ScopedState.h:145
std::vector< double > reals(void) const
Return the real values corresponding to this state. If a conversion is not possible, an exception is thrown.
Definition: ScopedState.h:356
ScopedState(const ScopedState< O > &other)
Copy constructor that allows instantiation from states of other type.
Definition: ScopedState.h:108
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:223
ScopedState(const 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:126
void enforceBounds(void)
Enforce the bounds on the maintained state.
Definition: ScopedState.h:342