如何设置SLAM建图时的激光数据来源

[1058898985]

hi鱼总，

我在给机器人建模的时候用了两个激光，但是在建图的过程种出现如图所示的结果。我想是不是没有配置好建图时候的激光数据来源。想请问一下这个应该怎么来配置。

[小鱼]

@1058898985 后来不是检查到了小车在原地还会动，应该是你的建模有问题，检查下。

[1058898985]

@小鱼 谢谢，目前在定位建模的问题到底出在哪儿。我调用您例程里的URDF文件车不会动。但是调用我自己的模型就会以一个很小的速度动。

[小鱼]

@1058898985 你可以试试我的代码能不能跑，可以跑就仔细对比下URDF。

fishbot地址：https://github.com/fishros/fishbot

[1058898985]

@小鱼 鱼总，调用你的模型文件没有问题。但是我的就不行。我找了好几天了，没定位到问题。帮忙看看我的URDF

<?xml version="1.0"?>
<robot name="fishbot">

<!-- Robot Footprint -->
<link name="base_footprint"/>

<joint name="base_joint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0.0 0.0 0.19" rpy="0 0 0"/>
</joint>

<!-- base link -->
<link name="base_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<box size="0.9 0.7 0.3"/>
</geometry>
<material name="yellow">
<color rgba="1 0.4 0 0.5" />
</material>
</visual>
<collision>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<box size="0.9 0.7 0.3"/>
</geometry>
<material name="yellow">
<color rgba="1 0.4 0 0.5" />
</material>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.0122666" ixy="0" ixz="0" iyy="0.0122666" iyz="0" izz="0.02"/>
</inertial>
</link>

<!-- head left laser link -->
<link name="head_left_laser_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.10" radius="0.04"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>

<collision>
  <origin xyz="0 0 0" rpy="0 0 0"/>
  <geometry>
    <cylinder length="0.10" radius="0.04"/>
  </geometry>
  <material name="black">
    <color rgba="0.0 0.0 0.0 0.5" /> 
  </material>
</collision>

<inertial>
  <mass value="0.1"/>
  <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>

</link>

<!-- head left laser joint -->
<joint name="head_left_laser_joint" type="fixed">
<parent link="base_link" />
<child link="head_left_laser_link" />
<origin xyz="0.40 0.30 0.175" />
</joint>

<!-- end right laser link -->
<link name="end_right_laser_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.10" radius="0.04"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>

<collision>
  <origin xyz="0 0 0" rpy="0 0 0"/>
  <geometry>
    <cylinder length="0.10" radius="0.04"/>
  </geometry>
  <material name="black">
    <color rgba="0.0 0.0 0.0 0.5" /> 
  </material>
</collision>
<inertial>
<mass value="0.1"/>
  <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>

</link>

<!-- end right laser joint -->
<joint name="end_right_laser_joint" type="fixed">
<parent link="base_link" />
<child link="end_right_laser_link" />
<origin xyz="-0.40 -0.30 0.175" />
</joint>

<!-- imu link -->
<link name="imu_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
</visual>

<collision>
  <origin xyz="0 0 0.0" rpy="0 0 0"/>
  <geometry>
	    <box size="0.02 0.02 0.02"/>
  </geometry>
</collision>
<inertial>
  <mass value="0.1"/>
  <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>

</link>

<!-- imu joint -->
<joint name="imu_joint" type="fixed">
<parent link="base_link" />
<child link="imu_link" />
<origin xyz="0 0 0.1" />
</joint>

<!-- left wheel link -->
<link name="left_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="1.57079 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.08"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>

<collision>
    <origin xyz="0 0 0" rpy="1.57079 0 0"/>
    <geometry>
      <cylinder length="0.04" radius="0.08"/>
    </geometry>
      <material name="black">
        <color rgba="0.0 0.0 0.0 0.5" /> 
      </material>
</collision>
<inertial>
  <mass value="0.2"/>
  <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>

</link>

