Run with a slam system

[narutojxl]

Hi @TurtleZhong,
Thanks for your sharing, i'm trying to run the gazebo with lio-sam. When robot is stationary at origin, lio-sam begins to drift in z axis. I am not familar with gazebo, i am not sure i set the params correctly. Look forward for your suggestions and thanks in advance. I modify these params.

• comment joystick teleop.launch in spawn_husky.launch.
• disable enable_ekf in husky_control/launch/control.launch.
• config vlp-16 params. <VLP-16 parent="base_link" name="velodyne" topic="/points_raw" hz="10" samples="1800" min_range="0.1" gpu="false" organize_cloud="true"> in husky.urdf.xacro.
• lookup extrinsic between laser and imu. /imu/data in base_link frame_id,

➜ [/home/jxl] rosrun tf tf_echo velodyne base_link
At time 0.000
- Translation: [-0.081, 0.000, -0.913]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY (radian) [0.000, -0.000, 0.000]
in RPY (degree) [0.000, -0.000, 0.000]

• modify imu noise and add yaw noise in husky.urdf.xacro.

    <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
      <robotNamespace>$(arg robot_namespace)</robotNamespace>
      <updateRate>50.0</updateRate>
      <bodyName>base_link</bodyName>
      <topicName>imu/data</topicName>
      <accelDrift>0.005 0.005 0.005</accelDrift>
      <accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise> <!--default: 0.05 -->
      <!-- <rateDrift>0.005 0.005 0.005 </rateDrift> -->
      <!-- <rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise> -->
      <!-- <headingDrift>0.005</headingDrift> -->
      <!-- <headingGaussianNoise>0.005</headingGaussianNoise> -->
      <yawDrift>0.0005 0.0005 0.0005</yawDrift>
      <yawGaussianNoise>0.005 0.005 0.005</yawGaussianNoise> 
      <gaussianNoise>0.005</gaussianNoise>
    </plugin>

• set lio-sam params, imu noise is the default.

sensor: velodyne
N_SCAN: 16
Horizon_SCAN: 1800
imuAccNoise: 3.9939570888238808e-03
imuGyrNoise: 1.5636343949698187e-03
imuAccBiasN: 6.4356659353532566e-05
imuGyrBiasN: 3.5640318696367613e-05
imuGravity: 9.80511
imuRPYWeight: 0.01
extrinsicTrans: [-0.081, 0.000, -0.913]
extrinsicRot: [1, 0, 0,
0, 1, 0,
0, 0, 1]
extrinsicRPY: [1, 0, 0,
0, 1, 0,
0, 0, 1]

Best regards
narutojxl

[narutojxl]

I want to change hz="5", samples="1800", organize_cloud="false" according to velodyne_simulator, but i find there is no topic named /points_raw, but the only topic /points, defined in GazeboRosVelodyneLaser.cpp, which generates libgazebo_ros_velodyne_laser.so used in VLP-16.urdf.xacro.

LVIO-SAM/sim_env/husky/husky_description/urdf/husky.urdf.xacro

Lines 295 to 298 in a2df3a1

	<xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
	<VLP-16 parent="base_link" name="velodyne" topic="/points_raw" hz="10" samples="440" gpu="false">
	<origin xyz="0.4 0 0.5" rpy="0 0 0" />
	</VLP-16>

I'm not familar with xacro syntax, try to add xacro: in VLP-16 tag, tf will complains link 'velodyne_base_link' is not unique. and then crash. Does anybody know how to do to make params take effect? Thanks for your help and time.

<xacro:VLP-16 parent="base_link" name="velodyne" topic="/points_raw" hz="5" samples="1800" organize_cloud="false" >
    <origin xyz="0.4 0 0.5" rpy="0 0 0" /> 
</xacro:VLP-16>

Update 1:
It seems this <VLP-16> tag code in husky.urdf.xacro does not work, the working place is in

LVIO-SAM/sim_env/husky/husky_description/urdf/husky.urdf.xacro

Line 62 in a2df3a1

