Amcl Cartographer 双定位

1、使用记录

１）执行双定位，执行过程中未报错。注意各个launch文件执行顺序。
２）ＡＭＣＬ运行过程中，ＳＣＡＮ与地图匹配没carto好，感觉慢半拍，不
影响使用。
３）Amcl 定位精度稍差，但无首次定位漂移的问题，carto有这问题。
４）carto 在ｉｍｕ加laser , 定位还行，进行搬移后，还是能准确定位。
５）ｃａｒｔｏ启动首次定位容易飘，无法到达指定位置。
６）ｃｐｕ占用率在增加了amcl后变化不大，最主要还是carto负荷大。
７）ａｍｃｌ维持常规配置 map - odom - baselink-laser。
８）主要修改carto 的配置，反过来还未调试成功。
９）增加超声后，移动过程中没纯激光流畅，但影响在空旷位置不明显。
在狭窄位置有卡顿，还是能走出来。后期考虑设置为可开启关闭。
１０）在carto 使用odom进行搬移后，靠自身重定位困难，除非回到原点，
很容易重定位成功。

2、修改文件

一、firstdev.launch
１）增加了carto 使用的静态ＴＦ，imu 不需要，将ＩＭＵ的frame_id 改为与baselink
相同即可。
　
其他未改变。

二、backpack_2d.lua
１）更地图帧名。
２）更改了tracking与published 帧名，其实跟踪帧跟踪的是激光与ＩＭＵ，需要
ｔｏｐｉｃ与帧名，ＩＭＵ可以直接用baselink名，无需ＴＦ，如果一定要（增加ＩＭＵ的ＴＦ有可能会造成ＲＯＳ时间报警，需要ＩＭＵ及时发布话题。
但ｓｃａｎ需要增加ＴＦ，不然carto 无法找到原帧（父帧）跟踪，因为激光与baselink不一定重合。
３）published帧也需要，ｃａｒｔｏ需要输出ＭＡＰ到baselink的ＴＦ变换。
４）ｏｄｏｍ不需要，ｏｄｏｍ帧名应该不关键。

三、ｓｄｗａ.sh
1）主要启动顺序，不然会报警。

四、carto_localization.launch
１）修改了ｃａｒｔｏ的激光话题，如果用baselink下的激光话题，容易冲突。
２）修改了cartographer_occupancy_grid_node　的输出地图话题。不然与
map_server　话题冲突，虽然帧名不一样。
３）其他未改动。

五、socket_node
1）增加了py_scan话题，原因如４．１。
２）监听ｓｃａｎ话题，修改帧名后发出。以后需改到ｃ＋＋文件中，增加
执行速度。

六、修改了hi216_imu_node．
将imu话题的帧名改为py_base_link，原因如２．２。

文件：
carto_localization.launch

<launch>
  <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/zjz/maps/mymap.pbstream"
      output="screen">
      <remap from="/scan" to="/py_scan" />	
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" >
	<remap from = "/map" to = "/carto_map" />
  </node>

  <arg name="map_file" default="/home/zjz/maps/mymap.yaml"/>
  <node pkg="map_server" name="map_server"  type="map_server" args="$(arg map_file)"> 
  </node>

  <include file="/home/zjz/rsvel/navparas/amcl.launch" />

  <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/zjz/rsvel/navparas/dwa_paras/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="/home/zjz/rsvel/navparas/dwa_paras/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="/home/zjz/rsvel/navparas/dwa_paras/local_costmap_params.yaml" command="load" />
    <rosparam file="/home/zjz/rsvel/navparas/dwa_paras/global_costmap_params.yaml" command="load" />
    <rosparam file="/home/zjz/rsvel/navparas/dwa_paras/move_base_params.yaml" command="load" />
    <rosparam file="/home/zjz/rsvel/navparas/dwa_paras/dwa_local_planner_params.yaml" command="load"/>
    <rosparam file="/home/zjz/rsvel/navparas/dwa_paras/global_planner_params.yaml" command="load" />
  </node>
</launch>

文件：backpack_2d.lua

  map_frame = "c_map",
  tracking_frame = "py_base_link",	
  published_frame = "py_base_link",
  odom_frame = "py_odom",
  provide_odom_frame = false,	
  publish_frame_projected_to_2d = false,
  use_odometry = false,

文件：firstdev.launch

<node pkg="tf" type="static_transform_publisher" name="py_base_link_to_laser" args="0.04 0 0.2 0 0 0  py_base_link py_scan 25"/>

 <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.04 0 0.2 0 0 0  base_link laser 25"/>
 <node pkg="tf" type="static_transform_publisher" name="base_link_to_range_fl" args="0.185 0.09 0.145 0 0 0  base_link ultrasound_fl  50"/>
 <node pkg="tf" type="static_transform_publisher" name="base_link_to_range_fr" args="0.185 -0.09 0.145 0 0 0  base_link ultrasound_fr  50"/>
 <node pkg="tf" type="static_transform_publisher" name="base_link_to_range_ml" args="-0.0965 0.21 0.145 1.57 0 0  base_link ultrasound_ml  100"/>
 <node pkg="tf" type="static_transform_publisher" name="base_link_to_range_mr" args="-0.0965 -0.21 0.145 -1.57 0 0  base_link ultrasound_mr  100"/>
 <node pkg="tf" type="static_transform_publisher" name="base_link_to_range_bl" args="-0.235 0.078 0.145 3.14 0 0  base_link ultrasound_bl  100"/>
 <node pkg="tf" type="static_transform_publisher" name="base_link_to_range_br" args="-0.235 -0.078 0.145 3.14 0 0  base_link ultrasound_br  100"/>

 <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf"> 
   <param name="output_frame" value="odom"/> 
   <param name="base_footprint_frame" value="base_link"/>
   <param name="freq" value="30.0"/>
   <param name="sensor_timeout" value="1.0"/> 
   <param name="odom_used" value="true"/>
   <param name="imu_used" value="true"/> 
   <param name="vo_used" value="false"/> 
   <param name="debug" value="false"/> 
   <param name="self_diagnose" value="false"/> 
   <remap from="odom" to="vel_odom" />
 </node>
 <node pkg="socket_node" type="socket_node_class.py" name="socket_node" output="screen"/>

</launch>

文件：sdwa.sh

#!/bin/bash

cd rsvel
cd launch
roslaunch my_serial_node.launch &
echo "*********** sleep 5 second ***********"
sleep 5
roslaunch firstdev.launch &
echo "*********** sleep 5 second ***********"
sleep 4
roslaunch slamware.launch &
echo "*********** sleep 4 second ***********"
sleep 4
roslaunch carto_localization.launch 
exit 0