¶ 智能清洁机器人（地面清洁）调试指南
¶ 竞赛介绍
智能清洁机器人是一种专门设计用于执行清洁任务的自动化机器人，又被称为环境提升机器人，它们被广泛应用于各类社区服务环境中，如商场、办公楼、酒店等，旨在减轻人力负担，提高清洁效率，并提升场景的卫生状况。智能清洁机器人通过搭载激光雷达，立体视觉相机、超声/TOF模块等感知传感器、搭配机载自主定位系统，路径规划系统和智能算法来自主感知周围环境和自主导航完成清洁任务。它们能够识别障碍物、避免碰撞，并规划有效地清洁路径。智能清洁机器人可以用于多种清洁任务，包括地面清洁、桌面清洁、立面清洁、卫生间清洁等。
为了贴合智能清洁机器人的行业前景，提升学生相关技术深度与动手实践能力，设立智能清洁机器人比赛。通过这个比赛，期望参赛者能够展示出创新的机器人技术和解决当前清洁机器人行业实际问题的能力，为智能清洁领域的发展作出贡献。
¶ 竞赛场地及说明
编写智能移动机器人的程序，完成对指定区域的清洁，同时绕过不可清洁垃圾并记录拍照，机器人从出发点出发，清理可清洁区域的垃圾，绕过不可清洁区域的垃圾，完成清扫任务
¶ 比赛例程使用说明
比赛例程已经封装好了一个单独的ros功能包里,为demo_ws下的demo_clean_robot功能包，下面所有程序改动都在该功能包里修改：
- 参照软件上手指南里的Cartographer建图，运行8-cartographer.sh文件，使用键盘控制机器人运动，对比赛场地进行建图，保存；
- 进入robot_shell/w2c目录下，运行14-demo_clean_robot.sh即可启动比赛demo程序：
- 将机器人放到起始位置，运行命令，任务即可启动：
rostopic pub /clean_task std_msgs/String "data: 'start'"
- 关于清洁机器人路径规划，我们根据比赛规则手动对路径进行规划，如图所示：
接下来，在clean_task.py中的 init_path函数中，我们手动将航点通过列表来进行维护，其中列表内四个参数分别为
x、y、yaw、flag，其中位置信息可以通过导航中使用键盘移动同时订阅 amcl_pose话题来获取：
rostopic echo /amcl_pose
（这里yaw角度填入的是角度，需要提前转换下，也可以自己改动程序，直接输入四元数，或者自己预估一个角度也行）
# 初始化导航路径，这里用坐标点来进行路径规划，模拟扫地机器人全覆盖路径规划
def init_path(self):
self.path = []
# 这里路径列表存储的是元祖对象，参数分别为 x,y,yaw,flag 0：表示清扫区域点位，1：表示识别障碍物区域点位 2：表示绕开不可清扫范围区域的点
self.path.append([0.77, 0, 0, 1])
self.path.append([1.25, 0, 0, 0])
self.path.append([1.75, 0, 0, 1])
self.path.append([2.15, 0, -90, 0])
self.path.append([2.15, -0.3, -90, 1])
self.path.append([2.15, -0.7, -90, 0])
self.path.append([2.15, -1, -90, 1])
self.path.append([2.15, -1.3, -180, 0])
self.path.append([1.6, -1.3, -180, 1])
self.path.append([1.25, -1.3, 90, 0])
self.path.append([1.25, -1.0, 90, 1])
self.path.append([1.25, -0.7, 180, 0])
self.path.append([0.61, -0.7, -180, 1])
self.path.append([0.22, -0.7, -90, 0])
self.path.append([0, -1.3, -90, 0])
视觉识别不可清扫垃圾，这里我们使用的是基于OpenCV的颜色、形状识别（这里使用的是红色），来确定垃圾物料块在摄像头画面中的位置；OpenCV的颜色、形状识别这里我们使用HSV颜色空间来识别图像内的物料颜色，同时使用OpenCV形态学检测，霍夫圆检测来检测图像内物料块颜色的圆形物体，为了方便调试，我们在摄像头画面上嵌入了拖动条组件，并且单击画面中的要识别的物料，可以在控制台打印该物料块的HSV值：
 |
 |