<!-- left wheel joint -->
<joint name="left_wheel_joint" type="continuous">
<parent link="base_link" />
<child link="left_wheel_link" />
<origin xyz="0 0.33 -0.11" />
<axis xyz="0 1 0" />
</joint>

<!-- right wheel link -->
<link name="right_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="1.57079 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.08"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.57079 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.08"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>
</link>

<!-- right wheel joint -->
<joint name="right_wheel_joint" type="continuous">
<parent link="base_link" />
<child link="right_wheel_link" />
<origin xyz="0 -0.33 -0.11" />
<axis xyz="0 1 0" />
</joint>

<!-- caster head left wheel link -->
<link name="caster_head_left_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.04"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.04"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</collision>
<inertial>
<mass value="0.02"/>
<inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>
</link>

<!-- caster head left wheel joint -->
<joint name="caster_head_left_joint" type="fixed">
<parent link="base_link" />
<child link="caster_head_left_link" />
<origin xyz="0.37 0.275 -0.15" />
</joint>

<!--caster head left wheel gazebo -->>
<gazebo reference="caster_head_left_link">
<material>Gazebo/Black</material>
</gazebo>

<gazebo reference="caster_head_left_link">
<mu1 value="0.0"/>
<mu2 value="0.0"/>
<kp value="1000000.0" />
<kd value="10.0" />
<!-- <fdir1 value="0 0 1"/> -->
</gazebo>

<!-- caster head right wheel link -->
<link name="caster_head_right_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.04"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>

<collision>
  <origin xyz="0 0 0" rpy="0 0 0"/>
  <geometry>
    <sphere radius="0.04"/>
  </geometry>
  <material name="black">
    <color rgba="0.0 0.0 0.0 0.5" /> 
  </material>
</collision>

<inertial>
  <mass value="0.02"/>
  <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>

</link>

<!-- caster head right wheel joint -->
<joint name="caster_head_right_joint" type="fixed">
<parent link="base_link" />
<child link="caster_head_right_link" />
<origin xyz="0.37 -0.275 -0.15" />
</joint>

<!--caster head right wheel gazebo -->>
<gazebo reference="caster_head_right_link">
<material>Gazebo/Black</material>
</gazebo>

<gazebo reference="caster_head_right_link">
<mu1 value="0.0"/>
<mu2 value="0.0"/>
<kp value="1000000.0" />
<kd value="10.0" />
<!-- <fdir1 value="0 0 1"/> -->
</gazebo>

<!-- caster end left wheel link -->
<link name="caster_end_left_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.04"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>

<collision>
  <origin xyz="0 0 0" rpy="0 0 0"/>
  <geometry>
    <sphere radius="0.04"/>
  </geometry>
  <material name="black">
    <color rgba="0.0 0.0 0.0 0.5" /> 
  </material>
</collision>

<inertial>
  <mass value="0.02"/>
  <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>

</link>

<!-- caster end left wheel joint -->
<joint name="caster_end_left_joint" type="fixed">
<parent link="base_link" />
<child link="caster_end_left_link" />
<origin xyz="-0.37 0.275 -0.15" />
</joint>

<!--caster end left wheel gazebo -->>
<gazebo reference="caster_end_left_link">
<material>Gazebo/Black</material>
</gazebo>

<gazebo reference="caster_end_left_link">
<mu1 value="0.0"/>
<mu2 value="0.0"/>
<kp value="1000000.0" />
<kd value="10.0" />
<!-- <fdir1 value="0 0 1"/> -->
</gazebo>

<!-- caster end right wheel link -->
<link name="caster_end_right_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.04"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>

<collision>
  <origin xyz="0 0 0" rpy="0 0 0"/>
  <geometry>
    <sphere radius="0.04"/>
  </geometry>
  <material name="black">
    <color rgba="0.0 0.0 0.0 0.5" /> 
  </material>
</collision>

<inertial>
  <mass value="0.02"/>
  <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
</inertial>

</link>

