42 const geometry_msgs::Pose2D &pose,
43 const nav_2d_msgs::Twist2D &vel,
44 const geometry_msgs::Pose2D &goal,
45 const nav_2d_msgs::Path2D &global_plan) {
55 unsigned int start_index = 0;
56 double distance_to_start = std::numeric_limits<double>::infinity();
57 bool started_path =
false;
58 for (
unsigned int i = 0; i < plan.size(); i++) {
59 double g_x = plan[i].x;
60 double g_y = plan[i].y;
61 unsigned int map_x, map_y;
66 if (distance_to_start > distance) {
68 distance_to_start = distance;
75 }
else if (started_path) {
83 ROS_ERROR_NAMED(
"PathDistPrunedCritic",
"None of the points of the global plan were in the local costmap.");
90 nav_2d_msgs::Path2D global_plan_pruned;
91 global_plan_pruned.header = global_plan.header;
92 global_plan_pruned.poses = std::vector<geometry_msgs::Pose2D>(
93 plan.begin() + start_index,
bool prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel, const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan) override
nav_core2::BasicCostmap costmap
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
static const unsigned char NO_INFORMATION
Scores trajectories based on how far from the global path they end up, where the global path is prune...
double poseDistance(const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1)
nav_core2::Costmap::Ptr costmap_
nav_2d_msgs::Path2D adjustPlanResolution(const nav_2d_msgs::Path2D &global_plan_in, double resolution)
#define ROS_ERROR_NAMED(name,...)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool worldToGridBounded(const NavGridInfo &info, double wx, double wy, unsigned int &mx, unsigned int &my)
virtual bool prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel, const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan) override