How to diagnose when a URDF problem stops the gazebo_ros_control from receiving

asked 2019-10-14 18:14:08 -0500

horseatinweeds gravatar image

updated 2019-10-18 06:10:31 -0500

I have a URDF ackermann vehicle that works just fine, until I put the LiDAR on top of it. The LiDAR works. I can visualize the point cloud in RVIZ, but Gazebo stops responding to the messages I publish to the joint controllers. I echo the joint topic and see the messages going in, but the wheels don't move.

UPDATE:
That is the problem. [WARN] [1571345774.639031, 0.000000]: Controller Spawner couldn't find the expected controller_manager ROS interface.

How can I diagnose this odd issue? Here is my xacro file. The trouble maker is the last bit, where I define the sensor link for the Velodyne laser. This same bit works in other robots... Any ideas? Thanks.

    <?xml version="1.0"?>
    <robot name="gem" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!-- Vehicle Dimensions -->
    <xacro:property name="wheel_base_length" value="1.753"/>
    <xacro:property name="wheel_base_width" value="1.245"/>
    <xacro:property name="wheel_diameter" value="0.584"/><!--.292 -->
    <xacro:property name="wheel_thickness" value="0.178"/>

    <!-- Macros -->
    <xacro:macro name="wheel" params="name radius width material caster_offset mass">
      <link name="${name}">         
            <collision> 
              <origin xyz="0 0 0" rpy="0 1.5708 1.5708" />
              <geometry> 
               <cylinder length="${width}" radius="${radius}"/> 
              </geometry> 
            </collision>
            <visual> 
              <origin xyz="0 0 0" rpy="0 1.5708 1.5708" /> 
              <geometry> 
                <cylinder length="${width}" radius="${radius}"/> 
              </geometry> 
              <material name="black"/> 
            </visual>
            <inertial> 
              <origin xyz="0 0 0" rpy="0 1.5708 1.5708" /> 
              <mass value="0.2"/> 
              <!--<cylinder_inertia m="0.2" r="0.3" h="0.1"/> -->
          <cylinder_inirtia m="${mass}" r="${radius}" h="${width}" />
          <!--<inertia ixx="0.23" ixy="0" ixz="0" iyy="0.23" iyz="0" izz="0.4"/>-->
          </inertial> 
        </link> 
      <gazebo reference="${name}"> 
        <mu1 value="2.0"/> 
        <mu2 value="2.0"/> 
        <kp  value="10000000.0" /> 
        <kd  value="1.0" /> 
        <fdir1 value="0 1 0"/> 
        <material>${material}</material> 
      </gazebo>
    </xacro:macro>

    <xacro:macro name="wheel_steer" params="lr lr_reflect">
      <xacro:wheel name="${lr}" radius="${wheel_diameter/2}" width="${wheel_thickness}" material="Gazebo/Blue" caster_offset="0" mass="6"/>
      <xacro:wheel name="${lr}_assembly" radius="0.1" width="0.001" material="Gazebo/White" caster_offset="-.5" mass="0.5" />

      <joint name="${lr}_hinge" type="revolute"> 
        <parent link="chassis"/> 
        <child link="${lr}_assembly"/> 
        <origin xyz="${wheel_base_length/2} ${lr_reflect*(wheel_base_width/2)} ${wheel_diameter/2}" rpy="0 0 0" />  
        <axis xyz="0 0 1" rpy="0 0 0" /> 
        <limit effort="100" velocity="1" lower="-1" upper="1"/> 
        <dynamics damping="0.0" friction="0.0"/> 
      </joint> 
      <joint name="${lr}_rotate" type="continuous"> 
        <parent link="${lr}_assembly"/> 
        <child link="${lr}"/> 
        <origin xyz="0.0 ${lr_reflect*(wheel_thickness/2)} 0.0" rpy="0 0 0" />  
        <axis xyz="0 1 0" rpy="0 0 0" /> 
        <limit effort="100" velocity="50"/> 
        <dynamics damping ...
(more)
edit retag flag offensive close merge delete

Comments

