Nav2 Navigation Stack - kilted  kilted
ROS 2 Navigation Stack
Public Member Functions | Protected Attributes | List of all members
nav2_route::PathConverter Class Reference

An helper to convert the route into dense paths. More...

#include <nav2_route/include/nav2_route/path_converter.hpp>

Public Member Functions

 PathConverter ()=default
 A constructor for nav2_route::PathConverter.
 
 ~PathConverter ()=default
 A destructor for nav2_route::PathConverter.
 
void configure (nav2_util::LifecycleNode::SharedPtr node)
 Configure the object. More...
 
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. More...
 
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. More...
 

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_
 

Detailed Description

An helper to convert the route into dense paths.

Definition at line 37 of file path_converter.hpp.

Member Function Documentation

◆ configure()

void nav2_route::PathConverter::configure ( nav2_util::LifecycleNode::SharedPtr  node)

Configure the object.

Parameters
nodeNode to use to get params and create interfaces

Definition at line 27 of file path_converter.cpp.

◆ densify()

nav_msgs::msg::Path nav2_route::PathConverter::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_infoRe-Route info in case partial path to be populated
frameFrame ID from planning
nowTime
Returns
Path of the route

Definition at line 45 of file path_converter.cpp.

References nav2_route::CornerArc::getCornerEnd(), nav2_route::CornerArc::getCornerStart(), nav2_route::CornerArc::interpolateArc(), interpolateEdge(), and nav2_route::CornerArc::isCornerValid().

Here is the call graph for this function:

◆ interpolateEdge()

void nav2_route::PathConverter::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
x0X initial
y0Y initial
x1X final
y1Y final
posesPose vector reference to populate

Definition at line 137 of file path_converter.cpp.

Referenced by densify().

Here is the caller graph for this function:

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