Forward Declaration. More...

#include <visualization.h>

Public Member Functions
void	initialize (ros::NodeHandle &nh, const TebConfig &cfg)
	Initializes the class and registers topics.
	TebVisualization ()
	Default constructor.
	TebVisualization (ros::NodeHandle &nh, const TebConfig &cfg)
	Constructor that initializes the class and registers topics.
Publish to topics
void	publishGlobalPlan (const std::vector< geometry_msgs::PoseStamped > &global_plan) const
	Publish a given global plan to the ros topic ../../global_plan.
void	publishLocalPlan (const std::vector< geometry_msgs::PoseStamped > &local_plan) const
	Publish a given local plan to the ros topic ../../local_plan.
void	publishLocalPlanAndPoses (const TimedElasticBand &teb) const
	Publish Timed_Elastic_Band related stuff (local plan, pose sequence).
void	publishRobotFootprintModel (const PoseSE2 &current_pose, const BaseRobotFootprintModel &robot_model, const std::string &ns="RobotFootprintModel")
	Publish the visualization of the robot model.
void	publishObstacles (const ObstContainer &obstacles) const
	Publish obstacle positions to the ros topic ../../teb_markers.
void	publishViaPoints (const std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > &via_points, const std::string &ns="ViaPoints") const
	Publish via-points to the ros topic ../../teb_markers.
