35 #ifndef DLUX_GLOBAL_PLANNER_TRACEBACK_H 36 #define DLUX_GLOBAL_PLANNER_TRACEBACK_H 42 #include <nav_2d_msgs/Path2D.h> 79 const geometry_msgs::Pose2D& start,
const geometry_msgs::Pose2D& goal,
80 double& path_cost) = 0;
90 nav_2d_msgs::Path2D world_path;
91 world_path.header.frame_id = info.
frame_id;
92 world_path.poses.resize(original.poses.size());
93 for (
unsigned int i = 0; i < original.poses.size(); i++)
104 #endif // DLUX_GLOBAL_PLANNER_TRACEBACK_H
nav_2d_msgs::Path2D mapPathToWorldPath(const nav_2d_msgs::Path2D &original, const nav_grid::NavGridInfo &info)
Helper function to convert a path in Grid coordinates to World coordinates.
virtual ~Traceback()=default
Plugin interface for using the potential to trace a path from the start position back to the goal pos...
virtual void initialize(ros::NodeHandle &private_nh, CostInterpreter::Ptr cost_interpreter)
Initialize function.
CostInterpreter::Ptr cost_interpreter_
std::shared_ptr< CostInterpreter > Ptr
virtual nav_2d_msgs::Path2D getPath(const PotentialGrid &potential_grid, const geometry_msgs::Pose2D &start, const geometry_msgs::Pose2D &goal, double &path_cost)=0
Create the path from start to goal.