实体机器人识别虚拟环境中障碍物
之前的内容已经实现了虚拟机器人识别实体机器人的功能，接下来就是实体机器人如何识别虚拟环境中的障碍物（包括虚拟环境中的障碍物和其他虚拟机器人）。
我做的是基于雷达的，所以主要要处理的是雷达的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就可以了