diff --git a/Changelog.md b/Changelog.md index 33b42afb6b..27dd62016f 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,103 @@ ## Gazebo Sim 8.x +### Gazebo Sim 8.5.0 (2024-06-26) + +1. Backport: Adding cone primitives + * [Pull request #2404](https://github.com/gazebosim/gz-sim/pull/2404) + +1. Permit to run gz sim -g on Windows + * [Pull request #2382](https://github.com/gazebosim/gz-sim/pull/2382) + +1. Parse voxel resolution SDF param when decomposing meshes + * [Pull request #2445](https://github.com/gazebosim/gz-sim/pull/2445) + +1. Fix model command api test + * [Pull request #2444](https://github.com/gazebosim/gz-sim/pull/2444) + +1. Add tutorial for using the Pose component + * [Pull request #2219](https://github.com/gazebosim/gz-sim/pull/2219) + +1. Do not update sensors if it a triggered sensor + * [Pull request #2443](https://github.com/gazebosim/gz-sim/pull/2443) + +### Gazebo Sim 8.4.0 (2024-06-12) + +1. Add pause run tutorial + * [Pull request #2383](https://github.com/gazebosim/gz-sim/pull/2383) + +1. Fix warning message to show precise jump back in time duration + * [Pull request #2435](https://github.com/gazebosim/gz-sim/pull/2435) + +1. Optimize rendering sensor pose updates + * [Pull request #2425](https://github.com/gazebosim/gz-sim/pull/2425) + +1. Remove a few extra zeros from some sdf files + * [Pull request #2426](https://github.com/gazebosim/gz-sim/pull/2426) + +1. Use VERSION_GREATER_EQUAL in cmake logic + * [Pull request #2418](https://github.com/gazebosim/gz-sim/pull/2418) + +1. Support mesh optimization when using AttachMeshShapeFeature + * [Pull request #2417](https://github.com/gazebosim/gz-sim/pull/2417) + +1. Rephrase cmake comment about CMP0077 + * [Pull request #2419](https://github.com/gazebosim/gz-sim/pull/2419) + +1. Fix CMake warnings in Noble + * [Pull request #2397](https://github.com/gazebosim/gz-sim/pull/2397) + +1. Update sensors with pending trigger immediately in Sensors system + * [Pull request #2408](https://github.com/gazebosim/gz-sim/pull/2408) + +1. Add missing algorithm include + * [Pull request #2414](https://github.com/gazebosim/gz-sim/pull/2414) + +1. Add Track and Follow options in gui EntityContextMenu + * [Pull request #2402](https://github.com/gazebosim/gz-sim/pull/2402) + +1. ForceTorque system: improve readability + * [Pull request #2403](https://github.com/gazebosim/gz-sim/pull/2403) + +1. LTA Dynamics System + * [Pull request #2241](https://github.com/gazebosim/gz-sim/pull/2241) + +1. Remove Empty Test File + * [Pull request #2396](https://github.com/gazebosim/gz-sim/pull/2396) + +1. Fix GCC/CMake warnings for Noble + * [Pull request #2375](https://github.com/gazebosim/gz-sim/pull/2375) + +1. Fix warn unused variable in test + * [Pull request #2388](https://github.com/gazebosim/gz-sim/pull/2388) + +1. Fix name of gz-fuel_tools in package.xml + * [Pull request #2386](https://github.com/gazebosim/gz-sim/pull/2386) + +1. Add package.xml + * [Pull request #2337](https://github.com/gazebosim/gz-sim/pull/2337) + +1. Fix namespace and class links in documentation references that use namespace `gz` + * [Pull request #2385](https://github.com/gazebosim/gz-sim/pull/2385) + +1. Fix ModelPhotoShootTest test failures + * [Pull request #2294](https://github.com/gazebosim/gz-sim/pull/2294) + +1. Enable StoreResolvedURIs when loading SDF + * [Pull request #2349](https://github.com/gazebosim/gz-sim/pull/2349) + +1. Drop python3-disttutils from apt packages files + * [Pull request #2374](https://github.com/gazebosim/gz-sim/pull/2374) + +1. Added example world for `DopplerVelocityLogSystem` + * [Pull request #2373](https://github.com/gazebosim/gz-sim/pull/2373) + +1. Fix Gazebo/White and refactored MaterialParser + * [Pull request #2302](https://github.com/gazebosim/gz-sim/pull/2302) + +1. Support for Gazebo materials + * [Pull request #2269](https://github.com/gazebosim/gz-sim/pull/2269) + ### Gazebo Sim 8.3.0 (2024-04-11) 1. Use relative install paths for plugin shared libraries and gz-tools data diff --git a/examples/worlds/shapes.sdf b/examples/worlds/shapes.sdf index 3a0e0f5dc8..5d020a8c32 100644 --- a/examples/worlds/shapes.sdf +++ b/examples/worlds/shapes.sdf @@ -1,5 +1,5 @@ - + diff --git a/include/gz/sim/components/Gravity.hh b/include/gz/sim/components/Gravity.hh index 82ba3d036a..a4b8ac352d 100644 --- a/include/gz/sim/components/Gravity.hh +++ b/include/gz/sim/components/Gravity.hh @@ -36,6 +36,11 @@ namespace components /// \brief Store the gravity acceleration. using Gravity = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.Gravity", Gravity) + + /// \brief Store the gravity acceleration. + using GravityEnabled = Component; + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.GravityEnabled", GravityEnabled) } } } diff --git a/src/Conversions.cc b/src/Conversions.cc index 8c72d403e0..d5c054ada9 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -286,6 +286,10 @@ msgs::Geometry gz::sim::convert(const sdf::Geometry &_in) } } } + else if (_in.Type() == sdf::GeometryType::EMPTY) + { + out.set_type(msgs::Geometry::EMPTY); + } else { gzerr << "Geometry type [" << static_cast(_in.Type()) @@ -902,7 +906,7 @@ msgs::Atmosphere gz::sim::convert(const sdf::Atmosphere &_in) out.set_type(msgs::Atmosphere::ADIABATIC); } // todo(anyone) add mass density to sdf::Atmosphere? - // out.set_mass_density(_in.MassDensity());k + // out.set_mass_density(_in.MassDensity()); return out; } @@ -1735,15 +1739,7 @@ msgs::ParticleEmitter gz::sim::convert(const sdf::ParticleEmitter &_in) } } - /// \todo(nkoenig) Modify the particle_emitter.proto file to - /// have a topic field. - if (!_in.Topic().empty()) - { - auto header = out.mutable_header()->add_data(); - header->set_key("topic"); - header->add_value(_in.Topic()); - } - + out.mutable_topic()->set_data(_in.Topic()); out.mutable_particle_scatter_ratio()->set_data(_in.ScatterRatio()); return out; } @@ -1798,15 +1794,8 @@ sdf::ParticleEmitter gz::sim::convert(const msgs::ParticleEmitter &_in) out.SetColorRangeImage(_in.color_range_image().data()); if (_in.has_particle_scatter_ratio()) out.SetScatterRatio(_in.particle_scatter_ratio().data()); - - for (int i = 0; i < _in.header().data_size(); ++i) - { - auto data = _in.header().data(i); - if (data.key() == "topic" && data.value_size() > 0) - { - out.SetTopic(data.value(0)); - } - } + if (_in.has_topic()) + out.SetTopic(_in.topic().data()); return out; } @@ -1824,10 +1813,7 @@ msgs::Projector gz::sim::convert(const sdf::Projector &_in) out.set_fov(_in.HorizontalFov().Radian()); out.set_texture(_in.Texture().empty() ? "" : asFullPath(_in.Texture(), _in.FilePath())); - - auto header = out.mutable_header()->add_data(); - header->set_key("visibility_flags"); - header->add_value(std::to_string(_in.VisibilityFlags())); + out.set_visibility_flags(_in.VisibilityFlags()); return out; } @@ -1844,26 +1830,7 @@ sdf::Projector gz::sim::convert(const msgs::Projector &_in) out.SetHorizontalFov(math::Angle(_in.fov())); out.SetTexture(_in.texture()); out.SetRawPose(msgs::Convert(_in.pose())); - - /// \todo(anyone) add "visibility_flags" field to projector.proto - for (int i = 0; i < _in.header().data_size(); ++i) - { - auto data = _in.header().data(i); - if (data.key() == "visibility_flags" && data.value_size() > 0) - { - try - { - out.SetVisibilityFlags(std::stoul(data.value(0))); - } - catch (...) - { - gzerr << "Failed to parse projector : " - << data.value(0) << ". Using default value: 0xFFFFFFFF." - << std::endl; - out.SetVisibilityFlags(0xFFFFFFFF); - } - } - } + out.SetVisibilityFlags(_in.visibility_flags()); return out; } diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index e14cf6b21b..5d0f5345ba 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -1060,10 +1060,7 @@ TEST(Conversions, ParticleEmitter) EXPECT_EQ(math::Color(0.4f, 0.5f, 0.6f), msgs::Convert(emitterMsg.color_end())); EXPECT_EQ("range_image", emitterMsg.color_range_image().data()); - - auto header = emitterMsg.header().data(0); - EXPECT_EQ("topic", header.key()); - EXPECT_EQ("my_topic", header.value(0)); + EXPECT_EQ("my_topic", emitterMsg.topic().data()); EXPECT_FLOAT_EQ(0.9f, emitterMsg.particle_scatter_ratio().data()); @@ -1113,10 +1110,7 @@ TEST(Conversions, Projector) EXPECT_NEAR(30, projectorMsg.far_clip(), 1e-3); EXPECT_NEAR(0.4, projectorMsg.fov(), 1e-3); EXPECT_EQ("projector.png", projectorMsg.texture()); - - auto header = projectorMsg.header().data(0); - EXPECT_EQ("visibility_flags", header.key()); - EXPECT_EQ(0xFF, std::stoul(header.value(0))); + EXPECT_EQ(0xFF, projectorMsg.visibility_flags()); // Convert the message back to SDF. sdf::Projector projector2 = convert(projectorMsg); @@ -1222,3 +1216,13 @@ TEST(Conversions, MsgsPluginToSdf) EXPECT_EQ(innerXml, sdfPlugins[1].Contents()[0]->ToString("")); EXPECT_EQ(innerXml2, sdfPlugins[1].Contents()[1]->ToString("")); } + +///////////////////////////////////////////////// +TEST(Conversions, GeometryEmpty) +{ + sdf::Geometry geometry; + geometry.SetType(sdf::GeometryType::EMPTY); + + auto geometryMsg = convert(geometry); + EXPECT_EQ(msgs::Geometry::EMPTY, geometryMsg.type()); +} diff --git a/src/ModelCommandAPI_TEST.cc b/src/ModelCommandAPI_TEST.cc index 95f5b8b4a4..36e088680a 100644 --- a/src/ModelCommandAPI_TEST.cc +++ b/src/ModelCommandAPI_TEST.cc @@ -608,7 +608,7 @@ TEST(ModelCommandAPI, GZ_UTILS_TEST_DISABLED_ON_MAC(RgbdCameraSensor)) // Run without blocking. server.Run(false, 0, false); - // Tested command: gz model -m altimeter_mode -l link -s altimeter_sensor + // Tested command: gz model -m rgbd_camera -l rgbd_camera_link -s rgbd_camera { const std::string cmd = kGzModelCommand + "-m rgbd_camera -l rgbd_camera_link -s rgbd_camera"; @@ -657,7 +657,7 @@ TEST(ModelCommandAPI, GZ_UTILS_TEST_DISABLED_ON_MAC(RgbdCameraSensor)) " - Lens intrinsics Fy: 277\n" " - Lens intrinsics Cx: 160\n" " - Lens intrinsics Cy: 120\n" - " - Lens intrinsics skew: 1\n" + " - Lens intrinsics skew: 0\n" " - Visibility mask: 4294967295\n"; EXPECT_EQ(expectedOutput, output); } diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 35a3246107..894d624acd 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -587,6 +587,13 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) linkEntity, components::WindMode(_link->EnableWind())); } + if (!_link->EnableGravity()) + { + // If disable gravity, create a GravityEnabled component to the entity + this->dataPtr->ecm->CreateComponent( + linkEntity, components::GravityEnabled(false)); + } + // Visuals for (uint64_t visualIndex = 0; visualIndex < _link->VisualCount(); ++visualIndex) diff --git a/src/Util.cc b/src/Util.cc index d5eb58bc65..83ce8faf14 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -892,18 +892,18 @@ const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf, auto &meshManager = *common::MeshManager::Instance(); std::size_t maxConvexHulls = 16u; std::size_t voxelResolution = 200000u; + if (_meshSdf.ConvexDecomposition()) + { + // limit max number of convex hulls to generate + maxConvexHulls = _meshSdf.ConvexDecomposition()->MaxConvexHulls(); + voxelResolution = _meshSdf.ConvexDecomposition()->VoxelResolution(); + } if (_meshSdf.Optimization() == sdf::MeshOptimization::CONVEX_HULL) { /// create 1 convex hull for the whole submesh maxConvexHulls = 1u; } - else if (_meshSdf.ConvexDecomposition()) - { - // set convex decomposition params: max number of convex hulls - // and voxel resolution - maxConvexHulls = _meshSdf.ConvexDecomposition()->MaxConvexHulls(); - voxelResolution = _meshSdf.ConvexDecomposition()->VoxelResolution(); - } + // Check if MeshManager contains the decomposed mesh already. If not // add it to the MeshManager so we do not need to decompose it again. const std::string convexMeshName = diff --git a/src/cmd/cmdsim.rb.in b/src/cmd/cmdsim.rb.in index 64ee638c7b..65946d3494 100755 --- a/src/cmd/cmdsim.rb.in +++ b/src/cmd/cmdsim.rb.in @@ -598,12 +598,6 @@ See https://github.com/gazebosim/gz-sim/issues/168 for more info." options['record-period'], options['seed']) # Otherwise run the gui else options['gui'] - if plugin.end_with? ".dll" - puts "`gz sim` currently only works with the -s argument on Windows. -See https://github.com/gazebosim/gz-sim/issues/168 for more info." - exit(-1) - end - ENV['RMT_PORT'] = '1501' Importer.runGui(options['gui_config'], options['file'], options['wait_gui'], options['render_engine_gui'], diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index f01c72dfb9..59d81052f8 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -508,8 +508,8 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joints haven't been identified yet, look for them diff --git a/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc index a7c60109fd..80065691d5 100644 --- a/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc +++ b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc @@ -802,8 +802,8 @@ void AdvancedLiftDrag::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } if (!this->dataPtr->initialized) diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index 7dba18337f..3a99824e76 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -132,8 +132,8 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/air_speed/AirSpeed.cc b/src/systems/air_speed/AirSpeed.cc index 73c9e92466..39904b9575 100644 --- a/src/systems/air_speed/AirSpeed.cc +++ b/src/systems/air_speed/AirSpeed.cc @@ -134,8 +134,8 @@ void AirSpeed::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index ecd3ee1ea1..7f45d2899f 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -132,8 +132,8 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/apply_joint_force/ApplyJointForce.cc b/src/systems/apply_joint_force/ApplyJointForce.cc index 19add069df..b7ea5eba18 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.cc +++ b/src/systems/apply_joint_force/ApplyJointForce.cc @@ -134,8 +134,8 @@ void ApplyJointForce::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joint hasn't been identified yet, look for it diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 357b01df97..523950b172 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -550,8 +550,8 @@ void LinearBatteryPlugin::Update(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } if (_info.paused) diff --git a/src/systems/contact/Contact.cc b/src/systems/contact/Contact.cc index f4b8019f0c..0dcea6a8b4 100644 --- a/src/systems/contact/Contact.cc +++ b/src/systems/contact/Contact.cc @@ -276,8 +276,8 @@ void Contact::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } if (!_info.paused) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 04fc968279..b65af3fce6 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -396,8 +396,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joints haven't been identified yet, look for them diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index 71a2e2800b..8792f514d5 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -166,8 +166,8 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index eebbe77b6c..6026cda124 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -136,8 +136,8 @@ void Imu::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 40f5caac33..cf8f54187c 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -265,8 +265,8 @@ void JointController::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joints haven't been identified yet, look for them diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index b2835160ff..4b36a5076c 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -339,8 +339,8 @@ void JointPositionController::PreUpdate( if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joints haven't been identified yet, look for them diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index c83837c5ff..902bfec942 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -550,8 +550,8 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } if (!this->dataPtr->initialized) diff --git a/src/systems/log/LogRecord.cc b/src/systems/log/LogRecord.cc index 8cbeadbfb1..3361c407f4 100644 --- a/src/systems/log/LogRecord.cc +++ b/src/systems/log/LogRecord.cc @@ -669,8 +669,8 @@ void LogRecord::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Publish only once diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc index a4e5a7f195..a460e620c2 100644 --- a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc @@ -405,12 +405,11 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( }; // create services for this source - const auto fullName = scopedName(entity, _ecm); - auto validName = transport::TopicUtils::AsValidTopic(fullName); + const auto validName = topicFromScopedName(entity, _ecm, false); if (validName.empty()) { gzerr << "Failed to create valid topics with entity scoped name [" - << fullName << "]" << std::endl; + << scopedName(entity, _ecm) << "]" << std::endl; return; } if (!this->node.Advertise(validName + "/play", playSrvCb)) @@ -504,7 +503,7 @@ void LogicalAudioSensorPluginPrivate::CreateMicrophone( // create the detection publisher for this microphone auto pub = this->node.Advertise( - scopedName(entity, _ecm) + "/detection"); + topicFromScopedName(entity, _ecm, false) + "/detection"); if (!pub) { gzerr << "Error creating a detection publisher for microphone " diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index 1ddfe2146f..ded8c9c36f 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -134,8 +134,8 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index 499664a046..72474d5cb9 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -98,7 +98,7 @@ static constexpr const int8_t inclination_table[13][37] = \ { 71, 71, 72, 73, 75, 77, 78, 80, 81, 81, 80, 79, 77, 76, 74, 73, 73, 73, 73, 73, 73, 74, 74, 75, 76, 77, 78, 78, 78, 78, 77, 75, 73, 72, 71, 71, 71 }, // NOLINT }; -// strength data in centi-Tesla +// strength data in centi gauss static constexpr const int8_t strength_table[13][37] = \ { { 62, 60, 58, 56, 54, 52, 49, 46, 43, 41, 38, 36, 34, 32, 31, 31, 30, 30, 30, 31, 33, 35, 38, 42, 46, 51, 55, 59, 62, 64, 66, 67, 67, 66, 65, 64, 62 }, // NOLINT @@ -134,6 +134,12 @@ class gz::sim::systems::MagnetometerPrivate /// True if the rendering component is initialized public: bool initialized = false; + /// \brief True if the magnetic field is reported in gauss rather than tesla. + public: bool useUnitsGauss = true; + + /// \brief True if the magnetic field earth frame is NED rather than ENU. + public: bool useEarthFrameNED = true; + /// \brief Create sensor /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU @@ -226,7 +232,7 @@ class gz::sim::systems::MagnetometerPrivate return get_table_data(lat, lon, inclination_table); } - // return magnetic field strength in centi-Tesla + // return magnetic field strength in centi-Gauss float get_mag_strength(float lat, float lon) { return get_table_data(lat, lon, strength_table); @@ -242,6 +248,29 @@ Magnetometer::Magnetometer() : System(), dataPtr( ////////////////////////////////////////////////// Magnetometer::~Magnetometer() = default; +////////////////////////////////////////////////// +void Magnetometer::Configure(const Entity &/*_entity*/, + const std::shared_ptr &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + if (_sdf->HasElement("use_units_gauss")) + { + this->dataPtr->useUnitsGauss = _sdf->Get("use_units_gauss"); + } + gzdbg << "Magnetometer: using param [use_units_gauss: " + << this->dataPtr->useUnitsGauss << "]." + << std::endl; + + if (_sdf->HasElement("use_earth_frame_ned")) + { + this->dataPtr->useEarthFrameNED = _sdf->Get("use_earth_frame_ned"); + } + gzdbg << "Magnetometer: using param [use_earth_frame_ned: " + << this->dataPtr->useEarthFrameNED << "]." + << std::endl; +} + ////////////////////////////////////////////////// void Magnetometer::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) @@ -274,8 +303,8 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); @@ -430,7 +459,7 @@ void MagnetometerPrivate::Update( auto latLonEle = sphericalCoordinates(_entity, _ecm); if (!latLonEle) { - gzwarn << "Failed to update NavSat sensor enity [" << _entity + gzwarn << "Failed to update Magnetometer sensor enity [" << _entity << "]. Spherical coordinates not set." << std::endl; return true; } @@ -446,16 +475,32 @@ void MagnetometerPrivate::Update( get_mag_inclination( lat_rad * 180 / GZ_PI, lon_rad * 180 / GZ_PI) * GZ_PI / 180; - // Magnetic strength (10^5xnanoTesla) + // Magnetic strength in gauss (10^5 nano tesla = 10^-2 centi gauss) float strength_ga = 0.01f * get_mag_strength(lat_rad * 180 / GZ_PI, lon_rad * 180 / GZ_PI); - // Magnetic filed components are calculated by http://geomag.nrcan.gc.ca/mag_fld/comp-en.php - float H = strength_ga * cosf(inclination_rad); - float Z = tanf(inclination_rad) * H; - float X = H * cosf(declination_rad); - float Y = H * sinf(declination_rad); + // Magnetic intensity measured in telsa + float strength_tesla = 1.0E-4 * strength_ga; + + // Magnetic field components are calculated in world NED frame using: + // http://geomag.nrcan.gc.ca/mag_fld/comp-en.php + float H = cosf(inclination_rad); + H *= this->useUnitsGauss ? strength_ga : strength_tesla; + float Z_ned = tanf(inclination_rad) * H; + float X_ned = H * cosf(declination_rad); + float Y_ned = H * sinf(declination_rad); + + float X = X_ned; + float Y = Y_ned; + float Z = Z_ned; + if (!this->useEarthFrameNED) + { + // Use ENU convention for earth frame. + X = Y_ned; + Y = X_ned; + Z = -1.0 * Z_ned; + } math::Vector3d magnetic_field_I(X, Y, Z); it->second->SetWorldMagneticField(magnetic_field_I); @@ -494,6 +539,7 @@ void MagnetometerPrivate::RemoveMagnetometerEntities( } GZ_ADD_PLUGIN(Magnetometer, System, + Magnetometer::ISystemConfigure, Magnetometer::ISystemPreUpdate, Magnetometer::ISystemPostUpdate ) diff --git a/src/systems/magnetometer/Magnetometer.hh b/src/systems/magnetometer/Magnetometer.hh index 854eb44a3b..e2e1c5d166 100644 --- a/src/systems/magnetometer/Magnetometer.hh +++ b/src/systems/magnetometer/Magnetometer.hh @@ -37,6 +37,7 @@ namespace systems /// current location. class Magnetometer: public System, + public ISystemConfigure, public ISystemPreUpdate, public ISystemPostUpdate { @@ -46,6 +47,12 @@ namespace systems /// \brief Destructor public: ~Magnetometer() override; + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + /// Documentation inherited public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 2f585307c7..732bc07b1d 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -333,8 +333,8 @@ void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joints haven't been identified yet, look for them diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.cc b/src/systems/multicopter_control/MulticopterVelocityControl.cc index 96f547fa4d..d352a68a7a 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.cc +++ b/src/systems/multicopter_control/MulticopterVelocityControl.cc @@ -349,8 +349,8 @@ void MulticopterVelocityControl::PreUpdate( if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Nothing left to do if paused. diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index 12dcab4c86..5a711272bd 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -399,8 +399,8 @@ void MulticopterMotorModel::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joint or links haven't been identified yet, look for them diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index f438a9d240..dd6ac144f7 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -128,8 +128,8 @@ void NavSat::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 8473328320..fe5270ebe2 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -59,7 +59,9 @@ class gz::sim::systems::OdometryPublisherPrivate public: transport::Node node; /// \brief Model interface + //! [modelDeclaration] public: Model model{kNullEntity}; + //! [modelDeclaration] /// \brief Name of the world-fixed coordinate frame for the odometry message. public: std::string odomFrame; @@ -133,12 +135,14 @@ OdometryPublisher::OdometryPublisher() } ////////////////////////////////////////////////// +//! [Configure] void OdometryPublisher::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, EventManager &/*_eventMgr*/) { this->dataPtr->model = Model(_entity); + //! [Configure] if (!this->dataPtr->model.Valid(_ecm)) { @@ -233,8 +237,10 @@ void OdometryPublisher::Configure(const Entity &_entity, } else { + //! [definePub] this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopicValid); + //! [definePub] gzmsg << "OdometryPublisher publishing odometry on [" << odomTopicValid << "]" << std::endl; } @@ -297,17 +303,8 @@ void OdometryPublisher::PreUpdate(const gz::sim::UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; - } - - // Create the pose component if it does not exist. - auto pos = _ecm.Component( - this->dataPtr->model.Entity()); - if (!pos) - { - _ecm.CreateComponent(this->dataPtr->model.Entity(), - components::Pose()); + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } } @@ -347,7 +344,9 @@ void OdometryPublisherPrivate::UpdateOdometry( } // Construct the odometry message and publish it. + //! [declarePoseMsg] msgs::Odometry msg; + //! [declarePoseMsg] const std::chrono::duration dt = std::chrono::steady_clock::time_point(_info.simTime) - lastUpdateTime; @@ -357,7 +356,10 @@ void OdometryPublisherPrivate::UpdateOdometry( return; // Get and set robotBaseFrame to odom transformation. + //! [worldPose] const math::Pose3d rawPose = worldPose(this->model.Entity(), _ecm); + //! [worldPose] + //! [setPoseMsg] math::Pose3d pose = rawPose * this->offset; msg.mutable_pose()->mutable_position()->set_x(pose.Pos().X()); msg.mutable_pose()->mutable_position()->set_y(pose.Pos().Y()); @@ -366,6 +368,7 @@ void OdometryPublisherPrivate::UpdateOdometry( { msg.mutable_pose()->mutable_position()->set_z(pose.Pos().Z()); } + //! [setPoseMsg] // Get linear and angular displacements from last updated pose. double linearDisplacementX = pose.Pos().X() - this->lastUpdatePose.Pos().X(); @@ -476,7 +479,9 @@ void OdometryPublisherPrivate::UpdateOdometry( this->lastOdomPubTime = _info.simTime; if (this->odomPub.Valid()) { + //! [publishMsg] this->odomPub.Publish(msg); + //! [publishMsg] } // Generate odometry with covariance message and publish it. diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index cab9bd6d14..33c8fd23af 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -76,6 +76,13 @@ namespace systems /// - ``: Standard deviation of the Gaussian noise to be added /// to pose and twist messages. This element is optional, and the default /// value is 0. + /// + /// ## Components + /// + /// This system uses the following components: + /// + /// - gz::sim::components::Pose: Pose represented by gz::math::Pose3d. Used + /// to calculate the odometry to publish. class OdometryPublisher : public System, public ISystemConfigure, diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 998c4a1d62..ed2b24acf2 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -622,6 +622,14 @@ class gz::sim::systems::PhysicsPrivate public: struct SolverFeatureList : gz::physics::FeatureList< gz::physics::Solver>{}; + ////////////////////////////////////////////////// + // CollisionPairMaxContacts + /// \brief Feature list for setting and getting the max total contacts for + /// collision pairs + public: struct CollisionPairMaxContactsFeatureList : + gz::physics::FeatureList< + gz::physics::CollisionPairMaxContacts>{}; + ////////////////////////////////////////////////// // Nested Models /// \brief Feature list to construct nested models @@ -646,7 +654,8 @@ class gz::sim::systems::PhysicsPrivate NestedModelFeatureList, CollisionDetectorFeatureList, SolverFeatureList, - WorldModelFeatureList + WorldModelFeatureList, + CollisionPairMaxContactsFeatureList >; /// \brief A map between world entity ids in the ECM to World Entities in @@ -1049,6 +1058,33 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, } } + auto physicsComp = + _ecm.Component(_entity); + if (physicsComp) + { + auto maxContactsFeature = + this->entityWorldMap.EntityCast< + CollisionPairMaxContactsFeatureList>(_entity); + if (!maxContactsFeature) + { + static bool informed{false}; + if (!informed) + { + gzdbg << "Attempting to set physics options, but the " + << "phyiscs engine doesn't support feature " + << "[CollisionPairMaxContacts]. " + << "Options will be ignored." + << std::endl; + informed = true; + } + } + else + { + maxContactsFeature->SetCollisionPairMaxContacts( + physicsComp->Data().MaxContacts()); + } + } + // World Model proxy (used for joints directly under in SDF) auto worldModelFeature = this->entityWorldMap.EntityCast(_entity); @@ -1310,6 +1346,15 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm, link.SetInertial(inertial->Data()); } + // get link gravity + const components::GravityEnabled *gravityEnabled = + _ecm.Component(_entity); + if (nullptr != gravityEnabled) + { + // gravityEnabled set in SdfEntityCreator::CreateEntities() + link.SetEnableGravity(gravityEnabled->Data()); + } + auto constructLinkFeature = this->entityModelMap.EntityCast( _parent->Data()); diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 58661d4816..28fbbdc8d4 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -252,8 +252,7 @@ void PosePublisher::Configure(const Entity &_entity, this->dataPtr->usePoseV = _sdf->Get("use_pose_vector_msg", this->dataPtr->usePoseV).first; - std::string poseTopic = scopedName(_entity, _ecm) + "/pose"; - poseTopic = transport::TopicUtils::AsValidTopic(poseTopic); + std::string poseTopic = topicFromScopedName(_entity, _ecm, false) + "/pose"; if (poseTopic.empty()) { poseTopic = "/pose"; @@ -297,8 +296,8 @@ void PosePublisher::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Nothing left to do if paused. diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 84769ca3f2..601a4b3c9d 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -1034,6 +1034,11 @@ std::chrono::steady_clock::duration SensorsPrivate::NextUpdateTime( continue; } + if (rs->IsTriggered()) + { + continue; + } + std::chrono::steady_clock::duration time; // if sensor's next update tims is less or equal to current sim time then // it's in the process of being updated by the render loop diff --git a/src/systems/touch_plugin/TouchPlugin.cc b/src/systems/touch_plugin/TouchPlugin.cc index 91007cfbcc..d9cab9fd1d 100644 --- a/src/systems/touch_plugin/TouchPlugin.cc +++ b/src/systems/touch_plugin/TouchPlugin.cc @@ -251,8 +251,8 @@ void TouchPluginPrivate::Update(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } { diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index b5a1ccbde1..7af66e743b 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -188,8 +188,8 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Nothing left to do if paused. diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index 5ef1e51aa4..e237d0b1bc 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/src/systems/wind_effects/WindEffects.cc @@ -716,8 +716,8 @@ void WindEffects::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Process commands diff --git a/test/integration/actor_trajectory.cc b/test/integration/actor_trajectory.cc index 7a6819ba28..901789ce2e 100644 --- a/test/integration/actor_trajectory.cc +++ b/test/integration/actor_trajectory.cc @@ -99,7 +99,7 @@ class ActorFixture : public InternalFixture> // Load the actor_trajectory.sdf world that animates a box (actor) to follow // a trajectory. Verify that the box pose changes over time on the rendering // side. -TEST_F(ActorFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(ActorTrajectoryNoMesh)) +TEST_F(ActorFixture, ActorTrajectoryNoMesh) { sim::ServerConfig serverConfig; diff --git a/test/integration/camera_sensor_background_from_scene.cc b/test/integration/camera_sensor_background_from_scene.cc index 88d55cfc67..d53234258f 100644 --- a/test/integration/camera_sensor_background_from_scene.cc +++ b/test/integration/camera_sensor_background_from_scene.cc @@ -77,7 +77,7 @@ void cameraCb(const msgs::Image & _msg) ///////////////////////////////////////////////// // Test sensors use the background color of by default TEST_F(CameraSensorBackgroundFixture, - GZ_UTILS_TEST_DISABLED_ON_MAC(RedBackgroundFromScene)) + RedBackgroundFromScene) { const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "camera_sensor_scene_background.sdf"); diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc index 40544b92c4..8636346180 100644 --- a/test/integration/reset_sensors.cc +++ b/test/integration/reset_sensors.cc @@ -140,8 +140,8 @@ common::Image toImage(const msgs::Image &_msg) } ///////////////////////////////////////////////// -/// This test checks that that the sensors system handles cases where entities -/// are removed and then added back +/// This test checks that that air-pressure and camera sensor systems +/// handle Reset events TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) { gz::sim::ServerConfig serverConfig; diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 5c0185d5c9..41508e9466 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -1091,9 +1091,7 @@ TEST_P(SceneBroadcasterTest, EXPECT_DOUBLE_EQ(0.5, projector.fov()); EXPECT_NE(std::string::npos, projector.texture().find("path/to/dummy_image.png")); - auto header = projector.header().data(0); - EXPECT_EQ("visibility_flags", header.key()); - EXPECT_EQ(0x01, std::stoul(header.value(0))); + EXPECT_EQ(0x01, projector.visibility_flags()); } } diff --git a/test/integration/sensors_system.cc b/test/integration/sensors_system.cc index 84e95e94f6..a116d54cf7 100644 --- a/test/integration/sensors_system.cc +++ b/test/integration/sensors_system.cc @@ -183,7 +183,7 @@ void testDefaultTopics(const std::vector &_topics) ///////////////////////////////////////////////// /// This test checks that that the sensors system handles cases where entities /// are removed and then added back -TEST_F(SensorsFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleRemovedEntities)) +TEST_F(SensorsFixture, HandleRemovedEntities) { gz::sim::ServerConfig serverConfig; diff --git a/test/integration/sensors_system_battery.cc b/test/integration/sensors_system_battery.cc index 4eb35ec92f..49bfd0cd0b 100644 --- a/test/integration/sensors_system_battery.cc +++ b/test/integration/sensors_system_battery.cc @@ -83,7 +83,7 @@ class SensorsFixture : public InternalFixture> ///////////////////////////////////////////////// // Battery -TEST_F(SensorsFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(SensorsBatteryState)) +TEST_F(SensorsFixture, SensorsBatteryState) { const auto sdfPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "sensors_system_battery.sdf"); diff --git a/test/integration/sensors_system_update_rate.cc b/test/integration/sensors_system_update_rate.cc index 9df4de1d86..55d9130b13 100644 --- a/test/integration/sensors_system_update_rate.cc +++ b/test/integration/sensors_system_update_rate.cc @@ -76,7 +76,7 @@ class SensorsFixture : public InternalFixture> }; ///////////////////////////////////////////////// -TEST_F(SensorsFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(UpdateRate)) +TEST_F(SensorsFixture, UpdateRate) { gz::sim::ServerConfig serverConfig; diff --git a/test/integration/thermal_sensor_system.cc b/test/integration/thermal_sensor_system.cc index 16dfde6f6e..9ad00dcdaf 100644 --- a/test/integration/thermal_sensor_system.cc +++ b/test/integration/thermal_sensor_system.cc @@ -74,7 +74,7 @@ void thermalCb(const msgs::Image &_msg) ///////////////////////////////////////////////// TEST_F(ThermalSensorTest, - GZ_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystemInvalidConfig)) + ThermalSensorSystemInvalidConfig) { // Start server ServerConfig serverConfig; diff --git a/test/integration/thermal_system.cc b/test/integration/thermal_system.cc index 1e9155514c..bc5e81692f 100644 --- a/test/integration/thermal_system.cc +++ b/test/integration/thermal_system.cc @@ -49,7 +49,7 @@ class ThermalTest : public InternalFixture<::testing::Test> }; ///////////////////////////////////////////////// -TEST_F(ThermalTest, GZ_UTILS_TEST_DISABLED_ON_MAC(TemperatureComponent)) +TEST_F(ThermalTest, TemperatureComponent) { // Start server ServerConfig serverConfig; @@ -173,7 +173,7 @@ TEST_F(ThermalTest, GZ_UTILS_TEST_DISABLED_ON_MAC(TemperatureComponent)) } ///////////////////////////////////////////////// -TEST_F(ThermalTest, GZ_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystem)) +TEST_F(ThermalTest, ThermalSensorSystem) { // Start server ServerConfig serverConfig; diff --git a/test/integration/wide_angle_camera.cc b/test/integration/wide_angle_camera.cc index d2857bdccd..32dd00bd92 100644 --- a/test/integration/wide_angle_camera.cc +++ b/test/integration/wide_angle_camera.cc @@ -61,7 +61,7 @@ void imageCb(const msgs::Image &_msg) ///////////////////////////////////////////////// // The test checks the Wide Angle Camera readings -TEST_F(WideAngleCameraTest, GZ_UTILS_TEST_DISABLED_ON_MAC(WideAngleCameraBox)) +TEST_F(WideAngleCameraTest, WideAngleCameraBox) { // Start server ServerConfig serverConfig; diff --git a/tutorials.md.in b/tutorials.md.in index 0fa2b9f2ab..e88b8f7137 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -15,6 +15,7 @@ The Tutorials section contains step-by-step instructions with self-contained exa * \subpage apply_force_torque "Apply Force and Torque": Apply forces and/or torques to models during simulation through the GUI. * \subpage mouse_drag "Mouse Drag": Move models by dragging them in the scene using forces and torques. * \subpage model_command "Model Command": Use the CLI to get information about the models in a simulation. +* \subpage pause_run_simulation "Pause and Run simulation": Use Gazebo transport API to pause and run simulation. * \subpage reset_simulation Reset simulation * \subpage headless_rendering "Headless rendering": Access the GPU on a remote machine to produce sensor data without an X server. diff --git a/tutorials/component_pose.md b/tutorials/component_pose.md new file mode 100644 index 0000000000..34d440e12e --- /dev/null +++ b/tutorials/component_pose.md @@ -0,0 +1,80 @@ +\page posecomponent Case Study: Using the Pose Component + +We will show how to use the gz::sim::components::Pose component in a system. + +An example usage of the component can be found in the +gz::sim::systems::OdometryPublisher system +([source code](https://github.com/gazebosim/gz-sim/tree/gz-sim8/src/systems/odometry_publisher)), +which reads the pose component of a model through the Model entity, uses the +pose for some calculations, and then publishes the result as a message. + +More usage can be found in the +[integration test](https://github.com/gazebosim/gz-sim/blob/gz-sim8/test/integration/odometry_publisher.cc) +for the system, with test worlds `odometry*.sdf` +[here](https://github.com/gazebosim/gz-sim/tree/main/test/worlds). + +### Objects of interest + +- gz::sim::components::Pose: A component containing pose information +- gz::math::Pose3d: The actual data underlying a pose component +- gz::sim::systems::OdometryPublisher: A system that reads the pose component + of a model +- gz::sim::Model: The type underlying a model entity (gz::sim::Entity) + +### Find the model entity + +First, we will need access to an entity, the \ref gz::sim::Model entity in this +case. +`OdometryPublisher` happens to be a system meant to be specified under `` +in the SDF, so at the time `Configure()` is called, it has access to a model +entity from which we can extract a \ref gz::sim::Model: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc modelDeclaration +\snippet src/systems/odometry_publisher/OdometryPublisher.cc Configure + +### Read the pose component + +Once we have the handle to an entity, we can access components associated with +it. +A component may have been created at the time the world is loaded, or you may +create a component at runtime if it does not exist yet. + +In this case, we use the model entity found above to access its pose component, +which is created by default on every model entity. + +In `PostUpdate()`, which happens after physics has updated, we can get the +world pose of a model through gz::sim::worldPose, by passing in the model +entity and the entity component manager. + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc worldPose + +It returns the raw data to us in the gz::math::Pose3d type, which is also the +data type underlying a pose component. +We can perform calculations on the gz::math::Pose3d type, not the +gz::sim::components::Pose type, which is just a wrapper. + +### Use the pose component + +Now we can use the pose data as we like. + +Here, we manipulate the pose and package the result into a gz::msgs::Odometry +message to be published: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc declarePoseMsg + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc setPoseMsg + +See the source code for setting other fields of the message, such as twist and +the header. + +The message is then published: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc publishMsg + +where `odomPub` is defined in `Configure()`: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc definePub + +Outside the scope of this tutorial, the odometry publisher system also +calculates the covariance and publishes a pose vector on a TF topic. +See the source code to learn more. diff --git a/tutorials/files/pause_run_simlation/gui_pause_run.png b/tutorials/files/pause_run_simlation/gui_pause_run.png new file mode 100644 index 0000000000..49dd539653 Binary files /dev/null and b/tutorials/files/pause_run_simlation/gui_pause_run.png differ diff --git a/tutorials/pause_run_simulation.md b/tutorials/pause_run_simulation.md new file mode 100644 index 0000000000..ef757b75a0 --- /dev/null +++ b/tutorials/pause_run_simulation.md @@ -0,0 +1,57 @@ +\page pause_run_simulation Pause and Run simulation + +A Gazebo transport API is exposed to allow starting and stopping the simulation. +It's possible to call this API using the command line or through the GUI. + +To repeat this demo, run the `default` world: +```bash +gz sim default.sdf +``` + +## Transport API + +When Gazebo is run headless, this is an easy way to start the simulation. + +To pause and play over the transport API, we should call the service `/world//control` and fill the request message type +`gz.msgs.WorldControl`. This service returns a `gz.msgs.Boolean` with the status of the request (true means everything was fine, false otherwise). + +The `WorldControl` message contains a `pause` field for starting and stopping the simulation. + +To start the simulation: + +```bash +gz service -s /world/default/control --reqtype gz.msgs.WorldControl --reptype gz.msgs.Boolean --timeout 3000 --req 'pause: false' +``` + +To pause the simulation: + +```bash +gz service -s /world/default/control --reqtype gz.msgs.WorldControl --reptype gz.msgs.Boolean --timeout 3000 --req 'pause: true' +``` + +When paused, time will stop in Gazebo, and the physics will not be running. + +To check the current state of the simulator, check the `paused` field in `/stats` like so: +```bash +gz topic --echo --topic /stats -n 1 +``` +If the simulator is currently paused but was running before, we would see something similar to this: +```text +sim_time { + sec: 8 + nsec: 707000000 +} +real_time { + sec: 8 + nsec: 824323281 +} +iterations: 8707 +real_time_factor: 0.998022916602211 +``` + + +## GUI + +We included a button in the `World Control` plugin allowing to start and stop the simulation from the GUI. + +@image html files/pause_run_simulation/gui_pause_run.png diff --git a/tutorials/reset_simulation.md b/tutorials/reset_simulation.md index 7a62a3fa3d..bf8ae121ab 100644 --- a/tutorials/reset_simulation.md +++ b/tutorials/reset_simulation.md @@ -4,6 +4,11 @@ The Reset Gazebo transport API is exposed to allow resetting simulation to time It's possible to call this API using the command line or through the GUI. In addition to the API, we have also expanded the simulation system API with a Reset interface. +To repeat this demo, run the `rolling_shapes.sdf` file: +```bash +gz sim rolling_shapes.sdf +``` + ## Reset interface System authors may now choose to implement the Reset interface to have a more intelligent @@ -16,7 +21,7 @@ Follow the tutorial \subpage createsystemplugins to see how to support Reset by ## Transport API To invoke reset over transport API, we should call the service `/world//control` and fill the request message type -`gz.msgs.WorldControl`. This service returns a `gz.msgs.Boolean` with the status of the reset (true means everything was fine, false otherwise) +`gz.msgs.WorldControl`. This service returns a `gz.msgs.Boolean` with the status of the reset (true means everything was fine, false otherwise). The `WorldControl` message now contains a `reset` field for resetting the world: diff --git a/tutorials/using_components.md b/tutorials/using_components.md index 717beea566..1c91d47e46 100644 --- a/tutorials/using_components.md +++ b/tutorials/using_components.md @@ -72,3 +72,4 @@ The rest of the tutorial is case studies that walk through the usage of specific components. - \subpage jointforcecmdcomponent "JointForceCmd" +- \subpage posecomponent "Pose"