Class TebVisualization

Class Documentation

class TebVisualization

Forward Declaration.

Visualize stuff from the hateb_local_planner

Publish to topics

ros::Publisher global_plan_pub_

Publisher for the global plan.

ros::Publisher local_plan_pub_

Publisher for the local plan.

ros::Publisher local_traj_pub_

Publisher for the local traj.

ros::Publisher agents_global_plans_pub_

Publisher for the local plan.

ros::Publisher agents_local_plans_pub_

Publisher for the local plan.

ros::Publisher agents_local_trajs_pub_

Publisher for the agents local plans.

ros::Publisher teb_poses_pub_
ros::Publisher teb_fp_poses_pub_

Publisher for the trajectory pose sequence.

ros::Publisher agents_tebs_poses_pub_
ros::Publisher agents_tebs_fp_poses_pub_

Publisher for the trajectory pose sequence.

ros::Publisher teb_marker_pub_

Publisher for visualization markers.

ros::Publisher feedback_pub_

Publisher for optimization feedback messages.

ros::Publisher mode_text_pub_

Publisher for current planner mode text visualization.

ros::Publisher robot_traj_time_pub_

Publisher for robot’s time to goal with optimized trajectory (until end of trajectory)

ros::Publisher robot_path_time_pub_

Publisher for robot’s full time to goal (until goal, using path + traj)

ros::Publisher robot_next_pose_pub_

Publisher for robot’s predicted next pose.

ros::Publisher agent_next_pose_pub_

Publisher for agents’ predicted next poses.

ros::Publisher agent_trajs_time_pub_

Publisher for agents’ time to goal with optimized trajectory (until end of trajectory)

ros::Publisher agent_paths_time_pub_

Publisher for agents’ full time to goal (until goal, using path + traj)

ros::Publisher agent_marker_pub_

Publisher for agent visualization markers.

ros::Publisher agent_arrow_pub_

Publisher for agent direction arrow markers.

ros::Subscriber tracked_agents_sub_

Subscriber for tracked agents data input.

std::vector<double> vel_robot_

Store robot velocity history (for bar visualization)

std::vector<double> vel_agent_

Store agent velocity history (for bar visualization)

tf::TransformListener tf_

Transform listener for coordinate frame transformations.

ros::Publisher ttg_pub_

Publisher for time-to-goal information.

std::string ns_

Name space of the robot.

std::string tracked_agents_sub_topic_

Tracked agents sub topic.

const HATebConfig *cfg_

Config class that stores and manages all related parameters.

bool initialized_

Keeps track about the correct initialization of this class.

ros::Timer clearing_timer_

Timer for periodically clearing old visualization markers.

bool last_publish_robot_global_plan_

Last publish state of robot’s global plan.

bool last_publish_robot_local_plan_

Last publish state of robot’s local plan.

bool last_publish_robot_local_plan_poses_

Last publish state of robot’s local plan poses.

bool last_publish_robot_local_plan_fp_poses_

Last publish state of robot’s local plan footprint poses.

bool last_publish_agents_global_plans_

Last publish state of agents’ global plans.

bool last_publish_agents_local_plans_

Last publish state of agents’ local plans.

bool last_publish_agents_local_plan_poses_

Last publish state of agents’ local plan poses.

bool last_publish_agents_local_plan_fp_poses_

Last publish state of agents’ local plan footprint poses.

mutable int last_robot_fp_poses_idx_

Index of last published robot footprint pose.

int last_agent_fp_poses_idx_

Index of last published agent footprint pose.

void 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

void publishAgentGlobalPlans(const std::vector<AgentPlanCombined> &agents_plans) const

Publish a given agent’s global plan to the ros topic.

Parameters:

agents_plans – Vector comntaining the AgentPlanCombined

void 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

void publishTrackedAgents(const cohan_msgs::TrackedAgentsConstPtr &agents)

Publish visualization markers for tracked agents in the environment.

Parameters:

agents – Pointer to the tracked agents message containing agent positions, velocities, and properties

void publishLocalPlanAndPoses(const TimedElasticBand &teb, const BaseFootprintModel &robot_model, double fp_size, const std_msgs::ColorRGBA &color = toColorMsg(0.5, 0.0, 0.8, 0.0))

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

void publishAgentLocalPlansAndPoses(const std::map<uint64_t, TimedElasticBand> &agents_tebs_map, const BaseFootprintModel &agent_model, double fp_size, const std_msgs::ColorRGBA &color = toColorMsg(0.5, 0.0, 0.8, 0.0))

Publish local plans and pose sequences for multiple agents.

Similar to publishLocalPlanAndPoses but handles multiple agents. Publishes local plans and pose sequences for each agent in the map to their respective ROS topics.

Parameters:

  • agents_tebs_map – Map of agent IDs to their Timed Elastic Band trajectories

  • agent_model – The footprint model used for all agents

  • fp_size – Size of the footprint for visualization

  • color – Color to use for visualization markers (RGBA)

void publishTrajectory(const PlanTrajCombined &plan_traj_combined)

Publish a complete trajectory including plan segments before and after optimization.

Publishes visualization of the complete trajectory which includes:

  • The plan segment before optimization

  • The optimized trajectory

  • The plan segment after optimization

Parameters:

plan_traj_combined – Combined plan and trajectory data structure containing all segments

void publishAgentTrajectories(const std::vector<AgentPlanTrajCombined> &agents_plans_combined)

Publish trajectories for multiple agents.

Publishes visualization of complete trajectories for multiple agents, each including:

  • The plan segment before optimization

  • The optimized trajectory

  • The plan segment after optimization

Parameters:

agents_plans_combined – Vector of combined plan and trajectory data for each agent

void publishRobotFootprintModel(const PoseSE2 &current_pose, const BaseFootprintModel &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 BaseFootprintModel

  • ns – Namespace for the marker objects

  • color – Color of the footprint

void publishInfeasibleRobotPose(const PoseSE2 &current_pose, const BaseFootprintModel &robot_model)

Publish the robot footprints related to infeasible poses.

Parameters:

  • current_pose – Current pose of the robot

  • robot_model – Subclass of BaseFootprintModel

void publishObstacles(const ObstContainer &obstacles) const

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

Parameters:

obstacles – Obstacle container

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.

Parameters:

via_points – via-point container

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.

Remark

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.

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.

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() );

Remark

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

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

Parameters:

  • teb_planner – Container of boost::shared_ptr< TebOptPlannerPtr >

  • ns – Namespace for the marker objects

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)

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

void 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

void publishMode(int Mode)
static std_msgs::ColorRGBA toColorMsg(double a, double r, double g, double b)

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

static void setMarkerColour(visualization_msgs::Marker &marker, double itr, double n)
bool printErrorWhenNotInitialized() const

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

void clearingTimerCB(const ros::TimerEvent &event)

Callback function for the clearing timer that removes outdated visualization markers.

This protected method is called periodically to remove visualization markers that are no longer needed, preventing visualization clutter and memory buildup

Parameters:

event – The timer event containing timing information about the callback

Public Functions

TebVisualization()

Default constructor.

Remark

do not forget to call initialize()

TebVisualization(ros::NodeHandle &nh, const HATebConfig &cfg)

Constructor that initializes the class and registers topics.

Parameters:

  • nh – local ros::NodeHandle

  • cfg – const reference to the HATebConfig class for parameters

void initialize(ros::NodeHandle &nh, const HATebConfig &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 HATebConfig class for parameters