Управление 4-х колесным роботом в Gazebo и RViz

Я пытаюсь заставить своего 4-х колесного робота двигаться внутри пакета моделирования Gazebo, а также в RViz.

Я создал необходимые файлы .launch для Gazebo, Rviz и robot_control, но по-прежнему не вижу ни одной темы, связанной с моими колесными шарнирами.

Когда я пытаюсь запустить контроллер, я получаю следующие предупреждения/ошибки:

[ WARN] [1583095864.913036340]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[INFO] [1583095865.315061, 0.000000]: Controller Spawner: Waiting for service controller_manager/load_controller
0x1a2e560 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0xfe3480) ): Attempt to set a screen on a child window.
0x1a22620 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0xfe3480) ): Attempt to set a screen on a child window.
0x1a23560 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0xfe3480) ): Attempt to set a screen on a child window.
0x1a2e9f0 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0xfe3480) ): Attempt to set a screen on a child window.
[WARN] [1583095895.473658, 2216.849000]: Controller Spawner couldn't find the expected controller_manager ROS interface.

Беседка.запуск:

<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="verbose" value="true" />
  </include>

  <node
    name="tf_footprint_base"
    pkg="tf"
    type="static_transform_publisher"
    args="0 0 0 0 0 0 base_link base_footprint 40" />


   <!--  ******************** Robot Model ********************  -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find robot)/urdf/robot.urdf'"/>

  <node
    name="spawn_model"
    pkg="gazebo_ros"
    type="spawn_model"
    args="-file $(find robot)/urdf/robot.urdf -urdf -model robot"
    output="screen" />

  <node
    name="fake_joint_calibration"
    pkg="rostopic"
    type="rostopic"
    args="pub /calibrated std_msgs/Bool true" />
</launch>

RViz.запуск:

<launch>
  <arg
    name="model" />
  <arg
    name="gui"
    default="False" />
  <param
    name="robot_description"
    textfile="$(find robot)/urdf/robot.urdf" />
  <param
    name="use_gui"
    value="$(arg gui)" />
  <node
    name="joint_state_publisher"
    pkg="joint_state_publisher"
    type="joint_state_publisher" />
  <node
    name="robot_state_publisher"
    pkg="robot_state_publisher"
    type="state_publisher" />
  <node
    name="rviz"
    pkg="rviz"
    type="rviz"
    args="-d $(find robot)/urdf.rviz" />
</launch>

robot_control.запуск:

<launch>

<!-- Get description of robot-->

<param name = 'robot_description' command="cat $(find robot)/urdf/robot.urdf" />


<!-- Send fake joint values -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="FALSE"/>
</node>

<!-- Show in Rviz-->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find robot)/urdf.rviz"/>


  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find robot)/config/robot_control.yaml" command="load"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen" ns="/robot">
    <remap from="/joint_states" to="/robot/joint_states" />
  </node>

<!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/robot" args="joint_state_controller FRJ_velocity_controller FLJ_velocity_controller BRJ_velocity_controller BLJ_velocity_controller FLJ_position_controller FRJ_position_controller BLJ_position_controller BRJ_position_controller "/>

</launch>

