Skip to content

Commit b626b6d

Browse files
peci1peci1
authored
Implement proper tracked motion model (#958)
* Absolem: fix bad merge. * Implement more plausible model of tracks. * Merge upstream thermal cam settings for MARV SC3 * Update with latest changes. * Removed empty wheel slip plugin. * Use power_draining_topic to start battery drain for tracked vehicles.
1 parent 8c1e339 commit b626b6d

File tree

17 files changed

+10656
-29434
lines changed

17 files changed

+10656
-29434
lines changed

‎submitted_models/ctu_cras_norlab_absolem_sensor_config_1/config/common.yaml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,8 @@
11
revision: 2014 # 2014 for TRADR robot, 2021 for upgraded robot
22

3+
wheels_instead_of_tracks: False
4+
track_mu: 10
5+
36
big_collision_box_on_top: False
47
big_collision_box_height: 0.5
58
big_collision_box_width: 0.5

‎submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/common.rb‎

Lines changed: 109 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,16 @@
11
def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _additionalSDF, _max_velocity=0.4, _max_acceleration=3)
2+
# read robot configuration from the config/ dir to access robot geometry information
3+
require 'yaml'
4+
confdir = File.join(__dir__, '..', 'config')
5+
config_yamls = [ File.join(confdir, 'common.yaml'), File.join(confdir, 'sdf.yaml') ]
6+
config = Hash.new
7+
for config_yaml in config_yamls
8+
conf = YAML.load_file(config_yaml)
9+
config.merge!(conf)
10+
end
11+
12+
useWheels = config["wheels_instead_of_tracks"]
13+
214
numTrackWheels = 8
315
numFlipperWheels = 5
416

@@ -22,53 +34,109 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
2234
diffdriveJoints[i] = ""
2335
end
2436
wheelSlip = ""
37+
trackControllers = ""
38+
trackLinks = ""
39+
power_draining_topics = ""
2540

2641
for track in tracks
27-
for wheel in 1..numTrackWheels
28-
diffdriveJoints[0] += "<#{track}_joint>#{track}_track_wheel#{wheel}_j</#{track}_joint>\n"
29-
wheelSlip += <<-HEREDOC
30-
<wheel link_name="#{track}_track_wheel#{wheel}">
31-
<slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
32-
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
33-
<wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
34-
<wheel_radius>#{wheelRadiuses[0]}</wheel_radius>
35-
</wheel>
36-
HEREDOC
42+
if useWheels
43+
for wheel in 1..numTrackWheels
44+
diffdriveJoints[0] += "<#{track}_joint>#{track}_track_wheel#{wheel}_j</#{track}_joint>\n"
45+
wheelSlip += <<-HEREDOC
46+
<wheel link_name="#{track}_track_wheel#{wheel}">
47+
<slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
48+
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
49+
<wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
50+
<wheel_radius>#{wheelRadiuses[0]}</wheel_radius>
51+
</wheel>
52+
HEREDOC
53+
end
54+
else
55+
trackControllers += <<-HEREDOC
56+
<plugin name='ignition::gazebo::systems::TrackController' filename='libignition-gazebo-track-controller-system.so'>
57+
<link>#{track}_track</link>
58+
<min_velocity>-#{_max_velocity}</min_velocity>
59+
<max_velocity>#{_max_velocity}</max_velocity>
60+
<min_acceleration>-#{_max_acceleration}</min_acceleration>
61+
<max_acceleration>#{_max_acceleration}</max_acceleration>
62+
</plugin>
63+
HEREDOC
64+
trackLinks += "<#{track}_track><link>#{track}_track</link></#{track}_track>"
65+
power_draining_topics += "<power_draining_topic>/model/#{_name}/link/#{track}_track/track_cmd_vel</power_draining_topic>"
3766
end
3867
for flipper in flippersOfTrack[track]
39-
for wheel in 1..numFlipperWheels
40-
diffdriveJoints[wheel-1] += "<#{track}_joint>#{flipper}_flipper_wheel#{wheel}_j</#{track}_joint>\n"
41-
wheelSlip += <<-HEREDOC
42-
<wheel link_name="#{flipper}_flipper_wheel#{wheel}">
43-
<slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
44-
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
45-
<wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
46-
<wheel_radius>#{wheelRadiuses[wheel-1]}</wheel_radius>
47-
</wheel>
68+
if useWheels
69+
for wheel in 1..numFlipperWheels
70+
diffdriveJoints[wheel-1] += "<#{track}_joint>#{flipper}_flipper_wheel#{wheel}_j</#{track}_joint>\n"
71+
wheelSlip += <<-HEREDOC
72+
<wheel link_name="#{flipper}_flipper_wheel#{wheel}">
73+
<slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
74+
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
75+
<wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
76+
<wheel_radius>#{wheelRadiuses[wheel-1]}</wheel_radius>
77+
</wheel>
78+
HEREDOC
79+
end
80+
else
81+
trackControllers += <<-HEREDOC
82+
<plugin name='ignition::gazebo::systems::TrackController' filename='libignition-gazebo-track-controller-system.so'>
83+
<link>#{flipper}_flipper</link>
84+
<min_velocity>-#{_max_velocity}</min_velocity>
85+
<max_velocity>#{_max_velocity}</max_velocity>
86+
<min_acceleration>-#{_max_acceleration}</min_acceleration>
87+
<max_acceleration>#{_max_acceleration}</max_acceleration>
88+
</plugin>
4889
HEREDOC
90+
trackLinks += "<#{track}_track><link>#{flipper}_flipper</link></#{track}_track>"
91+
power_draining_topics += "<power_draining_topic>/model/#{_name}/link/#{flipper}_flipper/track_cmd_vel</power_draining_topic>"
4992
end
5093
end
5194
end
5295

