cartographer + velodyne64 + IMU建图

13 篇文章 7 订阅

从周一晚上开始到,现在是周五晚上,断断续续搞了好几天了,终于跑通了,我也是醉了。不过好的是,一个一个问题,最后还是弄懂了,还是有点激动了。抓紧时间锻炼自己的动手能力吧!!渣渣。

我跑了Kitti的raw data和自己录的数据,下面以自己录的数据整个流程及文件配置。我自己录的数据为Velodyne64 E和IMU原始数据。

0  cartographer_rosbag_validate -bag_filename your_bag.bag

如图,我的数据bag是有问题的,到现在我也不知到这个问题咋解决。不过现在知道了没关系,不影响建图。刚开始的时候可是心里一点底都没有,因为你不知道这个问题会造成什么结果。

 frame_id "velo_link" on topic /velodyne_points has serialization time
1552041177.349049635 but sensor time 57002.398437500 differing by -1.55198e+09 s.

 

1  .lua文件:配置传感器数量

这里需要改的设置的有

velodyne64.lua

  1. 四个frame: map_frame = "map",tracking_frame = "imu_link",published_frame = "base_link",odom_frame = "odom",
  2. num_point_clouds = 1(三维激光数量)
  3. num_subdivisions_per_laser_scan = 1(这个特么我也不知道是干啥的,看着像是2d激光的参数应该设为0,一开始我设的0,结果报错 Check failed: options.num_subdivisions_per_laser_scan >=1)
  4. TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1(这个全局变量貌似表示一次扫描有多少次数据
include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66

return options

2  .urdf文件:配置link 和joint,有几个传感器就配置几个。

如下,一个IMU和一个Velodyne。分割线上半部分不知道干啥的,下半部分是设置IMU和Velodyne的frame_id名字,以及他们的父frame_id的。base_link是父frame_id。

velodyne64.urdf

<robot name="cartographer_velodyne64_3d">
  <material name="orange">
    <color rgba="1.0 0.5 0.2 1" />
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 1" />
  </material>

  <link name="imu_link">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="velo_link">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

-----------------我是分割线,不知道注释格式,配置的时候把本行删掉------------------

  <link name="base_link" />

  <joint name="imu_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="imu_link" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>
<joint name="velodyne64_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="velo_link" />
    <origin xyz="-0.0795 1.2967 0.5389" rpy="0. 0. 0." />
  </joint>

</robot>

3  .launch文件:配置数据bag地址,remap topic为指定名称,在这里包含上述的velodyne64.luavelodyne64.urdf文件。

1、选用离线模式结点cartographer_offline_node!!!这个一开始没有注意,我在在线模式下不行,希望有朋友能交流下。

2、在录制bag时,传感器数据的frame_id必须和velodyne64.urdf文件里的”_link“一样,要对应起来,如下

sensor_msgs::PointCloud2::Ptr output_msg=boost::make_shared<sensor_msgs::PointCloud2>();
output_msg->header.frame_id="velo_link";

sensor_msgs::Imu imuData;
imuData.header.frame_id="imu_link";

3、topic名字更改使用remap,把自己bag里的topic名字换成需要的topic名字 。

velodyne64.launch


<launch>

 <param name="/use_sim_time" value="true" />

  <arg name="bag_filename" default="/media/localization/新加卷/school/20190306siyuan_RAWIMU_noFanzhun_gy.bag" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />

  <node name="cartographer_offline_node" pkg="cartographer_ros"
      type="cartographer_offline_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basenames velodyne64.lua
          -urdf_filenames $(find cartographer_ros)/urdf/velodyne64.urdf
          -bag_filenames $(arg bag_filename)"
      output="screen">
      <remap from="points2" to="/velodyne_points" />
      <remap from="imu" to="/imu/data" />
  </node>


  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

</launch>

4、运行

roslaunch cartographer_ros velodyne64.launch 

运行完后会在源数据bag同目录下生成.pbstream文件,这个文件用于获得最终地图。

5、存地图。

此时需要配置两个文件 

(1)assert_velodyne64.lua

VOXEL_SIZE = 5e-2

include "transform.lua"

options = {
  tracking_frame = "base_link",
  pipeline = {
    {
      action = "min_max_range_filter",
      min_range = 1.,
      max_range = 60.,
    },
    {
      action = "dump_num_points",
    },
    {
      action = "fixed_ratio_sampler",
      sampling_ratio = 0.01,
    },
    
    {
      action = "write_pcd",
      filename = "cartor_siyuanMap.pcd"
    },

    {
      action = "color_points",
      frame_id = "velo_link",
      color = { 255., 0., 0. },
    },
  }
}

return options

这里主要是 "write_pcd"这条命令,filename就是最终要生成pcd地图的名字官方上有介绍各种命令,但是给的demo里面没有写这句话,写的都是生成三视图的命令。我一开始也没看懂,还在纳闷为什么只有图片,我也没有用相机啊。。。后来看的蓝鲸的那篇博客才明白了,自己添加一条命令就行。

(2)assets_writer_velodyne64.launch

<launch>
<arg name="bag_filenames" default="/media/localization/新加卷/school/20190306siyuan_RAWIMU_noFanzhun_gy.bag"/>
<arg name="pose_graph_filename" default="$(arg bag_filenames).pbstream"/>

  <node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"
      type="cartographer_assets_writer" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename assert_velodyne64.lua
          -urdf_filename $(find cartographer_ros)/urdf/velodyne64.urdf
          -bag_filenames $(arg bag_filenames)
          -pose_graph_filename $(arg pose_graph_filename)"
      output="screen">
  </node>
</launch>

bag_filenames就是数据bag的名字,pose_graph_filename是offline slam模式下生成的最终优化过的位姿文件。

在这个launch文件里面就包含了上面的assert_velodyne64.lua

配置完后运行

roslaunch cartographer_ros assets_writer_velodyne64.launch 

即可生成pcd格式的地图。

最终生成的地图如下
 

 


最后在这里记录一下自己写下的Bug,希望能长点记性!我不是在写程序,而是在写bug!

今天下午原本都打算先搁置一段时间再看,太崩溃了,又不要求全部搞懂,只是跑通都没成功,深深对自己动手能力感到无力。结果不经意间发现了蓝鲸机器人的这篇博文,算是找到了救命稻草,把我从崩溃边缘拉回来了(主要是bug 1,看了那篇博文之后才发现,真心感谢!!)

Bug 1

remap写反了,如果要是放在rosbag play 那里的话,是 

  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)"  >  
      <remap from="/velodyne_points" to="points2" />
      <remap from="/imu/data" to="imu" />
 </node>