URDF-файл:

    <?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from robot.xacro                    | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!--This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner ([email protected]) 
     Commit Version: 1.5.1-0-g916b5db  Build Version: 1.5.7152.31018
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
  <link name="base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00390777224516517 -0.032446267219719 0.184169550820421"/>
      <mass value="4.20121630268732"/>
      <inertia ixx="0.0149000386129946" ixy="-4.66831001352174E-09" ixz="5.23920338795194E-08" iyy="0.0234359493013497" iyz="0.000771538751024883" izz="0.0286744535302635"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/base_link.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.529411764705882 0.549019607843137 0.549019607843137 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/base_link.STL"/>
      </geometry>
    </collision>
  </link>
  <link name="FL">
    <inertial>
      <origin rpy="0 0 0" xyz="-5.55111512312578E-17 9.10729824887824E-18 -6.93889390390723E-18"/>
      <mass value="0.0615219544751675"/>
      <inertia ixx="5.54433425808419E-05" ixy="-1.45453466603006E-20" ixz="-1.00225538664655E-21" iyy="3.00921775305435E-05" iyz="2.309188417276E-21" izz="3.00921775305435E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/FL.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.298039215686275 0.298039215686275 0.298039215686275 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/FL.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="FLJ" type="continuous">
    <origin rpy="1.5707963267949 0 0" xyz="-0.162088045105054 0.0440194465480433 -0.0035605397876617"/>
    <parent link="base_link"/>
    <child link="FL"/>
    <axis xyz="-1 0 0"/>
  </joint>
  <link name="FR">
    <inertial>
      <origin rpy="0 0 0" xyz="2.77555756156289E-17 -5.3776427755281E-17 3.46944695195361E-17"/>
      <mass value="0.0615219544751675"/>
      <inertia ixx="5.54433425808419E-05" ixy="-1.73616162087809E-20" ixz="-8.97406720914896E-21" iyy="3.00921775305435E-05" iyz="-2.24902265994014E-21" izz="3.00921775305435E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/FR.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.298039215686275 0.298039215686275 0.298039215686275 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/FR.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="FRJ" type="continuous">
    <origin rpy="1.5707963267949 0 0" xyz="0.169947769597777 0.0440194465480434 -0.00356053978766185"/>
    <parent link="base_link"/>
    <child link="FR"/>
    <axis xyz="-1 0 0"/>
  </joint>
  <link name="BL">
    <inertial>
      <origin rpy="0 0 0" xyz="-5.55111512312578E-17 7.85396053748499E-16 -1.38777878078145E-17"/>
      <mass value="0.0615219544751675"/>
      <inertia ixx="5.54433425808419E-05" ixy="-5.54431358512462E-21" ixz="2.27097066282367E-22" iyy="3.00921775305435E-05" iyz="2.73146067339844E-21" izz="3.00921775305435E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/BL.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.298039215686275 0.298039215686275 0.298039215686275 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/BL.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="BLJ" type="continuous">
    <origin rpy="1.5707963267949 0 0" xyz="-0.162088045105054 -0.0759805534519569 -0.00356053978766179"/>
    <parent link="base_link"/>
    <child link="BL"/>
    <axis xyz="-1 0 0"/>
  </joint>
  <link name="BR">
    <inertial>
      <origin rpy="0 0 0" xyz="-8.32667268468867E-17 7.03430369508595E-16 2.77555756156289E-17"/>
      <mass value="0.0615219544751675"/>
      <inertia ixx="5.54433425808419E-05" ixy="-7.26178700863739E-21" ixz="-6.24920580783796E-21" iyy="3.00921775305435E-05" iyz="-3.51228039755728E-21" izz="3.00921775305435E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/BR.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.298039215686275 0.298039215686275 0.298039215686275 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/BR.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="BRJ" type="continuous">
    <origin rpy="1.5708 0 0" xyz="0.16991 -0.075981 -0.0035605"/>
    <parent link="base_link"/>
    <child link="BR"/>
    <axis xyz="-1 0 0"/>
  </joint>
  <link name="TB">
    <inertial>
      <origin rpy="0 0 0" xyz="-7.8063E-18 1.3878E-17 0.001"/>
      <mass value="0.36807"/>
      <inertia ixx="0.00019523" ixy="7.0972E-20" ixz="-4.0656E-22" iyy="0.0026688" iyz="4.5169E-23" izz="0.0028638"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/TB.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.52941 0.54902 0.54902 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/TB.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="TBJ" type="fixed">
    <origin rpy="0 0 0" xyz="0.003912 -0.086602 0.46844"/>
    <parent link="base_link"/>
    <child link="TB"/>
    <axis xyz="0 0 0"/>
  </joint>
  <!--             LIDAR                     -->
  <gazebo reference="hokuyo_link">
    <sensor name="head_hokuyo_sensor" type="ray">
      <pose> 0 0 0 0 0 0 </pose>
      <visualize>true</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-1.57079</min_angle>
            <max_angle>1.57079</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.1</min>
          <max>30</max>
          <resolution>0.1</resolution>
        </range>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_head_hokuyo_controller">
        <topicName>/scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="TB_Lidar" type="fixed">
    <axis xyz="0 1 0"/>
    <parent link="TB"/>
    <child link="hokuyo_link"/>
    <origin rpy="0 0 1.57079633" xyz="0 0 0.005"/>
  </joint>
  <!-- Hokuyo Laser -->
  <link name="hokuyo_link">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
    </collision>
  </link>
  <!--                 Gazebo Ros Control Plugin                   -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <robotNamespace>/robot</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>
  <!--                 Gazebo Transmission                        -->
  <transmission name="FLT">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor1">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="FLJ">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="FRT">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor2">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="FRJ">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="BLT">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor3">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="BLJ">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="BRT">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor4">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="BRJ">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <!--                Differential Drive          -->
  <gazebo>
    <plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
      <leftJoint>FLJ</leftJoint>
      <legacyMode>false</legacyMode>
      <rightJoint>FRJ</rightJoint>
      <robotBaseFrame>base_link</robotBaseFrame>
      <wheelSeperation>0.25</wheelSeperation>
      <wheelDiameter>0.07</wheelDiameter>
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>
  </gazebo>
</robot>

$ростопический список

/calibrated
/clicked_point
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/initialpose
/joint_states
/move_base_simple/goal
/robot/joint_states
/rosout
/rosout_agg
/scan
/tf
/tf_static

Я вижу свою модель в RViz правильно, так же как и в Gazebo, но не могу отдавать команды на колеса, так как темы не существует.