实体机器人识别虚拟环境中障碍物

之前的内容已经实现了虚拟机器人识别实体机器人的功能，接下来就是实体机器人如何识别虚拟环境中的障碍物（包括虚拟环境中的障碍物和其他虚拟机器人）。

我做的是基于雷达的，所以主要要处理的是雷达的scan话题

我的虚拟机器人命名空间在Vir_wheeltec_01下，实体机器人的命名空间在wheeltec_01下。

因此需要将虚拟机器人的雷达/Vir_wheeltec_01/scan 内容与/wheeltec_01/scan进行融合，由于虚拟机器人的雷达范围更大，因此融合的逻辑是以/wheeltec_01/scan作为掩模，把所有的障碍物都封存在新的雷达话题中/wheeltec_01/fused_scan，这一部分的功能封存在实体机器人的工作空间下的turn_on_wheeltec_robot功能包的scan_filter.py文件中

#!/usr/bin/env python
# -*- coding: utf-8 -*-import rospy
import math
from sensor_msgs.msg import LaserScan
from message_filters import ApproximateTimeSynchronizer, Subscriber
from threading import Lockclass ScanFusionNode:def __init__(self):# 初始化发布器self.fused_scan_pub = rospy.Publisher('/wheeltec_01/fused_scan', LaserScan, queue_size=10)# 创建时间同步器real_sub = Subscriber('/wheeltec_01/scan', LaserScan)virtual_sub = Subscriber('/Vir_wheeltec_01/scan', LaserScan)self.ts = ApproximateTimeSynchronizer([real_sub, virtual_sub], queue_size=5, slop=0.05)self.ts.registerCallback(self.sync_callback)rospy.loginfo("scan_filter node initialized")def sync_callback(self, real_scan, virtual_scan):try:fused_scan = self.fuse_scans(real_scan, virtual_scan)self.fused_scan_pub.publish(fused_scan)except Exception as e:rospy.logerr("Fusion error: %s", str(e))def fuse_scans(self, real_scan, virtual_scan):# 参数校验if not self.validate_scans(real_scan, virtual_scan):raise ValueError("Invalid scan parameters")# 创建融合后的消息fused_scan = LaserScan()fused_scan.header = real_scan.headerfused_scan.header.stamp = rospy.Time.now()fused_scan.angle_min = real_scan.angle_minfused_scan.angle_max = real_scan.angle_maxfused_scan.angle_increment = real_scan.angle_incrementfused_scan.time_increment = real_scan.time_incrementfused_scan.scan_time = real_scan.scan_timefused_scan.range_min = real_scan.range_minfused_scan.range_max = real_scan.range_max# 计算虚拟扫描角度步长virtual_angle_step = (virtual_scan.angle_max - virtual_scan.angle_min) / len(virtual_scan.ranges)# 执行融合fused_ranges = []for i in range(len(real_scan.ranges)):# 获取实体数据real_range = real_scan.ranges[i]# 映射到虚拟扫描索引current_angle = real_scan.angle_min + i * real_scan.angle_incrementvirtual_idx = int((current_angle - virtual_scan.angle_min) / virtual_angle_step)virtual_idx = max(0, min(virtual_idx, len(virtual_scan.ranges)-1))virtual_range = virtual_scan.ranges[virtual_idx]# 有效性检查if math.isnan(real_range) or math.isinf(real_range):real_range = fused_scan.range_maxif math.isnan(virtual_range) or math.isinf(virtual_range):virtual_range = fused_scan.range_max# 融合逻辑fused_range = min(real_range, virtual_range) if virtual_range <= fused_scan.range_max else real_rangefused_ranges.append(fused_range)fused_scan.ranges = fused_rangesreturn fused_scandef validate_scans(self, real_scan, virtual_scan):# 校验角度范围是否重叠if (real_scan.angle_min > virtual_scan.angle_max) or (real_scan.angle_max < virtual_scan.angle_min):rospy.logwarn("Scan angle ranges do not overlap!")return Falsereturn Trueif __name__ == '__main__':try:rospy.init_node('scan_filter')node = ScanFusionNode()rospy.spin()except rospy.ROSInterruptException:pass