<xacro:include filename="$(find husky_description)/urdf/accessories/vlp16_mount.urdf.xacro"/>

[narutojxl]

The drift is caused by extrinsic between velodyne and imu frame, see here .

[Gatsby23]

Meet the same problem as you. The extrinsic between velodyne and imu frame is:
R = Identity
t = [-0.4, 0, -0.5]. Right?

[narutojxl]

• The problem about imu axis not aligned with binded frame axis problem is caused by world file, when using with cmu indoor.world. The reason i dont figure out.
• Currently i use mir_robot model as husky model has some problems: the feedback wheel odom is not correct and rotate in place not normally, see here. I'm not sure whether author has solve this problem.
• Fortunately mir_robot author has solved mir_robot noise problem, see here, maybe you can try this model.
• The extrinsic between velodyne and imu, i get from rosrun tf tf_echo [velodyne_frame] [imu_frame].

[Gatsby23]

• The problem about imu axis not aligned with binded frame axis problem is caused by world file, when using with cmu indoor.world. The reason i dont figure out.
• Currently i use mir_robot model as husky model has some problems: the feedback wheel odom is not correct and rotate in place not normally, see here. I'm not sure whether author has solve this problem.
• Fortunately mir_robot author has solved mir_robot noise problem, see here, maybe you can try this model.
• The extrinsic between velodyne and imu, i get from rosrun tf tf_echo [velodyne_frame] [imu_frame].

Yes, When I run the command tf_echo. I get like this :

• Translation: [-0.4, 0.000, -0.5]
• Rotation: in Quaternion [0.707, 0.000, 0.707, 1.000]
  in RPY (radian) [pi, -pi/2, 0.000]
  in RPY (degree) [3.14, -1.57, 0.000]

When I use this in my program, the trajectory result is not correct. So I want to figure out the correct parameter about this simulation

[Gatsby23]

• The problem about imu axis not aligned with binded frame axis problem is caused by world file, when using with cmu indoor.world. The reason i dont figure out.
• Currently i use mir_robot model as husky model has some problems: the feedback wheel odom is not correct and rotate in place not normally, see here. I'm not sure whether author has solve this problem.
• Fortunately mir_robot author has solved mir_robot noise problem, see here, maybe you can try this model.
• The extrinsic between velodyne and imu, i get from rosrun tf tf_echo [velodyne_frame] [imu_frame].

不好意思，为了怕表述不清楚，这边就中文给您回复了。
看您的回复，好像IMU的参数不仅仅跟husky的xacro文件有关，和world也有关？
我现在在测试我算法的时候发现两个问题：
1.IMU参数：发现IMU的陀螺仪零偏一直为0，不知道怎么给他添加噪声。
2.激光和IMU的外参：我用tf来看的话旋转是有参数的，但是看xacro文件里，旋转实际上是0。并且把这个外参放到程序中，程序轨迹直接飘了。所以这边好奇你是怎么设置参数的？

[narutojxl]

您客气了。以下是我的个人想法，希望有所帮助。

好像IMU的参数不仅仅跟husky的xacro文件有关，和world也有关？

我发现在使用不同world时，有的world在最开始时刻，机器人模型会抖动一下, 可以先加载world，分开加载机器人model，比如husky或者mir_robot。在使用imu的时候，原本在gazebo imu插件中通过参数设置，让imu sensor 绑定到velodyne所在的坐标系，但是发现由于抖动后，imu box(在rviz中通过imu插件rviz_imu_plugin)所在的轴和imu topic的frame_id所在的坐标系不对齐。 imu的acc和gyro数据是在imu box所在轴下的，但是通过tf_echo查询的时候查的是imu topic的frame_id和雷达坐标系的外参。所以，在加载world后，你确认下imu box 和imu topic上的frame_id 坐标系是不是对齐的。

发现IMU的陀螺仪零偏一直为0，不知道怎么给他添加噪声。

如果是需要增加imu gyro测量噪声， 你可以把gazebo imu插件对应的源码下载下来，看是否有现成的参数直接可以设置，如果没有，在代码里面根据自己的想法加一下。

