目的：丢弃move_base，使用自己的DWA，本着不造轮子的想法（我也造不出轮子），大部分源码用大佬们的开源，更关注修改整合后的效果
研究路径：
- 1.基于之前的Astar和Bspline得到全局路径
- 2.使用turtlebot3两轮全向小车模型
- 3.基于clean_turtle3_robot的next_goal发布逻辑
- 4.基于dwa_planner项目的dwa算法
- 5.基于ipa项目的形态学分割和牛耕覆盖算法
- 6.牛耕覆盖路径优化（分段平滑处理）
- 7.整合所有
前情回顾：
先前完成了上述(1-3)+move_base的仿真，也完成了5的修改，目前想法是将move_base丢弃采用单独的dwa，也是为了之后在实机上使用的时候更加方便，更不依赖ros（最后的话估计要丢掉ros，裁剪出消息机制）
代码仓库（9.30开始更新）：
https://gitee.com/upcgyl/dwa_-clean_-robot.git
9.30 更新
当前问题：直接将dwa丢入工程内后，出现一些不适配项目的问题，后续将从下述现象修改：
- 1.机器人起点方向不确定导致第一个目标点DWA规划的路径较差，拟修改初始机器人目标点为当前点且方向为当前点到第一个目标点的方向
- 2.该算法逻辑为到达目标点前减速，由于已经有全局路径，所以尽量保证在整个过程中匀速运动，拟修改calc_cmd_vel函数中后半部分
- 3.到达目标点时出现原地打转现象，后续查找原因
- 4.next_goal和dwa中均存在到达目标点判断，后续思考是否加入状态机制避免重复判断
10.11 更新
代码仓库修改：将next_goal分离为单独的功能包，新增分区和覆盖的依赖代码
对于9.30的4个问题先不做考虑，将next_goal的到达距离减小后可以在视觉上稍微优化2现象，先考虑分割路径如何发布目标点，由于bcpp中没有设计每个区域之间连接的方式，所以第一步需要将各区域之间通过planner连接然后将所有点作为goal进行发布先看一下效果，存在几个明显的问题
- 1.分区后的单独区域覆盖的路径存在穿墙现象导致机器人撞墙dwa死机
- 2.转弯效果不好，覆盖率也不够（先不考虑外围代价地图区域）
- 3.机器人初始位置不是（0,0）坐标
思考，是否可以从这几个方向进行修改：
1.减小分区的最大面积阈值，使分得的区域更加的小
2.对房间进行膨胀
3.联通距离小于机器人的障碍物
10.14更新
解决问题3机器人初始坐标设置问题
在代码中，我认为地图的中心点为机器人的初始点，于是将start_point设置为msg.info.origin.position.x + msg.info.width * 0.5 而在amcl和gazebo中我的initial_pose 为(0,0)，这是不正确的，事实上，我如果想让机器以房间的中心为初始点的话，那我的initial_pose应该设置为：(msg.info.origin.position.x + msg.info.width 0.5) map_resolution + msg.info.origin.position.x，例如我的地图大小是384X384，分辨率是0.05，地图起始坐标为(-10,-10)，那么我的中点坐标应该为（-10+384/2）x0.05 - 10 也就是（-0.9，-0.9）
对于问题1 bcpp穿墙现象
尝试将每两个点都用A * 去规划，效果很差，同时会增加没用的距离消耗，该方案取消
git分支T10_14：修改bcpp传入参数后效果：
当前留存的大问题：
1.运动时的路径与实际规划存在位置偏差；
2.转角点较少且没有进行路径优化
3.初始角度问题
4.在全房间的时候依旧会出现穿墙的效果
效果解释：
房间边界留出的空间用来后续补充沿边规划的算法，因为扫地机器人当前的算法一般都是先沿边后覆盖
10.15更新
/ 该部分修改实际测试无用，可以直接跳过看下一部分 /
删掉next_goal中的path_callback中的自己修改过的一些东西后，初始不会出现转角效果较差的问题了，会根据目标点先转方向后移动
更新后的path_callback:
void path_callback(const nav_msgs::Path &path)
{
ROS_INFO("path_callback Success!");
if ((planned_path.Path.size() == 0) || (path.poses.size() != taille_last_path))
{
planned_path.Path.clear();
new_path = true;
/* // 使用路径消息的第一个点作为目标方向
if (path.poses.size() > 1) // 确保有至少两个点
{
float initial_angle = atan2(path.poses[1].pose.position.y - path.poses[0].pose.position.y,
path.poses[1].pose.position.x - path.poses[0].pose.position.x);
ROS_INFO("y1=%f, x1=%f, y0=%f, x0=%f", path.poses[1].pose.position.y, path.poses[1].pose.position.x,
path.poses[0].pose.position.y, path.poses[0].pose.position.x);
ROS_INFO("initial_angle=%f", initial_angle);
goal_msgs.header.frame_id = "odom";
goal_msgs.header.stamp = ros::Time::now();
goal_msgs.pose.position.x = path.poses[0].pose.position.x;
goal_msgs.pose.position.y = path.poses[0].pose.position.y;
goal_msgs.pose.position.z = 0;
goal_msgs.pose.orientation = tf::createQuaternionMsgFromYaw(initial_angle);
// 发布初始目标方向
GoalPoint_pub.publish(goal_msgs);
} */
for (int i = 0; i < path.poses.size(); i++)
{
planned_path.addGoal(path.poses[i].pose.position.x, path.poses[i].pose.position.y, false);
}
taille_last_path = path.poses.size();
}
}修改min_cell_width计算方式为const int min_cell_width = half_grid_spacing_as_int;，
使min_cell_width变小，更多的区域不会被合并，cell合并更依赖于设定的面积大小，修改后路径：
单从该测试用例看效果已经优于之前的情况了
增加单个区域cell之间的连接，减少穿墙效果：主要修改computeBoustrophedonPath函数
10.15修改后的代码放到了T10_15分支上了，整体是跑不起来的，dwa部分还有问题，下班
10.18更新
把pathplan.cpp重构，修改为coverage_planner.cpp更加整洁，代码已推送至T10_18分支
目前牛耕覆盖还存在问题：
1.房间最开始的分割方向固定为主要方向，导致在测试地图的最中间的最大的房子分割效果不好，后续增加家具等后可能分割效果会好一些；
10.19
对于单个房间进行覆盖时算法还得改，考虑看如何将单个房间再划分为几个小区域，主要是一些不规则的墙造成的，类似于上图中的右下角的房间和中间房间的区别；造成这个现象的原因是牛耕的逻辑：牛耕进行区分cell是通过寻找突出的黑色像素，然后将这一行作为一个分割，这样就造成了中间房间的中间区域较长（实际上这个并没有问题），但是由于中间房间的左上和左下区域不满足要求，所以被合并到了中间区域。事实上，我觉得应该先将房间分为左右两个cell然后再进行横向的分割
10.19更新
增加两目标点之间的距离检测，超过0.2的话就补路径，这样的话不会穿墙，但是实际覆盖效率低
