2d_costmap.h 9.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299
  1. // 导航代价图,用于机器人路径规划和自动避障
  2. // 作者:HeJunzhen
  3. // 时间:2020/07/15
  4. // 成都河狸智能科技有限责任公司
  5. #ifndef NAVIGATION_COSTMAP_ROS_H_
  6. #define NAVIGATION_COSTMAP_ROS_H_
  7. #include <queue>
  8. #include <vector>
  9. #include <string>
  10. #include <boost/thread.hpp>
  11. #include <boost/thread/mutex.hpp>
  12. #include <ros/ros.h>
  13. #include <geometry_msgs/Pose2D.h>
  14. #include <sensor_msgs/PointCloud.h>
  15. #include <nav_msgs/OccupancyGrid.h>
  16. #include "common/common.h"
  17. #include <std_msgs/UInt32.h>
  18. typedef uint32_t uint32;
  19. typedef unsigned char uchar;
  20. namespace nav
  21. {
  22. namespace costmap
  23. {
  24. // 栅格
  25. struct Cell
  26. {
  27. Cell(float dist, int index, uint32 x, uint32 y, uint32 sx, uint32 sy) : distance_(dist), index_(index), x_(x), y_(y), sx_(sx), sy_(sy) {}
  28. Cell() {}
  29. float distance_; // 到障碍物的距离
  30. uint32 index_; // 地图数组索引号
  31. uint32 x_, y_, sx_, sy_; // x_:x坐标 y_:y坐标 sx_:最近障碍物x坐标 sy_:最近障碍物y坐标
  32. };
  33. inline bool operator<(const Cell &a, const Cell &b)
  34. {
  35. return a.distance_ > b.distance_;
  36. }
  37. // 代价值定义
  38. enum COST_VALUE
  39. {
  40. PERDEFINED_PLAN = 0, // 预规划路径在代价地图代价值
  41. FREE_SPACE = 100, // 空闲区域代价值
  42. INSCRIBED_INFLATED_OBSTACLE = 253, // 地图边界代价值
  43. LETHAL_OBSTACLE, // 障碍物代价值
  44. NO_INFORMATION, // 未知区域代价值
  45. };
  46. class Pose
  47. {
  48. float x;
  49. float y;
  50. float theta;
  51. };
  52. // 机器人定位点,需要计算连续两次数据的时间差来判断当前数据是否可用
  53. class PoseWithTime : public Pose
  54. {
  55. public:
  56. ros::Time time; // 记录收到数据时的时间
  57. bool use; // 数据是否可用
  58. };
  59. // 代价地图
  60. class CostMap
  61. {
  62. public:
  63. CostMap(CostmapInfo costmap_config); // 传入代价地图配置信息
  64. ~CostMap();
  65. public:
  66. void restart(); // 收到新的地图需要重启动代价图
  67. void run(); // 代价图需要接收到地图后才能启动,否则一直等待
  68. void stop(); // 停止
  69. void deleteAll(); // 释放代价图内存
  70. // 获取机器人当前位姿
  71. // 传入:pose(引用)位姿
  72. // 返回:true:当前数据可用 false:当前数据不可用
  73. bool getRobotPose(Pose &pose);
  74. // 预设置代价图路径
  75. // 输入:路径
  76. // 返回:true:设置成功 false:设置失败
  77. bool setCostPath(const Path path);
  78. // 设置代价地图栅格的代价值
  79. // 输入:x:栅格的x坐标 y:栅格的y坐标 value:设置的代价值 mandatory:是否强制设置(覆盖原先障碍物 true:强制 false:不强制)
  80. // 返回:true:设置成功 false:设置失败
  81. bool setCostValue(uint32 x, uint32 y, uchar value, bool mandatory = false);
  82. // 获取栅格内的代价值
  83. // 输入: x:栅格x坐标 y:栅格y坐标
  84. // 返回: 栅格的代价值
  85. uchar getCost(uint32 x, uint32 y);
  86. // 获取整个代价地图
  87. // 返回: 整个代价地图的指针
  88. uchar *getCostmap();
  89. // 获取代价地图X轴的长度
  90. // 返回: 代价地图X轴的长度
  91. uint32 getSizeInCellsX() { return size_x_; };
  92. // 获取代价地图Y轴的长度
  93. // 返回: 代价地图Y轴的长度
  94. uint32 getSizeInCellsY() { return size_y_; };
  95. // 获取代价地图原点坐标的X值
  96. // 返回: 代价地图原点坐标的X值
  97. float getOriginX() { return origin_x_; };
  98. // 获取代价地图原点坐标的Y值
  99. // 返回: 代价地图原点坐标的Y值
  100. float getOriginY() { return origin_y_; };
  101. // 获取代价地图的分辨率
  102. // 返回: 代价地图的分辨率
  103. float getResolution() { return resolution_; };
  104. // 世界坐标转地图坐标
  105. // 输入: wx:世界坐标X值 wy:世界坐标Y值 mx:地图坐标X值(引用) my:地图坐标Y值(引用)
  106. // 输出: true:转换成功 false:转换失败
  107. bool worldToMap(float wx, float wy, uint32 &mx, uint32 &my);
  108. // 地图坐标转世界坐标
  109. // 输入: mx:地图坐标X值 my:地图坐标Y值 wx:世界坐标X值(引用) wy:世界坐标Y值(引用)
  110. // 输出: true:转换成功 false:转换失败
  111. bool mapToWorld(uint32 mx, uint32 my, float &wx, float &wy);
  112. private:
  113. void initMap(); // 初始化代价地图
  114. void resetMap(); // 清空代价地图
  115. void pubCostmap(); // 发布代价地图
  116. // 计算距离内有几个栅格数
  117. // 输入: dist:长度距离
  118. // 返回: 长度距离内的栅格数
  119. int cellDistance(float dist);
  120. // 根据距离边界或障碍物的长度计算代价值
  121. // 输入: distance:距离边界或障碍物的长度
  122. // 返回: 计算的代价值
  123. uchar computeCost(float distance);
  124. // 代价区域核(原理跟高斯核一样)
  125. void getInflationAreaCost();
  126. // 将传入地图转换为代价地图
  127. // 输入: map:原始地图
  128. // 返回: 对应的代价地图
  129. uchar *setInflationMap(uchar *map);
  130. // 计算栅格代价值
  131. // 输入: mx:栅格x坐标 my:栅格y坐标 src_x:最近障碍物x坐标 src_y:最近障碍物y坐标
  132. uchar costLookup(int mx, int my, int src_x, int src_y);
  133. // 计算两个栅格的距离
  134. // 输入: mx:a栅格x坐标 mx:a栅格y坐标 mx:b栅格x坐标 mx:b栅格y坐标
  135. // 返回: a栅格到b栅格的距离
  136. float distanceLookup(int mx, int my, int src_x, int src_y);
  137. // 计算障碍物点在膨胀范围内的栅格,如果在则存储
  138. // 输入: index当前栅格在地图内的坐标 mx:栅格x值 my:栅格y值 src_x:最近障碍物x值 src_y:最近障碍物y值
  139. void enqueue(int index, int mx, int my, int src_x, int src_y);
  140. // 初始地图代价值赋值
  141. // 输入: value 原始地图栅格值
  142. // 返回: 对应的代价值
  143. uchar interpretValue(uchar value);
  144. private:
  145. // 接收地图
  146. void incomingMap(const nav_msgs::OccupancyGridConstPtr &map);
  147. // 接收机器人定位后坐标
  148. void locationCallback(const geometry_msgs::Pose2DConstPtr &pose);
  149. // 接收传感器点云数据
  150. void sensorCloudCallback(const sensor_msgs::PointCloudConstPtr &cloud);
  151. // 更新代价地图
  152. void updateCostMap();
  153. private:
  154. // 代价地图(含传感器数据)
  155. uchar *costmap_;
  156. // 原始代价地图(不含传感器数据)
  157. uchar *originCostmap_;
  158. std::vector<ros::Time> obstacleUpdateTimeMap_;
  159. // 原始地图
  160. uchar *originmap_;
  161. // 遍历地图
  162. bool *seen_;
  163. // 地图传入开始标志位
  164. bool run_;
  165. // 腐蚀半径(低代价值) 小于等于这个距离的代价根据距离计算
  166. float inflation_radius_;
  167. // 腐蚀半径(高代价值) 小于等于这个距离的代价 = 253
  168. float inscribe_radius_;
  169. // 腐蚀半径内的栅格数
  170. uint32 inflation_cells_;
  171. uint32 inscribe_cells_;
  172. float sensor_duration_;
  173. // 地图分辨率
  174. float resolution_;
  175. // 地图X轴长度
  176. uint32 size_x_;
  177. // 地图Y轴长度
  178. uint32 size_y_;
  179. // 代价图原点X坐标
  180. float origin_x_;
  181. // 代价图原点Y坐标
  182. float origin_y_;
  183. // 代价图更新频率
  184. uint32 frequency_;
  185. // 定位数据读写锁
  186. boost::shared_mutex locat_mutex_;
  187. // 传感器数据读写锁
  188. boost::shared_mutex sensor_mutex_;
  189. // 代价图读写锁
  190. boost::shared_mutex costmap_mutex_;
  191. // 代价地图更新线程
  192. boost::thread *update_cost_;
  193. // 机器人当前坐标
  194. PoseWithTime robot_pose_;
  195. // 腐蚀高斯核存储
  196. uchar **cached_cost_;
  197. float **cached_dist_;
  198. // 腐蚀距离内的栅格存储
  199. space::BlockQueue<Cell> bin_cells_;
  200. // 传感器点云数据
  201. std::vector<Cell> sensor_cells_;
  202. // 预设值的代价路径
  203. std::vector<int> cost_path_;
  204. private:
  205. ros::NodeHandle nh_;
  206. // 代价地图发布
  207. ros::Publisher costmap_publisher_;
  208. // 原始地图接收
  209. ros::Subscriber map_subscriber_;
  210. // 传感器数据接收
  211. ros::Subscriber sensor_subscriber_;
  212. // 定位坐标接收
  213. ros::Subscriber location_subscriber_;
  214. };
  215. }
  216. }
  217. #endif