#include <wave_front_planner.h>

Inheritance diagram for wave_front_planner::WaveFrontPlanner:

Public Types
typedef boost::shared_ptr< wave_front_planner::WaveFrontPlanner >	Ptr

Public Types inherited from mbf_mesh_core::MeshPlanner
typedef boost::shared_ptr< mbf_mesh_core::MeshPlanner >	Ptr

Public Types inherited from mbf_abstract_core::AbstractPlanner
typedef boost::shared_ptr< ::mbf_abstract_core::AbstractPlanner >	Ptr

Public Member Functions
virtual bool	cancel ()
	Requests the planner to cancel, e.g. if it takes too much time. More...

virtual bool	initialize (const std::string &name, const boost::shared_ptr< mesh_map::MeshMap > &mesh_map_ptr)
	Initializes the planner plugin with a user configured name and a shared pointer to the mesh map. More...

virtual uint32_t	makePlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, double tolerance, std::vector< geometry_msgs::PoseStamped > &plan, double &cost, std::string &message)
	Compute a continuous vector field and geodesic path on the mesh's surface. More...

	WaveFrontPlanner ()
	Constructor. More...

virtual	~WaveFrontPlanner ()
	Destructor. More...

Public Member Functions inherited from mbf_mesh_core::MeshPlanner
virtual	~MeshPlanner ()

Public Member Functions inherited from mbf_abstract_core::AbstractPlanner
virtual	~AbstractPlanner ()

Protected Member Functions
void	computeVectorMap ()
	Computes the vector field in a post processing. It rotates the predecessor edges by the stored angles. More...

void	reconfigureCallback (wave_front_planner::WaveFrontPlannerConfig &cfg, uint32_t level)
	Dynamic reconfigure callback. More...

uint32_t	waveFrontPropagation (const mesh_map::Vector &start, const mesh_map::Vector &goal, const lvr2::DenseEdgeMap< float > &edge_weights, const lvr2::DenseVertexMap< float > &costs, std::list< std::pair< mesh_map::Vector, lvr2::FaceHandle >> &path, lvr2::DenseVertexMap< float > &distances, lvr2::DenseVertexMap< lvr2::VertexHandle > &predecessors)
	Computes a wavefront propagation from the start until it reached the goal. More...

uint32_t	waveFrontPropagation (const mesh_map::Vector &start, const mesh_map::Vector &goal, std::list< std::pair< mesh_map::Vector, lvr2::FaceHandle >> &path)
	Computes a wavefront propagation from the start until it reached the goal. More...

bool	waveFrontUpdate (lvr2::DenseVertexMap< float > &distances, const lvr2::DenseEdgeMap< float > &edge_weights, const lvr2::VertexHandle &v1, const lvr2::VertexHandle &v2, const lvr2::VertexHandle &v3)

bool	waveFrontUpdateWithS (lvr2::DenseVertexMap< float > &distances, const lvr2::DenseEdgeMap< float > &edge_weights, const lvr2::VertexHandle &v1, const lvr2::VertexHandle &v2, const lvr2::VertexHandle &v3)

Protected Member Functions inherited from mbf_mesh_core::MeshPlanner
	MeshPlanner ()

Protected Member Functions inherited from mbf_abstract_core::AbstractPlanner
	AbstractPlanner ()

Private Attributes
std::atomic_bool	cancel_planning
	flag if cancel has been requested More...

WaveFrontPlannerConfig	config
	the current dynamic reconfigure planner configuration More...

dynamic_reconfigure::Server< wave_front_planner::WaveFrontPlannerConfig >::CallbackType	config_callback
	dynamic reconfigure callback function binding More...

lvr2::DenseVertexMap< lvr2::FaceHandle >	cutting_faces
	the face which is cut by the computed line to the source More...

lvr2::DenseVertexMap< float >	direction
	theta angles to the source of the wave front propagation More...

bool	first_config
	indicates if dynamic reconfigure has been called the first time More...