5396
diffDrive = ""
54-
for wheel in 0...numFlipperWheels
55-
# we only want odometry from the first diffdrive
56-
no_odom = ""
57-
if wheel > 0
58-
no_odom = "<odom_topic>unused_odom</odom_topic>\n"
59-
end
60-
diffDrive += <<-HEREDOC
61-
<plugin filename="ignition-gazebo-diff-drive-system" name="ignition::gazebo::systems::DiffDrive">
62-
#{diffdriveJoints[wheel]}
63-
<wheel_separation>#{wheelSeparation}</wheel_separation>
64-
<wheel_radius>#{wheelRadiuses[wheel]}</wheel_radius>
65-
<topic>/model/#{_name}/cmd_vel_relay</topic>
66-
<min_velocity>-#{_max_velocity}</min_velocity>
67-
<max_velocity>#{_max_velocity}</max_velocity>
68-
<min_acceleration>-#{_max_acceleration}</min_acceleration>
69-
<max_acceleration>#{_max_acceleration}</max_acceleration>
70-
#{no_odom}
71-
</plugin>
97+
trackedVehicle = ""
98+
wheelSlipPlugin = ""
99+
if useWheels
100+
for wheel in 0...numFlipperWheels
101+
# we only want odometry from the first diffdrive
102+
no_odom = ""
103+
if wheel > 0
104+
no_odom = "<odom_topic>unused_odom</odom_topic>\n"
105+
end
106+
diffDrive += <<-HEREDOC
107+
<plugin filename="ignition-gazebo-diff-drive-system" name="ignition::gazebo::systems::DiffDrive">
108+
#{diffdriveJoints[wheel]}
109+
<wheel_separation>#{wheelSeparation}</wheel_separation>
110+
<wheel_radius>#{wheelRadiuses[wheel]}</wheel_radius>
111+
<topic>/model/#{_name}/cmd_vel_relay</topic>
112+
<min_velocity>-#{_max_velocity}</min_velocity>
113+
<max_velocity>#{_max_velocity}</max_velocity>
114+
<min_acceleration>-#{_max_acceleration}</min_acceleration>
115+
<max_acceleration>#{_max_acceleration}</max_acceleration>
116+
#{no_odom}
117+
</plugin>
118+
HEREDOC
119+
end
120+
wheelSlipPlugin = <<-HEREDOC
121+
<plugin filename="ignition-gazebo-wheel-slip-system" name="ignition::gazebo::systems::WheelSlip">
122+
#{wheelSlip}
123+
</plugin>
124+
HEREDOC
125+
else
126+
trackedVehicle += <<-HEREDOC
127+
<plugin name="ignition::gazebo::systems::TrackedVehicle" filename="ignition-gazebo-tracked-vehicle-system">
128+
#{trackLinks}
129+
<tracks_separation>#{wheelSeparation}</tracks_separation>
130+
<tracks_height>0.18094</tracks_height>
131+
<steering_efficiency>0.5</steering_efficiency>
132+
<topic>/model/#{_name}/cmd_vel_relay</topic>
133+
<linear_velocity>
134+
<min_velocity>-#{_max_velocity}</min_velocity>
135+
<max_velocity>#{_max_velocity}</max_velocity>
136+
<min_acceleration>-#{_max_acceleration}</min_acceleration>
137+
<max_acceleration>#{_max_acceleration}</max_acceleration>
138+
</linear_velocity>
139+
</plugin>
72140
HEREDOC
73141
end
74142

@@ -85,6 +153,8 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
85153
<uri>#{_modelURI}</uri>
86154
<!-- Diff drive -->
87155
#{diffDrive}
156+
#{trackedVehicle}
157+
#{trackControllers}
88158
<!-- Publish robot state information -->
89159
<plugin filename="libignition-gazebo-pose-publisher-system.so"
90160
name="ignition::gazebo::systems::PosePublisher">
@@ -110,6 +180,7 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
110180
<smooth_current_tau>1.9499</smooth_current_tau>
111181
<power_load>4.95</power_load>
112182
<start_on_motion>true</start_on_motion>
183+
#{power_draining_topics}
113184
</plugin>
114185
<!-- Gas Sensor plugin -->
115186
<plugin filename="libGasEmitterDetectorPlugin.so"
@@ -119,9 +190,7 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
119190
<type>gas</type>
120191
</plugin>
121192
<!-- Wheel slip -->
122-
<plugin filename="ignition-gazebo-wheel-slip-system" name="ignition::gazebo::systems::WheelSlip">
123-
#{wheelSlip}
124-
</plugin>
193+
#{wheelSlipPlugin}
125194
#{_additionalSDF}
126195
</include>
127196
</sdf>

0 commit comments

Comments
 (0)