123456789101112131415161718192021222324252627282930313233343536373839 |
- #include <iostream>
- #include <ros/ros.h>
- // #include <vector.h>
- #include <costmap_2d/costmap_2d_ros.h>
- #include "teb/path_smoother.h"
- #include <nav_msgs/Path.h>
- namespace teb_planner_ns
- {
- class TEB_Planner
- {
- public:
- TEB_Planner();
- ~TEB_Planner();
- void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros);
- 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);
- private:
- std::vector<geometry_msgs::PoseStamped> global_plan_;
- std::vector<geometry_msgs::PoseStamped> local_plan_;
- path_smoother::PathSmoother path_smoother_;
- costmap_2d::Costmap2DROS *costmap_ros_;
- std::vector<double> duration_; // TEB每段轨迹的持续时间
- std::vector<geometry_msgs::PoseStamped> pre_local_plan_;
- geometry_msgs::PoseStamped robot_pose_;
- nav_msgs::Path pub_local_plan_;
- ros::Publisher plan_pub_;
- bool use_clamp_spline_flag;
- bool choiceLocalPlanPoint(std::vector<geometry_msgs::PoseStamped> &local_plan);
- void trajectoryOpt(const geometry_msgs::PoseStamped &robot_pose, std::vector<geometry_msgs::PoseStamped> &plan);
- void copyVectorPoseStamped(const std::vector<geometry_msgs::PoseStamped> &copied_vec, std::vector<geometry_msgs::PoseStamped> &vec);
- void myWorldToMap(std::vector<geometry_msgs::PoseStamped> &local_plan);
- void myMapToWorld(std::vector<geometry_msgs::PoseStamped> &local_plan);
- };
- };
|