37 #ifndef OMPL_BASE_SCOPED_STATE_
38 #define OMPL_BASE_SCOPED_STATE_
40 #include "ompl/base/SpaceInformation.h"
41 #include <boost/concept_check.hpp>
55 template<
class T = StateSpace>
59 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
62 BOOST_CONCEPT_ASSERT((boost::Convertible<typename T::StateType*, State*>));
75 State *s = space_->allocState();
89 State *s = space_->allocState();
101 State *s = space_->allocState();
103 space_->copyState(s, static_cast<const State*>(other.
get()));
110 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
111 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
119 State *s = space_->allocState();
121 space_->copyState(s, static_cast<const State*>(other.
get()));
128 State *s = space_->allocState();
129 space_->copyState(s, state);
141 space_->freeState(state_);
155 space_->freeState(state_);
158 State *s = space_->allocState();
160 space_->copyState(s, static_cast<const State*>(other.
get()));
168 if (other != static_cast<State*>(state_))
176 space_->copyState(static_cast<State*>(state_), other);
184 if (&other != static_cast<State*>(state_))
192 space_->copyState(static_cast<State*>(state_), &other);
201 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
202 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
210 if (reinterpret_cast<const void*>(&other) != reinterpret_cast<const void*>(
this))
212 space_->freeState(state_);
215 State *s = space_->allocState();
217 space_->copyState(s, static_cast<const State*>(other.
get()));
225 for (
unsigned int i = 0 ; i < reals.size() ; ++i)
226 if (
double *va = space_->getValueAddressAtIndex(state_, i))
236 unsigned int index = 0;
237 while (
double *va = space_->getValueAddressAtIndex(state_, index++))
246 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
247 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
255 return space_->equalStates(static_cast<const State*>(state_), static_cast<const State*>(other.
get()));
262 return !(*
this == other);
275 double *val = space_->getValueAddressAtIndex(state_, index);
284 const double *val = space_->getValueAddressAtIndex(state_, index);
293 const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
294 std::map<std::string, StateSpace::ValueLocation>::const_iterator it = vm.find(name);
297 double *val = space_->getValueAddressAtLocation(state_, it->second);
301 throw Exception(
"Name '" + name +
"' not known");
307 const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
308 std::map<std::string, StateSpace::ValueLocation>::const_iterator it = vm.find(name);
311 const double *val = space_->getValueAddressAtLocation(state_, it->second);
315 throw Exception(
"Name '" + name +
"' not known");
322 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
323 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
330 return space_->distance(static_cast<const State*>(state_), state);
337 sampler_ = space_->allocStateSampler();
338 sampler_->sampleUniform(state_);
344 space_->enforceBounds(state_);
350 return space_->satisfiesBounds(state_);
356 std::vector<double>
reals(
void)
const
358 std::vector<double> r;
359 unsigned int index = 0;
360 while (
double *va = space_->getValueAddressAtIndex(state_, index++))
366 void print(std::ostream &out = std::cout)
const
368 space_->printState(state_, out);
497 std::ostream& operator<<(std::ostream &out, const ScopedState<T> &state)
511 template<
class T,
class Y>
515 copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
527 template<
class T,
class Y>
539 template<
class T,
class Y>
double distance(const ScopedState< O > &other) const
Compute the distance to another state.
void print(std::ostream &out=std::cout) const
Print this state to a stream.
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...
StateType * operator()(void) const
Returns a pointer to the contained state (used for Python bindings)
ScopedState(const StateSpacePtr &space)
Given the state space that we are working with, allocate a state.
ScopedState< T > & operator=(const ScopedState< O > &other)
Assignment operator that allows conversion of states.
Definition of a scoped state.
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.
A boost shared pointer wrapper for ompl::base::StateSampler.
boost::shared_ptr< ScopedState<> > ScopedStatePtr
Shared pointer to a ScopedState<>
double & operator[](const unsigned int index)
Access the indexth double value this state contains.
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.
double operator[](const std::string &name) const
Access a double value from this state contains using its name.
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(const SpaceInformationPtr &si)
Given the space that we are working with, allocate a state from the corresponding state space...
ScopedState< T > & operator=(const State &other)
Assignment operator.
ScopedState< T > & operator=(const ScopedState< T > &other)
Assignment operator.
double operator[](const unsigned int index) const
Access the indexth double value this state contains.
bool satisfiesBounds(void) const
Check if the maintained state satisfies bounds.
bool operator==(const ScopedState< O > &other) const
Checks equality of two states.
StateType * operator->(void)
Returns a pointer to the contained state.
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...
ScopedState(const ScopedState< T > &other)
Copy constructor.
void random(void)
Set this state to a random value (uniform)
StateType * get(void)
Returns a pointer to the contained state.
Definition of an abstract state.
double distance(const State *state) const
Compute the distance to another state.
T::StateType StateType
The type of the contained state.
The exception type for ompl.
bool operator!=(const ScopedState< O > &other) const
Checks equality of two states.
const StateType * operator->(void) const
Returns a pointer to the contained state.
StateType & operator*(void)
De-references to the contained state.
ScopedState< T > & operator=(const State *other)
Assignment operator.
~ScopedState(void)
Free the memory of the internally allocated state.
const StateType & operator*(void) const
De-references to the contained state.
const StateSpacePtr & getSpace(void) const
Get the state space that the state corresponds to.
std::vector< double > reals(void) const
Return the real values corresponding to this state. If a conversion is not possible, an exception is thrown.
ScopedState(const ScopedState< O > &other)
Copy constructor that allows instantiation from states of other type.
ScopedState< T > & operator=(const std::vector< double > &reals)
Partial assignment operator. Only sets the double values of the state to specified real values...
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...
void enforceBounds(void)
Enforce the bounds on the maintained state.