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

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

我做的是基于雷达的，所以主要要处理的是雷达的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 Lock

class 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.header
        fused_scan.header.stamp = rospy.Time.now()
        fused_scan.angle_min = real_scan.angle_min
        fused_scan.angle_max = real_scan.angle_max
        fused_scan.angle_increment = real_scan.angle_increment
        fused_scan.time_increment = real_scan.time_increment
        fused_scan.scan_time = real_scan.scan_time
        fused_scan.range_min = real_scan.range_min
        fused_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_increment
            virtual_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_max
            if 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_range
            fused_ranges.append(fused_range)
        
        fused_scan.ranges = fused_ranges
        return fused_scan

    def 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 False
        return True

if __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: LaserScan
  topic: /wheeltec_01/fused_scan
  marking: true
  clearing: 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就可以了