float	goal_dist_offset
	an offset that determines how far beyond the goal (robot's position) is propagated. More...

std::string	map_frame
	the map coordinate frame / system id More...

mesh_map::MeshMap::Ptr	mesh_map
	shared pointer to the mesh map More...

std::string	name
	the user defined plugin name More...

ros::Publisher	path_pub
	publisher for the backtracked path More...

lvr2::DenseVertexMap< float >	potential
	potential field / scalar distance field to the seed More...

lvr2::DenseVertexMap< lvr2::VertexHandle >	predecessors
	predecessors while wave propagation More...

ros::NodeHandle	private_nh
	the private node handle with the user defined namespace (name) More...

bool	publish_face_vectors
	whether to also publish direction vectors at the triangle centers More...

bool	publish_vector_field
	whether to publish the vector field or not More...

boost::shared_ptr< dynamic_reconfigure::Server< wave_front_planner::WaveFrontPlannerConfig > >	reconfigure_server_ptr
	shared pointer to dynamic reconfigure server More...

lvr2::DenseVertexMap< mesh_map::Vector >	vector_map
	stores the current vector map containing vectors pointing to the seed More...

Detailed Description

Definition at line 49 of file wave_front_planner.h.

Member Typedef Documentation

◆ Ptr

typedef boost::shared_ptr<wave_front_planner::WaveFrontPlanner> wave_front_planner::WaveFrontPlanner::Ptr

Definition at line 52 of file wave_front_planner.h.

Constructor & Destructor Documentation

◆ WaveFrontPlanner()

wave_front_planner::WaveFrontPlanner::WaveFrontPlanner ( )

Constructor.

Definition at line 53 of file wave_front_planner.cpp.

◆ ~WaveFrontPlanner()

wave_front_planner::WaveFrontPlanner::~WaveFrontPlanner ( )

virtual

Destructor.

Definition at line 57 of file wave_front_planner.cpp.

Member Function Documentation

◆ cancel()

bool wave_front_planner::WaveFrontPlanner::cancel ( )

virtual

Requests the planner to cancel, e.g. if it takes too much time.

Returns: true if cancel has been successfully requested, false otherwise

Implements mbf_mesh_core::MeshPlanner.

Definition at line 126 of file wave_front_planner.cpp.

◆ computeVectorMap()

void wave_front_planner::WaveFrontPlanner::computeVectorMap ( )

protected

Computes the vector field in a post processing. It rotates the predecessor edges by the stored angles.

Definition at line 170 of file wave_front_planner.cpp.

◆ initialize()

bool wave_front_planner::WaveFrontPlanner::initialize	(	const std::string &	name,
		const boost::shared_ptr< mesh_map::MeshMap > &	mesh_map_ptr
	)

virtual

Initializes the planner plugin with a user configured name and a shared pointer to the mesh map.

Parameters

name	The user configured name, which is used as namespace for parameters, etc.
mesh_map_ptr	A shared pointer to the mesh map instance to access attributes and helper functions, etc.

Returns: true if the plugin has been initialized successfully

Implements mbf_mesh_core::MeshPlanner.

Definition at line 132 of file wave_front_planner.cpp.

◆ makePlan()

uint32_t wave_front_planner::WaveFrontPlanner::makePlan	(	const geometry_msgs::PoseStamped &	start,
		const geometry_msgs::PoseStamped &	goal,
		double	tolerance,
		std::vector< geometry_msgs::PoseStamped > &	plan,
		double &	cost,
		std::string &	message
	)

virtual

Compute a continuous vector field and geodesic path on the mesh's surface.

Parameters

start	The start pose
goal	The goal pose
tolerance	The goal tolerance, TODO is currently not used
plan	The computed plan
cost	The computed cost for the plan
message	a detailed outcome message

Returns: result outcome code, see the GetPath action definition

Implements mbf_mesh_core::MeshPlanner.

Definition at line 61 of file wave_front_planner.cpp.

◆ reconfigureCallback()

void wave_front_planner::WaveFrontPlanner::reconfigureCallback	(	wave_front_planner::WaveFrontPlannerConfig &	cfg,
		uint32_t	level
	)

protected

Dynamic reconfigure callback.

Definition at line 158 of file wave_front_planner.cpp.

◆ waveFrontPropagation() [1/2]

uint32_t wave_front_planner::WaveFrontPlanner::waveFrontPropagation	(	const mesh_map::Vector &	start,
		const mesh_map::Vector &	goal,
		const lvr2::DenseEdgeMap< float > &	edge_weights,
		const lvr2::DenseVertexMap< float > &	costs,
		std::list< std::pair< mesh_map::Vector, lvr2::FaceHandle >> &	path,
		lvr2::DenseVertexMap< float > &	distances,
		lvr2::DenseVertexMap< lvr2::VertexHandle > &	predecessors
	)

protected

Computes a wavefront propagation from the start until it reached the goal.

Parameters

start	The seed of the wave, i.e. the robot's goal pose
goal	The goal of the wavefront, where it will stop propagating
edge_weights	The edge weights map to use for vertex distances in a triangle
costs	The combined vertex costs to use during the propagation
path	The backtracked path
distances	The computed distances
predecessors	The backtracked predecessors

Returns: a ExePath action related outcome code

Definition at line 523 of file wave_front_planner.cpp.

◆ waveFrontPropagation() [2/2]

uint32_t wave_front_planner::WaveFrontPlanner::waveFrontPropagation	(	const mesh_map::Vector &	start,
		const mesh_map::Vector &	goal,
		std::list< std::pair< mesh_map::Vector, lvr2::FaceHandle >> &	path
	)

protected

Computes a wavefront propagation from the start until it reached the goal.

Parameters

start	The seed of the wave, i.e. the robot's goal pose
goal	The goal of the wavefront, where it will stop propagating
path	The backtracked path

Returns: a ExePath action related outcome code

Definition at line 207 of file wave_front_planner.cpp.

◆ waveFrontUpdate()

bool wave_front_planner::WaveFrontPlanner::waveFrontUpdate	(	lvr2::DenseVertexMap< float > &	distances,
		const lvr2::DenseEdgeMap< float > &	edge_weights,
		const lvr2::VertexHandle &	v1,
		const lvr2::VertexHandle &	v2,
		const lvr2::VertexHandle &	v3
	)

inlineprotected

Fast Marching Method update step using the Law of Cosines to determine if the direction vector is cutting the current triangle

Parameters

distances	Distance map to the goal which stores the current state of all distances to the goal
edge_weights	Distances assigned to each edge
v1	The first vertex of the triangle
v2	The second vertex of the triangle
v3	The thrid vertex of the triangle

Returns: true if the newly computed distance is shorter than before and if the current triangle is cut

Definition at line 334 of file wave_front_planner.cpp.

◆ waveFrontUpdateWithS()

bool wave_front_planner::WaveFrontPlanner::waveFrontUpdateWithS	(	lvr2::DenseVertexMap< float > &	distances,
		const lvr2::DenseEdgeMap< float > &	edge_weights,
		const lvr2::VertexHandle &	v1,
		const lvr2::VertexHandle &	v2,
		const lvr2::VertexHandle &	v3
	)

inlineprotected

Fast Marching Method update step using the Hesse normal form to determine if the direction vector is cutting the current triangle

Parameters

distances	Distance map to the goal which stores the current state of all distances to the goal
edge_weights	Distances assigned to each edge
v1	The first vertex of the triangle
v2	The second vertex of the triangle
v3	The thrid vertex of the triangle

Returns: true if the newly computed distance is shorter than before and if the current triangle is cut

Definition at line 214 of file wave_front_planner.cpp.

Member Data Documentation

◆ cancel_planning

std::atomic_bool wave_front_planner::WaveFrontPlanner::cancel_planning

private

flag if cancel has been requested

Definition at line 170 of file wave_front_planner.h.

◆ config

WaveFrontPlannerConfig wave_front_planner::WaveFrontPlanner::config

private

the current dynamic reconfigure planner configuration

Definition at line 197 of file wave_front_planner.h.

◆ config_callback

dynamic_reconfigure::Server<wave_front_planner::WaveFrontPlannerConfig>::CallbackType wave_front_planner::WaveFrontPlanner::config_callback

private

dynamic reconfigure callback function binding

Definition at line 191 of file wave_front_planner.h.

◆ cutting_faces

lvr2::DenseVertexMap<lvr2::FaceHandle> wave_front_planner::WaveFrontPlanner::cutting_faces

private

the face which is cut by the computed line to the source

Definition at line 206 of file wave_front_planner.h.

◆ direction

lvr2::DenseVertexMap<float> wave_front_planner::WaveFrontPlanner::direction

private

theta angles to the source of the wave front propagation

Definition at line 200 of file wave_front_planner.h.

◆ first_config

bool wave_front_planner::WaveFrontPlanner::first_config

private

indicates if dynamic reconfigure has been called the first time

Definition at line 194 of file wave_front_planner.h.

◆ goal_dist_offset

float wave_front_planner::WaveFrontPlanner::goal_dist_offset

private

an offset that determines how far beyond the goal (robot's position) is propagated.

Definition at line 185 of file wave_front_planner.h.

◆ map_frame

std::string wave_front_planner::WaveFrontPlanner::map_frame

private

the map coordinate frame / system id

Definition at line 182 of file wave_front_planner.h.

◆ mesh_map

mesh_map::MeshMap::Ptr wave_front_planner::WaveFrontPlanner::mesh_map

private

shared pointer to the mesh map

Definition at line 161 of file wave_front_planner.h.

◆ name

std::string wave_front_planner::WaveFrontPlanner::name

private

the user defined plugin name

Definition at line 164 of file wave_front_planner.h.

◆ path_pub

ros::Publisher wave_front_planner::WaveFrontPlanner::path_pub

private

publisher for the backtracked path

Definition at line 173 of file wave_front_planner.h.

◆ potential

lvr2::DenseVertexMap<float> wave_front_planner::WaveFrontPlanner::potential

private

potential field / scalar distance field to the seed

Definition at line 212 of file wave_front_planner.h.

◆ predecessors

lvr2::DenseVertexMap<lvr2::VertexHandle> wave_front_planner::WaveFrontPlanner::predecessors

private

predecessors while wave propagation

Definition at line 203 of file wave_front_planner.h.

◆ private_nh

ros::NodeHandle wave_front_planner::WaveFrontPlanner::private_nh

private

the private node handle with the user defined namespace (name)

Definition at line 167 of file wave_front_planner.h.

◆ publish_face_vectors

bool wave_front_planner::WaveFrontPlanner::publish_face_vectors

private

whether to also publish direction vectors at the triangle centers

Definition at line 179 of file wave_front_planner.h.

◆ publish_vector_field

bool wave_front_planner::WaveFrontPlanner::publish_vector_field

private

whether to publish the vector field or not

Definition at line 176 of file wave_front_planner.h.

◆ reconfigure_server_ptr

boost::shared_ptr<dynamic_reconfigure::Server<wave_front_planner::WaveFrontPlannerConfig> > wave_front_planner::WaveFrontPlanner::reconfigure_server_ptr

private

shared pointer to dynamic reconfigure server

Definition at line 188 of file wave_front_planner.h.

◆ vector_map

lvr2::DenseVertexMap<mesh_map::Vector> wave_front_planner::WaveFrontPlanner::vector_map

private

stores the current vector map containing vectors pointing to the seed

Definition at line 209 of file wave_front_planner.h.

The documentation for this class was generated from the following files:

Public Types

Public Member Functions

Protected Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ Ptr

Constructor & Destructor Documentation

◆ WaveFrontPlanner()

◆ ~WaveFrontPlanner()

Member Function Documentation

◆ cancel()

◆ computeVectorMap()

◆ initialize()

◆ makePlan()

◆ reconfigureCallback()

◆ waveFrontPropagation() [1/2]

◆ waveFrontPropagation() [2/2]

◆ waveFrontUpdate()

◆ waveFrontUpdateWithS()

Member Data Documentation

◆ cancel_planning

◆ config

◆ config_callback

◆ cutting_faces

◆ direction

◆ first_config

◆ goal_dist_offset

◆ map_frame

◆ mesh_map

◆ name

◆ path_pub

◆ potential

◆ predecessors

◆ private_nh

◆ publish_face_vectors

◆ publish_vector_field

◆ reconfigure_server_ptr

◆ vector_map