37 #ifndef BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_ 38 #define BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_ 41 #include <nav_msgs/Odometry.h> 42 #include <nav_msgs/Path.h> 43 #include <geometry_msgs/PoseStamped.h> 44 #include <geometry_msgs/Twist.h> 45 #include <geometry_msgs/Point.h> 88 void prunePlan(
const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan);
101 const std::vector<geometry_msgs::PoseStamped>& global_plan,
102 const geometry_msgs::PoseStamped& global_robot_pose,
104 const std::string& global_frame,
105 std::vector<geometry_msgs::PoseStamped>& transformed_plan);
116 const std::vector<geometry_msgs::PoseStamped>& global_plan,
117 const std::string& global_frame,
118 geometry_msgs::PoseStamped &goal_pose);
134 const std::vector<geometry_msgs::PoseStamped>& global_plan,
136 const std::string& global_frame,
137 geometry_msgs::PoseStamped& global_pose,
138 const nav_msgs::Odometry& base_odom,
139 double rot_stopped_vel,
double trans_stopped_vel,
140 double xy_goal_tolerance,
double yaw_goal_tolerance);
149 bool stopped(
const nav_msgs::Odometry& base_odom,
150 const double& rot_stopped_velocity,
151 const double& trans_stopped_velocity);
bool transformGlobalPlan(const tf2_ros::Buffer &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const geometry_msgs::PoseStamped &global_robot_pose, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, std::vector< geometry_msgs::PoseStamped > &transformed_plan)
Transforms the global plan of the robot from the planner frame to the frame of the costmap...
bool getGoalPose(const tf2_ros::Buffer &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const std::string &global_frame, geometry_msgs::PoseStamped &goal_pose)
Returns last pose in plan.
double getGoalPositionDistance(const geometry_msgs::PoseStamped &global_pose, double goal_x, double goal_y)
return squared distance to check if the goal position has been achieved
bool stopped(const nav_msgs::Odometry &base_odom, const double &rot_stopped_velocity, const double &trans_stopped_velocity)
Check whether the robot is stopped or not.
double getGoalOrientationAngleDifference(const geometry_msgs::PoseStamped &global_pose, double goal_th)
return angle difference to goal to check if the goal orientation has been achieved ...
bool isGoalReached(const tf2_ros::Buffer &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, geometry_msgs::PoseStamped &global_pose, const nav_msgs::Odometry &base_odom, double rot_stopped_vel, double trans_stopped_vel, double xy_goal_tolerance, double yaw_goal_tolerance)
Check if the goal pose has been achieved.
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, const ros::Publisher &pub)
Publish a plan for visualization purposes.
void prunePlan(const geometry_msgs::PoseStamped &global_pose, std::vector< geometry_msgs::PoseStamped > &plan, std::vector< geometry_msgs::PoseStamped > &global_plan)
Trim off parts of the global plan that are far enough behind the robot.