template<typename GraphType >
void	publishGraph (const GraphType &graph, const std::string &ns_prefix="Graph")
	Publish a boost::adjacency_list (boost's graph datatype) via markers.
template<typename BidirIter >
void	publishPathContainer (BidirIter first, BidirIter last, const std::string &ns="PathContainer")
	Publish multiple 2D paths (each path given as a point sequence) from a container class.
void	publishTebContainer (const std::vector< boost::shared_ptr< TebOptimalPlanner > > &teb_planner, const std::string &ns="TebContainer")
	Publish multiple Tebs from a container class (publish as marker message).
void	publishFeedbackMessage (const std::vector< boost::shared_ptr< TebOptimalPlanner > > &teb_planners, unsigned int selected_trajectory_idx, const ObstContainer &obstacles)
	Publish a feedback message (multiple trajectory version)
void	publishFeedbackMessage (const TebOptimalPlanner &teb_planner, const ObstContainer &obstacles)
	Publish a feedback message (single trajectory overload)
Protected Member Functions
bool	printErrorWhenNotInitialized () const
	Small helper function that checks if initialize() has been called and prints an error message if not.
Protected Attributes
const TebConfig *	cfg_
	Config class that stores and manages all related parameters.
ros::Publisher	feedback_pub_
	Publisher for the feedback message for analysis and debug purposes.
ros::Publisher	global_plan_pub_
	Publisher for the global plan.
bool	initialized_
	Keeps track about the correct initialization of this class.
ros::Publisher	local_plan_pub_
	Publisher for the local plan.
ros::Publisher	teb_marker_pub_
	Publisher for visualization markers.
ros::Publisher	teb_poses_pub_
	Publisher for the trajectory pose sequence.

Detailed Description

Forward Declaration.

Visualize stuff from the teb_local_planner

Definition at line 79 of file visualization.h.

Constructor & Destructor Documentation

teb_local_planner::TebVisualization::TebVisualization ( )

Default constructor.

Remarks:: do not forget to call initialize()

Definition at line 46 of file visualization.cpp.

teb_local_planner::TebVisualization::TebVisualization	(	ros::NodeHandle &	nh,
		const TebConfig &	cfg
	)

Constructor that initializes the class and registers topics.

Parameters:

nh	local ros::NodeHandle
cfg	const reference to the TebConfig class for parameters

Definition at line 50 of file visualization.cpp.

Member Function Documentation

void teb_local_planner::TebVisualization::initialize	(	ros::NodeHandle &	nh,
		const TebConfig &	cfg
	)

Initializes the class and registers topics.

Call this function if only the default constructor has been called before.

Parameters:

nh	local ros::NodeHandle
cfg	const reference to the TebConfig class for parameters

Definition at line 55 of file visualization.cpp.

bool teb_local_planner::TebVisualization::printErrorWhenNotInitialized ( ) const [inline, protected]

Small helper function that checks if initialize() has been called and prints an error message if not.

Returns:: true if not initialized, false if everything is ok

Definition at line 405 of file visualization.cpp.

void teb_local_planner::TebVisualization::publishFeedbackMessage	(	const std::vector< boost::shared_ptr< TebOptimalPlanner > > &	teb_planners,
		unsigned int	selected_trajectory_idx,
		const ObstContainer &	obstacles
	)

Publish a feedback message (multiple trajectory version)

The feedback message contains the all planned trajectory candidates (e.g. if planning in distinctive topologies is turned on). Each trajectory is composed of the sequence of poses, the velocity profile and temporal information. The feedback message also contains a list of active obstacles.

Parameters:

teb_planners	container with multiple tebs (resp. their planner instances)
selected_trajectory_idx	Idx of the currently selected trajectory in `teb_planners`
obstacles	Container of obstacles

Definition at line 353 of file visualization.cpp.

void teb_local_planner::TebVisualization::publishFeedbackMessage	(	const TebOptimalPlanner &	teb_planner,
		const ObstContainer &	obstacles
	)

Publish a feedback message (single trajectory overload)

The feedback message contains the planned trajectory that is composed of the sequence of poses, the velocity profile and temporal information. The feedback message also contains a list of active obstacles.

Parameters:

teb_planner	the planning instance
obstacles	Container of obstacles

Definition at line 383 of file visualization.cpp.

void teb_local_planner::TebVisualization::publishGlobalPlan ( const std::vector< geometry_msgs::PoseStamped > & global_plan ) const

Publish a given global plan to the ros topic ../../global_plan.

Parameters:

global_plan Pose array describing the global plan

Definition at line 75 of file visualization.cpp.

template<typename GraphType >

void teb_local_planner::TebVisualization::publishGraph	(	const GraphType &	graph,
		const std::string &	ns_prefix = `"Graph"`
	)

Publish a boost::adjacency_list (boost's graph datatype) via markers.

Remarks:: Make sure that vertices of the graph contain a member pos as Eigen::Vector2d type to query metric position values.

Parameters:

graph	Const reference to the boost::adjacency_list (graph)
ns_prefix	Namespace prefix for the marker objects (the strings "Edges" and "Vertices" will be appended)

Template Parameters:

GraphType boost::graph object in which vertices has the field/member pos.

Definition at line 48 of file visualization.hpp.

void teb_local_planner::TebVisualization::publishLocalPlan ( const std::vector< geometry_msgs::PoseStamped > & local_plan ) const

Publish a given local plan to the ros topic ../../local_plan.

Parameters:

local_plan Pose array describing the local plan

Definition at line 81 of file visualization.cpp.

void teb_local_planner::TebVisualization::publishLocalPlanAndPoses ( const TimedElasticBand & teb ) const

Publish Timed_Elastic_Band related stuff (local plan, pose sequence).

Given a Timed_Elastic_Band instance, publish the local plan to ../../local_plan and the pose sequence to ../../teb_poses.

Parameters:

teb	const reference to a Timed_Elastic_Band

Definition at line 88 of file visualization.cpp.

void teb_local_planner::TebVisualization::publishObstacles ( const ObstContainer & obstacles ) const

Publish obstacle positions to the ros topic ../../teb_markers.

Todo:: Move filling of the marker message to polygon class in order to avoid checking types.

Parameters:

obstacles Obstacle container

Definition at line 147 of file visualization.cpp.

template<typename BidirIter >

void teb_local_planner::TebVisualization::publishPathContainer	(	BidirIter	first,
		BidirIter	last,
		const std::string &	ns = `"PathContainer"`
	)

Publish multiple 2D paths (each path given as a point sequence) from a container class.

Provide a std::vector< std::vector< T > > in which T.x() and T.y() exist and std::vector could be individually substituded by std::list / std::deque /...

A common point-type for object T could be Eigen::Vector2d.

T could be also a raw pointer std::vector< std::vector< T* > >.

        typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > PathType; // could be a list or deque as well ...
    std::vector<PathType> path_container(2); // init 2 empty paths; the container could be a list or deque as well ...
        // Fill path_container.at(0) with Eigen::Vector2d elements, we skip that here
        // Fill path_container.at(1) with Eigen::Vector2d elements, we skip that here
    publishPathContainer( path_container.begin(), path_container.end() );

Remarks:: Actually the underlying path does not necessarily need to be a Eigen::Vector2d sequence. Eigen::Vector2d can be replaced with any datatype that implement public x() and y() methods.

Parameters:

first	Bidirectional iterator pointing to the begin of the path
last	Bidirectional iterator pointing to the end of the path
ns	Namespace for the marker objects (the strings "Edges" and "Vertices" will be appended)

Template Parameters:

BidirIter Bidirectional iterator to a 2D path (sequence of Eigen::Vector2d elements) in a container

Definition at line 179 of file visualization.hpp.

void teb_local_planner::TebVisualization::publishRobotFootprintModel	(	const PoseSE2 &	current_pose,
		const BaseRobotFootprintModel &	robot_model,
		const std::string &	ns = `"RobotFootprintModel"`
	)

Publish the visualization of the robot model.

Parameters:

current_pose	Current pose of the robot
robot_model	Subclass of BaseRobotFootprintModel
ns	Namespace for the marker objects

Definition at line 122 of file visualization.cpp.

void teb_local_planner::TebVisualization::publishTebContainer	(	const std::vector< boost::shared_ptr< TebOptimalPlanner > > &	teb_planner,
		const std::string &	ns = `"TebContainer"`
	)

Publish multiple Tebs from a container class (publish as marker message).

Parameters:

teb_planner	Container of boost::shared_ptr< TebOptPlannerPtr >
ns	Namespace for the marker objects

Definition at line 308 of file visualization.cpp.

void teb_local_planner::TebVisualization::publishViaPoints	(	const std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > &	via_points,
		const std::string &	ns = `"ViaPoints"`
	)		const