35 #ifndef NAV_2D_UTILS_PATH_OPS_H 36 #define NAV_2D_UTILS_PATH_OPS_H 38 #include <nav_2d_msgs/Path2D.h> 45 double poseDistance(
const geometry_msgs::Pose2D& pose0,
const geometry_msgs::Pose2D& pose1);
50 double getPlanLength(
const nav_2d_msgs::Path2D& plan,
const unsigned int start_index = 0);
55 double getPlanLength(
const nav_2d_msgs::Path2D& plan,
const geometry_msgs::Pose2D& query_pose);
64 nav_2d_msgs::Path2D
adjustPlanResolution(
const nav_2d_msgs::Path2D& global_plan_in,
double resolution);
75 nav_2d_msgs::Path2D
compressPlan(
const nav_2d_msgs::Path2D& input_path,
double epsilon = 0.1);
84 void addPose(nav_2d_msgs::Path2D& path,
double x,
double y,
double theta = 0.0);
88 #endif // NAV_2D_UTILS_PATH_OPS_H void addPose(nav_2d_msgs::Path2D &path, double x, double y, double theta=0.0)
Convenience function to add a pose to a path in one line.
A set of utility functions for Bounds objects interacting with other messages/types.
double poseDistance(const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1)
Calculate the linear distance between two poses.
nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D &input_path, double epsilon=0.1)
Decrease the length of the plan by eliminating colinear points.
nav_2d_msgs::Path2D adjustPlanResolution(const nav_2d_msgs::Path2D &global_plan_in, double resolution)
Increase plan resolution to match that of the costmap by adding points linearly between points...
double getPlanLength(const nav_2d_msgs::Path2D &plan, const unsigned int start_index=0)
Calculate the length of the plan, starting from the given index.