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
ros::Publisher	global_plan_pub_
	Publisher for the global plan. More...

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

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

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

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

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

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

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	publishRobotFootprint (const PoseSE2 &current_pose, const std::vector< geometry_msgs::Point > &footprint, const std::string &ns, const std_msgs::ColorRGBA &color)

void	publishInfeasibleRobotPose (const PoseSE2 &infeasible_pose, const BaseRobotFootprintModel &robot_model, const std::vector< geometry_msgs::Point > &footprint)
	Publish the robot footprint model and the actual robot footprint at an infeasible pose. More...

void	publishObstacles (const ObstContainer &obstacles, double scale=0.1) 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 std_msgs::ColorRGBA	toColorMsg (double a, double r, double g, double b)
	Helper function to generate a color message from single values. More...

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

Detailed Description

Forward Declaration.

Visualize stuff from the teb_local_planner

Definition at line 116 of file visualization.h.

Constructor & Destructor Documentation

◆ TebVisualization() [1/2]

teb_local_planner::TebVisualization::TebVisualization ( )

Default constructor.

Remarks: do not forget to call initialize()

Definition at line 82 of file visualization.cpp.

◆ TebVisualization() [2/2]

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 86 of file visualization.cpp.

Member Function Documentation

◆ initialize()

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 91 of file visualization.cpp.

◆ printErrorWhenNotInitialized()

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 566 of file visualization.cpp.

◆ publishFeedbackMessage() [1/2]

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 480 of file visualization.cpp.

◆ publishFeedbackMessage() [2/2]

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 522 of file visualization.cpp.

◆ publishGlobalPlan()

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 111 of file visualization.cpp.

◆ publishGraph()

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 84 of file visualization.hpp.

◆ publishInfeasibleRobotPose()

void teb_local_planner::TebVisualization::publishInfeasibleRobotPose	(	const PoseSE2 &	infeasible_pose,
		const BaseRobotFootprintModel &	robot_model,
		const std::vector< geometry_msgs::Point > &	footprint
	)

Publish the robot footprint model and the actual robot footprint at an infeasible pose.

Parameters

infeasible_pose	Infeasible pose to visualize
robot_model	Subclass of BaseRobotFootprintModel
footprint	Actual robot footprint

Definition at line 204 of file visualization.cpp.

◆ publishLocalPlan()

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 117 of file visualization.cpp.

◆ publishLocalPlanAndPoses()

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 124 of file visualization.cpp.

◆ publishObstacles()

void teb_local_planner::TebVisualization::publishObstacles	(	const ObstContainer &	obstacles,
		double	scale = `0.1`
	)		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
scale	Size of the non-circular obstacles

Definition at line 212 of file visualization.cpp.

◆ publishPathContainer()

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 215 of file visualization.hpp.

◆ publishRobotFootprint()

void teb_local_planner::TebVisualization::publishRobotFootprint	(	const PoseSE2 &	current_pose,
		const std::vector< geometry_msgs::Point > &	footprint,
		const std::string &	ns,
		const std_msgs::ColorRGBA &	color
	)

Definition at line 183 of file visualization.cpp.

◆ publishRobotFootprintModel()

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 158 of file visualization.cpp.

◆ publishTebContainer()

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 429 of file visualization.cpp.

◆ publishViaPoints()

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 396 of file visualization.cpp.

◆ toColorMsg()

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 556 of file visualization.cpp.

Member Data Documentation

◆ cfg_

const TebConfig* teb_local_planner::TebVisualization::cfg_

protected

Config class that stores and manages all related parameters.

Definition at line 301 of file visualization.h.

◆ feedback_pub_

ros::Publisher teb_local_planner::TebVisualization::feedback_pub_

protected

Publisher for the feedback message for analysis and debug purposes.

Definition at line 299 of file visualization.h.

◆ global_plan_pub_

ros::Publisher teb_local_planner::TebVisualization::global_plan_pub_

protected

Publisher for the global plan.

Definition at line 295 of file visualization.h.

◆ initialized_

bool teb_local_planner::TebVisualization::initialized_

protected

Keeps track about the correct initialization of this class.

Definition at line 303 of file visualization.h.

◆ local_plan_pub_

ros::Publisher teb_local_planner::TebVisualization::local_plan_pub_

protected

Publisher for the local plan.

Definition at line 296 of file visualization.h.

◆ teb_marker_pub_

ros::Publisher teb_local_planner::TebVisualization::teb_marker_pub_

protected

Publisher for visualization markers.

Definition at line 298 of file visualization.h.

◆ teb_poses_pub_

ros::Publisher teb_local_planner::TebVisualization::teb_poses_pub_

protected

Publisher for the trajectory pose sequence.

Definition at line 297 of file visualization.h.

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

Public Member Functions

Publish to topics

Detailed Description

Constructor & Destructor Documentation

◆ TebVisualization() [1/2]

◆ TebVisualization() [2/2]

Member Function Documentation

◆ initialize()

◆ printErrorWhenNotInitialized()

◆ publishFeedbackMessage() [1/2]

◆ publishFeedbackMessage() [2/2]

◆ publishGlobalPlan()

◆ publishGraph()

◆ publishInfeasibleRobotPose()

◆ publishLocalPlan()

◆ publishLocalPlanAndPoses()

◆ publishObstacles()

◆ publishPathContainer()

◆ publishRobotFootprint()

◆ publishRobotFootprintModel()

◆ publishTebContainer()

◆ publishViaPoints()

◆ toColorMsg()

Member Data Documentation

◆ cfg_

◆ feedback_pub_

◆ global_plan_pub_

◆ initialized_

◆ local_plan_pub_

◆ teb_marker_pub_

◆ teb_poses_pub_