teb.h 1.6 KB

123456789101112131415161718192021222324252627282930313233343536373839
  1. #include <iostream>
  2. #include <ros/ros.h>
  3. // #include <vector.h>
  4. #include <costmap_2d/costmap_2d_ros.h>
  5. #include "teb/path_smoother.h"
  6. #include <nav_msgs/Path.h>
  7. namespace teb_planner_ns
  8. {
  9. class TEB_Planner
  10. {
  11. public:
  12. TEB_Planner();
  13. ~TEB_Planner();
  14. void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros);
  15. bool makePlan(const geometry_msgs::PoseStamped &robot_pose, const std::vector<geometry_msgs::PoseStamped> &global_plan, std::vector<geometry_msgs::PoseStamped> &local_plan, const bool is_forward);
  16. private:
  17. std::vector<geometry_msgs::PoseStamped> global_plan_;
  18. std::vector<geometry_msgs::PoseStamped> local_plan_;
  19. path_smoother::PathSmoother path_smoother_;
  20. costmap_2d::Costmap2DROS *costmap_ros_;
  21. std::vector<double> duration_; // TEB每段轨迹的持续时间
  22. std::vector<geometry_msgs::PoseStamped> pre_local_plan_;
  23. geometry_msgs::PoseStamped robot_pose_;
  24. nav_msgs::Path pub_local_plan_;
  25. ros::Publisher plan_pub_;
  26. bool use_clamp_spline_flag;
  27. bool choiceLocalPlanPoint(std::vector<geometry_msgs::PoseStamped> &local_plan);
  28. void trajectoryOpt(const geometry_msgs::PoseStamped &robot_pose, std::vector<geometry_msgs::PoseStamped> &plan);
  29. void copyVectorPoseStamped(const std::vector<geometry_msgs::PoseStamped> &copied_vec, std::vector<geometry_msgs::PoseStamped> &vec);
  30. void myWorldToMap(std::vector<geometry_msgs::PoseStamped> &local_plan);
  31. void myMapToWorld(std::vector<geometry_msgs::PoseStamped> &local_plan);
  32. };
  33. };