Class PathConverter

Class Documentation

class PathConverter

An helper to convert the route into dense paths.

Public Functions

PathConverter() = default

A constructor for nav2_route::PathConverter.

~PathConverter() = default

A destructor for nav2_route::PathConverter.

void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node)

Configure the object.

Parameters:

nodeNode to use to get params and create interfaces

nav_msgs::msg::Path densify(const Route &route, const ReroutingState &rerouting_info, const std::string &frame, const rclcpp::Time &now)

Convert a Route into a dense path.

Parameters:
  • routeRoute object

  • rerouting_info – Re-Route info in case partial path to be populated

  • frame – Frame ID from planning

  • now – Time

Returns:

Path of the route

void interpolateEdge(float x0, float y0, float x1, float y1, std::vector<geometry_msgs::msg::PoseStamped> &poses)

Convert an individual edge into a dense line.

Parameters:
  • x0 – X initial

  • y0 – Y initial

  • x1 – X final

  • y1 – Y final

  • poses – Pose vector reference to populate

Protected Attributes

rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr path_pub_
rclcpp::Logger logger_ = {rclcpp::get_logger("PathConverter")}
float density_
float smoothing_radius_
bool smooth_corners_