Open
Description
I am trialing a local model with rgbd sensors in the urban circuits:
simple_urban_01
simple_urban_02
urban_circuit_02
I get an exception and core dump when crossing the the first turn down the tunnel from the team base room soon after scoring has started. I did not get this with X3_SENSOR_CONFIG_1 configuration. I am running with the localModel argument set to true.
Dbg] [EntityComponentManager.cc:1091] Updated state thread iterators: 8 threads processing around 66 components each.
[Msg] Scoring has Started
terminate called after throwing an instance of 'std::out_of_range'
what(): map::at
Stack trace (most recent call last) in thread 226:
#15 Object "", at 0xffffffffffffffff, in
#14 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f54d6da271e, in clone
#13 Object "/lib/x86_64-linux-gnu/libpthread.so.0", at 0x7f54d6a696da, in start_thread
#12 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f54d3a6e6de, in std::error_code::default_error_condition() const
#11 Object "/usr/lib/x86_64-linux-gnu/libignition-gazebo4.so.4", at 0x7f54d268b209, in GetRecordPluginElem(sdf::v10::Root&)
#10 Object "/usr/lib/x86_64-linux-gnu/libignition-gazebo4.so.4", at 0x7f54d2695dea, in ignition::gazebo::v4::SimulationRunner::Run(unsigned long)
#9 Object "/usr/lib/x86_64-linux-gnu/libignition-gazebo4.so.4", at 0x7f54d26956e6, in ignition::gazebo::v4::SimulationRunner::Step(ignition::gazebo::v4::UpdateInfo const&)
#8 Object "/usr/lib/x86_64-linux-gnu/libignition-gazebo4.so.4", at 0x7f54d268fc51, in ignition::gazebo::v4::SimulationRunner::UpdateSystems()
#7 Object "/home/developer/subt_ws/install/lib/libGameLogicPlugin.so", at 0x7f54a101d583, in subt::GameLogicPlugin::PreUpdate(ignition::gazebo::v4::UpdateInfo const&, ignition::gazebo::v4::EntityComponentManager&)
#6 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f54d3a3f836, in
#5 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f54d3a43d53, in __cxa_throw
#4 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f54d3a43b20, in std::terminate()
#3 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f54d3a43ae5, in std::rethrow_exception(std::__exception_ptr::exception_ptr)
#2 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f54d3a3d956, in
#1 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f54d6cc1920, in abort
#0 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f54d6cbffb7, in gsignal
Aborted (Signal sent by tkill() 52 1000)
./run_sim.bash: line 6: 52 Aborted (core dumped) ign launch -v 4 $@
The SDF for the model includes additional rgbd sensors beneath the existing eg add:
<sensor name="camera_port" type="rgbd_camera">
<pose>0 0.1 0 0 0 1.570796</pose>
<always_on>1</always_on>
<update_rate>20</update_rate>
<camera name="camera_port">
<horizontal_fov>1.0472</horizontal_fov>
<lens>
<intrinsics>
<!-- fx = fy = width / ( 2 * tan (hfov / 2 ) ) -->
<fx>554.3</fx>
<fy>554.3</fy>
<!-- cx = ( width + 1 ) / 2 -->
<cx>320.5</cx>
<!-- cy = ( height + 1 ) / 2 -->
<cy>240.5</cy>
<s>0</s>
</intrinsics>
</lens>
<distortion>
<k1>0.0</k1>
<k2>0.0</k2>
<k3>0.0</k3>
<p1>0.0</p1>
<p2>0.0</p2>
<center>0.5 0.5</center>
</distortion>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.01</near>
<far>300</far>
</clip>
<depth_camera>
<clip>
<near>0.1</near>
<far>10</far>
</clip>
</depth_camera>
<noise>
<type>gaussian</type>
<mean>0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
<sensor name="camera_starboard" type="rgbd_camera">
<pose>0 -0.1 0 0 0 -1.570796</pose>
<always_on>1</always_on>
<update_rate>20</update_rate>
<camera name="camera_starboard">
<horizontal_fov>1.0472</horizontal_fov>
<lens>
<intrinsics>
<!-- fx = fy = width / ( 2 * tan (hfov / 2 ) ) -->
<fx>554.3</fx>
<fy>554.3</fy>
<!-- cx = ( width + 1 ) / 2 -->
<cx>320.5</cx>
<!-- cy = ( height + 1 ) / 2 -->
<cy>240.5</cy>
<s>0</s>
</intrinsics>
</lens>
<distortion>
<k1>0.0</k1>
<k2>0.0</k2>
<k3>0.0</k3>
<p1>0.0</p1>
<p2>0.0</p2>
<center>0.5 0.5</center>
</distortion>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.01</near>
<far>300</far>
</clip>
<depth_camera>
<clip>
<near>0.1</near>
<far>10</far>
</clip>
</depth_camera>
<noise>
<type>gaussian</type>
<mean>0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
<sensor name="camera_down" type="rgbd_camera">
<pose>0 0 -0.05 0 1.570796 0</pose>
<always_on>1</always_on>
<update_rate>20</update_rate>
<camera name="camera_down">
<horizontal_fov>1.0472</horizontal_fov>
<lens>
<intrinsics>
<!-- fx = fy = width / ( 2 * tan (hfov / 2 ) ) -->
<fx>554.3</fx>
<fy>554.3</fy>
<!-- cx = ( width + 1 ) / 2 -->
<cx>320.5</cx>
<!-- cy = ( height + 1 ) / 2 -->
<cy>240.5</cy>
<s>0</s>
</intrinsics>
</lens>
<distortion>
<k1>0.0</k1>
<k2>0.0</k2>
<k3>0.0</k3>
<p1>0.0</p1>
<p2>0.0</p2>
<center>0.5 0.5</center>
</distortion>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.01</near>
<far>300</far>
</clip>
<depth_camera>
<clip>
<near>0.1</near>
<far>10</far>
</clip>
</depth_camera>
<noise>
<type>gaussian</type>
<mean>0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>