Forward Declaration. More...

#include <visualization.h>

Public Member Functions
void	initialize (ros::NodeHandle &nh, const TebConfig &cfg)
	Initializes the class and registers topics. More...

	TebVisualization ()
	Default constructor. More...

	TebVisualization (ros::NodeHandle &nh, const TebConfig &cfg)
	Constructor that initializes the class and registers topics. More...

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. More...

void	publishLocalPlan (const std::vector< geometry_msgs::PoseStamped > &local_plan) const
	Publish a given local plan to the ros topic ../../local_plan. More...

void	publishLocalPlanAndPoses (const TimedElasticBand &teb) const
	Publish Timed_Elastic_Band related stuff (local plan, pose sequence). More...

void	publishRobotFootprintModel (const PoseSE2 &current_pose, const BaseRobotFootprintModel &robot_model, const std::string &ns="RobotFootprintModel", const std_msgs::ColorRGBA &color=toColorMsg(0.5, 0.0, 0.8, 0.0))
	Publish the visualization of the robot model. More...

void	publishInfeasibleRobotPose (const PoseSE2 &current_pose, const BaseRobotFootprintModel &robot_model)
	Publish the robot footprints related to infeasible poses. More...

void	publishObstacles (const ObstContainer &obstacles) const
	Publish obstacle positions to the ros topic ../../teb_markers. More...

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. More...

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. More...

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. More...

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). More...

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) More...

void	publishFeedbackMessage (const TebOptimalPlanner &teb_planner, const ObstContainer &obstacles)
	Publish a feedback message (single trajectory overload) More...

Static Public Member Functions
static std_msgs::ColorRGBA	toColorMsg (double a, double r, double g, double b)
	Helper function to generate a color message from single values. More...

Protected Member Functions
bool	printErrorWhenNotInitialized () const
	Small helper function that checks if initialize() has been called and prints an error message if not. More...

Protected Attributes
const TebConfig *	cfg_
	Config class that stores and manages all related parameters. More...

ros::Publisher	feedback_pub_
	Publisher for the feedback message for analysis and debug purposes. More...

ros::Publisher	global_plan_pub_
	Publisher for the global plan. More...

bool	initialized_
	Keeps track about the correct initialization of this class. More...

ros::Publisher	local_plan_pub_
	Publisher for the local plan. More...

ros::Publisher	teb_marker_pub_
	Publisher for visualization markers. More...

ros::Publisher	teb_poses_pub_
	Publisher for the trajectory pose sequence. More...

Detailed Description

Forward Declaration.

Visualize stuff from the teb_local_planner

Definition at line 80 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

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 473 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 387 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 429 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::publishInfeasibleRobotPose	(	const PoseSE2 &	current_pose,
		const BaseRobotFootprintModel &	robot_model
	)

Publish the robot footprints related to infeasible poses.

Parameters

current_pose	Current pose of the robot
robot_model	Subclass of BaseRobotFootprintModel

Definition at line 147 of file visualization.cpp.

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 153 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"`,
		const std_msgs::ColorRGBA &	color = `toColorMsg(0.5, 0.0, 0.8, 0.0)`
	)

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
color	Color of the footprint

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 336 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

Publish via-points to the ros topic ../../teb_markers.

Parameters

via_points via-point container

Definition at line 303 of file visualization.cpp.

std_msgs::ColorRGBA teb_local_planner::TebVisualization::toColorMsg	(	double	a,
		double	r,
		double	g,
		double	b
	)

static

Helper function to generate a color message from single values.

Parameters

a	Alpha value
r	Red value
g	Green value
b	Blue value

Returns: Color message

Definition at line 463 of file visualization.cpp.

Member Data Documentation

const TebConfig* teb_local_planner::TebVisualization::cfg_

protected

Config class that stores and manages all related parameters.

Definition at line 259 of file visualization.h.

ros::Publisher teb_local_planner::TebVisualization::feedback_pub_

protected

Publisher for the feedback message for analysis and debug purposes.

Definition at line 257 of file visualization.h.

ros::Publisher teb_local_planner::TebVisualization::global_plan_pub_

protected

Publisher for the global plan.

Definition at line 253 of file visualization.h.

bool teb_local_planner::TebVisualization::initialized_

protected

Keeps track about the correct initialization of this class.

Definition at line 261 of file visualization.h.

ros::Publisher teb_local_planner::TebVisualization::local_plan_pub_

protected

Publisher for the local plan.

Definition at line 254 of file visualization.h.

ros::Publisher teb_local_planner::TebVisualization::teb_marker_pub_

protected

Publisher for visualization markers.

Definition at line 256 of file visualization.h.

ros::Publisher teb_local_planner::TebVisualization::teb_poses_pub_

protected

Publisher for the trajectory pose sequence.

Definition at line 255 of file visualization.h.

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

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation