网站质量,北京开发公司,站长工具成品源码,网站建站过程分析之前的内容已经实现了虚拟机器人识别实体机器人的功能#xff0c;接下来就是实体机器人如何识别虚拟环境中的障碍物#xff08;包括虚拟环境中的障碍物和其他虚拟机器人#xff09;。
我做的是基于雷达的#xff0c;所以主要要处理的是雷达的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_size10)# 创建时间同步器real_sub Subscriber(/wheeltec_01/scan, LaserScan)virtual_sub Subscriber(/Vir_wheeltec_01/scan, LaserScan)self.ts ApproximateTimeSynchronizer([real_sub, virtual_sub], queue_size5, slop0.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 namescan_filter pkgyour_package_name typescan_filter.py outputscreen remap fromscan to/wheeltec_01/scan / remap fromfused_scan to/wheeltec_01/fused_scan / /node”
launch!-- 使用命名空间 wheeltec_01 --group nswheeltec_01!-- 开启机器人底层相关节点 同时开启导航功能 --include file$(find turn_on_wheeltec_robot)/launch/turn_on_wheeltec_robot.launcharg namenavigation defaulttrue//include!-- turn on lidar 开启雷达 --include file$(find turn_on_wheeltec_robot)/launch/wheeltec_lidar.launch /!-- 启动扫描数据融合节点 scan_filter.py --node namescan_filter pkgyour_package_name typescan_filter.py outputscreenremap fromscan to/wheeltec_01/scan /remap fromfused_scan to/wheeltec_01/fused_scan //node!-- 设置需要用于导航的地图 --arg namemap_file default$(find turn_on_wheeltec_robot)/map/WHEELTEC.yaml/node namemap_server_for_test pkgmap_server typemap_server args$(arg map_file)/!-- 开启用于导航的自适应蒙特卡洛定位 amcl --include file$(find turn_on_wheeltec_robot)/launch/include/amcl.launch node pkgtf typestatic_transform_publisher namewheeltec_world_to_global_01 args-9 -4 0 0 0 0 world /map 100 /!-- MarkerArray功能节点 --node namesend_mark pkgturn_on_wheeltec_robot typesend_mark.py/!-- move_base 节点检测 --node namenode_detection pkgturn_on_wheeltec_robot typenode_ping.pyparam namenode_name typestring value/move_base//node/group !-- 结束命名空间 wheeltec_01 --
/launch并且启动时需要先在主机启动虚拟小车gazeboroslaunch wxfpublish testSimulation.launch
接着启动小车的导航文件roslaunch turn_on ... wxfnavigation.launch(可以teb补全因为具体的我忘记了)
最终结果 此外地图文件更换成map并且修改实体小车下map中的位置把rviz中实体小车到world的映射改成0000就可以了
