Loading...
Searching...
No Matches
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
45namespace 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 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
137 {
138 space_->freeState(state_);
139 }
140
142 const StateSpacePtr &getSpace() const
143 {
144 return space_;
145 }
146
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
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
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
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
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
394 {
395 return state_;
396 }
397
399 const StateType *get() const
400 {
401 return state_;
402 }
403
406 {
407 return state_;
408 }
409
410 private:
411 StateSpacePtr space_;
412 StateSamplerPtr sampler_;
413 StateType *state_;
414 };
415
489
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>
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
540
541 template <class T>
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
The exception type for ompl.
Definition Exception.h:47
Definition of a scoped state.
Definition ScopedState.h:57
ScopedState< T > & operator=(const double value)
Partial assignment operator. Only sets the double values of the state to a fixed value.
const StateSpacePtr & getSpace() const
void print(std::ostream &out=std::cout) const
Print this state to a stream.
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...
~ScopedState()
Free the memory of the internally allocated state.
StateType & operator*()
De-references to the contained state.
ScopedState< T > & operator=(const State *other)
Assignment operator.
typename SE3StateSpace::StateType StateType
Definition ScopedState.h:66
bool satisfiesBounds() const
Check if the maintained state satisfies bounds.
StateType * operator->()
Returns a pointer to the contained state.
void random()
Set this state to a random value (uniform)
double operator[](const unsigned int index) const
Access the indexth double value this state contains.
double operator[](const std::string &name) const
Access a double value from this state contains using its name.
void enforceBounds()
Enforce the bounds on the maintained state.
bool operator==(const ScopedState< O > &other) const
Checks equality of two states.
double & operator[](const std::string &name)
Access a double value from this state contains using its name.
ScopedState< T > & operator=(const State &other)
Assignment operator.
ScopedState(StateSpacePtr space)
Given the state space that we are working with, allocate a state.
Definition ScopedState.h:84
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(const ScopedState< O > &other)
Copy constructor that allows instantiation from states of other type.
double distance(const ScopedState< O > &other) const
Compute the distance to another state.
ScopedState< T > & operator=(const ScopedState< O > &other)
Assignment operator that allows conversion of states.
std::vector< double > reals() const
StateType * operator()() const
Returns a pointer to the contained state (used for Python bindings)
bool operator!=(const ScopedState< O > &other) const
Checks equality of two states.
ScopedState(const ScopedState< T > &other)
Copy constructor.
Definition ScopedState.h:96
const StateType & operator*() const
De-references to the contained state.
double & operator[](const unsigned int index)
Access the indexth double value this state contains.
const StateType * operator->() const
Returns a pointer to the contained state.
ScopedState< T > & operator=(const std::vector< double > &reals)
Partial assignment operator. Only sets the double values of the state to specified real values.
const StateType * get() const
Returns a pointer to the contained state.
double distance(const State *state) const
Compute the distance to another state.
ScopedState< T > & operator=(const ScopedState< T > &other)
Assignment operator.
StateType * get()
Returns a pointer to the contained state.
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...
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
Definition of an abstract state.
Definition State.h:50
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...
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...
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....
This namespace contains sampling based planning routines shared by both planning under geometric cons...
std::ostream & operator<<(std::ostream &stream, Cost c)
Output operator for Cost.
Definition Cost.cpp:39
std::shared_ptr< ScopedState<> > ScopedStatePtr
Shared pointer to a ScopedState<>
Main namespace. Contains everything in this library.
STL namespace.