请教一个关于SLAM的问题

回复本帖可奖励1枚铜币哦! 每人最多可获奖1次!
奖池剩余9(中奖概率10%)
大家好!我是一个刚接触ros的小白,现在毕设题目和自动驾驶中的SLAM有点关系。设备会有gps、imu和激光雷达,但是目前没有实地环境,只能在仿真环境中使用。
看了tutorial后,我对此仍然没有多少头绪,所以买了博主“古月居”的书,借鉴了他的书中的“使用gmapping+激光雷达+里程计”这部分的代码,完成了第一个demo。其中激光雷达和里程计都来源于是gazebo软件中的插件。
接下来,我查阅资料,参考了https://answers.ros.org/question/258307/gazebo-simulation-gps-fix/,将gps加入进了gazebo。
我的gazebo代码如下,几乎全都是借鉴的古月居的代码,除了gps那部分。由于初来乍到,不知道怎么很方便的贴代码(好像点了“代码”就全都是代码了,如果大家无法忍受看非格式化的代码的话在此表示抱歉,并求解这里贴代码的正确姿势)
<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!-- Defining the colors used in this robot -->
    <material name="Black">
        <color rgba="0 0 0 1" />
    </material>

    <material name="White">
        <color rgba="1 1 1 1" />
    </material>

    <material name="Blue">
        <color rgba="0 0 1 1" />
    </material>

    <material name="Red">
        <color rgba="1 0 0 1" />
    </material>

    <!-- PROPERTY LIST -->
    <!--All units in m-kg-s-radians unit system -->
    <xacro:property name="M_PI" value="3.1415926535897931" />

    <!-- Main body length, width, height and mass -->
    <xacro:property name="base_mass" value="0.5" />
    <xacro:property name="base_link_radius" value="0.13" />
    <xacro:property name="base_link_length" value="0.005" />

    <xacro:property name="motor_x" value="-0.05" />

    <!-- Caster radius and mass -->
    <xacro:property name="caster_radius" value="0.016" />
    <xacro:property name="caster_mass" value="0.01" />
    <xacro:property name="caster_joint_origin_x" value="-0.12" />

    <!-- Wheel radius, height and mass -->
    <xacro:property name="wheel_radius" value="0.033" />
    <xacro:property name="wheel_height" value="0.017" />
    <xacro:property name="wheel_mass" value="0.1" />

    <!-- plate height and mass -->
    <xacro:property name="plate_mass" value="0.05" />
    <xacro:property name="plate_height" value="0.07" />
    <xacro:property name="standoff_x" value="0.12" />
    <xacro:property name="standoff_y" value="0.10" />

    <!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0" iyy="${2*m*r*r/5}" iyz="0" izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="box_inertial_matrix" params="m w h d">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(h*h+d*d)/12}" ixy = "0" ixz = "0"
                iyy="${m*(w*w+d*d)/12}" iyz = "0"
                izz="${m*(w*w+h*h)/12}" />
        </inertial>
    </xacro:macro>

    <!-- Macro for wheel joint -->
    <xacro:macro name="wheel" params="lr translateY">
        <!-- lr: left, right -->
        <link name="wheel_${lr}_link" />
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0  0 " />
                <geometry>
                    <cylinder />
                </geometry>
                <material name="Black" />
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0 " />
                <geometry>
                    <cylinder />
                </geometry>
            </collision>
            <cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_height}" />
        </link />

        <gazebo reference="wheel_${lr}_link">
            <material>Gazebo/Black</material>
        </gazebo>

        <joint name="base_to_wheel_${lr}_joint" type="continuous">
            <parent link="base_link" />
            <child link="wheel_${lr}_link" />
            <origin xyz="${motor_x} ${translateY * base_link_radius} 0" rpy="0 0 0" />
            <axis xyz="0 1 0" rpy="0  0" />
        </joint>

        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="wheel_${lr}_joint_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="base_to_wheel_${lr}_joint" />
            <actuator name="wheel_${lr}_joint_motor">
                <hardwareinterface>VelocityJointInterface</hardwareinterface>
                <mechanicalreduction>1</mechanicalreduction>
            </actuator>
        </transmission>
    </xacro:macro>

    <!-- Macro for caster joint -->
    <xacro:macro name="caster" params="fb translateX">
        <!-- fb: front, back -->
        <link name="${fb}_caster_link" />
            <visual>
                <origin xyz="0 0 0 " rpy="0 0 0" />
                <geometry>
                    <sphere radius="${caster_radius}" />
                </geometry>
                <material name="Black" />
            </visual>  
            <collision>
                <geometry>
                    <sphere radius="${caster_radius}" />
                </geometry>
                <origin xyz="0 0 0 " rpy="0 0 0" />
            </collision>      
            <sphere_inertial_matrix m="${caster_mass}" r="${caster_radius}" />
        </link />

        <gazebo reference="${fb}_caster_link">
            <material>Gazebo/Black</material>
        </gazebo>

        <joint name="base_to_${fb}_caster_joint" type="fixed">
            <parent link="base_link" />
            <child link="${fb}_caster_link" />
            <origin xyz="${translateX*caster_joint_origin_x} 0 ${-caster_radius}" rpy="0 0 0" />
        </joint>
    </xacro:macro>

    <!-- Macro for plate joint -->
    <xacro:macro name="plate" params="num parent">
        <link name="plate_${num}_link" />
            <cylinder_inertial_matrix m="0.1" r="${base_link_radius}" h="${base_link_length}" />

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <cylinder />
                </geometry>
                <material name="yellow" />
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <cylinder />
                </geometry>
            </collision>
        </link />
        <gazebo reference="plate_${num}_link">
            <material>Gazebo/Blue</material>
        </gazebo>
        <joint name="plate_${num}_joint" type="fixed">
            <origin xyz="0 0 ${plate_height}" rpy="0 0 0" />
            <parent link="${parent}" />
            <child link="plate_${num}_link" />
        </joint>
    </xacro:macro>

    <!-- Macro for standoff joint -->
    <xacro:macro name="mrobot_standoff_2in" params="parent number x_loc y_loc z_loc">
        <joint name="standoff_2in_${number}_joint" type="fixed">
            <origin xyz="${x_loc} ${y_loc} ${z_loc}" rpy="0 0 0" />
            <parent link="${parent}" />
            <child link="standoff_2in_${number}_link" />
        </joint>

        <link name="standoff_2in_${number}_link" />
            <inertial>
                <mass value="0.001" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001" />
            </inertial>

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.01 0.07" />
                </geometry>
                <material name="black">
                    <color rgba="0.16 0.17 0.15 0.9" />
                </material>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.01 0.07" />
                </geometry>
            </collision>
        </link />
    </xacro:macro>

    <!-- BASE-FOOTPRINT -->
    <!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin -->
    <xacro:macro name="mrobot_body">
        <link name="base_footprint" />
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001" />
                </geometry>
            </visual>
        </link />

        <joint name="base_footprint_joint" type="fixed">
            <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0" />
            <parent link="base_footprint" />
            <child link="base_link" />
        </joint>

        <!-- BASE-LINK -->
        <!--Actual body/chassis of the robot-->
        <link name="base_link" />
            <cylinder_inertial_matrix m="${base_mass}" r="${base_link_radius}" h="${base_link_length}" />
            <visual>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder />
                </geometry>
                <material name="yellow" />
            </visual>

            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder />
                </geometry>
            </collision>  
        </link />
        <gazebo reference="base_link">
            <material>Gazebo/Blue</material>
        </gazebo>

        <!-- Wheel Definitions -->
        <wheel lr="right" translatey="1" />
        <wheel lr="left" translatey="-1" />

        <!-- Casters Definitions -->
        <caster fb="front" translatex="-1" />

        <!-- plates and standoff Definitions -->
        <mrobot_standoff_2in parent="base_link" number="1" x_loc="-${standoff_x/2 + 0.03}" y_loc="-${standoff_y - 0.03}" z_loc="${plate_height/2}" />
        <mrobot_standoff_2in parent="base_link" number="2" x_loc="-${standoff_x/2 + 0.03}" y_loc="${standoff_y - 0.03}" z_loc="${plate_height/2}" />
        <mrobot_standoff_2in parent="base_link" number="3" x_loc="${standoff_x/2}" y_loc="-${standoff_y}" z_loc="${plate_height/2}" />
        <mrobot_standoff_2in parent="base_link" number="4" x_loc="${standoff_x/2}" y_loc="${standoff_y}" z_loc="${plate_height/2}" />
        <mrobot_standoff_2in parent="standoff_2in_1_link" number="5" x_loc="0" y_loc="0" z_loc="${plate_height}" />
        <mrobot_standoff_2in parent="standoff_2in_2_link" number="6" x_loc="0" y_loc="0" z_loc="${plate_height}" />
        <mrobot_standoff_2in parent="standoff_2in_3_link" number="7" x_loc="0" y_loc="0" z_loc="${plate_height}" />
        <mrobot_standoff_2in parent="standoff_2in_4_link" number="8" x_loc="0" y_loc="0" z_loc="${plate_height}" />

        <!-- plate Definitions -->
        <plate num="1" parent="base_link" />
        <plate num="2" parent="plate_1_link" />

        <!-- controller -->
        <gazebo>
            <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
                <rosdebuglevel>Debug</rosdebuglevel>
                <publishwheeltf>true</publishwheeltf>
                <robotnamespace>/</robotnamespace>
                <publishtf>1</publishtf>
                <publishwheeljointstate>true</publishwheeljointstate>
                <alwayson>true</alwayson>
                <updaterate>100.0</updaterate>
                <legacymode>true</legacymode>
                <leftjoint>base_to_wheel_left_joint</leftjoint>
                <rightjoint>base_to_wheel_right_joint</rightjoint>
                <wheelseparation>${base_link_radius*2}</wheelseparation>
                <wheeldiameter>${2*wheel_radius}</wheeldiameter>
                <broadcasttf>1</broadcasttf>
                <wheeltorque>30</wheeltorque>
                <wheelacceleration>1.8</wheelacceleration>
                <commandtopic>cmd_vel</commandtopic>
                <odometryframe>odom</odometryframe>
                <odometrytopic>odom</odometrytopic>
                <robotbaseframe>base_footprint</robotbaseframe>
            </plugin>
                        <plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
                                <alwayson>true</alwayson>
                                <updaterate>1.0</updaterate>
                                <bodyname>base_link</bodyname>
                                <topicname>/fix</topicname>
                                <velocitytopicname>/fix_velocity</velocitytopicname>
                                <drift>5.0 5.0 5.0</drift>
                                <gaussiannoise>0.1 0.1 0.1</gaussiannoise>
                                <velocitydrift>0 0 0</velocitydrift>
                                <velocitygaussiannoise>0.1 0.1 0.1</velocitygaussiannoise>
                        </plugin>
        </gazebo>

    </xacro:macro>
