Implement proper tracked motion model

submitted_models/ctu_cras_norlab_absolem_sensor_config_1/config/common.yaml

@@ -1,5 +1,8 @@
 revision: 2014  # 2014 for TRADR robot, 2021 for upgraded robot
 
+wheels_instead_of_tracks: False
+track_mu: 10
+
 big_collision_box_on_top: False
 big_collision_box_height: 0.5
 big_collision_box_width: 0.5

submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/common.rb

@@ -1,4 +1,16 @@
 def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _additionalSDF, _max_velocity=0.4, _max_acceleration=3)
+  # read robot configuration from the config/ dir to access robot geometry information
+  require 'yaml'
+  confdir = File.join(__dir__, '..', 'config')
+  config_yamls = [ File.join(confdir, 'common.yaml'), File.join(confdir, 'sdf.yaml') ]
+  config = Hash.new
+  for config_yaml in config_yamls
+    conf = YAML.load_file(config_yaml)
+    config.merge!(conf)
+  end
+
+  useWheels = config["wheels_instead_of_tracks"]
+
   numTrackWheels = 8
   numFlipperWheels = 5
 
@@ -22,53 +34,109 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
     diffdriveJoints[i] = ""
   end
   wheelSlip = ""
+  trackControllers = ""
+  trackLinks = ""
+  power_draining_topics = ""
 
   for track in tracks
-    for wheel in 1..numTrackWheels
-      diffdriveJoints[0] += "<#{track}_joint>#{track}_track_wheel#{wheel}_j</#{track}_joint>\n"
-      wheelSlip += <<-HEREDOC
-        <wheel link_name="#{track}_track_wheel#{wheel}">
-          <slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
-          <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
-          <wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
-          <wheel_radius>#{wheelRadiuses[0]}</wheel_radius>
-        </wheel>
-      HEREDOC
+    if useWheels
+        for wheel in 1..numTrackWheels
+          diffdriveJoints[0] += "<#{track}_joint>#{track}_track_wheel#{wheel}_j</#{track}_joint>\n"
+          wheelSlip += <<-HEREDOC
+            <wheel link_name="#{track}_track_wheel#{wheel}">
+              <slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
+              <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+              <wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
+              <wheel_radius>#{wheelRadiuses[0]}</wheel_radius>
+            </wheel>
+          HEREDOC
+        end
+    else
+        trackControllers += <<-HEREDOC
+          <plugin name='ignition::gazebo::systems::TrackController' filename='libignition-gazebo-track-controller-system.so'>
+              <link>#{track}_track</link>
+              <min_velocity>-#{_max_velocity}</min_velocity>
+              <max_velocity>#{_max_velocity}</max_velocity>
+              <min_acceleration>-#{_max_acceleration}</min_acceleration>
+              <max_acceleration>#{_max_acceleration}</max_acceleration>
+          </plugin>
+        HEREDOC
+        trackLinks += "<#{track}_track><link>#{track}_track</link></#{track}_track>"
+        power_draining_topics += "<power_draining_topic>/model/#{_name}/link/#{track}_track/track_cmd_vel</power_draining_topic>"
     end
     for flipper in flippersOfTrack[track]
-      for wheel in 1..numFlipperWheels
-        diffdriveJoints[wheel-1] += "<#{track}_joint>#{flipper}_flipper_wheel#{wheel}_j</#{track}_joint>\n"
-        wheelSlip += <<-HEREDOC
-          <wheel link_name="#{flipper}_flipper_wheel#{wheel}">
-            <slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
-            <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
-            <wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
-            <wheel_radius>#{wheelRadiuses[wheel-1]}</wheel_radius>
-          </wheel>
+      if useWheels
+          for wheel in 1..numFlipperWheels
+            diffdriveJoints[wheel-1] += "<#{track}_joint>#{flipper}_flipper_wheel#{wheel}_j</#{track}_joint>\n"
+            wheelSlip += <<-HEREDOC
+              <wheel link_name="#{flipper}_flipper_wheel#{wheel}">
+                <slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
+                <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
+                <wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
+                <wheel_radius>#{wheelRadiuses[wheel-1]}</wheel_radius>
+              </wheel>
+            HEREDOC
+          end
+      else
+        trackControllers += <<-HEREDOC
+          <plugin name='ignition::gazebo::systems::TrackController' filename='libignition-gazebo-track-controller-system.so'>
+              <link>#{flipper}_flipper</link>
+              <min_velocity>-#{_max_velocity}</min_velocity>
+              <max_velocity>#{_max_velocity}</max_velocity>
+              <min_acceleration>-#{_max_acceleration}</min_acceleration>
+              <max_acceleration>#{_max_acceleration}</max_acceleration>
+          </plugin>
         HEREDOC
