Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Dima/cleanup #247

Open
wants to merge 2 commits into
base: melodic-devel
Choose a base branch
from
Open

Dima/cleanup #247

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1907879
remove redundant functions
Nov 9, 2020
9206571
use unified config-interface
Nov 9, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
140 changes: 29 additions & 111 deletions include/teb_local_planner/g2o_types/base_teb_edges.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,28 @@

namespace teb_local_planner
{



/**
* @brief Common interface for setting/getting the TebConfig.
*
* Class offers a setter/getter interface used by the edges of this library.
*/
struct ConfigInterface{

inline const TebConfig* getTebConfig() const noexcept
{
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

getter actually not required, so it could be dropped

return cfg_;
}

inline void setTebConfig(const TebConfig& _cfg) noexcept
{
cfg_ = &_cfg;
}

protected:
const TebConfig* cfg_;
};

/**
* @class BaseTebUnaryEdge
* @brief Base edge connecting a single vertex in the TEB optimization problem
Expand All @@ -67,25 +87,11 @@ namespace teb_local_planner
* @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
*/
template <int D, typename E, typename VertexXi>
class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>,
public ConfigInterface
{
public:

using typename g2o::BaseUnaryEdge<D, E, VertexXi>::ErrorVector;
using g2o::BaseUnaryEdge<D, E, VertexXi>::computeError;

/**
* @brief Compute and return error / cost value.
*
* This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
* @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
*/
ErrorVector& getError()
{
computeError();
return _error;
}


/**
* @brief Read values from input stream
*/
Expand All @@ -104,23 +110,6 @@ class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
return os.good();
}

/**
* @brief Assign the TebConfig class for parameters.
* @param cfg TebConfig class
*/
void setTebConfig(const TebConfig& cfg)
{
cfg_ = &cfg;
}

protected:

using g2o::BaseUnaryEdge<D, E, VertexXi>::_error;
using g2o::BaseUnaryEdge<D, E, VertexXi>::_vertices;

const TebConfig* cfg_; //!< Store TebConfig class for parameters

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

Expand All @@ -135,24 +124,10 @@ class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
* @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
*/
template <int D, typename E, typename VertexXi, typename VertexXj>
class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>,
public ConfigInterface
{
public:

using typename g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::ErrorVector;
using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::computeError;

/**
* @brief Compute and return error / cost value.
*
* This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
* @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
*/
ErrorVector& getError()
{
computeError();
return _error;
}

/**
* @brief Read values from input stream
Expand All @@ -172,23 +147,6 @@ class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
return os.good();
}

/**
* @brief Assign the TebConfig class for parameters.
* @param cfg TebConfig class
*/
void setTebConfig(const TebConfig& cfg)
{
cfg_ = &cfg;
}

protected:

using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_error;
using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_vertices;

const TebConfig* cfg_; //!< Store TebConfig class for parameters

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

Expand All @@ -204,34 +162,11 @@ class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
* @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
*/
template <int D, typename E>
class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>
class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>,
public ConfigInterface
{
public:

using typename g2o::BaseMultiEdge<D, E>::ErrorVector;
using g2o::BaseMultiEdge<D, E>::computeError;

// Overwrites resize() from the parent class
virtual void resize(size_t size)
{
g2o::BaseMultiEdge<D, E>::resize(size);

for(std::size_t i=0; i<_vertices.size(); ++i)
_vertices[i] = NULL;
}

/**
* @brief Compute and return error / cost value.
*
* This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
* @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
*/
ErrorVector& getError()
{
computeError();
return _error;
}

/**
* @brief Read values from input stream
*/
Expand All @@ -250,23 +185,6 @@ class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>
return os.good();
}

/**
* @brief Assign the TebConfig class for parameters.
* @param cfg TebConfig class
*/
void setTebConfig(const TebConfig& cfg)
{
cfg_ = &cfg;
}

protected:

using g2o::BaseMultiEdge<D, E>::_error;
using g2o::BaseMultiEdge<D, E>::_vertices;

const TebConfig* cfg_; //!< Store TebConfig class for parameters

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

Expand Down