I don't know for sure or what is happening, but the velodyne_description files also load Gazebo plugins in their .xacros. It could be that what you have in your model is conflicting somehow with what the VLP xacro is doing.

You may have already guessed that, but to test, you could disable the Gazebo-plugin in the VLP .xacro and see whether that changes anything.

gvdhoorn gravatar image gvdhoorn  ( 2019-10-16 02:04:25 -0500 )edit

I do not know if this matters but the definition of the transmission since ROS Indigo is:

  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="ros_control">
      <robotNamespace>robot_ns</robotNamespace>
      <robotParam>robot_description</robotParam>
      <controlPeriod>0.001</controlPeriod>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>    
  </gazebo>

<transmission name="trans_name">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="joint_name">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="joint_motor">
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
 </transmission>

Seems like to me, that you are not able to use the controllers cause they are not loaded in RobotHW.

Weasfas gravatar image Weasfas  ( 2019-10-16 04:48:37 -0500 )edit

That is the problem. [WARN] [1571345774.639031, 0.000000]: Controller Spawner couldn't find the expected controller_manager ROS interface.

This warning came up after i improved some other things about the urdf that were causing warnings. I tried using your example here:

<gazebo> <plugin filename="libgazebo_ros_control.so" name="ros_control"> <robotnamespace>/ack</robotnamespace> <robotparam>/ack/gem</robotparam> <controlperiod>0.001</controlperiod> <robotsimtype>gazebo_ros_control/DefaultRobotHWSim</robotsimtype> <legacymodens>true</legacymodens> </plugin>
</gazebo>

My namespace is /ack and my robot name is gem. I'm running out of ideas...

For one experiment I generated the urdf from my xacro, It's the temp2.txt file here: https://github.com/benwarrick/simple_...

The xacro is gem_1kg_LiDAR.xacro

I added the output from the launch in my original post. Any advice is appreciated. Thanks.

horseatinweeds gravatar image horseatinweeds  ( 2019-10-17 18:41:23 -0500 )edit

Please update your question with this sort of information and make sure to format it properly. Right now it's very hard to read, and comments are too limited to contain this sort of update.

Edit your original question. Use the edit button/link for that.

gvdhoorn gravatar image gvdhoorn  ( 2019-10-18 04:51:25 -0500 )edit

Hi,

The repo you posted is not accessible. It would be good if you give read access.

On the other hand, I think the problem does not rely on the VLP-16 instantiation, and the plugin seems to launch correctly. That warning can be caused by several things, like lack of computing power that force the controller manager to not have enough time to load, conflicts when loading plugins (like the VLP-16)...

I am using the files you provided and I think you may have some collisions problems between links. My Gazebo is unable to start both the model and the controllers.

Also ensure you have installed gazebo_ros_pkgs and ros_control, ros_controllers.

Furthermore, you are running robot_state_publisher and joint_state_publisher in a different namespace, this is not advisable unless you do the proper remaps.

Weasfas gravatar image Weasfas  ( 2019-10-18 05:36:56 -0500 )edit

Hi Weasfas, thanks. I will update and try using one namespace for both robot_state_publisher and joint_state_publisher. Also, I set my repo to Public. (I thought I already had...)

The computer resources I think are fine. I have another robot using joint publishers along with this same LiDAR setup. Maybe it is a conflict. I will have to do more experiments.

horseatinweeds gravatar image horseatinweeds  ( 2019-10-18 06:16:33 -0500 )edit

Hi @horseatinweeds

After doing some tests with your package I found a several problems that, once fixed, let me display and control the base on Gazebo.

First of all, you need to take care of the tag <selfCollide> in the gazebo reference, because is causing self colliding problems in your model.

Next, you will need to change the VLP-16 description to adjust to your environment: you are loading a samples:=1875 Lidar, that is huge depending on the machine and resources allocate by Gazebo, Try to lower that number and launch the simulation. With those fixes you will be able to launch it and Gazebo will have enough time to load the controllers.

Weasfas gravatar image Weasfas  ( 2019-10-18 07:04:44 -0500 )edit