Gazebo SITL Modifying Rover Model

PX4 Autopilot
1

Hey there, I am simply trying to modify the rover model for SITL purposes, but I’m running into an issue that doesn’t make sense to me. I have modified the code and included the mavlink and sensor plugins, but when I run ‘make px4_sitl gazebo_rover’, I’m getting the following error:

    [0/4] Performing build step for 'sitl_gazebo'
ninja: no work to do.
[3/4] cd /home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/build/px4_sitl_default/tmp && /home...irmware_Test/Firmware /home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/build/px4_sitl_default
SITL ARGS
sitl_bin: /home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/build/px4_sitl_default/bin/px4
debugger: none
program: gazebo
model: rover
src_path: /home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware
build_path: /home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/build/px4_sitl_default
GAZEBO_PLUGIN_PATH :/home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/build/px4_sitl_default/build_gazebo
GAZEBO_MODEL_PATH :/home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/Tools/sitl_gazebo/models
LD_LIBRARY_PATH /home/cde01/catkin_ws/devel/lib:/opt/ros/melodic/lib:/home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/build/px4_sitl_default/build_gazebo
gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:734: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = gazebo::event::Connection; typename boost::detail::sp_member_access<T>::type = gazebo::event::Connection*]: Assertion `px != 0' failed.
/home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/Tools/sitl_run.sh: line 97: 16125 Aborted                 (core dumped) gzserver "${src_path}/Tools/sitl_gazebo/worlds/${model}.world"
SITL COMMAND: "/home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/build/px4_sitl_default/bin/px4" "/home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware"/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -t "/home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware"/test_data
INFO  [px4] Creating symlink /home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/ROMFS/px4fmu_common -> /home/cde01/Documents/PX4/Gazebo_Firmware_Test/Firmware/build/px4_sitl_default/tmp/rootfs/etc

Here is my rover.sdf model:

<?xml version='1.0'?>
<sdf version='1.5'>
  <model name="rover">
  <static>false</static>

	<link name='chassis'>
    <inertial>
      <mass>15</mass>
      <inertia>
        <ixx>3</ixx>
        <ixy>0.0</ixy>
        <iyy>7</iyy>
        <ixz>0.0</ixz>
        <iyz>0.0</iyz>
        <izz>7</izz>
      </inertia>
      <pose>0 0 0 0 0 0</pose>
    </inertial>
		<pose>0 0 .1 0 0 0</pose>
		<collision name='chassis_collision'>
			<geometry>
				<box>
					<size>.5 .3 .1</size>
				</box>
			</geometry>
		</collision>

    <visual name='chassis_visual'>
			<geometry>
				<box>
					<size>.5 .3 .1</size>
				</box>
			</geometry>
		</visual>


    <collision name='front_caster_collision'>
      <pose>0.2 0 -.05 0 0 0</pose>
      <geometry>
          <sphere>
              <radius>.05</radius>
          </sphere>
      </geometry>
      <surface>
          <friction>
              <ode>
                  <mu>0</mu>
                  <mu2>0</mu2>
                  <slip1>1.0</slip1>
                  <slip2>1.0</slip2>
              </ode>
          </friction>
      </surface>
    </collision>

		<visual name='front_caster_visual'>
      <pose>0.2 0 -.05 0 0 0</pose>
      <geometry>
          <sphere>
              <radius>.05</radius>
          </sphere>
      </geometry>
    </visual>

    <collision name='back_caster_collision'>
      <pose>-0.2 0 -.05 0 0 0</pose>
      <geometry>
          <sphere>
              <radius>.05</radius>
          </sphere>
      </geometry>
      <surface>
          <friction>
              <ode>
                  <mu>0</mu>
                  <mu2>0</mu2>
                  <slip1>1.0</slip1>
                  <slip2>1.0</slip2>
              </ode>
          </friction>
      </surface>
    </collision>

		<visual name='back_caster_visual'>
      <pose>-0.2 0 -.05 0 0 0</pose>
      <geometry>
          <sphere>
              <radius>.05</radius>
          </sphere>
      </geometry>
    </visual>

	</link>

<link name="right_wheel">
        <pose>0 -0.18 .1 0 1.5707 1.5707</pose>
        <inertial>
          <mass>.5</mass>
          <inertia>
            <ixx>0.05</ixx>
            <ixy>0.0</ixy>
            <iyy>0.05</iyy>
            <ixz>0.0</ixz>
            <iyz>0.0</iyz>
            <izz>0.1</izz>
          </inertia>
        </inertial>
        <collision name="right_wheel_collision">
          <geometry>
            <cylinder>
              <radius>.1</radius>
              <length>.05</length>
            </cylinder>
					</geometry>
          <max_contacts>1</max_contacts>
				<surface>
          <friction>
            <ode>
              <mu>1</mu>
              <mu2>1</mu2>
              <fdir1>0 0 1</fdir1>
              <slip1>0</slip1>
              <slip2>0</slip2>
            </ode>
          </friction>
          <contact>
            <ode>
              <min_depth>0.005</min_depth>
              <kp>1e8</kp>
            </ode>
          </contact>
        </surface>
				</collision>
        <visual name="right_wheel_visual">
          <geometry>
            <cylinder>
              <radius>.1</radius>
              <length>.05</length>
            </cylinder>
		</geometry>
		<material>
          <script>
            <uri>model://polaris_ranger_xp900/materials/scripts</uri>
            <uri>model://polaris_ranger_xp900/materials/textures</uri>
            <name>PolarisXP900/Diffuse</name>
          </script>
        </material>
        </visual>
</link>


<link name="left_wheel">
        <pose>0 0.18 .1 0 1.5707 1.5707</pose>
        <inertial>
          <mass>.5</mass>
          <inertia>
            <ixx>0.05</ixx>
            <ixy>0.0</ixy>
            <iyy>0.05</iyy>
            <ixz>0.0</ixz>
            <iyz>0.0</iyz>
            <izz>0.1</izz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <cylinder>
              <radius>.1</radius>
              <length>.05</length>
            </cylinder>
          </geometry>
          <max_contacts>1</max_contacts>
          <surface>
            <friction>
              <ode>
                <mu>1.0</mu>
                <mu2>1.0</mu2>
                <fdir1>0 0 1</fdir1>
                <slip1>0</slip1>
                <slip2>0</slip2>
              </ode>
            </friction>
            <contact>
              <ode>
                <min_depth>0.005</min_depth>
                <kp>1e8</kp>
              </ode>
            </contact>
          </surface>
				</collision>
        <visual name="left_wheel_visual">
          <geometry>
            <cylinder>
              <radius>.1</radius>
              <length>.05</length>
            </cylinder>
		      </geometry>
		<material>
          <script>
            <uri>model://polaris_ranger_xp900/materials/scripts</uri>
            <uri>model://polaris_ranger_xp900/materials/textures</uri>
            <name>PolarisXP900/Diffuse</name>
          </script>
        </material>
        </visual>
</link>

	<joint type='revolute' name='right_wheel_joint'>
        <pose>0 0 -0.025 0 0 0</pose>
        <child>right_wheel</child>
        <parent>chassis</parent>
        <axis>
          <xyz>0 1 0</xyz>
        </axis>
      </joint>

	  <joint type='revolute' name='left_wheel_joint'>
      <pose>0 0 -0.025 0 0 0</pose>
      <child>left_wheel</child>
      <parent>chassis</parent>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>


    <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
      <robotNamespace/>
      <pubRate>50</pubRate>
      <baroTopic>/baro</baroTopic>
    </plugin>

    <plugin name='gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
      <robotNamespace></robotNamespace>
      <linkName>rover/imu_link</linkName>
      <imuTopic>/imu</imuTopic>
      <gyroscopeNoiseDensity>0.0003394</gyroscopeNoiseDensity>
      <gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>
      <gyroscopeBiasCorrelationTime>1000.0</gyroscopeBiasCorrelationTime>
      <gyroscopeTurnOnBiasSigma>0.0087</gyroscopeTurnOnBiasSigma>
      <accelerometerNoiseDensity>0.004</accelerometerNoiseDensity>
      <accelerometerRandomWalk>0.006</accelerometerRandomWalk>
      <accelerometerBiasCorrelationTime>300.0</accelerometerBiasCorrelationTime>
      <accelerometerTurnOnBiasSigma>0.196</accelerometerTurnOnBiasSigma>
    </plugin>

    <plugin name="gps_plugin" filename="libgazebo_gps_plugin.so">
      <robotNamespace></robotNamespace>
      <gpsNoise>true</gpsNoise>
    </plugin>

    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
      <robotNamespace/>
      <pubRate>100</pubRate>
      <noiseDensity>0.0004</noiseDensity>
      <randomWalk>6.4e-06</randomWalk>
      <biasCorrelationTime>600</biasCorrelationTime>
      <magTopic>/mag</magTopic>
    </plugin>

    <plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
      <robotNamespace></robotNamespace>
      <imuSubTopic>/imu</imuSubTopic>
      <gpsSubTopic>/gps</gpsSubTopic>
      <magSubTopic>/mag</magSubTopic>
      <baroSubTopic>/baro</baroSubTopic>
      <mavlink_addr>INADDR_ANY</mavlink_addr>
      <mavlink_udp_port>14560</mavlink_udp_port>
      <serialEnabled>false</serialEnabled>
      <serialDevice>/dev/ttyACM0</serialDevice>
      <baudRate>921600</baudRate>
      <qgc_addr>INADDR_ANY</qgc_addr>
      <qgc_udp_port>14550</qgc_udp_port>
      <hil_mode>false</hil_mode>
      <hil_state_level>false</hil_state_level>
      <enable_lockstep>true</enable_lockstep>
      <use_tcp>true</use_tcp>
      <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
      <control_channels>
        <channel name="rear_left_wheel_drive">
          <input_index>5</input_index>
          <input_offset>0</input_offset>
          <input_scaling>40</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
          <joint_name>left_wheel_joint</joint_name>
          <joint_control_pid>
            <p>50</p>
            <i>20</i>
            <d>2</d>
            <iMax>800</iMax>
            <iMin>-800</iMin>
            <cmdMax>1200</cmdMax>
            <cmdMin>-1200</cmdMin>
          </joint_control_pid>
        </channel>
        <channel name="rear_right_wheel_drive">
          <input_index>6</input_index>
          <input_offset>0</input_offset>
          <input_scaling>40</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
          <joint_name>right_wheel_joint</joint_name>
          <joint_control_pid>
            <p>50</p>
            <i>20</i>
            <d>2</d>
            <iMax>800</iMax>
            <iMin>-800</iMin>
            <cmdMax>1200</cmdMax>
            <cmdMin>-1200</cmdMin>
          </joint_control_pid>
        </channel>
      </control_channels>
    </plugin>

  </model>
</sdf>

Any help would be greatly appreciated.

2

@Jaeyoung-Lim have you seen this before?

3

@JulianOes Yeap, @McKayW Are you running gazebo11? this is not officially supported yet. Running gazebo9 will fix the problem

4

@Jaeyoung-Lim I am currently running gazebo 9

5

@McKayW Is your rover.sdf file different from what is already in the repo? If this is the case, one of your gazebo dependencies are the wrong version

6

Yes, it’s a custom rover.sdf file, but it’s in the same location as the one in the repo. You can see the contents of the file in my initial comment. Do you have any idea what dependencies could be missing? I am not including anything that’s not already in the rover.sdf that’s in the repo.

7

@McKayW Can you run the normal rover.sdf file then? Would be helpful if you show what exactly you changed rather than me comparing it line by line

8

@Jaeyoung-Lim yes I have no problems running the default rover.sdf file.

Yes I understand. All I can say is that the whole rover model itself is different, the only things that are the same are the sensor and MAVLink interface plugins. Naturally, I have changed the control channels to point to the joints of my rover but that’s about it. Although, I can’t imagine that changing the links and joints would cause this connection failure, assuming that the control channels are pointing to the joints of the rover. Any ideas?

Thankyou in advance for your help, I understand these questions can be frustrating to answer sometimes.

9

@McKayW No problem,

The error you are seeing is gazebo complaining it cannot find one of the resources. I would suggest you go back to the original rover sdf and try to pull in your changes one by one to see what exactly is causing your error. Then it would be a lot easier to narrow the issue down

10

@Jaeyoung-Lim so it turns out that if I open a new terminal and run an instance of gzserver before starting the px4 daemon, then the connection issue goes away. It seems as though there is some sort of script that is not run which would boot up gzserver? However, when I do this, the gazebo GUI comes up but my model is not there. I imagine whatever script is not being run is also responsible for loading the model as well. Do you have any thoughts as to what might be the issue here?

11

@Jaeyoung-Lim what’s more is that I can see the line that is failing is:

gzserver "${src_path}/Tools/sitl_gazebo/worlds/${model}.world" & SIM_PID='echo $!'

This seems to return a connection issue when it’s run from sitl_run.sh. However, when I run this command in isolation in a terminal, it doesn’t return any issues.

12

@McKayW You can add gzserver --verbose to further check what it is complaining about

13

@Jaeyoung-Lim so I had more of a dig around yesterday arvo and it turns out that the IMU plugin requires a link in order to be defined. So gzserver was failing because it wasn’t loading the plugin correctly. Still unsure as to why it wasn’t returning issues when I ran it in isolation, but it seems to be all fixed now.

Thankyou so much for your help mate, really appreciate you taking the time to reply to me.

14

@Jaeyoung-Lim so after being able to load my model, I’ve come across a different issue. When I punch in make px4_sitl gazebo_rover with my custom rover.sdf file, everything loads up, but the rover’s wheels are vibrating (small in magnitude, but relatively quickly). This results in my rover sliding all over the place and I’m wondering if you might know why this is? I have examined the actuator_controls_0 and actuator_outputs and they are not sending anything. I have double checked that the “input index” parameter in the control channel inside the mavlink interface plugin in the .sdf file has been set to the correct output from the mixer file. I’m a bit at a loss as to what might be causing this. Do you have any thoughts on where I should be looking?

Thanks mate.

15

Found the issue. The D gain on the velocity controller was too high for my model.