当前位置：首页 > wzjs >正文

机构网站建设兰州网络seo公司

wzjs 2025/7/20 4:07:16

机构网站建设,兰州网络seo公司,dw做网站的所有流程,珠海集团网站建设之前的内容已经实现了虚拟机器人识别实体机器人的功能，接下来就是实体机器人如何识别虚拟环境中的障碍物（包括虚拟环境中的障碍物和其他虚拟机器人）。我做的是基于雷达的，所以主要要处理的是雷达的scan话题我的虚拟机器人命名…

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

我做的是基于雷达的，所以主要要处理的是雷达的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 文件路径/文件名）

“
    <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补全，因为具体的我忘记了)

最终结果