99 lines
2.7 KiB
C++
99 lines
2.7 KiB
C++
/***
|
|
* @brief: cleaning robot path planning
|
|
* @author: Wang
|
|
* @date: 20170702
|
|
***/
|
|
|
|
#ifndef CLEANINGPATHPLANNING_H
|
|
#define CLEANINGPATHPLANNING_H
|
|
|
|
#include <opencv2/opencv.hpp>
|
|
#include <opencv2/core/core.hpp>
|
|
#include <opencv2/highgui/highgui.hpp>
|
|
#include <opencv2/imgproc/imgproc.hpp>
|
|
#include <vector>
|
|
#include <string>
|
|
|
|
#include "tf/tf.h"
|
|
#include "tf/transform_listener.h"
|
|
#include <costmap_2d/costmap_2d.h>
|
|
#include <costmap_2d/costmap_2d_ros.h>
|
|
#include <geometry_msgs/PoseStamped.h>
|
|
#include <nav_msgs/Path.h>
|
|
|
|
using namespace cv;
|
|
using namespace std;
|
|
|
|
constexpr double PI =3.14159;
|
|
|
|
struct cellIndex
|
|
{
|
|
int row;
|
|
int col;
|
|
double theta; //{0, 45,90,135,180,225,270,315} 角度信息 hjr 注
|
|
};
|
|
|
|
/*************************************************
|
|
*
|
|
* 读取栅格地图并根据占据信息获取其对应的空闲（可行走）空间，
|
|
* 按照遍历算法规划行走路线。
|
|
*
|
|
* **********************************************/
|
|
class CleaningPathPlanning
|
|
{
|
|
public:
|
|
//CleaningPathPlanning() = delete;
|
|
CleaningPathPlanning(costmap_2d::Costmap2DROS *costmap2d_ros);
|
|
|
|
vector<geometry_msgs::PoseStamped> GetPathInROS();
|
|
vector<geometry_msgs::PoseStamped> GetBorderTrackingPathInROS();
|
|
|
|
void SetCoveredGrid(double wx,double wy);
|
|
int GetSizeOfCell(){return this->SIZE_OF_CELL;}
|
|
|
|
//for visualization
|
|
void PublishCoveragePath();
|
|
void PublishGrid();
|
|
private:
|
|
//helper functions.
|
|
bool initializeMats();
|
|
bool initializeCoveredGrid();
|
|
void getCellMatAndFreeSpace(Mat srcImg, Mat &cellMat,vector<cellIndex> &freeSpaceVec);
|
|
void initializeNeuralMat(Mat cellMat, Mat neuralizedMat);
|
|
void writeResult(Mat resultmat,vector<cellIndex> pathVec);
|
|
void writeResult(cv::Mat resultmat,std::vector<cv::Point2i> pathVec);
|
|
void mainPlanningLoop();
|
|
double distance(Point2i pta,Point2i ptb);
|
|
bool findElement(vector<cv::Point2i> pointsVec,cv::Point2i pt, int&index);
|
|
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
|
|
bool cellContainsPoint(cv::Point2i pt,cellIndex cell);
|
|
|
|
void GetBorderTrackingPathInCV(vector<cv::Point2i>&resultVec);
|
|
vector<cellIndex> GetPathInCV();
|
|
|
|
|
|
bool initialized_;
|
|
Mat srcMap_;
|
|
Mat cellMat_;
|
|
Mat neuralizedMat_;
|
|
vector<cellIndex> freeSpaceVec_;
|
|
vector<cellIndex> pathVec_;
|
|
vector<geometry_msgs::PoseStamped> pathVecInROS_;
|
|
|
|
double resolution_;
|
|
ros::Publisher plan_pub_;
|
|
ros::Publisher grid_pub_;
|
|
nav_msgs::OccupancyGrid covered_path_grid_;
|
|
|
|
//tf::TransformListener &tf_;
|
|
tf::Stamped<tf::Pose> initPose_;
|
|
|
|
costmap_2d::Costmap2D* costmap2d_;
|
|
costmap_2d::Costmap2DROS* costmap2d_ros_;
|
|
|
|
int SIZE_OF_CELL; //must be odd number.
|
|
int GRID_COVERED_VALUE;
|
|
};
|
|
|
|
#endif // CLEANINGPATHPLANNING_H
|