ROS中小车前进与后退方向颠倒或者与转向之间混乱

这些天在用xacro编写自己的小车模型的时候，发现一个非常奇怪的现象，小车在gazebo和rviz中显示都是正常的，各个部件之间位置都正确，节点之间的关系也正确，gazebo的小车控制插件中各个joint的关系也正常。但是使用teleop_twist_control进行控制的时候，明明按的是i前进，小车却在后退，按，控制后退却在前进。

搜罗很多，但一无所获，最后思考ROS中唯一能够决定小车前进还是后退的一个因素就是小车的坐标系。所以我打开rviz中查看小车的坐标系如下图：

这就是问题所在，小车车轮坐标系和车中的其他部件之间完全是混乱的！！！这就是问题所在，所以需要将车轮的坐标系调到标准的XYZ坐标形式，就比如laser_link那样。

在调节坐标轴形式上，就很有讲究，最好的方法就是更改link的origin中rpy值，而不要去改joint中的rpy值。也就是保证joint中rpy始终为"0 0 0"是最好的。比如上图中front_right_wheel，也就是右前轮为例，原先的urdf描述文件为：

<link name="front_right_wheel">
        <inertial>
            <origin xyz="1.9051E-06 -2.3183E-07 -0.00064079" rpy="0 0 0"/>
            <mass value="0.051532"/>
            <inertia ixx="1.4595E-05" ixy="-4.3975E-10" ixz="4.415E-10"
                     iyy="1.4594E-05" iyz="-1.7476E-09" izz="2.4264E-05"/>
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://yahboomcar_description/meshes/mecanum/front_right_wheel.STL"/>
            </geometry>
            <material name="">
                <color rgba="0.7 0.7 0.7 1"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://yahboomcar_description/meshes/mecanum/front_right_wheel.STL"/>
            </geometry>
        </collision>
    </link>

    <joint name="front_right_joint" type="continuous">
        <origin xyz="0.08 -0.0845 -0.0389" rpy="-1.5703 0 3.14159"/>
        <parent link="base_link"/>
        <child link="front_right_wheel"/>
        <axis xyz="0 0 1" rpy="0 0 0"/>
        <limit effort="100" velocity="1"/>
    </joint>

更改思路应该是：

1.将joint中rpy值都设置为0，查看车轮位置为：

然后，调整link中的rpy值，而不是调整joint中的！！！这个很关键。因为旋转joint的话只是旋转了tf，也就是图中可以看到的红绿蓝坐标轴，而不是车轮本身。如图中所示，只需要顺时针绕X轴旋转90度即可。调整后，需要注意的一点是要将axis中的"0 0 1"改为"0 1 0"。也就是让gazebo和rviz知道是要绕y轴转动。

另外，需要注意的是visual、inertial以及collision三个标签中都rpy要同步调节，让碰撞和惯性矩阵与车轮重合，否则会出现下图状况：

图中黄色的就是碰撞模块，粉色的就是惯性模块。如果不同调节，就无法准确模拟车轮运动与碰撞检测。

附上更改后右前轮的urdf文件：

<link name="front_right_wheel">
        <inertial>
            <origin xyz="1.9051E-06 -2.3183E-07 -0.00064079" rpy="${pi/2} 0 0"/>
            <mass value="0.051532"/>
            <inertia ixx="1.4595E-05" ixy="-4.3975E-10" ixz="4.415E-10"
                     iyy="1.4594E-05" iyz="-1.7476E-09" izz="2.4264E-05"/>
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
            <geometry>
                <mesh filename="package://yahboomcar_description/meshes/mecanum/front_right_wheel.STL"/>
            </geometry>
            <material name="">
                <color rgba="0.7 0.7 0.7 1"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
            <geometry>
                <mesh filename="package://yahboomcar_description/meshes/mecanum/front_right_wheel.STL"/>
            </geometry>
        </collision>
    </link>

    <joint name="front_right_joint" type="continuous">
        <origin xyz="0.08 -0.0845 -0.0389" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="front_right_wheel"/>
        <axis xyz="0 0 1" rpy="0 0 0"/>
        <limit effort="100" velocity="1"/>
    </joint>

按照上述步骤调节好所有部件后，小车的所有joint坐标系为：

至此，小车前进与后退方向就正确了。