Visualizes trajectories for debugging. More...

#include <nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp>

Collaboration diagram for mppi::TrajectoryVisualizer:

Public Member Functions
	TrajectoryVisualizer ()=default
	Constructor for mppi::TrajectoryVisualizer.

void	on_configure (nav2::LifecycleNode::WeakPtr parent, const std::string &name, const std::string &frame_id, ParametersHandler *parameters_handler)
	Configure trajectory visualizer. More...

void	on_cleanup ()
	Cleanup object on shutdown.

void	on_activate ()
	Activate object.

void	on_deactivate ()
	Deactivate object.

void	add (const Eigen::ArrayXXf &trajectory, const std::string &marker_namespace, const builtin_interfaces::msg::Time &cmd_stamp)
	Add an optimal trajectory to visualize. More...

void	add (const models::Trajectories &trajectories, const Eigen::ArrayXf &costs, const std::vector< bool > &collisions, const builtin_interfaces::msg::Time &stamp)
	Add candidate trajectories colored by cost gradient. More...

void	visualize ()
	Visualize the plan.

void	reset ()
	Reset object.

Protected Member Functions
void	addCostColoredTrajectory (size_t trajectory_idx, const models::Trajectories &trajectories, float normalized_cost, bool in_collision, const builtin_interfaces::msg::Time &stamp)
	Create a LINE_STRIP marker for a single trajectory colored by normalized cost. More...

Static Protected Member Functions
static std_msgs::msg::ColorRGBA	costToColor (float normalized)
	Convert a normalized cost [0,1] to a green->yellow->red color. More...

Protected Attributes
std::string	frame_id_

nav2::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	trajectories_publisher_

nav2::Publisher< nav_msgs::msg::Path >::SharedPtr	optimal_path_pub_

std::unique_ptr< nav_msgs::msg::Path >	optimal_path_

std::unique_ptr< visualization_msgs::msg::MarkerArray >	points_

int	marker_id_ = 0

ParametersHandler *	parameters_handler_

size_t	trajectory_step_ {0}

size_t	time_step_ {0}

rclcpp::Logger	logger_ {rclcpp::get_logger("MPPIController")}

Detailed Description

Visualizes trajectories for debugging.

Definition at line 40 of file trajectory_visualizer.hpp.

Member Function Documentation

◆ add() [1/2]

void mppi::TrajectoryVisualizer::add	(	const Eigen::ArrayXXf &	trajectory,
		const std::string &	marker_namespace,
		const builtin_interfaces::msg::Time &	cmd_stamp
	)

Add an optimal trajectory to visualize.

Parameters

trajectory Optimal trajectory

Definition at line 62 of file trajectory_visualizer.cpp.

Referenced by nav2_mppi_controller::MPPIController::visualize().

Here is the caller graph for this function:

◆ add() [2/2]

void mppi::TrajectoryVisualizer::add	(	const models::Trajectories &	trajectories,
		const Eigen::ArrayXf &	costs,
		const std::vector< bool > &	collisions,
		const builtin_interfaces::msg::Time &	stamp
	)

Add candidate trajectories colored by cost gradient.

Parameters

trajectories	Candidate trajectories
costs	Cost per trajectory for the selected layer
collisions	Per-trajectory collision flags
stamp	Timestamp for markers

Definition at line 110 of file trajectory_visualizer.cpp.

References addCostColoredTrajectory().

Here is the call graph for this function:

◆ addCostColoredTrajectory()

void mppi::TrajectoryVisualizer::addCostColoredTrajectory	(	size_t	trajectory_idx,
		const models::Trajectories &	trajectories,
		float	normalized_cost,
		bool	in_collision,
		const builtin_interfaces::msg::Time &	stamp
	)

protected

Create a LINE_STRIP marker for a single trajectory colored by normalized cost.

Parameters

trajectory_idx	Row index into the trajectories arrays
trajectories	Trajectory data
normalized_cost	Cost value in [0, 1] range
in_collision	Whether this trajectory is in collision
stamp	Timestamp

Definition at line 153 of file trajectory_visualizer.cpp.

References costToColor().

Referenced by add().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ costToColor()

std_msgs::msg::ColorRGBA mppi::TrajectoryVisualizer::costToColor ( float normalized )

staticprotected

Convert a normalized cost [0,1] to a green->yellow->red color.

Parameters

normalized Value in [0, 1]

Returns: ColorRGBA

Definition at line 188 of file trajectory_visualizer.cpp.

Referenced by addCostColoredTrajectory().

Here is the caller graph for this function:

◆ on_configure()

void mppi::TrajectoryVisualizer::on_configure	(	nav2::LifecycleNode::WeakPtr	parent,
		const std::string &	name,
		const std::string &	frame_id,
		ParametersHandler *	parameters_handler
	)