Program Listing for File ScopedState.h
↰ Return to documentation for file (src/ompl/base/ScopedState.h)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Rice University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Ioan Sucan */
#ifndef OMPL_BASE_SCOPED_STATE_
#define OMPL_BASE_SCOPED_STATE_
#include "ompl/base/SpaceInformation.h"
#include <boost/concept_check.hpp>
#include <iostream>
#include <utility>
namespace ompl
{
namespace base
{
template <class T = StateSpace>
class ScopedState
{
BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
BOOST_CONCEPT_ASSERT((boost::Convertible<typename T::StateType *, State *>));
public:
using StateType = typename T::StateType;
explicit ScopedState(const SpaceInformationPtr &si) : space_(si->getStateSpace())
{
State *s = space_->allocState();
// ideally, this should be a dynamic_cast and we
// should throw an exception in case of
// failure. However, RTTI may not be available across
// shared library boundaries, so we do not use it
state_ = static_cast<StateType *>(s);
}
explicit ScopedState(StateSpacePtr space) : space_(std::move(space))
{
State *s = space_->allocState();
// ideally, this should be a dynamic_cast and we
// should throw an exception in case of
// failure. However, RTTI may not be available across
// shared library boundaries, so we do not use it
state_ = static_cast<StateType *>(s);
}
ScopedState(const ScopedState<T> &other) : space_(other.getSpace())
{
State *s = space_->allocState();
state_ = static_cast<StateType *>(s);
space_->copyState(s, static_cast<const State *>(other.get()));
}
template <class O>
ScopedState(const ScopedState<O> &other) : space_(other.getSpace())
{
BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
// ideally, we should use a dynamic_cast and throw an
// exception in case other.get() does not cast to
// const StateType*. However, RTTI may not be
// available across shared library boundaries, so we
// do not use it
State *s = space_->allocState();
state_ = static_cast<StateType *>(s);
space_->copyState(s, static_cast<const State *>(other.get()));
}
ScopedState(StateSpacePtr space, const State *state) : space_(std::move(space))
{
State *s = space_->allocState();
space_->copyState(s, state);
// ideally, this should be a dynamic_cast and we
// should throw an exception in case of
// failure. However, RTTI may not be available across
// shared library boundaries, so we do not use it
state_ = static_cast<StateType *>(s);
}
~ScopedState()
{
space_->freeState(state_);
}
const StateSpacePtr &getSpace() const
{
return space_;
}
ScopedState<T> &operator=(const ScopedState<T> &other)
{
if (&other != this)
{
space_->freeState(state_);
space_ = other.getSpace();
State *s = space_->allocState();
state_ = static_cast<StateType *>(s);
space_->copyState(s, static_cast<const State *>(other.get()));
}
return *this;
}
ScopedState<T> &operator=(const State *other)
{
if (other != static_cast<State *>(state_))
{
// ideally, we should use a dynamic_cast and throw an
// exception in case other does not cast to
// const StateType*. However, RTTI may not be
// available across shared library boundaries, so we
// do not use it
space_->copyState(static_cast<State *>(state_), other);
}
return *this;
}
ScopedState<T> &operator=(const State &other)
{
if (&other != static_cast<State *>(state_))
{
// ideally, we should use a dynamic_cast and throw an
// exception in case &other does not cast to
// const StateType*. However, RTTI may not be
// available across shared library boundaries, so we
// do not use it
space_->copyState(static_cast<State *>(state_), &other);
}
return *this;
}
template <class O>
ScopedState<T> &operator=(const ScopedState<O> &other)
{
BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
// ideally, we should use a dynamic_cast and throw an
// exception in case other.get() does not cast to
// const StateType*. However, RTTI may not be
// available across shared library boundaries, so we
// do not use it
if (reinterpret_cast<const void *>(&other) != reinterpret_cast<const void *>(this))
{
space_->freeState(state_);
space_ = other.getSpace();
State *s = space_->allocState();
state_ = static_cast<StateType *>(s);
space_->copyState(s, static_cast<const State *>(other.get()));
}
return *this;
}
ScopedState<T> &operator=(const std::vector<double> &reals)
{
for (unsigned int i = 0; i < reals.size(); ++i)
if (double *va = space_->getValueAddressAtIndex(state_, i))
*va = reals[i];
else
break;
return *this;
}
ScopedState<T> &operator=(const double value)
{
unsigned int index = 0;
while (double *va = space_->getValueAddressAtIndex(state_, index++))
*va = value;
return *this;
}
template <class O>
bool operator==(const ScopedState<O> &other) const
{
BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
// ideally, we should use a dynamic_cast and throw an
// exception in case other.get() does not cast to
// const StateType*. However, RTTI may not be
// available across shared library boundaries, so we
// do not use it
return space_->equalStates(static_cast<const State *>(state_), static_cast<const State *>(other.get()));
}
template <class O>
bool operator!=(const ScopedState<O> &other) const
{
return !(*this == other);
}
const ScopedState<> operator[](const StateSpacePtr &s) const;
double &operator[](const unsigned int index)
{
double *val = space_->getValueAddressAtIndex(state_, index);
if (val == nullptr)
throw Exception("Index out of bounds");
return *val;
}
double operator[](const unsigned int index) const
{
const double *val = space_->getValueAddressAtIndex(state_, index);
if (val == nullptr)
throw Exception("Index out of bounds");
return *val;
}
double &operator[](const std::string &name)
{
const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
auto it = vm.find(name);
if (it != vm.end())
{
double *val = space_->getValueAddressAtLocation(state_, it->second);
if (val != nullptr)
return *val;
}
throw Exception("Name '" + name + "' not known");
}
double operator[](const std::string &name) const
{
const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
auto it = vm.find(name);
if (it != vm.end())
{
const double *val = space_->getValueAddressAtLocation(state_, it->second);
if (val != nullptr)
return *val;
}
throw Exception("Name '" + name + "' not known");
}
template <class O>
double distance(const ScopedState<O> &other) const
{
BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
return distance(other.get());
}
double distance(const State *state) const
{
return space_->distance(static_cast<const State *>(state_), state);
}
void random()
{
if (!sampler_)
sampler_ = space_->allocStateSampler();
sampler_->sampleUniform(state_);
}
void enforceBounds()
{
space_->enforceBounds(state_);
}
bool satisfiesBounds() const
{
return space_->satisfiesBounds(state_);
}
std::vector<double> reals() const
{
std::vector<double> r;
unsigned int index = 0;
while (double *va = space_->getValueAddressAtIndex(state_, index++))
r.push_back(*va);
return r;
}
void print(std::ostream &out = std::cout) const
{
space_->printState(state_, out);
}
StateType &operator*()
{
return *state_;
}
const StateType &operator*() const
{
return *state_;
}
StateType *operator->()
{
return state_;
}
const StateType *operator->() const
{
return state_;
}
StateType *get()
{
return state_;
}
const StateType *get() const
{
return state_;
}
StateType *operator()() const
{
return state_;
}
private:
StateSpacePtr space_;
StateSamplerPtr sampler_;
StateType *state_;
};
template <class T>
inline std::ostream &operator<<(std::ostream &out, const ScopedState<T> &state)
{
state.print(out);
return out;
}
template <class T, class Y>
inline ScopedState<T> &operator<<(ScopedState<T> &to, const ScopedState<Y> &from)
{
copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
return to;
}
template <class T, class Y>
inline const ScopedState<T> &operator>>(const ScopedState<T> &from, ScopedState<Y> &to)
{
copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
return from;
}
template <class T, class Y>
inline const ScopedState<> operator^(const ScopedState<T> &a, const ScopedState<Y> &b)
{
ScopedState<> r(a.getSpace() + b.getSpace());
return r << a << b;
}
template <class T>
const ScopedState<> ScopedState<T>::operator[](const StateSpacePtr &s) const
{
ScopedState<> r(s);
return r << *this;
}
using ScopedStatePtr = std::shared_ptr<ScopedState<>>;
} // namespace base
} // namespace ompl
#endif