Class DWBPublisher

Class Documentation

class DWBPublisher

Consolidation of all the publishing logic for the DWB Local Planner.

Right now, it can publish 1) The Global Plan (as passed in using setPath) 2) The Local Plan (after it is calculated) 3) The Transformed Global Plan (since it may be different than the global) 4) The Full LocalPlanEvaluation 5) Markers representing the different trajectories evaluated 6) The CostGrid (in the form of a complex PointCloud2)

Public Functions

explicit DWBPublisher(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, const std::string &plugin_name)
nav2_util::CallbackReturn on_configure()
nav2_util::CallbackReturn on_activate()
nav2_util::CallbackReturn on_deactivate()
nav2_util::CallbackReturn on_cleanup()
inline bool shouldRecordEvaluation()

Does the publisher require that the LocalPlanEvaluation be saved.

Returns:

True if the Evaluation is needed to publish either directly or as trajectories

void publishEvaluation(std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results)

If the pointer is not null, publish the evaluation and trajectories as needed.

void publishLocalPlan(const std_msgs::msg::Header &header, const dwb_msgs::msg::Trajectory2D &traj)
void publishCostGrid(const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros, const std::vector<TrajectoryCritic::Ptr> critics)
void publishGlobalPlan(const nav_2d_msgs::msg::Path2D plan)
void publishTransformedPlan(const nav_2d_msgs::msg::Path2D plan)
void publishLocalPlan(const nav_2d_msgs::msg::Path2D plan)

Protected Functions

void publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation &results)
void publishGenericPlan(const nav_2d_msgs::msg::Path2D plan, rclcpp::Publisher<nav_msgs::msg::Path> &pub, bool flag)

Protected Attributes

bool publish_evaluation_
bool publish_global_plan_
bool publish_transformed_
bool publish_local_plan_
bool publish_trajectories_
bool publish_cost_grid_pc_
bool publish_input_params_
builtin_interfaces::msg::Duration marker_lifetime_
std::shared_ptr<LifecyclePublisher<dwb_msgs::msg::LocalPlanEvaluation>> eval_pub_
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> global_pub_
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> transformed_pub_
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> local_pub_
std::shared_ptr<LifecyclePublisher<visualization_msgs::msg::MarkerArray>> marker_pub_
std::shared_ptr<LifecyclePublisher<sensor_msgs::msg::PointCloud2>> cost_grid_pc_pub_
rclcpp_lifecycle::LifecycleNode::WeakPtr node_
rclcpp::Clock::SharedPtr clock_
std::string plugin_name_