20200921第三次C_MAP&D_MAP
在这里插入代码片
<launch>
<include file="$(find stdr_robot)/launch/robot_manager.launch" />
<node type="stdr_server_node" pkg="stdr_server" name="stdr_server" output="screen" args="$(find stdr_resources)/maps/sparse_obstacles.yaml">
</node>
<node pkg="stdr_robot" type="robot_handler" name="$(anon robot_spawn)" args="add $(find stdr_resources)/resources/robots/pandora_robot.yaml 1 2 0" />
<!--
<include file="$(find stdr_gui)/launch/stdr_gui.launch"/>
<node pkg="tf" type="static_transform_publisher" name="world2map" args="0 0 0 0 0 0 world map 100" />
-->
<node pkg="tf" type="static_transform_publisher" name="baselink_to_laser" args="0 0 0 0 0 0 base_link laser 20" />
<arg name="map_file" default="/home/zzz/maps/map_vWall.yaml"/>
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)">
<remap from="map" to="vWall_map"/>
</node>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_localization.lua
-load_state_filename /home/zzz/maps/Omap.pbstream"
output="screen">
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" >
<remap from="map" to="map_carto"/>
</node>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
<rosparam file="/home/zzz/rsvel/navparas/dwa_paras/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="/home/zzz/rsvel/navparas/dwa_paras/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="/home/zzz/rsvel/navparas/dwa_paras/local_costmap_params.yaml" command="load" />
<rosparam file="/home/zzz/rsvel/navparas/dwa_paras/global_costmap_params.yaml" command="load" />
<rosparam file="/home/zzz/rsvel/navparas/dwa_paras/move_base_params.yaml" command="load" />
<rosparam file="/home/zzz/rsvel/navparas/dwa_paras/dwa_local_planner_params.yaml" command="load"/>
<rosparam file="/home/zzz/rsvel/navparas/dwa_paras/global_planner_params.yaml" command="load" />
<remap from="cmd_vel" to="robot0/cmd_vel"/>
</node>
<node pkg="socket_node" type="socket_odom_scan.py" name="socket_node" output="screen"/>
<!--
<node pkg="socket_node" type="socket_node_class.py" name="socket_node" output="screen"/>
<node pkg="socket_node" type="socket_odom_scan.py" name="socket_node" output="screen"/>
-->
</launch>
dwa common公共配置文件
inflation_layer:
enabled: true
cost_scaling_factor: 5.0 # exponential rate at which the obstacle cost drops off (default: 10)
inflation_radius: 0.4 # max. distance from an obstacle at which costs are incurred for planning paths.
#global inflation radius = 0.5
static_layer:
enabled: true
map_topic: "/vWall_map"
global_costmap:
global_frame: /map_carto
robot_base_frame: /base_link
update_frequency: 5.0 #before: 5.0
publish_frequency: 0.5 #before 0.5
static_map: true
transform_tolerance: 0.20
inflation_radius: 0.40
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
20200722第二次C_MAP
类似方法一:在socket_node 内实现了,订阅STDR 的SCAN 和 ODOM。修改了frame后,再发布出来。
发布话题:需放在主程序中(不和订阅话题声明放一起。)
SCAN 在旋转时抖动严重,问题原因不明,原来未发现这种现象。
方法1:
1、写一个节点程序,订阅STDR 的SCAN 和 ODOM。修改了frame后,再发布出来。
2、编写launch文件,开始建图:
1)配置carto文件:关闭IMU,不提供ODOM 帧、修改地图帧为:map_carto。
2)配置launch文件:增加了一个静态TF,重映射cartographer_occupancy_grid_node为
3、DWA导航:launch文件:
公共文件:
建图lanch:
<launch>
<include file="$(find stdr_robot)/launch/robot_manager.launch" />
<node type="stdr_server_node" pkg="stdr_server" name="stdr_server" output="screen" args="$(find stdr_resources)/maps/sparse_obstacles.yaml"/>
<node pkg="tf" type="static_transform_publisher" name="world2map" args="0 0 0 0 0 0 world map 100" />
<!--<include file="$(find stdr_gui)/launch/stdr_gui.launch"/>-->
<node pkg="stdr_robot" type="robot_handler" name="$(anon robot_spawn)" args="add $(find stdr_resources)/resources/robots/pandora_robot.yaml 1 2 0" />
<node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="teleop_twist_keyboard" output="screen">
<remap from="cmd_vel" to="/robot0/cmd_vel" />
</node>
<!--<node pkg="tf" type="static_transform_publisher" name="odom_to_baselink" args="0 0 0 0 0 0 odom base_link 100" />-->
<node pkg="tf" type="static_transform_publisher" name="baselink_to_laser" args="0 0 0 0 0 0 base_link laser 100" />
<node pkg="scan_cut" type="scan_cut" name="scan_cut" output="screen">
</node>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory
$(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d.lua"
output="screen">
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05">
<remap from="map" to="map_carto" />
</node>
</launch>
方法2:
1、在socket 增加了odom,和scan 的重新转发。(应该不需要)
1.1、修改程序,增加的部分
from nav_msgs.msg import Odometry
2、stdr报错:主要原因是carto和STDR 都在发MAP话题。
3、robot_manager.launch 文件。
3.1、TF图(仿真ROSBAG)
3.2 backpack_2d.lua
4、启动roslaunch 文件。
<include file="$(find stdr_robot)/launch/robot_manager.launch"/>
<node type="stdr_server_node" pkg="stdr_server" name="stdr_server" output="screen" args="$(find stdr_resources)/maps/sparse_obstacles.yaml">
<remap from="map" to="sd_map" />
</node>
<node pkg="tf" type="static_transform_publisher" name="world2map" args="0 0 0 0 0 0 world sd_map 100" />
<node pkg="stdr_robot" type="robot_handler" name="$(anon robot_spawn)" args="add $(find stdr_resources)/resources/robots/pandora_robot.yaml 1 2 0" />
<!-- <param name="/use_sim_time" value="true" /> -->
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser_frame" args="0 0 0.185 0 0 0 base_link socket_scan 100"/>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory
$(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_stdr.lua"
output="screen">
<remap from="scan" to="socket_scan" />
<remap from="odom" to="socket_odom" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node pkg="socket_node" type="socket_node_class.py" name="socket_node" output="screen"/>
<node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="teleop_twist_keyboard" output="screen"/>
<node name="rviz" pkg="rviz" type="rviz" required="true" />
</launch>
6、下图为cpu使用率:开始平均45%,峰值80%。 后期平均:70%,峰值85%。
6.1、大图,默认参数,无IMU,图像歪的厉害,应该是socket-node增加了ODOM 和 SCAN 原因。
caoto 订阅的是socket_scan 和 socket_odom ,改回继续测试。
发现scan 只有5HZ, 应该为10hz(原因不明)
不知改了什么,建图有改善,但还是有偏斜。
USE IMU
6.2、carto调参:
总结:就第一个参数optimize_every_n_nodes = 90 改为45后,效果明显,无重影,其他效果不明显,
也许地图小了。用大图继续测试。
2)cpu% 有些改善,估计地图小了,效果不明显。改大图继续测试。
3)修改本地优化,效果不明显,大图继续测试,或修改PAD地图显示程序。
4)目的在保证建图效果的情况下,尽量减小CPU% , 如M1M1性能肯定弱。
5)在各种参数设置情况下,感觉最后建图效果差不多。
POSE_GRAPH.optimize_every_n_nodes = 90
POSE_GRAPH.global_sampling_ratio = 0.003
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.optimize_every_n_nodes = 45
POSE_GRAPH.global_sampling_ratio = 0.003
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.optimize_every_n_nodes = 90
POSE_GRAPH.global_sampling_ratio = 0.0015
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.optimize_every_n_nodes = 90
POSE_GRAPH.global_sampling_ratio = 0.003
POSE_GRAPH.constraint_builder.sampling_ratio = 0.15
POSE_GRAPH.optimize_every_n_nodes = 90
POSE_GRAPH.global_sampling_ratio = 0.003
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.constraint_builder.min_score = 0.75
POSE_GRAPH.optimize_every_n_nodes = 45
POSE_GRAPH.global_sampling_ratio = 0.0015
POSE_GRAPH.constraint_builder.sampling_ratio = 0.15
POSE_GRAPH.constraint_builder.min_score = 0.75