/velodyne_points 和/imu/data 是我发布的topic名称。但是要在订阅topic的节点里写的话就刚好反过来了!!!这个自己之前就有点混,虽然大概知道,还是吃了这个亏了!

  <node name="cartographer_offline_node" pkg="cartographer_ros"
      type="cartographer_offline_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basenames velodyne64.lua
          -urdf_filenames $(find cartographer_ros)/urdf/velodyne64.urdf
          -bag_filenames $(arg bag_filename)"
      output="screen">
      <remap from="points2" to="/velodyne_points" />
      <remap from="imu" to="/imu/data" />
  </node>

Bug 2

自己录的数据本身就有问题。存读数据搞反了,无语凝咽,自己写的bug,含着泪也得负责。

1、存数据时按yaw pitch roll顺序存的,然而我一直以为是roll pitch yaw顺序,读的时候就用的roll pitch yaw,导致最后程序虽能跑通,但是效果一直很差。对一个未知的东西,出现了bug,想到的原因有好多,一个一个试都不行,最后才想到用matlab画一下图看看,画了图才发现不只是顺序有问题,数据也有问题!(此时我已经惊了,天大的bug,我还调用这个错误的数据调了一天)。数据问题详细见2。

2、惯导文档上写的加速度需要乘以IMU的输出频率,而角速度没有写(如下图)。所以我输出的数据就是角速度没有乘以输出频率100。但是实际上角速度的值也要乘以IMU的输出频率!!!法克!

 
我录的是围着一栋建筑物走了一圈的正方形轨迹数据,原来错误的数据cartographer跑出来的是大概是一条直线到底,没有明显的90度拐弯,现在想一下应该就是角速度值比实际的小了100倍,基本都为零了,所以不会拐弯。不多当时我一度以为是IMU频率问题或者是时间戳的问题。我发布的IMU实际频率为30HZ,但是cartographer却显示为6HZ;时间戳提示

 这个到现在都没搞明白怎么回事,先记下以后再说吧。

评论 87
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值