我用tf来看的话旋转是有参数的，但是看xacro文件里，旋转实际上是0。并且把这个外参放到程序中，程序轨迹直接飘了。

这个问题可能跟第一个问题是同一个根源，我猜测哈 :)

[TurtleZhong]

您客气了。以下是我的个人想法，希望有所帮助。

好像IMU的参数不仅仅跟husky的xacro文件有关，和world也有关？

我发现在使用不同world时，有的world在最开始时刻，机器人模型会抖动一下, 可以先加载world，分开加载机器人model，比如husky或者mir_robot。在使用imu的时候，原本在gazebo imu插件中通过参数设置，让imu sensor 绑定到velodyne所在的坐标系，但是发现由于抖动后，imu box(在rviz中通过imu插件rviz_imu_plugin)所在的轴和imu topic的frame_id所在的坐标系不对齐。 imu的acc和gyro数据是在imu box所在轴下的，但是通过tf_echo查询的时候查的是imu topic的frame_id和雷达坐标系的外参。所以，在加载world后，你确认下imu box 和imu topic上的frame_id 坐标系是不是对齐的。

发现IMU的陀螺仪零偏一直为0，不知道怎么给他添加噪声。

如果是需要增加imu gyro测量噪声， 你可以把gazebo imu插件对应的源码下载下来，看是否有现成的参数直接可以设置，如果没有，在代码里面根据自己的想法加一下。

我用tf来看的话旋转是有参数的，但是看xacro文件里，旋转实际上是0。并且把这个外参放到程序中，程序轨迹直接飘了。

这个问题可能跟第一个问题是同一个根源，我猜测哈 :)

关于机器人模型会抖动的问题，这个确实是的，有可能是因为gazebo环境重力原因，有些时候模型是总某个高度掉到地面的，所以会抖，但是等所以稳定之后应该就没事了

[Gatsby23]

您客气了。以下是我的个人想法，希望有所帮助。

好像IMU的参数不仅仅跟husky的xacro文件有关，和world也有关？

我发现在使用不同world时，有的world在最开始时刻，机器人模型会抖动一下, 可以先加载world，分开加载机器人model，比如husky或者mir_robot。在使用imu的时候，原本在gazebo imu插件中通过参数设置，让imu sensor 绑定到velodyne所在的坐标系，但是发现由于抖动后，imu box(在rviz中通过imu插件rviz_imu_plugin)所在的轴和imu topic的frame_id所在的坐标系不对齐。 imu的acc和gyro数据是在imu box所在轴下的，但是通过tf_echo查询的时候查的是imu topic的frame_id和雷达坐标系的外参。所以，在加载world后，你确认下imu box 和imu topic上的frame_id 坐标系是不是对齐的。

发现IMU的陀螺仪零偏一直为0，不知道怎么给他添加噪声。

如果是需要增加imu gyro测量噪声， 你可以把gazebo imu插件对应的源码下载下来，看是否有现成的参数直接可以设置，如果没有，在代码里面根据自己的想法加一下。

我用tf来看的话旋转是有参数的，但是看xacro文件里，旋转实际上是0。并且把这个外参放到程序中，程序轨迹直接飘了。

这个问题可能跟第一个问题是同一个根源，我猜测哈 :)

这里没有看懂IMU box指的是什么，所以将rviz打开后，截图发上来，想问下这里说的imu box和imu topic对应的frame id应该如何看呢？

[narutojxl]

类似下面这个照片; 遥控机器人旋转, imu box也会跟着旋转；imu topic的frame id 我记得是通过imu gazebo plugins的一个参数可以设置，可以通过rostopic echo [imu_topic]查看设置的是否起作用。

[Gatsby23]

类似下面这个照片; 遥控机器人旋转, imu box也会跟着旋转；imu topic的frame id 我记得是通过imu gazebo plugins的一个参数可以设置，可以通过rostopic echo [imu_topic]查看设置的是否起作用。

OK，刚刚重新检查了一遍，我这边是直接使用默认环境和参数的，应该没有太大问题。多谢指教！