接着就是基于这个话题去生成cost_map

找到costmap_car_params.yaml文件，修改如下（主要是添加fused_scan，因为小车中原来是没有这一段的，move_base默认就是基于scan话题生成costmap，所以要显式修改）

# 机器人外形设置参数，直接影响代价地图
footprint: [[-0.133, -0.125], [-0.133, 0.125], [0.133, 0.125], [0.133, -0.125]]# 设置膨胀层参数，根据obstacle_layer、static_layer和footprint生成代价地图
inflation_layer:enabled: true  # 是否开启膨胀层cost_scaling_factor: 15  # 代价地图数值随与障碍物距离下降的比值，越大会导致路径规划越靠近障碍物inflation_radius: 0.25  # 机器人膨胀半径，影响路径规划，单位：m# 激光扫描传感器配置，确保配置正确的缩进
observation_sources: laser_scan_sensor
laser_scan_sensor:data_type: LaserScantopic: /wheeltec_01/fused_scanmarking: trueclearing: true

 由于修改后move_base会基于fused_scan生成cosmap所以要在movebase节点启动前就生成fused_scan雷达信息还需要修改实体小车的启动文件wxfnavigation.launch(可以teb补全，因为具体的我忘记了)，主要就是在启动雷达后加入（scan_filter.py要做成可执行文件 chomd +x 文件路径/文件名）

“<!-- 启动扫描数据融合节点 scan_filter.py -->
    <node name="scan_filter" pkg="your_package_name" type="scan_filter.py" output="screen">
      <remap from="scan" to="/wheeltec_01/scan" />
      <remap from="fused_scan" to="/wheeltec_01/fused_scan" />
    </node>”

<launch><!-- 使用命名空间 wheeltec_01 --><group ns="wheeltec_01"><!-- 开启机器人底层相关节点 同时开启导航功能 --><include file="$(find turn_on_wheeltec_robot)/launch/turn_on_wheeltec_robot.launch"><arg name="navigation" default="true"/></include><!-- turn on lidar 开启雷达 --><include file="$(find turn_on_wheeltec_robot)/launch/wheeltec_lidar.launch" /><!-- 启动扫描数据融合节点 scan_filter.py --><node name="scan_filter" pkg="your_package_name" type="scan_filter.py" output="screen"><remap from="scan" to="/wheeltec_01/scan" /><remap from="fused_scan" to="/wheeltec_01/fused_scan" /></node><!-- 设置需要用于导航的地图 --><arg name="map_file" default="$(find turn_on_wheeltec_robot)/map/WHEELTEC.yaml"/><node name="map_server_for_test" pkg="map_server" type="map_server" args="$(arg map_file)"/><!-- 开启用于导航的自适应蒙特卡洛定位 amcl --><include file="$(find turn_on_wheeltec_robot)/launch/include/amcl.launch" ><node pkg="tf" type="static_transform_publisher" name="wheeltec_world_to_global_01" args="-9 -4 0 0 0 0 world /map 100" /><!-- MarkerArray功能节点 --><node name='send_mark' pkg="turn_on_wheeltec_robot" type="send_mark.py"/><!-- move_base 节点检测 --><node name='node_detection' pkg="turn_on_wheeltec_robot" type="node_ping.py"><param name="node_name" type="string" value="/move_base"/></node></group> <!-- 结束命名空间 wheeltec_01 -->
</launch>

并且启动时，需要先在主机启动虚拟小车（gazebo）roslaunch wxfpublish testSimulation.launch

接着启动小车的导航文件roslaunch turn_on ... wxfnavigation.launch(可以teb补全，因为具体的我忘记了)

最终结果

此外地图文件更换成map，并且修改实体小车下map中的位置，把rviz中实体小车到world的映射改成0000就可以了