+        trackLinks += "<#{track}_track><link>#{flipper}_flipper</link></#{track}_track>"
+        power_draining_topics += "<power_draining_topic>/model/#{_name}/link/#{flipper}_flipper/track_cmd_vel</power_draining_topic>"
       end
     end
   end
 
   diffDrive = ""
-  for wheel in 0...numFlipperWheels
-    # we only want odometry from the first diffdrive
-    no_odom = ""
-    if wheel > 0
-      no_odom = "<odom_topic>unused_odom</odom_topic>\n"
-    end
-    diffDrive += <<-HEREDOC
-      <plugin filename="ignition-gazebo-diff-drive-system" name="ignition::gazebo::systems::DiffDrive">
-        #{diffdriveJoints[wheel]}
-        <wheel_separation>#{wheelSeparation}</wheel_separation>
-        <wheel_radius>#{wheelRadiuses[wheel]}</wheel_radius>
-        <topic>/model/#{_name}/cmd_vel_relay</topic>
-        <min_velocity>-#{_max_velocity}</min_velocity>
-        <max_velocity>#{_max_velocity}</max_velocity>
-        <min_acceleration>-#{_max_acceleration}</min_acceleration>
-        <max_acceleration>#{_max_acceleration}</max_acceleration>
-        #{no_odom}
-      </plugin>
+  trackedVehicle = ""
+  wheelSlipPlugin = ""
+  if useWheels
+      for wheel in 0...numFlipperWheels
+        # we only want odometry from the first diffdrive
+        no_odom = ""
+        if wheel > 0
+          no_odom = "<odom_topic>unused_odom</odom_topic>\n"
+        end
+        diffDrive += <<-HEREDOC
+          <plugin filename="ignition-gazebo-diff-drive-system" name="ignition::gazebo::systems::DiffDrive">
+            #{diffdriveJoints[wheel]}
+            <wheel_separation>#{wheelSeparation}</wheel_separation>
+            <wheel_radius>#{wheelRadiuses[wheel]}</wheel_radius>
+            <topic>/model/#{_name}/cmd_vel_relay</topic>
+            <min_velocity>-#{_max_velocity}</min_velocity>
+            <max_velocity>#{_max_velocity}</max_velocity>
+            <min_acceleration>-#{_max_acceleration}</min_acceleration>
+            <max_acceleration>#{_max_acceleration}</max_acceleration>
+            #{no_odom}
+          </plugin>
+        HEREDOC
+      end
+      wheelSlipPlugin = <<-HEREDOC
+        <plugin filename="ignition-gazebo-wheel-slip-system" name="ignition::gazebo::systems::WheelSlip">
+          #{wheelSlip}
+        </plugin>
+      HEREDOC
+  else
+    trackedVehicle += <<-HEREDOC
+        <plugin name="ignition::gazebo::systems::TrackedVehicle" filename="ignition-gazebo-tracked-vehicle-system">
+              #{trackLinks}
+              <tracks_separation>#{wheelSeparation}</tracks_separation>
+              <tracks_height>0.18094</tracks_height>
+              <steering_efficiency>0.5</steering_efficiency>
+              <topic>/model/#{_name}/cmd_vel_relay</topic>
+              <linear_velocity>
+                  <min_velocity>-#{_max_velocity}</min_velocity>
+                  <max_velocity>#{_max_velocity}</max_velocity>
+                  <min_acceleration>-#{_max_acceleration}</min_acceleration>
+                  <max_acceleration>#{_max_acceleration}</max_acceleration>
+              </linear_velocity>
+          </plugin>
     HEREDOC
   end
 
@@ -85,6 +153,8 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
       <uri>#{_modelURI}</uri>
       <!-- Diff drive -->
       #{diffDrive}
+      #{trackedVehicle}
+      #{trackControllers}
       <!-- Publish robot state information -->
       <plugin filename="libignition-gazebo-pose-publisher-system.so"
         name="ignition::gazebo::systems::PosePublisher">
@@ -110,6 +180,7 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
         <smooth_current_tau>1.9499</smooth_current_tau>
         <power_load>4.95</power_load>
         <start_on_motion>true</start_on_motion>
+        #{power_draining_topics}
       </plugin>
       <!-- Gas Sensor plugin -->
       <plugin filename="libGasEmitterDetectorPlugin.so"
@@ -119,9 +190,7 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
         <type>gas</type>
       </plugin>
       <!-- Wheel slip -->
-      <plugin filename="ignition-gazebo-wheel-slip-system" name="ignition::gazebo::systems::WheelSlip">
-        #{wheelSlip}
-      </plugin>
+      #{wheelSlipPlugin}
       #{_additionalSDF}
     </include>
     </sdf>