</robot>
由于gmapping库需要里程计,而设备没有里程计,于是我在http://wiki.ros.org/gps_common里查到gps_common库的utm_odometry_code能将gps信号转化为里程计。


之前的gmapping,我使用gazebo提供的/odom参数作为里程计输入,而现在,我试图使用gps_common库由gps信号输出一个里程计信号,记为/pred_odom topic。而就在这一步时,我遇到了困难,我的launch文件如下:

<launch>

    <!-- 设置launch文件的参数 -->
    <arg name="world_name" value="$(find mrobot_gazebo)/worlds/room.world"/>
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>

    <!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(arg world_name)" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!-- 加载机器人模型描述参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_gazebo)/urdf/mrobot_with_rplidar.urdf.xacro'" />

    <!-- 运行joint_state_publisher节点,发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>

    <!-- 运行robot_state_publisher节点,发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
        <param name="publish_frequency" type="double" value="50.0" />
    </node>

<node name="pred_odom" pkg="gps_common" type="utm_odometry_node">
<remap from="odom" to="pred_odom"/>
<param name="child_frame_id" type="string" value="base_footprint" />
</node>

    <!-- 在gazebo中加载机器人模型-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          args="-urdf -model mrobot -param robot_description"/>

</launch>

我希望odom和pred_odom是一致的,这样的话在我看来问题就有了初步的解决。可是,当我使用rostopic echo输出/odom与/pred_odom这两个topic时,却发现他们虽然twist一致,但position却完全不同(据我理解这似乎是坐标系未对齐导致的,但我也不知道该怎么做了)

输出/odom结果如下




输出/pred_odom结果如下





position的x,y的巨大差异,导致我无法拿pred_odom作为gmapping的输入,请问我该怎么做?
真心恳求诸位的帮忙!之前我一直都在闭门造车,很多东西都学的不扎实(几乎等于没学),还望不吝赐教!

juceyzhao 论坛版主 沙发

2019-03-05 09:42

时间戳这个因素考虑下

juceyzhao 论坛版主 板凳

2019-03-05 09:42

回帖奖励+ 1
时间戳这个因素考虑下
游客
登录后才可以回帖,登录 或者 注册