PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
8.21k stars 13.38k forks source link

SITL v1.14: gazebo sim crashes on start [Arch] #21386

Open PSandro opened 1 year ago

PSandro commented 1 year ago

Describe the bug

When I run px4_sitl with gazebo sim (v7.4.0) on the default world and any drone, gazebo opens up and the px4 simulator starts to initialize but at some point before the drone is visible in gazebo, there is an assertion error and stacktrace printed in the gazebo output; the simulation does not continue to initialize. No drone is shown. An important notice: I'm running on an ArchLinux.

To Reproduce

Steps to reproduce the behavior:

  1. Install ignition-gazebo on ArchLinux AUR
  2. use px4 with tag v1.14.0-beta2
  3. select a world that renders with ogre or edit Tools/simulation/gz/worlds/default.sdf to ogre
  4. run simulation with make px4_sitl gz_x500 4.1 (optional) enable gazebo verbose debug output
  5. gazebo tries to start but crashes at some point (see error)

Expected behavior

I would expect the simulation with gazebo to start normally as it does on my Ubuntu 22.04 machine.

Error/Logs

image Here is the full log:

make px4_sitl gz_x500

``` [sandro@arch ~/px4/px4]$ make px4_sitl gz_x500 [0/1] cd /home/sandro/px4/px4/build/px4_sitl_default/src/modules/simulation/gz_bridge && /usr/bin/cmake -E env PX4_SIM_MODEL=gz_x500 /home/sandro/px4/px4/build/px4_sitl_default/bin/px4 ______ __ __ ___ | ___ \ \ \ / / / | | |_/ / \ V / / /| | | __/ / \ / /_| | | | / /^\ \ \___ | \_| \/ \/ |_/ px4 starting. INFO [px4] startup script: /bin/sh etc/init.d-posix/rcS 0 INFO [init] found model autostart file as SYS_AUTOSTART=4001 INFO [param] selected parameter default file parameters.bson INFO [param] importing from 'parameters.bson' INFO [parameters] BSON document size 251 bytes, decoded 251 bytes (INT32:11, FLOAT:2) INFO [param] selected parameter backup file parameters_backup.bson INFO [dataman] data manager file './dataman' size is 7866640 bytes INFO [init] starting gazebo with world: /home/sandro/px4/px4/Tools/simulation/gz/worlds/default.sdf WARN [init] PX4_GZ_MODEL_NAME or PX4_GZ_MODEL not set using PX4_SIM_MODEL. INFO [gz_bridge] world: default, model name: x500_0, simulation model: x500 [Msg] Gazebo Sim Server v7.4.0 [Msg] Loading SDF world file[/home/sandro/px4/px4/Tools/simulation/gz/worlds/default.sdf]. [Msg] Serving entity system service on [/entity/system/add] [Dbg] [Physics.cc:869] Loaded [gz::physics::dartsim::Plugin] from library [/usr/lib/gz-physics-6/engine-plugins/libgz-physics-dartsim-plugin.so] [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::Physics] for entity [1] [Msg] Create service on [/world/default/create] [Msg] Remove service on [/world/default/remove] [Msg] Pose service on [/world/default/set_pose] [Msg] Pose service on [/world/default/set_pose_vector] [Msg] Light configuration service on [/world/default/light_config] [Msg] Physics service on [/world/default/set_physics] [Msg] SphericalCoordinates service on [/world/default/set_spherical_coordinates] [Msg] Enable collision service on [/world/default/enable_collision] [Msg] Disable collision service on [/world/default/disable_collision] [Msg] Material service on [/world/default/visual_config] [Msg] Material service on [/world/default/wheel_slip] [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::UserCommands] for entity [1] [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::SceneBroadcaster] for entity [1] [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::Contact] for entity [1] [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::Imu] for entity [1] [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::AirPressure] for entity [1] [Dbg] [Sensors.cc:501] Configuring Sensors system [Dbg] [Sensors.cc:414] SensorsPrivate::Run [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::Sensors] for entity [1] [Dbg] [Sensors.cc:392] SensorsPrivate::RenderThread started [Dbg] [Sensors.cc:234] Waiting for init [Msg] Loaded level [3] [Msg] Serving world controls on [/world/default/control], [/world/default/control/state] and [/world/default/playback/control] [Msg] Serving GUI information on [/world/default/gui/info] [Msg] World [default] initialized with [default_physics] physics profile. [Msg] Serving world SDF generation service on [/world/default/generate_world_sdf] [Msg] Serving world names on [/gazebo/worlds] [Msg] Resource path add service on [/gazebo/resource_paths/add]. [Msg] Resource path get service on [/gazebo/resource_paths/get]. [Msg] Resource path resolve service on [/gazebo/resource_paths/resolve]. [Msg] Resource paths published on [/gazebo/resource_paths]. [Msg] Server control service on [/server_control]. INFO [commander] LED: open /dev/led0 failed (22) WARN [health_and_arming_checks] Preflight Fail: Accel Sensor 0 missing WARN [health_and_arming_checks] Preflight Fail: barometer 0 missing WARN [health_and_arming_checks] Preflight Fail: ekf2 missing data WARN [health_and_arming_checks] Preflight Fail: Gyro Sensor 0 missing WARN [health_and_arming_checks] Preflight Fail: Compass Sensor 0 missing INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550 INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540 INFO [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030 INFO [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280 INFO [logger] logger started (mode=all) INFO [logger] Start file log (type: full) INFO [logger] [logger] ./log/2023-03-28/23_59_21.ulg INFO [logger] Opened full log file: ./log/2023-03-28/23_59_21.ulg INFO [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network) INFO [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network) INFO [px4] Startup script returned successfully pxh> [Msg] Found no publishers on /stats, adding root stats topic [Msg] Found no publishers on /clock, adding root clock topic [Dbg] [SimulationRunner.cc:513] Creating PostUpdate worker threads: 6 [Dbg] [SimulationRunner.cc:524] Creating postupdate worker thread (0) [Dbg] [SimulationRunner.cc:524] Creating postupdate worker thread (1) [Dbg] [SimulationRunner.cc:524] Creating postupdate worker thread (2) [Dbg] [SimulationRunner.cc:524] Creating postupdate worker thread (3) [Dbg] [SimulationRunner.cc:524] Creating postupdate worker thread (4) [Wrn] [MulticopterMotorModel.cc:257] No robotNamespace set using entity name. [Dbg] [MulticopterMotorModel.cc:375] Listening to topic: x500_0/command/motor_speed [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::MulticopterMotorModel] for entity [10] [Wrn] [MulticopterMotorModel.cc:257] No robotNamespace set using entity name. [Dbg] [MulticopterMotorModel.cc:375] Listening to topic: x500_0/command/motor_speed [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::MulticopterMotorModel] for entity [10] [Wrn] [MulticopterMotorModel.cc:257] No robotNamespace set using entity name. [Dbg] [MulticopterMotorModel.cc:375] Listening to topic: x500_0/command/motor_speed [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::MulticopterMotorModel] for entity [10] [Wrn] [MulticopterMotorModel.cc:257] No robotNamespace set using entity name. [Dbg] [MulticopterMotorModel.cc:375] Listening to topic: x500_0/command/motor_speed [Dbg] [SystemManager.cc:74] Loaded system [gz::sim::systems::MulticopterMotorModel] for entity [10] [Dbg] [UserCommands.cc:1263] Created entity [10] named [x500_0] /usr/include/c++/12.2.1/bits/stl_vector.h:1123: std::vector<_Tp, _Alloc>::reference std::vector<_Tp, _Alloc>::operator[](size_type) [with _Tp = dart::dynamics::Skeleton::DataCache; _Alloc = Eigen::aligned_allocator; reference = dart::dynamics::Skeleton::DataCache&; size_type = long unsigned int]: Assertion '__n < this->size()' failed. Stack trace (most recent call last): #31 Object "/usr/lib/libruby.so.3.0", at 0x7f2e1604760e, in #30 Object "/usr/lib/libruby.so.3.0", at 0x7f2e16044829, in #29 Object "/usr/lib/libruby.so.3.0", at 0x7f2e1603b57c, in #28 Object "/usr/lib/ruby/3.0.0/x86_64-linux/fiddle.so", at 0x7f2e161e3acc, in #27 Object "/usr/lib/libruby.so.3.0", at 0x7f2e1600e0f5, in rb_nogvl #26 Object "/usr/lib/ruby/3.0.0/x86_64-linux/fiddle.so", at 0x7f2e161e33bc, in #25 Object "/usr/lib/libffi.so.8", at 0x7f2e1594cb72, in ffi_call #24 Object "/usr/lib/libffi.so.8", at 0x7f2e15949f5d, in #23 Object "/usr/lib/libffi.so.8", at 0x7f2e1594d4f5, in #22 Object "/usr/lib/libgz-sim7-gz.so.7.4.0", at 0x7f2e120c0940, in runServer #21 Object "/usr/lib/libgz-sim7.so.7", at 0x7f2e10ff09c2, in #20 Object "/usr/lib/libgz-sim7.so.7", at 0x7f2e1100438b, in gz::sim::v7::SimulationRunner::Run(unsigned long) #19 Object "/usr/lib/libgz-sim7.so.7", at 0x7f2e11003a28, in gz::sim::v7::SimulationRunner::Step(gz::sim::v7::UpdateInfo const&) #18 Object "/usr/lib/libgz-sim7.so.7", at 0x7f2e10ff8d91, in gz::sim::v7::SimulationRunner::UpdateSystems() #17 Object "/usr/lib/gz-sim-7/plugins/libgz-sim-physics-system.so", at 0x7f2e04520a6b, in gz::sim::v7::systems::Physics::Update(gz::sim::v7::UpdateInfo const&, gz::sim::v7::EntityComponentManager&) #16 Object "/usr/lib/gz-sim-7/plugins/libgz-sim-physics-system.so", at 0x7f2e0450cce6, in gz::sim::v7::systems::PhysicsPrivate::CreatePhysicsEntities(gz::sim::v7::EntityComponentManager const&, bool) #15 Object "/usr/lib/gz-sim-7/plugins/libgz-sim-physics-system.so", at 0x7f2e0450bd0e, in gz::sim::v7::systems::PhysicsPrivate::CreateModelEntities(gz::sim::v7::EntityComponentManager const&, bool) #14 Object "/usr/lib/gz-sim-7/plugins/libgz-sim-physics-system.so", at 0x7f2e04597f10, in void gz::sim::v7::EntityComponentManager::EachNew, gz::sim::v7::components::ModelTag, gz::sim::v7::serializers::DefaultSerializer > >, gz::sim::v7::components::Component, std::allocator >, gz::sim::v7::components::NameTag, gz::sim::v7::serializers::StringSerializer>, gz::sim::v7::components::Component, gz::sim::v7::components::PoseTag, gz::sim::v7::serializers::DefaultSerializer > >, gz::sim::v7::components::Component > >(gz::sim::v7::EntityComponentManager::identity, gz::sim::v7::components::ModelTag, gz::sim::v7::serializers::DefaultSerializer > > const*, gz::sim::v7::components::Component, std::allocator >, gz::sim::v7::components::NameTag, gz::sim::v7::serializers::StringSerializer> const*, gz::sim::v7::components::Component, gz::sim::v7::components::PoseTag, gz::sim::v7::serializers::DefaultSerializer > > const*, gz::sim::v7::components::Component > const*)> >::type) const #13 Object "/usr/lib/gz-sim-7/plugins/libgz-sim-physics-system.so", at 0x7f2e04514315, in #12 Object "/usr/lib/gz-sim-7/plugins/libgz-sim-physics-system.so", at 0x7f2e0454a7d5, in gz::physics::sdf::ConstructSdfModel::World, gz::sim::v7::systems::PhysicsPrivate::MinimumFeatureList>::ConstructModel(sdf::v13::Model const&) #11 Object "/usr/lib/gz-physics-6/engine-plugins/libgz-physics-dartsim-plugin.so", at 0x7f2deed89174, in virtual thunk to gz::physics::dartsim::SDFFeatures::ConstructSdfModel(gz::physics::Identity const&, sdf::v13::Model const&) #10 Object "/usr/lib/gz-physics-6/engine-plugins/libgz-physics-dartsim-plugin.so", at 0x7f2deed87873, in gz::physics::dartsim::SDFFeatures::ConstructSdfModelImpl(unsigned long, sdf::v13::Model const&) #9 Object "/usr/lib/gz-physics-6/engine-plugins/libgz-physics-dartsim-plugin.so", at 0x7f2deed86783, in gz::physics::dartsim::SDFFeatures::ConstructSdfJoint(gz::physics::dartsim::ModelInfo const&, sdf::v13::Joint const&, dart::dynamics::BodyNode*, dart::dynamics::BodyNode*) #8 Object "/usr/lib/gz-physics-6/engine-plugins/libgz-physics-dartsim-plugin.so", at 0x7f2deed7a839, in dart::dynamics::RevoluteJoint* dart::dynamics::BodyNode::moveTo(dart::dynamics::BodyNode*, dart::dynamics::RevoluteJoint::Properties const&) #7 Object "/usr/lib/gz-physics-6/engine-plugins/libgz-physics-dartsim-plugin.so", at 0x7f2deed7a6bc, in dart::dynamics::RevoluteJoint* dart::dynamics::Skeleton::moveBodyNodeTree(dart::dynamics::BodyNode*, std::shared_ptr const&, dart::dynamics::BodyNode*, dart::dynamics::RevoluteJoint::Properties const&) #6 Object "/usr/lib/libdart.so.6.13", at 0x7f2dee892486, in dart::dynamics::Skeleton::moveBodyNodeTree(dart::dynamics::Joint*, dart::dynamics::BodyNode*, std::shared_ptr, dart::dynamics::BodyNode*) #5 Object "/usr/lib/libdart.so.6.13", at 0x7f2dee745d7d, in dart::dynamics::Entity::changeParentFrame(dart::dynamics::Frame*) #4 Object "/usr/lib/libdart.so.6.13", at 0x7f2dee716fef, in dart::dynamics::BodyNode::dirtyTransform() #3 Object "/usr/lib/libstdc++.so.6", at 0x7f2e11ed30e1, in std::__glibcxx_assert_fail(char const*, int, char const*, char const*) #2 Object "/usr/lib/libc.so.6", at 0x7f2e15ab053c, in abort #1 Object "/usr/lib/libc.so.6", at 0x7f2e15ac6ea7, in gsignal #0 Object "/usr/lib/libc.so.6", at 0x7f2e15b158ec, in Aborted (Signal sent by tkill() 317085 1000) WARN [health_and_arming_checks] Too many arming check events (1, 14 > 14). Not reporting all WARN [health_and_arming_checks] Preflight Fail: Accel Sensor 0 missing WARN [health_and_arming_checks] Preflight Fail: barometer 0 missing WARN [health_and_arming_checks] Preflight Fail: ekf2 missing data WARN [health_and_arming_checks] Preflight Fail: Gyro Sensor 0 missing WARN [health_and_arming_checks] Preflight Fail: Compass Sensor 0 missing ERROR [ekf2] start failed (-1) ```

Drone (please complete the following information):

Additional context

I tried to change the joints in the model.sdf from revolute to universal. The error log is similar. The same error occurs when starting the simulation headless.

oysstu commented 11 months ago

Hitting the same problem with a non-PX4 project using gz-harmonic. The problem is definitely in gz-physics/libdart. Have you reported this upstream?

oysstu commented 11 months ago

I think I figured out why it happens on Arch and not Ubuntu. See https://github.com/dartsim/dart/issues/1769