|---|
根据控制台打印的HSV值，拖动窗口中的按钮，来确定一个范围，并且将该值记录到程序中：
- 障碍物识别后，机器人还需要自动避让不可清洁区域，因此，我们把导航相关参数也放到了功能包下：
我们使用深度相机来识别以及避障，在代价地图中增加一层camera_layer，以下是代价地图参数 costmap_common_params.yaml:
# 深度相机识别的区域
camera_polygon: [ [ 0.15, 0.1 ], [ 0.15, -0.1 ], [ 0.5, -0.1 ], [ 0.5, 0.1 ] ]
# 机器人半径
robot_radius: 0.1
#设置膨胀层参数
#根据obstacle_layer、static_layer和footprint生成代价地图
inflation_layer:
#是否开启膨胀层
enabled: true
#代价地图数值随与障碍物距离下降的比值，越大会导致路径规划越靠近障碍物
cost_scaling_factor: 2.4
#机器人膨胀半径，影响路径规划，单位：m
inflation_radius: 1.2
#设置静态层参数
static_layer:
#是否开启静态层
enabled: true
#静态层的订阅的地图话题
map_topic: map
#地图话题中数据值为多少，会转换为静态层代价地图中的未知区域
unknown_cost_value: -1
#地图话题中数据值为多少，会转换为静态层代价地图中的完全占用区域
lethal_cost_threshold: 100
#是否仅把第一次订阅到的地图数据转换为静态层代价地图，无视后续订阅到的地图数据
first_map_only: false
#是否订阅话题 “map_topic”+“_updates”
subscribe_to_updates: false
#如果设置为false，地图话题中的未知区域在代价地图中会转换为自由区域
track_unknown_space: true
#如果设置为true，静态层代价地图只有未知、自由和完全占用三种情况
#如果设置为false，静态层代价地图可以有不同的占用程度
trinary_costmap: true
#设置障碍层参数
obstacle_layer:
#是否开启障碍层
enabled: true
#代价地图的高度
origin_z: 0.0
#高于多少的障碍物不加入观测范围，单位：m
max_obstacle_height: 0.35
#低于多少的障碍物不加入观测范围，单位：m
min_obstacle_height: 0.05
#障碍层的Z轴方格的高度
z_resolution: 0.05
#障碍层Z轴上有几个方格
z_voxels: 16
#被认为是“已知”的列中允许的未知单元格数
unknown_threshold: 15
#被认为是“自由”的列中允许的标记单元格数
mark_threshold: 0
#障碍层如何与其他地图层处理的方法。
#0：障碍层覆盖其它地图层； 1：障碍物最大化方法，即各层的占用方格会覆盖其他层的自由方格，这是最常用的方法
#99：不改变其它地图层，应该是使障碍层层无效的方法
combination_method: 1
#如果设置为true，障碍层代价地图会有未知、自由和完全占用三种情况
#如果设置为false，障碍层代价地图只有自由和完全占用两种情况
track_unknown_space: true
#多少范围内障碍物会被加入代价地图，单位：m
obstacle_range: 1.5
#多少范围内障碍物会被追踪，单位：m
raytrace_range: 2.0
#是否发布障碍层的投影地图层话题
publish_voxel_map: false
#设置障碍层的观测源名称，可以一次设置多个观测源observation_sources: scan, scan2, camera
observation_sources: scan
#设置对应观测源参数
scan:
#观测源数据话题名称
topic: scan
#观测源话题的数据格式，可以为LaserScan、PointCloud、PointCloud2
data_type: LaserScan
#是否使用该观测源清除自由空间
clearing: true
#是否使用该观测源添加障碍物
marking: true
# 深度相机图层
camera_layer:
data_type: PointCloud2
enabled: true
topic: "/cloud_concatenated"
pixel_decay: 20.0
- 接下来任务开始，机器人移动到目标点位置，通过ros service形式来请求不可清洁障碍物识别，服务端识别到不可清扫垃圾后，返回状态，并记录拍照（obstacle_detect.py）：
# 障碍物识别服务结果返回，并拍照记录
def detect_task_callback(self, msg):
self.isDetect = False
time.sleep(2)
if self.isDetect:
self.take_photo()
self.isDetect = False
return "1"
else:
return "0"
# 拍照
def take_photo(self):
file_name = calendar.timegm(time.gmtime())
path = "/home/bcsh/magic_ws/src/demo_clean_robot/photo/{}.jpg".format(file_name)
cv2.imwrite(path,self.img)
- 客户端收到反馈后，如果识别到不可清扫垃圾，则删除清扫区域航点，机器人前往下一个区域（clean_task.py）：
# 导航到某个目标点
def goto(self, p, index):
print("current x = {}, y= {}".format(p[0], p[1]))
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = p[0]
goal.target_pose.pose.position.y = p[1]
goal.target_pose.pose.position.z = 0
q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)
result = self.move_base.wait_for_result(rospy.Duration(60))
state = self.move_base.get_state()
# 通知机器人，到达目标点
if state == GoalStatus.SUCCEEDED:
print("arrive x = {}, y= {}".format(p[0], p[1]))
if p[3] == 1:
rospy.wait_for_service('/obstacle_detect_service')
query_service = rospy.ServiceProxy("/obstacle_detect_service", Obstacle)
response = query_service("detect")
if response.result == "1":
print("find obstacle")
# 识别到障碍物，删除清扫区域坐标点
del self.path[index]
rospy.sleep(1)
¶ 程序流程