<!-- caster end right wheel joint -->
<joint name="caster_end_right_joint" type="fixed">
<parent link="base_link" />
<child link="caster_end_right_link" />
<origin xyz="-0.37 -0.275 -0.15" />
</joint>

<!--caster end right wheel gazebo -->>
<gazebo reference="caster_end_right_link">
<material>Gazebo/Black</material>
</gazebo>

<gazebo reference="caster_end_right_link">
<mu1 value="0.0"/>
<mu2 value="0.0"/>
<kp value="1000000.0" />
<kd value="10.0" />
<!-- <fdir1 value="0 0 1"/> -->
</gazebo>

<gazebo>
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
<ros>
<namespace>/</namespace>
<remapping>cmd_vel:=cmd_vel</remapping>
<remapping>odom:=odom</remapping>
</ros>
<update_rate>30</update_rate>
<!-- wheels -->
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<!-- kinematics -->
<wheel_separation>0.66</wheel_separation>
<wheel_diameter>0.16</wheel_diameter>
<!-- limits -->
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<!-- output -->
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_base_frame>
</plugin>

  <plugin name="fishbot_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
    <ros>
      <remapping>~/out:=joint_states</remapping>
    </ros>
    <update_rate>30</update_rate>
    <joint_name>right_wheel_joint</joint_name>
    <joint_name>left_wheel_joint</joint_name>
  </plugin>    

</gazebo>

<gazebo reference="head_left_laser_link">
<material>Gazebo/Black</material>
</gazebo>

<gazebo reference="end_right_laser_link">
<material>Gazebo/Black</material>
</gazebo>

<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<ros>
<namespace>/</namespace>
<remapping>~/out:=imu</remapping>
</ros>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>

      <linear_acceleration>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </z>
      </linear_acceleration>
    </imu>
  </sensor>

</gazebo>

<gazebo reference="head_left_laser_link">
<sensor name="head_left_laser_sensor" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>5</update_rate>
<pose>0 0 0.075 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1.000000</resolution>
<min_angle>-1.570000</min_angle>
<max_angle>3.140000</max_angle>
</horizontal>
</scan>
<range>
<min>0.120000</min>
<max>20</max>
<resolution>0.015000</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>

  <plugin name="headleft_laserscan" filename="libgazebo_ros_ray_sensor.so">
    <ros>
      <remapping>~/out:=scan</remapping>
    </ros>
    <output_type>sensor_msgs/LaserScan</output_type>
    <frame_name>head_left_laser_link</frame_name>
  </plugin>
  </sensor>

</gazebo>

<gazebo reference="end_right_laser_link">
<sensor name="end_right_laser_sensor" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>5</update_rate>
<pose>0 0 0.075 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1.000000</resolution>
<min_angle>1.570000</min_angle>
<max_angle>6.280000</max_angle>
</horizontal>
</scan>
<range>
<min>0.120000</min>
<max>20</max>
<resolution>0.015000</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>

  <plugin name="endright_laserscan" filename="libgazebo_ros_ray_sensor.so">
    <ros>
      <remapping>~/out:=scan_end_right</remapping>
    </ros>
    <output_type>sensor_msgs/LaserScan</output_type>
    <frame_name>end_right_laser_link</frame_name>
  </plugin>
  </sensor>

</gazebo>

</robot>

[小鱼]

@1058898985 注意格式哈，看下昨晚发的最新文章，检查平整性和惯性相关参数是否正常。

[1058898985]

@小鱼修改了转动惯量，目前看不动了，但是左右驱动轮的joint还是在不停的跳动。

[小鱼]

@1058898985 你的机器人模型数据是随便设的吗？惯量、质量、轮子半径之类的，位置是否正确

[1058898985]

@小鱼 在 如何设置SLAM建图时的激光数据来源 中说：

@1058898985 你的机器人模型数据是随便设的吗？惯量、质量、轮子半径之类的，位置是否正确
外观数据正确，其他的数据我用solid work重新算一遍，几乎可以肯定就是这个问题。

[小鱼]

@1058898985 惯量摩擦力之类的改改看