From 5f9a27270d0294ff357564b91452133d3a2f9126 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 16 Aug 2022 18:57:34 -0700 Subject: [PATCH 001/160] =?UTF-8?q?=F0=9F=8E=88=203.14.0~pre1=20(#1650)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- CMakeLists.txt | 4 ++-- Changelog.md | 51 +++++++++++++++++++++++++++++++++++++++++++++++++- 2 files changed, 52 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 65d216f505..f638d5d30a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo3 VERSION 3.13.0) +project(ignition-gazebo3 VERSION 3.14.0) set (GZ_DISTRIBUTION "Citadel") #============================================================================ @@ -15,7 +15,7 @@ find_package(ignition-cmake2 2.8.0 REQUIRED) #============================================================================ # Configure the project #============================================================================ -ign_configure_project(VERSION_SUFFIX) +ign_configure_project(VERSION_SUFFIX pre1) #============================================================================ # Set project-specific options diff --git a/Changelog.md b/Changelog.md index 0d62e41cce..23d7336e66 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,4 +1,53 @@ -## Ignition Gazebo 3.x +## Gazebo Sim 3.x + +### Gazebo Sim 3.14.0 (2022-08-XX) + +1. Change `CODEOWNERS` and maintainer to Michael + * [Pull request #1644](https://github.com/gazebosim/gz-sim/pull/1644) + +1. Replace pose in `ViewAngle` with `GzPose` + * [Pull request #1641](https://github.com/gazebosim/gz-sim/pull/1641) + +1. Fix loading worlds from CLI + * [Pull request #1627](https://github.com/gazebosim/gz-sim/pull/1627) + +1. Quick start dialog + * [Pull request #1536](https://github.com/gazebosim/gz-sim/pull/1536) + +1. Quiet `libSDFormat` console on --verbose 0 + * [Pull request #1621](https://github.com/gazebosim/gz-sim/pull/1621) + +1. Add Ackermann Steering system (backport from Fortress) + * [Pull request #1613](https://github.com/gazebosim/gz-sim/pull/1613) + +1. New Apply Link Wrench system + * [Pull request #1593](https://github.com/gazebosim/gz-sim/pull/1593) + +1. Implement Component Inspector `Vector3` with common widget `Vector3` + * [Pull request #1569](https://github.com/gazebosim/gz-sim/pull/1569) + +1. Helper function to get an entity from an entity message + * [Pull request #1595](https://github.com/gazebosim/gz-sim/pull/1595) + +1. Ignition -> Gazebo + * [Pull request #1596](https://github.com/gazebosim/gz-sim/pull/1596) + +1. Add Model::CanonicalLink getter + * [Pull request #1594](https://github.com/gazebosim/gz-sim/pull/1594) + +1. Implement Pose3d with common widget pose + * [Pull request #1571](https://github.com/gazebosim/gz-sim/pull/1571) + +1. Test fixes and updates + * [Pull request #1545](https://github.com/gazebosim/gz-sim/pull/1545) + * [Pull request #1531](https://github.com/gazebosim/gz-sim/pull/1531) + * [Pull request #1599](https://github.com/gazebosim/gz-sim/pull/1599) + +1. Bash completion for flags + * [Pull request #1504](https://github.com/gazebosim/gz-sim/pull/1504) + +1. Add new `GZ_GUI_RESOURCE_PATH` to help message + * [Pull request #1470](https://github.com/gazebosim/gz-sim/pull/1470) ### Ignition Gazebo 3.13.0 (2022-06-01) From b0fcc0a98ede36862bedc0570811d70bef21f8df Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 17 Aug 2022 12:19:08 -0700 Subject: [PATCH 002/160] Remove redundant namespace references (#1635) Signed-off-by: methylDragon --- .../ignition/gazebo/EntityComponentManager.hh | 1 + .../gazebo/detail/EntityComponentManager.hh | 1 + src/Component_TEST.cc | 4 +- src/Conversions.cc | 126 +++--- src/Conversions_TEST.cc | 28 +- src/EntityComponentManager.cc | 14 +- src/EntityComponentManager_TEST.cc | 152 +++---- src/SdfEntityCreator_TEST.cc | 80 ++-- src/ServerConfig.cc | 36 +- src/ServerConfig_TEST.cc | 8 +- src/ServerPrivate.cc | 10 +- src/Server_TEST.cc | 36 +- src/SimulationRunner.cc | 28 +- src/SimulationRunner_TEST.cc | 378 +++++++++--------- src/SystemLoader_TEST.cc | 2 +- src/TestFixture.cc | 6 +- src/gui/GuiFileHandler.cc | 2 +- src/gui/GuiRunner.cc | 2 +- src/gui/Gui_TEST.cc | 8 +- src/gui/TmpIface.cc | 12 +- src/gui/plugins/align_tool/AlignTool.cc | 20 +- .../component_inspector/ComponentInspector.cc | 38 +- src/gui/plugins/entity_tree/EntityTree.cc | 6 +- src/gui/plugins/grid_config/GridConfig.cc | 4 +- .../JointPositionController.cc | 10 +- src/gui/plugins/modules/EntityContextMenu.cc | 18 +- .../playback_scrubber/PlaybackScrubber.cc | 2 +- src/gui/plugins/plot_3d/Plot3D.cc | 6 +- .../resource_spawner/ResourceSpawner.cc | 12 +- src/gui/plugins/scene3d/Scene3D.cc | 52 +-- src/gui/plugins/shapes/Shapes.cc | 2 +- src/gui/plugins/tape_measure/TapeMeasure.cc | 52 +-- .../transform_control/TransformControl.cc | 10 +- .../plugins/video_recorder/VideoRecorder.cc | 14 +- src/gui/plugins/view_angle/ViewAngle.cc | 14 +- src/ign.cc | 54 +-- src/network/PeerInfo.cc | 28 +- src/network/PeerTracker.cc | 2 +- src/rendering/MarkerManager.cc | 118 +++--- src/rendering/RenderUtil.cc | 4 +- src/rendering/SceneManager.cc | 6 +- .../ackermann_steering/AckermannSteering.cc | 34 +- .../apply_joint_force/ApplyJointForce.cc | 8 +- .../battery_plugin/LinearBatteryPlugin.cc | 14 +- src/systems/breadcrumbs/Breadcrumbs.cc | 6 +- src/systems/buoyancy/Buoyancy.cc | 10 +- .../CameraVideoRecorder.cc | 6 +- .../ColladaWorldExporter.cc | 10 +- src/systems/contact/Contact.cc | 2 +- .../detachable_joint/DetachableJoint.cc | 6 +- src/systems/diff_drive/DiffDrive.cc | 36 +- src/systems/imu/Imu.cc | 2 +- .../joint_controller/JointController.cc | 10 +- .../JointPositionController.cc | 10 +- .../JointStatePublisher.cc | 6 +- src/systems/lift_drag/LiftDrag.cc | 32 +- src/systems/log/LogPlayback.cc | 6 +- src/systems/log/LogRecord.cc | 8 +- .../log_video_recorder/LogVideoRecorder.cc | 18 +- .../LogicalAudioSensorPlugin.cc | 26 +- src/systems/magnetometer/Magnetometer.cc | 2 +- .../MulticopterVelocityControl.cc | 8 +- .../MulticopterMotorModel.cc | 14 +- .../performer_detector/PerformerDetector.cc | 6 +- src/systems/physics/EntityFeatureMap_TEST.cc | 4 +- src/systems/physics/Physics.cc | 126 +++--- src/systems/pose_publisher/PosePublisher.cc | 14 +- .../scene_broadcaster/SceneBroadcaster.cc | 36 +- src/systems/sensors/Sensors.cc | 8 +- src/systems/thermal/Thermal.cc | 4 +- src/systems/touch_plugin/TouchPlugin.cc | 2 +- .../track_controller/TrackController.cc | 50 +-- src/systems/tracked_vehicle/TrackedVehicle.cc | 50 +-- .../triggered_publisher/TriggeredPublisher.cc | 2 +- .../velocity_control/VelocityControl.cc | 28 +- src/systems/wheel_slip/WheelSlip.cc | 2 +- src/systems/wind_effects/WindEffects.cc | 2 +- test/integration/ackermann_steering_system.cc | 16 +- test/integration/air_pressure_system.cc | 6 +- test/integration/altimeter_system.cc | 6 +- test/integration/apply_joint_force_system.cc | 2 +- test/integration/battery_plugin.cc | 14 +- test/integration/breadcrumbs.cc | 30 +- test/integration/buoyancy.cc | 16 +- .../integration/camera_video_record_system.cc | 6 +- test/integration/components.cc | 14 +- test/integration/depth_camera.cc | 4 +- test/integration/detachable_joint.cc | 8 +- test/integration/diff_drive_system.cc | 16 +- test/integration/each_new_removed.cc | 2 +- test/integration/elevator_system.cc | 2 +- test/integration/entity_erase.cc | 4 +- test/integration/examples_build.cc | 12 +- test/integration/gpu_lidar.cc | 4 +- test/integration/imu_system.cc | 8 +- test/integration/joint_controller_system.cc | 16 +- .../joint_position_controller_system.cc | 8 +- test/integration/level_manager.cc | 22 +- .../level_manager_runtime_performers.cc | 22 +- test/integration/lift_drag_system.cc | 8 +- test/integration/log_system.cc | 18 +- .../logical_audio_sensor_plugin.cc | 6 +- test/integration/logical_camera_system.cc | 22 +- test/integration/magnetometer_system.cc | 8 +- test/integration/multicopter.cc | 18 +- test/integration/network_handshake.cc | 8 +- test/integration/performer_detector.cc | 4 +- test/integration/physics_system.cc | 184 ++++----- test/integration/play_pause.cc | 14 +- test/integration/pose_publisher_system.cc | 14 +- test/integration/rgbd_camera.cc | 4 +- test/integration/scene_broadcaster_system.cc | 34 +- test/integration/sdf_frame_semantics.cc | 22 +- test/integration/sdf_include.cc | 4 +- test/integration/sensors_system.cc | 4 +- test/integration/thermal_system.cc | 6 +- test/integration/tracked_vehicle_system.cc | 20 +- test/integration/user_commands.cc | 20 +- test/integration/velocity_control_system.cc | 8 +- test/integration/wheel_slip.cc | 54 +-- test/integration/wind_effects.cc | 6 +- test/performance/level_manager.cc | 10 +- test/performance/sdf_runner.cc | 18 +- test/plugins/TestSystem.cc | 2 +- 124 files changed, 1385 insertions(+), 1383 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index dc5ed1dce8..167fe8e30a 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -22,6 +22,7 @@ #include #include +#include #include #include #include diff --git a/include/ignition/gazebo/detail/EntityComponentManager.hh b/include/ignition/gazebo/detail/EntityComponentManager.hh index dcdb37324b..d08722ee80 100644 --- a/include/ignition/gazebo/detail/EntityComponentManager.hh +++ b/include/ignition/gazebo/detail/EntityComponentManager.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include #include diff --git a/src/Component_TEST.cc b/src/Component_TEST.cc index ea3bdd40a4..1b03f4e547 100644 --- a/src/Component_TEST.cc +++ b/src/Component_TEST.cc @@ -321,7 +321,7 @@ TEST_F(ComponentTest, OStream) EXPECT_EQ("Mass: 0", ostr.str()); } - // Component with a ignition::msgs type that gets serialized by the default + // Component with a msgs type that gets serialized by the default // serializer { using Custom = components::ComponentMassMatrix().Mass()); } - // Component with a ignition::msgs type that gets deserialized by the message + // Component with a msgs type that gets deserialized by the message // deserializer { using Custom = components::Component IGNITION_GAZEBO_VISIBLE -msgs::Entity_Type ignition::gazebo::convert(const std::string &_in) +msgs::Entity_Type gazebo::convert(const std::string &_in) { msgs::Entity_Type out = msgs::Entity_Type_NONE; @@ -108,7 +108,7 @@ msgs::Entity_Type ignition::gazebo::convert(const std::string &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -math::Pose3d ignition::gazebo::convert(const msgs::Pose &_in) +math::Pose3d gazebo::convert(const msgs::Pose &_in) { math::Pose3d out(_in.position().x(), _in.position().y(), @@ -125,7 +125,7 @@ math::Pose3d ignition::gazebo::convert(const msgs::Pose &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Collision ignition::gazebo::convert(const sdf::Collision &_in) +msgs::Collision gazebo::convert(const sdf::Collision &_in) { msgs::Collision out; out.set_name(_in.Name()); @@ -138,7 +138,7 @@ msgs::Collision ignition::gazebo::convert(const sdf::Collision &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -sdf::Collision ignition::gazebo::convert(const msgs::Collision &_in) +sdf::Collision gazebo::convert(const msgs::Collision &_in) { sdf::Collision out; out.SetName(_in.name()); @@ -150,7 +150,7 @@ sdf::Collision ignition::gazebo::convert(const msgs::Collision &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Geometry ignition::gazebo::convert(const sdf::Geometry &_in) +msgs::Geometry gazebo::convert(const sdf::Geometry &_in) { msgs::Geometry out; if (_in.Type() == sdf::GeometryType::BOX && _in.BoxShape()) @@ -214,7 +214,7 @@ msgs::Geometry ignition::gazebo::convert(const sdf::Geometry &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -sdf::Geometry ignition::gazebo::convert(const msgs::Geometry &_in) +sdf::Geometry gazebo::convert(const msgs::Geometry &_in) { sdf::Geometry out; if (_in.type() == msgs::Geometry::BOX && _in.has_box()) @@ -300,7 +300,7 @@ sdf::Geometry ignition::gazebo::convert(const msgs::Geometry &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Material ignition::gazebo::convert(const sdf::Material &_in) +msgs::Material gazebo::convert(const sdf::Material &_in) { msgs::Material out; msgs::Set(out.mutable_ambient(), _in.Ambient()); @@ -354,7 +354,7 @@ msgs::Material ignition::gazebo::convert(const sdf::Material &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -sdf::Material ignition::gazebo::convert(const msgs::Material &_in) +sdf::Material gazebo::convert(const msgs::Material &_in) { sdf::Material out; out.SetAmbient(msgs::Convert(_in.ambient())); @@ -393,7 +393,7 @@ sdf::Material ignition::gazebo::convert(const msgs::Material &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Actor ignition::gazebo::convert(const sdf::Actor &_in) +msgs::Actor gazebo::convert(const sdf::Actor &_in) { msgs::Actor out; out.mutable_entity()->set_name(_in.Name()); @@ -433,7 +433,7 @@ msgs::Actor ignition::gazebo::convert(const sdf::Actor &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -sdf::Actor ignition::gazebo::convert(const msgs::Actor &_in) +sdf::Actor gazebo::convert(const msgs::Actor &_in) { sdf::Actor out; out.SetName(_in.entity().name()); @@ -476,7 +476,7 @@ sdf::Actor ignition::gazebo::convert(const msgs::Actor &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Light ignition::gazebo::convert(const sdf::Light &_in) +msgs::Light gazebo::convert(const sdf::Light &_in) { msgs::Light out; out.set_name(_in.Name()); @@ -504,7 +504,7 @@ msgs::Light ignition::gazebo::convert(const sdf::Light &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -sdf::Light ignition::gazebo::convert(const msgs::Light &_in) +sdf::Light gazebo::convert(const msgs::Light &_in) { sdf::Light out; out.SetName(_in.name()); @@ -532,7 +532,7 @@ sdf::Light ignition::gazebo::convert(const msgs::Light &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::GUI ignition::gazebo::convert(const sdf::Gui &_in) +msgs::GUI gazebo::convert(const sdf::Gui &_in) { msgs::GUI out; @@ -571,12 +571,12 @@ msgs::GUI ignition::gazebo::convert(const sdf::Gui &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Time ignition::gazebo::convert( +msgs::Time gazebo::convert( const std::chrono::steady_clock::duration &_in) { msgs::Time out; - auto secNsec = ignition::math::durationToSecNsec(_in); + auto secNsec = math::durationToSecNsec(_in); out.set_sec(secNsec.first); out.set_nsec(secNsec.second); @@ -587,7 +587,7 @@ msgs::Time ignition::gazebo::convert( ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -std::chrono::steady_clock::duration ignition::gazebo::convert( +std::chrono::steady_clock::duration gazebo::convert( const msgs::Time &_in) { return std::chrono::seconds(_in.sec()) + std::chrono::nanoseconds(_in.nsec()); @@ -596,7 +596,7 @@ std::chrono::steady_clock::duration ignition::gazebo::convert( ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Inertial ignition::gazebo::convert(const math::Inertiald &_in) +msgs::Inertial gazebo::convert(const math::Inertiald &_in) { msgs::Inertial out; msgs::Set(out.mutable_pose(), _in.Pose()); @@ -613,7 +613,7 @@ msgs::Inertial ignition::gazebo::convert(const math::Inertiald &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -math::Inertiald ignition::gazebo::convert(const msgs::Inertial &_in) +math::Inertiald gazebo::convert(const msgs::Inertial &_in) { math::MassMatrix3d massMatrix; massMatrix.SetMass(_in.mass()); @@ -633,7 +633,7 @@ math::Inertiald ignition::gazebo::convert(const msgs::Inertial &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Axis ignition::gazebo::convert(const sdf::JointAxis &_in) +msgs::Axis gazebo::convert(const sdf::JointAxis &_in) { msgs::Axis out; msgs::Set(out.mutable_xyz(), _in.Xyz()); @@ -671,7 +671,7 @@ msgs::Axis ignition::gazebo::convert(const sdf::JointAxis &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -sdf::JointAxis ignition::gazebo::convert(const msgs::Axis &_in) +sdf::JointAxis gazebo::convert(const msgs::Axis &_in) { sdf::JointAxis out; out.SetXyz(msgs::Convert(_in.xyz())); @@ -696,7 +696,7 @@ sdf::JointAxis ignition::gazebo::convert(const msgs::Axis &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Scene ignition::gazebo::convert(const sdf::Scene &_in) +msgs::Scene gazebo::convert(const sdf::Scene &_in) { msgs::Scene out; // todo(anyone) add Name to sdf::Scene? @@ -712,7 +712,7 @@ msgs::Scene ignition::gazebo::convert(const sdf::Scene &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -sdf::Scene ignition::gazebo::convert(const msgs::Scene &_in) +sdf::Scene gazebo::convert(const msgs::Scene &_in) { sdf::Scene out; // todo(anyone) add SetName to sdf::Scene? @@ -728,7 +728,7 @@ sdf::Scene ignition::gazebo::convert(const msgs::Scene &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Atmosphere ignition::gazebo::convert(const sdf::Atmosphere &_in) +msgs::Atmosphere gazebo::convert(const sdf::Atmosphere &_in) { msgs::Atmosphere out; out.set_temperature(_in.Temperature().Kelvin()); @@ -746,7 +746,7 @@ msgs::Atmosphere ignition::gazebo::convert(const sdf::Atmosphere &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -sdf::Atmosphere ignition::gazebo::convert(const msgs::Atmosphere &_in) +sdf::Atmosphere gazebo::convert(const msgs::Atmosphere &_in) { sdf::Atmosphere out; out.SetTemperature(math::Temperature(_in.temperature())); @@ -762,16 +762,16 @@ sdf::Atmosphere ignition::gazebo::convert(const msgs::Atmosphere &_in) } ////////////////////////////////////////////////// -void ignition::gazebo::set(msgs::Time *_msg, +void gazebo::set(msgs::Time *_msg, const std::chrono::steady_clock::duration &_in) { - auto secNsec = ignition::math::durationToSecNsec(_in); + auto secNsec = math::durationToSecNsec(_in); _msg->set_sec(secNsec.first); _msg->set_nsec(secNsec.second); } ////////////////////////////////////////////////// -void ignition::gazebo::set(msgs::WorldStatistics *_msg, +void gazebo::set(msgs::WorldStatistics *_msg, const gazebo::UpdateInfo &_in) { set(_msg->mutable_sim_time(), _in.simTime); @@ -783,7 +783,7 @@ void ignition::gazebo::set(msgs::WorldStatistics *_msg, ////////////////////////////////////////////////// template<> -msgs::Physics ignition::gazebo::convert(const sdf::Physics &_in) +msgs::Physics gazebo::convert(const sdf::Physics &_in) { msgs::Physics out; out.set_max_step_size(_in.MaxStepSize()); @@ -793,7 +793,7 @@ msgs::Physics ignition::gazebo::convert(const sdf::Physics &_in) ////////////////////////////////////////////////// template<> -sdf::Physics ignition::gazebo::convert(const msgs::Physics &_in) +sdf::Physics gazebo::convert(const msgs::Physics &_in) { sdf::Physics out; out.SetRealTimeFactor(_in.real_time_factor()); @@ -802,7 +802,7 @@ sdf::Physics ignition::gazebo::convert(const msgs::Physics &_in) } ////////////////////////////////////////////////// -void ignition::gazebo::set(msgs::SensorNoise *_msg, const sdf::Noise &_sdf) +void gazebo::set(msgs::SensorNoise *_msg, const sdf::Noise &_sdf) { switch (_sdf.Type()) { @@ -831,7 +831,7 @@ void ignition::gazebo::set(msgs::SensorNoise *_msg, const sdf::Noise &_sdf) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -sdf::Noise ignition::gazebo::convert(const msgs::SensorNoise &_in) +sdf::Noise gazebo::convert(const msgs::SensorNoise &_in) { sdf::Noise out; @@ -864,7 +864,7 @@ sdf::Noise ignition::gazebo::convert(const msgs::SensorNoise &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) +msgs::Sensor gazebo::convert(const sdf::Sensor &_in) { msgs::Sensor out; out.set_name(_in.Name()); @@ -880,17 +880,17 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) msgs::MagnetometerSensor *sensor = out.mutable_magnetometer(); if (_in.MagnetometerSensor()->XNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_x_noise(), + gazebo::set(sensor->mutable_x_noise(), _in.MagnetometerSensor()->XNoise()); } if (_in.MagnetometerSensor()->YNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_y_noise(), + gazebo::set(sensor->mutable_y_noise(), _in.MagnetometerSensor()->YNoise()); } if (_in.MagnetometerSensor()->ZNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_z_noise(), + gazebo::set(sensor->mutable_z_noise(), _in.MagnetometerSensor()->ZNoise()); } } @@ -940,13 +940,13 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) if (_in.AltimeterSensor()->VerticalPositionNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_vertical_position_noise(), + gazebo::set(sensor->mutable_vertical_position_noise(), _in.AltimeterSensor()->VerticalPositionNoise()); } if (_in.AltimeterSensor()->VerticalVelocityNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_vertical_velocity_noise(), + gazebo::set(sensor->mutable_vertical_velocity_noise(), _in.AltimeterSensor()->VerticalVelocityNoise()); } } @@ -965,7 +965,7 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) if (_in.AirPressureSensor()->PressureNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_pressure_noise(), + gazebo::set(sensor->mutable_pressure_noise(), _in.AirPressureSensor()->PressureNoise()); } sensor->set_reference_altitude( @@ -986,38 +986,38 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) if (sdfImu->LinearAccelerationXNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set( + gazebo::set( sensor->mutable_linear_acceleration()->mutable_x_noise(), sdfImu->LinearAccelerationXNoise()); } if (sdfImu->LinearAccelerationYNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set( + gazebo::set( sensor->mutable_linear_acceleration()->mutable_y_noise(), sdfImu->LinearAccelerationYNoise()); } if (sdfImu->LinearAccelerationZNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set( + gazebo::set( sensor->mutable_linear_acceleration()->mutable_z_noise(), sdfImu->LinearAccelerationZNoise()); } if (sdfImu->AngularVelocityXNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set( + gazebo::set( sensor->mutable_angular_velocity()->mutable_x_noise(), sdfImu->AngularVelocityXNoise()); } if (sdfImu->AngularVelocityYNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set( + gazebo::set( sensor->mutable_angular_velocity()->mutable_y_noise(), sdfImu->AngularVelocityYNoise()); } if (sdfImu->AngularVelocityZNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set( + gazebo::set( sensor->mutable_angular_velocity()->mutable_z_noise(), sdfImu->AngularVelocityZNoise()); } @@ -1051,7 +1051,7 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) if (sdfLidar->LidarNoise().Type() != sdf::NoiseType::NONE) { - ignition::gazebo::set(sensor->mutable_noise(), sdfLidar->LidarNoise()); + gazebo::set(sensor->mutable_noise(), sdfLidar->LidarNoise()); } sensor->set_horizontal_samples(sdfLidar->HorizontalScanSamples()); sensor->set_horizontal_resolution(sdfLidar->HorizontalScanResolution()); @@ -1081,7 +1081,7 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -sdf::Sensor ignition::gazebo::convert(const msgs::Sensor &_in) +sdf::Sensor gazebo::convert(const msgs::Sensor &_in) { sdf::Sensor out; out.SetName(_in.name()); @@ -1098,17 +1098,17 @@ sdf::Sensor ignition::gazebo::convert(const msgs::Sensor &_in) { if (_in.magnetometer().has_x_noise()) { - sensor.SetXNoise(ignition::gazebo::convert( + sensor.SetXNoise(gazebo::convert( _in.magnetometer().x_noise())); } if (_in.magnetometer().has_y_noise()) { - sensor.SetYNoise(ignition::gazebo::convert( + sensor.SetYNoise(gazebo::convert( _in.magnetometer().y_noise())); } if (_in.magnetometer().has_z_noise()) { - sensor.SetZNoise(ignition::gazebo::convert( + sensor.SetZNoise(gazebo::convert( _in.magnetometer().z_noise())); } } @@ -1163,13 +1163,13 @@ sdf::Sensor ignition::gazebo::convert(const msgs::Sensor &_in) { if (_in.altimeter().has_vertical_position_noise()) { - sensor.SetVerticalPositionNoise(ignition::gazebo::convert( + sensor.SetVerticalPositionNoise(gazebo::convert( _in.altimeter().vertical_position_noise())); } if (_in.altimeter().has_vertical_velocity_noise()) { - sensor.SetVerticalVelocityNoise(ignition::gazebo::convert( + sensor.SetVerticalVelocityNoise(gazebo::convert( _in.altimeter().vertical_velocity_noise())); } } @@ -1188,7 +1188,7 @@ sdf::Sensor ignition::gazebo::convert(const msgs::Sensor &_in) { if (_in.air_pressure().has_pressure_noise()) { - sensor.SetPressureNoise(ignition::gazebo::convert( + sensor.SetPressureNoise(gazebo::convert( _in.air_pressure().pressure_noise())); } @@ -1212,19 +1212,19 @@ sdf::Sensor ignition::gazebo::convert(const msgs::Sensor &_in) if (_in.imu().linear_acceleration().has_x_noise()) { sensor.SetLinearAccelerationXNoise( - ignition::gazebo::convert( + gazebo::convert( _in.imu().linear_acceleration().x_noise())); } if (_in.imu().linear_acceleration().has_y_noise()) { sensor.SetLinearAccelerationYNoise( - ignition::gazebo::convert( + gazebo::convert( _in.imu().linear_acceleration().y_noise())); } if (_in.imu().linear_acceleration().has_z_noise()) { sensor.SetLinearAccelerationZNoise( - ignition::gazebo::convert( + gazebo::convert( _in.imu().linear_acceleration().z_noise())); } } @@ -1234,19 +1234,19 @@ sdf::Sensor ignition::gazebo::convert(const msgs::Sensor &_in) if (_in.imu().angular_velocity().has_x_noise()) { sensor.SetAngularVelocityXNoise( - ignition::gazebo::convert( + gazebo::convert( _in.imu().angular_velocity().x_noise())); } if (_in.imu().angular_velocity().has_y_noise()) { sensor.SetAngularVelocityYNoise( - ignition::gazebo::convert( + gazebo::convert( _in.imu().angular_velocity().y_noise())); } if (_in.imu().angular_velocity().has_z_noise()) { sensor.SetAngularVelocityZNoise( - ignition::gazebo::convert( + gazebo::convert( _in.imu().angular_velocity().z_noise())); } } @@ -1303,7 +1303,7 @@ sdf::Sensor ignition::gazebo::convert(const msgs::Sensor &_in) if (_in.lidar().has_noise()) { - sensor.SetLidarNoise(ignition::gazebo::convert( + sensor.SetLidarNoise(gazebo::convert( _in.lidar().noise())); } } @@ -1321,7 +1321,7 @@ sdf::Sensor ignition::gazebo::convert(const msgs::Sensor &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::WorldStatistics ignition::gazebo::convert(const gazebo::UpdateInfo &_in) +msgs::WorldStatistics gazebo::convert(const gazebo::UpdateInfo &_in) { msgs::WorldStatistics out; set(&out, _in); @@ -1331,7 +1331,7 @@ msgs::WorldStatistics ignition::gazebo::convert(const gazebo::UpdateInfo &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -gazebo::UpdateInfo ignition::gazebo::convert(const msgs::WorldStatistics &_in) +gazebo::UpdateInfo gazebo::convert(const msgs::WorldStatistics &_in) { gazebo::UpdateInfo out; out.iterations = _in.iterations(); @@ -1345,7 +1345,7 @@ gazebo::UpdateInfo ignition::gazebo::convert(const msgs::WorldStatistics &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -msgs::AxisAlignedBox ignition::gazebo::convert(const math::AxisAlignedBox &_in) +msgs::AxisAlignedBox gazebo::convert(const math::AxisAlignedBox &_in) { msgs::AxisAlignedBox out; msgs::Set(out.mutable_min_corner(), _in.Min()); @@ -1356,7 +1356,7 @@ msgs::AxisAlignedBox ignition::gazebo::convert(const math::AxisAlignedBox &_in) ////////////////////////////////////////////////// template<> IGNITION_GAZEBO_VISIBLE -math::AxisAlignedBox ignition::gazebo::convert(const msgs::AxisAlignedBox &_in) +math::AxisAlignedBox gazebo::convert(const msgs::AxisAlignedBox &_in) { math::AxisAlignedBox out; out.Min() = msgs::Convert(_in.min_corner()); diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index d3d906b277..a9c2cc038e 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -53,8 +53,8 @@ TEST(Conversions, Light) light.SetRawPose({3, 2, 1, 0, IGN_PI, 0}); light.SetPoseRelativeTo("world"); light.SetCastShadows(true); - light.SetDiffuse(ignition::math::Color(0.4f, 0.5f, 0.6f, 1.0)); - light.SetSpecular(ignition::math::Color(0.8f, 0.9f, 0.1f, 1.0)); + light.SetDiffuse(math::Color(0.4f, 0.5f, 0.6f, 1.0)); + light.SetSpecular(math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); light.SetConstantAttenuationFactor(0.5); light.SetLinearAttenuationFactor(0.1); @@ -224,10 +224,10 @@ TEST(Conversions, Time) TEST(Conversions, Material) { sdf::Material material; - material.SetDiffuse(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.4f)); - material.SetSpecular(ignition::math::Color(0.5f, 0.6f, 0.7f, 0.8f)); - material.SetAmbient(ignition::math::Color(0.9f, 1.0f, 1.1f, 1.2f)); - material.SetEmissive(ignition::math::Color(1.3f, 1.4f, 1.5f, 1.6f)); + material.SetDiffuse(math::Color(0.1f, 0.2f, 0.3f, 0.4f)); + material.SetSpecular(math::Color(0.5f, 0.6f, 0.7f, 0.8f)); + material.SetAmbient(math::Color(0.9f, 1.0f, 1.1f, 1.2f)); + material.SetEmissive(math::Color(1.3f, 1.4f, 1.5f, 1.6f)); material.SetLighting(true); sdf::Pbr pbr; @@ -308,7 +308,7 @@ TEST(Conversions, GeometryBox) geometry.SetType(sdf::GeometryType::BOX); sdf::Box boxShape; - boxShape.SetSize(ignition::math::Vector3d(1, 2, 3)); + boxShape.SetSize(math::Vector3d(1, 2, 3)); geometry.SetBoxShape(boxShape); auto geometryMsg = convert(geometry); @@ -375,7 +375,7 @@ TEST(Conversions, GeometryMesh) geometry.SetType(sdf::GeometryType::MESH); sdf::Mesh meshShape; - meshShape.SetScale(ignition::math::Vector3d(1, 2, 3)); + meshShape.SetScale(math::Vector3d(1, 2, 3)); meshShape.SetUri("file://watermelon"); meshShape.SetSubmesh("grape"); meshShape.SetCenterSubmesh(true); @@ -406,8 +406,8 @@ TEST(Conversions, GeometryPlane) geometry.SetType(sdf::GeometryType::PLANE); sdf::Plane planeShape; - planeShape.SetSize(ignition::math::Vector2d(1, 2)); - planeShape.SetNormal(ignition::math::Vector3d::UnitY); + planeShape.SetSize(math::Vector2d(1, 2)); + planeShape.SetNormal(math::Vector3d::UnitY); geometry.SetPlaneShape(planeShape); auto geometryMsg = convert(geometry); @@ -535,8 +535,8 @@ TEST(Conversions, JointAxis) TEST(Conversions, Scene) { sdf::Scene scene; - scene.SetAmbient(ignition::math::Color(0.1f, 0.2f, 0.3f, 0.4f)); - scene.SetBackground(ignition::math::Color(0.5f, 0.6f, 0.7f, 0.8f)); + scene.SetAmbient(math::Color(0.1f, 0.2f, 0.3f, 0.4f)); + scene.SetBackground(math::Color(0.5f, 0.6f, 0.7f, 0.8f)); scene.SetShadows(true); scene.SetGrid(true); scene.SetOriginVisual(true); @@ -585,7 +585,7 @@ TEST(Conversions, MagnetometerSensor) sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetUpdateRate(12.4); sensor.SetTopic("my_topic"); - sensor.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + sensor.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); sdf::Noise noise; noise.SetType(sdf::NoiseType::GAUSSIAN); @@ -625,7 +625,7 @@ TEST(Conversions, AltimeterSensor) sensor.SetType(sdf::SensorType::ALTIMETER); sensor.SetUpdateRate(12.4); sensor.SetTopic("my_topic"); - sensor.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + sensor.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); sdf::Noise noise; noise.SetType(sdf::NoiseType::GAUSSIAN); diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index e66d41e12c..0203a435e0 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -899,9 +899,9 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, } ////////////////////////////////////////////////// -ignition::msgs::SerializedState EntityComponentManager::ChangedState() const +msgs::SerializedState EntityComponentManager::ChangedState() const { - ignition::msgs::SerializedState stateMsg; + msgs::SerializedState stateMsg; // New entities for (const auto &entity : this->dataPtr->newlyCreatedEntities) @@ -922,7 +922,7 @@ ignition::msgs::SerializedState EntityComponentManager::ChangedState() const ////////////////////////////////////////////////// void EntityComponentManager::ChangedState( - ignition::msgs::SerializedStateMap &_state) const + msgs::SerializedStateMap &_state) const { // New entities for (const auto &entity : this->dataPtr->newlyCreatedEntities) @@ -986,11 +986,11 @@ void EntityComponentManagerPrivate::CalculateStateThreadLoad() } ////////////////////////////////////////////////// -ignition::msgs::SerializedState EntityComponentManager::State( +msgs::SerializedState EntityComponentManager::State( const std::unordered_set &_entities, const std::unordered_set &_types) const { - ignition::msgs::SerializedState stateMsg; + msgs::SerializedState stateMsg; for (const auto &it : this->dataPtr->entityComponents) { auto entity = it.first; @@ -1056,7 +1056,7 @@ void EntityComponentManager::State( ////////////////////////////////////////////////// void EntityComponentManager::SetState( - const ignition::msgs::SerializedState &_stateMsg) + const msgs::SerializedState &_stateMsg) { IGN_PROFILE("EntityComponentManager::SetState Non-map"); // Create / remove / update entities @@ -1160,7 +1160,7 @@ void EntityComponentManager::SetState( ////////////////////////////////////////////////// void EntityComponentManager::SetState( - const ignition::msgs::SerializedStateMap &_stateMsg) + const msgs::SerializedStateMap &_stateMsg) { IGN_PROFILE("EntityComponentManager::SetState Map"); // Create / remove / update entities diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 4b688a9e3e..3a56e0526a 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -38,31 +38,31 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { -using IntComponent = components::Component; +using IntComponent = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.IntComponent", IntComponent) -using UIntComponent = components::Component; +using UIntComponent = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.UIntComponent", UIntComponent) -using DoubleComponent = components::Component; +using DoubleComponent = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.DoubleComponent", DoubleComponent) using StringComponent = - components::Component; + Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.StringComponent", StringComponent) -using BoolComponent = components::Component; +using BoolComponent = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.BoolComponent", BoolComponent) -using Even = components::Component; +using Even = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Even", Even) -using Odd = components::Component; +using Odd = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Odd", Odd) struct Custom @@ -70,7 +70,7 @@ struct Custom int dummy{123}; }; -using CustomComponent = components::Component; +using CustomComponent = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CustomComponent", CustomComponent) } @@ -103,7 +103,7 @@ class EntityComponentManagerFixture ///////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, AdjacentMemorySingleComponentType) { - std::vector poses; + std::vector poses; std::vector keys; int count = 10; @@ -114,7 +114,7 @@ TEST_P(EntityComponentManagerFixture, AdjacentMemorySingleComponentType) // Create the components. for (int i = 0; i < count; ++i) { - poses.push_back(components::Pose(math::Pose3d( + poses.push_back(Pose(math::Pose3d( math::Rand::IntNormal(10, 5), math::Rand::IntNormal(100, 50), math::Rand::IntNormal(-100, 30), 0, 0, 0))); @@ -130,16 +130,16 @@ TEST_P(EntityComponentManagerFixture, AdjacentMemorySingleComponentType) // Check the component values. for (int i = 0; i < count; ++i) { - EXPECT_EQ(poses[i], *(manager.Component(keys[i]))); + EXPECT_EQ(poses[i], *(manager.Component(keys[i]))); } { - uintptr_t poseSize = sizeof(components::Pose); - const components::Pose *pose = nullptr, *prevPose = nullptr; + uintptr_t poseSize = sizeof(Pose); + const Pose *pose = nullptr, *prevPose = nullptr; // Check that each component is adjacent in memory for (int i = 0; i < count; ++i) { - pose = manager.Component(keys[i]); + pose = manager.Component(keys[i]); if (prevPose != nullptr) { EXPECT_EQ(poseSize, reinterpret_cast(pose) - @@ -153,9 +153,9 @@ TEST_P(EntityComponentManagerFixture, AdjacentMemorySingleComponentType) const math::Pose3d *poseData = nullptr, *prevPoseData = nullptr; for (int i = 0; i < count; ++i) { - poseData = &(manager.Component(keys[i])->Data()); + poseData = &(manager.Component(keys[i])->Data()); uintptr_t poseDataSize = sizeof(math::Pose3d) + - sizeof(components::BaseComponent); + sizeof(BaseComponent); if (prevPoseData != nullptr) { EXPECT_EQ(poseDataSize, reinterpret_cast(poseData) - @@ -169,7 +169,7 @@ TEST_P(EntityComponentManagerFixture, AdjacentMemorySingleComponentType) ///////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, AdjacentMemoryTwoComponentTypes) { - std::vector poses; + std::vector poses; std::vector ints; std::vector poseKeys; std::vector intKeys; @@ -182,7 +182,7 @@ TEST_P(EntityComponentManagerFixture, AdjacentMemoryTwoComponentTypes) // Create the components. for (int i = 0; i < count; ++i) { - poses.push_back(components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0))); + poses.push_back(Pose(math::Pose3d(1, 2, 3, 0, 0, 0))); ints.push_back(IntComponent(i)); poseKeys.push_back(manager.CreateComponent(entity, poses.back())); @@ -198,15 +198,15 @@ TEST_P(EntityComponentManagerFixture, AdjacentMemoryTwoComponentTypes) ASSERT_EQ(static_cast(count), poseKeys.size()); ASSERT_EQ(static_cast(count), intKeys.size()); - uintptr_t poseSize = sizeof(components::Pose); + uintptr_t poseSize = sizeof(Pose); uintptr_t intSize = sizeof(IntComponent); - const components::Pose *pose = nullptr, *prevPose = nullptr; + const Pose *pose = nullptr, *prevPose = nullptr; const IntComponent *it = nullptr, *prevIt = nullptr; // Check that each component is adjacent in memory for (int i = 0; i < count; ++i) { - pose = manager.Component(poseKeys[i]); + pose = manager.Component(poseKeys[i]); it = manager.Component(intKeys[i]); if (prevPose != nullptr) { @@ -307,7 +307,7 @@ TEST_P(EntityComponentManagerFixture, RemoveComponent) // adjacent to each other. TEST_P(EntityComponentManagerFixture, RemoveAdjacent) { - std::vector poses; + std::vector poses; std::vector keys; Entity entity = manager.CreateEntity(); @@ -317,19 +317,19 @@ TEST_P(EntityComponentManagerFixture, RemoveAdjacent) // Create the components. for (int i = 0; i < count; ++i) { - poses.push_back(components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0))); + poses.push_back(Pose(math::Pose3d(1, 2, 3, 0, 0, 0))); keys.push_back(manager.CreateComponent(entity, poses.back())); EXPECT_EQ(keys.back().second, i); } ASSERT_EQ(poses.size(), keys.size()); - uintptr_t poseSize = sizeof(components::Pose); - const components::Pose *pose = nullptr, *prevPose = nullptr; + uintptr_t poseSize = sizeof(Pose); + const Pose *pose = nullptr, *prevPose = nullptr; // Check that each component is adjacent in memory for (int i = 0; i < count; ++i) { - pose = manager.Component(keys[i]); + pose = manager.Component(keys[i]); if (prevPose != nullptr) { EXPECT_EQ(poseSize, reinterpret_cast(pose) - @@ -347,10 +347,10 @@ TEST_P(EntityComponentManagerFixture, RemoveAdjacent) EXPECT_FALSE(manager.RemoveComponent(entity, keys[1])); // Check that the two remaining components are still adjacent in memory - const components::Pose *pose1 = - manager.Component(keys[0]); - const components::Pose *pose3 = - manager.Component(keys[2]); + const Pose *pose1 = + manager.Component(keys[0]); + const Pose *pose3 = + manager.Component(keys[2]); EXPECT_EQ(poseSize, reinterpret_cast(pose3) - reinterpret_cast(pose1)); } @@ -366,33 +366,33 @@ TEST_P(EntityComponentManagerFixture, RemoveAddAdjacent) std::vector keys; keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0)))); + Pose(math::Pose3d(1, 2, 3, 0, 0, 0)))); keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(3, 1, 2, 0, 0, 0)))); + Pose(math::Pose3d(3, 1, 2, 0, 0, 0)))); keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(0, 10, 20, 0, 0, 0)))); + Pose(math::Pose3d(0, 10, 20, 0, 0, 0)))); - uintptr_t poseSize = sizeof(components::Pose); + uintptr_t poseSize = sizeof(Pose); // Remove the middle component. EXPECT_TRUE(manager.RemoveComponent(entity, keys[1])); // Add two more new component keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(101, 51, 520, 0, 0, 0)))); + Pose(math::Pose3d(101, 51, 520, 0, 0, 0)))); keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(1010, 81, 821, 0, 0, 0)))); + Pose(math::Pose3d(1010, 81, 821, 0, 0, 0)))); // Check that the components are all adjacent in memory - const components::Pose *pose1 = - manager.Component(keys[0]); - const components::Pose *pose2 = - manager.Component(keys[2]); - const components::Pose *pose3 = - manager.Component(keys[3]); - const components::Pose *pose4 = - manager.Component(keys[4]); + const Pose *pose1 = + manager.Component(keys[0]); + const Pose *pose2 = + manager.Component(keys[2]); + const Pose *pose3 = + manager.Component(keys[3]); + const Pose *pose4 = + manager.Component(keys[4]); EXPECT_EQ(poseSize, reinterpret_cast(pose2) - reinterpret_cast(pose1)); @@ -404,10 +404,10 @@ TEST_P(EntityComponentManagerFixture, RemoveAddAdjacent) reinterpret_cast(pose4) - reinterpret_cast(pose3)); // Check the values of the components. - EXPECT_EQ(components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0)), *pose1); - EXPECT_EQ(components::Pose(math::Pose3d(0, 10, 20, 0, 0, 0)), *pose2); - EXPECT_EQ(components::Pose(math::Pose3d(101, 51, 520, 0, 0, 0)), *pose3); - EXPECT_EQ(components::Pose(math::Pose3d(1010, 81, 821, 0, 0, 0)), *pose4); + EXPECT_EQ(Pose(math::Pose3d(1, 2, 3, 0, 0, 0)), *pose1); + EXPECT_EQ(Pose(math::Pose3d(0, 10, 20, 0, 0, 0)), *pose2); + EXPECT_EQ(Pose(math::Pose3d(101, 51, 520, 0, 0, 0)), *pose3); + EXPECT_EQ(Pose(math::Pose3d(1010, 81, 821, 0, 0, 0)), *pose4); } ///////////////////////////////////////////////// @@ -498,8 +498,8 @@ TEST_P(EntityComponentManagerFixture, ComponentValues) manager.CreateComponent(eDouble, DoubleComponent(0.123)); manager.CreateComponent(eIntDouble, IntComponent(456)); manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); - manager.CreateComponent(ePose, - components::Pose({1, 2, 3, 0, 0, 0})); + manager.CreateComponent(ePose, + Pose({1, 2, 3, 0, 0, 0})); manager.CreateComponent(eCustom, CustomComponent(Custom())); @@ -565,19 +565,19 @@ TEST_P(EntityComponentManagerFixture, ComponentValues) } { - const auto *value = manager.Component(ePose); + const auto *value = manager.Component(ePose); ASSERT_NE(nullptr, value); EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), value->Data()); - auto data = manager.ComponentData(ePose); + auto data = manager.ComponentData(ePose); EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), data); - EXPECT_TRUE(manager.SetComponentData(ePose, + EXPECT_TRUE(manager.SetComponentData(ePose, {4, 5, 6, 0, 0, 0})); - data = manager.ComponentData(ePose); + data = manager.ComponentData(ePose); EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 0), data); - EXPECT_FALSE(manager.SetComponentData(ePose, + EXPECT_FALSE(manager.SetComponentData(ePose, {4, 5, 6, 0, 0, 0})); } @@ -1515,11 +1515,11 @@ TEST_P(EntityComponentManagerFixture, EntityGraph) EXPECT_TRUE(manager.SetParentEntity(e6, e2)); EXPECT_TRUE(manager.SetParentEntity(e7, e2)); - EXPECT_FALSE(manager.SetParentEntity(e1, gazebo::Entity(1000))); - EXPECT_FALSE(manager.SetParentEntity(gazebo::Entity(1000), e1)); + EXPECT_FALSE(manager.SetParentEntity(e1, Entity(1000))); + EXPECT_FALSE(manager.SetParentEntity(Entity(1000), e1)); // Check their parents - EXPECT_EQ(gazebo::kNullEntity, manager.ParentEntity(e1)); + EXPECT_EQ(kNullEntity, manager.ParentEntity(e1)); EXPECT_EQ(e1, manager.ParentEntity(e2)); EXPECT_EQ(e1, manager.ParentEntity(e3)); EXPECT_EQ(e2, manager.ParentEntity(e4)); @@ -1528,8 +1528,8 @@ TEST_P(EntityComponentManagerFixture, EntityGraph) EXPECT_EQ(e2, manager.ParentEntity(e7)); // Detach from graph - EXPECT_TRUE(manager.SetParentEntity(e7, gazebo::kNullEntity)); - EXPECT_EQ(gazebo::kNullEntity, manager.ParentEntity(e7)); + EXPECT_TRUE(manager.SetParentEntity(e7, kNullEntity)); + EXPECT_EQ(kNullEntity, manager.ParentEntity(e7)); // Reparent EXPECT_TRUE(manager.SetParentEntity(e5, e3)); @@ -2210,11 +2210,11 @@ TEST_P(EntityComponentManagerFixture, StateMsgUpdateComponent) // create an entity and component auto entity = originalECMStateMap.CreateEntity(); - originalECMStateMap.CreateComponent(entity, components::IntComponent(1)); + originalECMStateMap.CreateComponent(entity, IntComponent(1)); int foundEntities = 0; - otherECMStateMap.Each( - [&](const Entity &, const components::IntComponent *) + otherECMStateMap.Each( + [&](const Entity &, const IntComponent *) { foundEntities++; return true; @@ -2226,8 +2226,8 @@ TEST_P(EntityComponentManagerFixture, StateMsgUpdateComponent) originalECMStateMap.State(stateMapMsg); otherECMStateMap.SetState(stateMapMsg); foundEntities = 0; - otherECMStateMap.Each( - [&](const Entity &, const components::IntComponent *_intComp) + otherECMStateMap.Each( + [&](const Entity &, const IntComponent *_intComp) { foundEntities++; EXPECT_EQ(1, _intComp->Data()); @@ -2237,12 +2237,12 @@ TEST_P(EntityComponentManagerFixture, StateMsgUpdateComponent) // modify a component and then share the update with the other ECM stateMapMsg.Clear(); - originalECMStateMap.SetComponentData(entity, 2); + originalECMStateMap.SetComponentData(entity, 2); originalECMStateMap.State(stateMapMsg); otherECMStateMap.SetState(stateMapMsg); foundEntities = 0; - otherECMStateMap.Each( - [&](const Entity &, const components::IntComponent *_intComp) + otherECMStateMap.Each( + [&](const Entity &, const IntComponent *_intComp) { foundEntities++; EXPECT_EQ(2, _intComp->Data()); @@ -2256,8 +2256,8 @@ TEST_P(EntityComponentManagerFixture, StateMsgUpdateComponent) EntityComponentManager otherECMState; foundEntities = 0; - otherECMState.Each( - [&](const Entity &, const components::IntComponent *) + otherECMState.Each( + [&](const Entity &, const IntComponent *) { foundEntities++; return true; @@ -2265,13 +2265,13 @@ TEST_P(EntityComponentManagerFixture, StateMsgUpdateComponent) EXPECT_EQ(0, foundEntities); entity = originalECMState.CreateEntity(); - originalECMState.CreateComponent(entity, components::IntComponent(1)); + originalECMState.CreateComponent(entity, IntComponent(1)); auto stateMsg = originalECMState.State(); otherECMState.SetState(stateMsg); foundEntities = 0; - otherECMState.Each( - [&](const Entity &, const components::IntComponent *_intComp) + otherECMState.Each( + [&](const Entity &, const IntComponent *_intComp) { foundEntities++; EXPECT_EQ(1, _intComp->Data()); @@ -2280,12 +2280,12 @@ TEST_P(EntityComponentManagerFixture, StateMsgUpdateComponent) EXPECT_EQ(1, foundEntities); stateMsg.Clear(); - originalECMState.SetComponentData(entity, 2); + originalECMState.SetComponentData(entity, 2); stateMsg = originalECMState.State(); otherECMState.SetState(stateMsg); foundEntities = 0; - otherECMState.Each( - [&](const Entity &, const components::IntComponent *_intComp) + otherECMState.Each( + [&](const Entity &, const IntComponent *_intComp) { foundEntities++; EXPECT_EQ(2, _intComp->Data()); diff --git a/src/SdfEntityCreator_TEST.cc b/src/SdfEntityCreator_TEST.cc index b074ec4cbd..605c04286d 100644 --- a/src/SdfEntityCreator_TEST.cc +++ b/src/SdfEntityCreator_TEST.cc @@ -56,7 +56,7 @@ using namespace ignition; using namespace gazebo; ///////////////////////////////////////////////// -class EntityCompMgrTest : public gazebo::EntityComponentManager +class EntityCompMgrTest : public EntityComponentManager { public: void ProcessEntityRemovals() { @@ -162,21 +162,21 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) if (modelCount == 1) { - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 1), + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1), _pose->Data()); EXPECT_EQ("box", _name->Data()); boxModelEntity = _entity; } else if (modelCount == 2) { - EXPECT_EQ(ignition::math::Pose3d(-1, -2, -3, 0, 0, 1), + EXPECT_EQ(math::Pose3d(-1, -2, -3, 0, 0, 1), _pose->Data()); EXPECT_EQ("cylinder", _name->Data()); cylModelEntity = _entity; } else if (modelCount == 3) { - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 1), + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 0, 1), _pose->Data()); EXPECT_EQ("sphere", _name->Data()); sphModelEntity = _entity; @@ -213,7 +213,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) if (linkCount == 1) { - EXPECT_EQ(ignition::math::Pose3d(0.1, 0.1, 0.1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.1, 0.1, 0.1, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_link", _name->Data()); @@ -224,7 +224,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (linkCount == 2) { - EXPECT_EQ(ignition::math::Pose3d(0.2, 0.2, 0.2, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.2, 0.2, 0.2, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_link", _name->Data()); @@ -235,7 +235,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (linkCount == 3) { - EXPECT_EQ(ignition::math::Pose3d(0.3, 0.3, 0.3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.3, 0.3, 0.3, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_link", _name->Data()); @@ -311,7 +311,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) if (collisionCount == 1) { - EXPECT_EQ(ignition::math::Pose3d(0.11, 0.11, 0.11, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.11, 0.11, 0.11, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_collision", _name->Data()); @@ -326,7 +326,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (collisionCount == 2) { - EXPECT_EQ(ignition::math::Pose3d(0.21, 0.21, 0.21, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.21, 0.21, 0.21, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_collision", _name->Data()); @@ -341,7 +341,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (collisionCount == 3) { - EXPECT_EQ(ignition::math::Pose3d(0.31, 0.31, 0.31, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.31, 0.31, 0.31, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_collision", _name->Data()); @@ -393,7 +393,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) if (visualCount == 1) { - EXPECT_EQ(ignition::math::Pose3d(0.12, 0.12, 0.12, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.12, 0.12, 0.12, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_visual", _name->Data()); @@ -417,7 +417,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (visualCount == 2) { - EXPECT_EQ(ignition::math::Pose3d(0.22, 0.22, 0.22, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.22, 0.22, 0.22, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_visual", _name->Data()); @@ -441,7 +441,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (visualCount == 3) { - EXPECT_EQ(ignition::math::Pose3d(0.32, 0.32, 0.32, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.32, 0.32, 0.32, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_visual", _name->Data()); @@ -486,7 +486,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) lightCount++; - EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 10, 0, 0, 0), _pose->Data()); EXPECT_EQ("sun", _name->Data()); @@ -496,19 +496,19 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) EXPECT_EQ("sun", _light->Data().Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, _light->Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 10, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ("", _light->Data().PoseRelativeTo()); EXPECT_TRUE(_light->Data().CastShadows()); - EXPECT_EQ(ignition::math::Color(0.8f, 0.8f, 0.8f, 1), + EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1), _light->Data().Diffuse()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(1000, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.9, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(-0.5, 0.1, -0.9), + EXPECT_EQ(math::Vector3d(-0.5, 0.1, -0.9), _light->Data().Direction()); return true; }); @@ -583,7 +583,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) EXPECT_EQ(worldEntity, _parent->Data()); EXPECT_EQ(worldEntity, this->ecm.ParentEntity(_entity)); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere", _name->Data()); sphModelEntity = _entity; @@ -614,7 +614,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) linkCount++; - EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_link", _name->Data()); EXPECT_EQ(sphModelEntity, _parent->Data()); @@ -653,7 +653,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) visualCount++; - EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_visual", _name->Data()); @@ -695,7 +695,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) // light attached to link if (lightCount == 1u) { - EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("link_light_point", _name->Data()); @@ -704,13 +704,13 @@ TEST_F(SdfEntityCreatorTest, CreateLights) EXPECT_EQ("link_light_point", _light->Data().Name()); EXPECT_EQ(sdf::LightType::POINT, _light->Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 1, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(ignition::math::Color(0.0f, 0.0f, 1.0f, 1), + EXPECT_EQ(math::Color(0.0f, 0.0f, 1.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(ignition::math::Color(0.1f, 0.1f, 0.1f, 1), + EXPECT_EQ(math::Color(0.1f, 0.1f, 0.1f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(2, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.05, _light->Data().ConstantAttenuationFactor()); @@ -720,7 +720,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) // directional light in the world else if (lightCount == 2u) { - EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 10, 0, 0, 0), _pose->Data()); EXPECT_EQ("directional", _name->Data()); @@ -729,25 +729,25 @@ TEST_F(SdfEntityCreatorTest, CreateLights) EXPECT_EQ("directional", _light->Data().Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, _light->Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 10, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_TRUE(_light->Data().CastShadows()); - EXPECT_EQ(ignition::math::Color(0.8f, 0.8f, 0.8f, 1), + EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1), _light->Data().Diffuse()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(100, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.9, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.2, -0.9), + EXPECT_EQ(math::Vector3d(0.5, 0.2, -0.9), _light->Data().Direction()); } // point light in the world else if (lightCount == 3u) { - EXPECT_EQ(ignition::math::Pose3d(0.0, -1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, -1.5, 3, 0, 0, 0), _pose->Data()); EXPECT_EQ("point", _name->Data()); @@ -756,13 +756,13 @@ TEST_F(SdfEntityCreatorTest, CreateLights) EXPECT_EQ("point", _light->Data().Name()); EXPECT_EQ(sdf::LightType::POINT, _light->Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(0, -1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, -1.5, 3, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(ignition::math::Color(1.0f, 0.0f, 0.0f, 1), + EXPECT_EQ(math::Color(1.0f, 0.0f, 0.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(ignition::math::Color(0.1f, 0.1f, 0.1f, 1), + EXPECT_EQ(math::Color(0.1f, 0.1f, 0.1f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(4, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.2, _light->Data().ConstantAttenuationFactor()); @@ -772,7 +772,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) // spot light in the world else if (lightCount == 4u) { - EXPECT_EQ(ignition::math::Pose3d(0.0, 1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 1.5, 3, 0, 0, 0), _pose->Data()); EXPECT_EQ("spot", _name->Data()); @@ -781,19 +781,19 @@ TEST_F(SdfEntityCreatorTest, CreateLights) EXPECT_EQ("spot", _light->Data().Name()); EXPECT_EQ(sdf::LightType::SPOT, _light->Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(0, 1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 1.5, 3, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(ignition::math::Color(0.0f, 1.0f, 0.0f, 1), + EXPECT_EQ(math::Color(0.0f, 1.0f, 0.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(5, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.3, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.4, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(0.0, 0.0, -1.0), + EXPECT_EQ(math::Vector3d(0.0, 0.0, -1.0), _light->Data().Direction()); EXPECT_DOUBLE_EQ(0.1, _light->Data().SpotInnerAngle().Radian()); EXPECT_DOUBLE_EQ(0.5, _light->Data().SpotOuterAngle().Radian()); @@ -1068,7 +1068,7 @@ size_t removedCount(EntityCompMgrTest &_manager) { size_t count = 0; _manager.EachRemoved( - [&](const ignition::gazebo::Entity &, const Ts *...) -> bool + [&](const Entity &, const Ts *...) -> bool { ++count; return true; diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index e6e7ee2b4f..db979e35fd 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -499,7 +499,7 @@ unsigned int ServerConfig::Seed() const void ServerConfig::SetSeed(unsigned int _seed) { this->dataPtr->seed = _seed; - ignition::math::Rand::Seed(_seed); + math::Rand::Seed(_seed); } ///////////////////////////////////////////////// @@ -800,7 +800,7 @@ parsePluginsFromDoc(const tinyxml2::XMLDocument &_doc) ///////////////////////////////////////////////// std::list -ignition::gazebo::parsePluginsFromFile(const std::string &_fname) +gazebo::parsePluginsFromFile(const std::string &_fname) { tinyxml2::XMLDocument doc; doc.LoadFile(_fname.c_str()); @@ -809,7 +809,7 @@ ignition::gazebo::parsePluginsFromFile(const std::string &_fname) ///////////////////////////////////////////////// std::list -ignition::gazebo::parsePluginsFromString(const std::string &_str) +gazebo::parsePluginsFromString(const std::string &_str) { tinyxml2::XMLDocument doc; doc.Parse(_str.c_str()); @@ -819,28 +819,28 @@ ignition::gazebo::parsePluginsFromString(const std::string &_str) ///////////////////////////////////////////////// std::list -ignition::gazebo::loadPluginInfo(bool _isPlayback) +gazebo::loadPluginInfo(bool _isPlayback) { std::list ret; // 1. Check contents of environment variable std::string envConfig; - bool configSet = ignition::common::env(gazebo::kServerConfigPathEnv, + bool configSet = common::env(kServerConfigPathEnv, envConfig, true); if (configSet) { - if (ignition::common::exists(envConfig)) + if (common::exists(envConfig)) { // Parse configuration stored in environment variable - ret = ignition::gazebo::parsePluginsFromFile(envConfig); + ret = gazebo::parsePluginsFromFile(envConfig); if (ret.empty()) { // This may be desired behavior, but warn just in case. // Some users may want to defer all loading until later // during runtime. - ignwarn << gazebo::kServerConfigPathEnv + ignwarn << kServerConfigPathEnv << " set but no plugins found\n"; } igndbg << "Loaded (" << ret.size() << ") plugins from file " << @@ -853,7 +853,7 @@ ignition::gazebo::loadPluginInfo(bool _isPlayback) // This may be desired behavior, but warn just in case. // Some users may want to defer all loading until late // during runtime. - ignwarn << gazebo::kServerConfigPathEnv + ignwarn << kServerConfigPathEnv << " set but no file found," << " no plugins loaded\n"; return ret; @@ -871,27 +871,27 @@ ignition::gazebo::loadPluginInfo(bool _isPlayback) } std::string defaultConfigDir; - ignition::common::env(IGN_HOMEDIR, defaultConfigDir); - defaultConfigDir = ignition::common::joinPaths(defaultConfigDir, ".ignition", + common::env(IGN_HOMEDIR, defaultConfigDir); + defaultConfigDir = common::joinPaths(defaultConfigDir, ".ignition", "gazebo"); - auto defaultConfig = ignition::common::joinPaths(defaultConfigDir, + auto defaultConfig = common::joinPaths(defaultConfigDir, configFilename); - if (!ignition::common::exists(defaultConfig)) + if (!common::exists(defaultConfig)) { - auto installedConfig = ignition::common::joinPaths( + auto installedConfig = common::joinPaths( IGNITION_GAZEBO_SERVER_CONFIG_PATH, configFilename); - if (!ignition::common::createDirectories(defaultConfigDir)) + if (!common::createDirectories(defaultConfigDir)) { ignerr << "Failed to create directory [" << defaultConfigDir << "]." << std::endl; return ret; } - if (!ignition::common::exists(installedConfig)) + if (!common::exists(installedConfig)) { ignerr << "Failed to copy installed config [" << installedConfig << "] to default config [" << defaultConfig << "]." @@ -899,7 +899,7 @@ ignition::gazebo::loadPluginInfo(bool _isPlayback) << std::endl; return ret; } - else if (!ignition::common::copyFile(installedConfig, defaultConfig)) + else if (!common::copyFile(installedConfig, defaultConfig)) { ignerr << "Failed to copy installed config [" << installedConfig << "] to default config [" << defaultConfig << "]." @@ -914,7 +914,7 @@ ignition::gazebo::loadPluginInfo(bool _isPlayback) } } - ret = ignition::gazebo::parsePluginsFromFile(defaultConfig); + ret = gazebo::parsePluginsFromFile(defaultConfig); if (ret.empty()) { diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index 733a55ef37..c97c0922cd 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -179,11 +179,11 @@ TEST(ParsePluginsFromFile, PlaybackConfig) TEST(LoadPluginInfo, FromEmptyEnv) { // Set environment to something that doesn't exist - ASSERT_TRUE(common::setenv(gazebo::kServerConfigPathEnv, "foo")); + ASSERT_TRUE(common::setenv(kServerConfigPathEnv, "foo")); auto plugins = loadPluginInfo(); EXPECT_EQ(0u, plugins.size()); - EXPECT_TRUE(common::unsetenv(gazebo::kServerConfigPathEnv)); + EXPECT_TRUE(common::unsetenv(kServerConfigPathEnv)); } ////////////////////////////////////////////////// @@ -192,7 +192,7 @@ TEST(LoadPluginInfo, FromValidEnv) auto validPath = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "server_valid2.config"); - ASSERT_TRUE(common::setenv(gazebo::kServerConfigPathEnv, validPath)); + ASSERT_TRUE(common::setenv(kServerConfigPathEnv, validPath)); auto plugins = loadPluginInfo(); ASSERT_EQ(2u, plugins.size()); @@ -211,7 +211,7 @@ TEST(LoadPluginInfo, FromValidEnv) EXPECT_EQ("TestModelSystem", plugin->Filename()); EXPECT_EQ("ignition::gazebo::TestModelSystem", plugin->Name()); - EXPECT_TRUE(common::unsetenv(gazebo::kServerConfigPathEnv)); + EXPECT_TRUE(common::unsetenv(kServerConfigPathEnv)); } ////////////////////////////////////////////////// diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index ba9f0f5b36..65d7915bcf 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -315,7 +315,7 @@ void ServerPrivate::AddRecordPlugin(const ServerConfig &_config) { sdfCompressPath = std::string(sdfRecordPath); if (!std::string(1, sdfCompressPath.back()).compare( - ignition::common::separator(""))) + common::separator(""))) { // Remove the separator at end of path sdfCompressPath = sdfCompressPath.substr(0, @@ -439,7 +439,7 @@ void ServerPrivate::SetupTransport() } ////////////////////////////////////////////////// -bool ServerPrivate::WorldsService(ignition::msgs::StringMsg_V &_res) +bool ServerPrivate::WorldsService(msgs::StringMsg_V &_res) { std::lock_guard lock(this->worldsMutex); @@ -455,7 +455,7 @@ bool ServerPrivate::WorldsService(ignition::msgs::StringMsg_V &_res) ////////////////////////////////////////////////// bool ServerPrivate::ServerControlService( - const ignition::msgs::ServerControl &_req, msgs::Boolean &_res) + const msgs::ServerControl &_req, msgs::Boolean &_res) { _res.set_data(false); @@ -498,7 +498,7 @@ bool ServerPrivate::ServerControlService( ////////////////////////////////////////////////// void ServerPrivate::AddResourcePathsService( - const ignition::msgs::StringMsg_V &_req) + const msgs::StringMsg_V &_req) { std::vector paths; for (int i = 0; i < _req.data_size(); ++i) @@ -521,7 +521,7 @@ void ServerPrivate::AddResourcePathsService( ////////////////////////////////////////////////// bool ServerPrivate::ResourcePathsService( - ignition::msgs::StringMsg_V &_res) + msgs::StringMsg_V &_res) { _res.Clear(); diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 0b3f44740c..7cff29395f 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -54,7 +54,7 @@ class ServerFixture : public InternalFixture<::testing::TestWithParam> ///////////////////////////////////////////////// TEST_P(ServerFixture, DefaultServerConfig) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; EXPECT_TRUE(serverConfig.SdfFile().empty()); EXPECT_TRUE(serverConfig.SdfString().empty()); EXPECT_FALSE(serverConfig.UpdateRate()); @@ -111,7 +111,7 @@ TEST_P(ServerFixture, ServerConfigPluginInfo) pluginInfo.SetName("interface"); pluginInfo.SetSdf(nullptr); - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.AddPlugin(pluginInfo); const std::list &plugins = serverConfig.Plugins(); @@ -265,7 +265,7 @@ TEST_P(ServerFixture, ServerConfigSensorPlugin) ///////////////////////////////////////////////// TEST_P(ServerFixture, SdfServerConfig) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfString(TestWorldSansPhysics::World()); EXPECT_TRUE(serverConfig.SdfFile().empty()); @@ -366,7 +366,7 @@ TEST_P(ServerFixture, ServerConfigLogRecordCompress) ///////////////////////////////////////////////// TEST_P(ServerFixture, SdfStringServerConfig) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -580,7 +580,7 @@ TEST_P(ServerFixture, RunOncePaused) ///////////////////////////////////////////////// TEST_P(ServerFixture, RunNonBlockingMultiple) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfString(TestWorldSansPhysics::World()); gazebo::Server server(serverConfig); @@ -682,7 +682,7 @@ TEST_P(ServerFixture, ServerControlStop) // See: https://github.com/gazebosim/gz-sim/issues/1544 TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(TwoServersNonBlocking)) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfString(TestWorldSansPhysics::World()); gazebo::Server server1(serverConfig); @@ -723,7 +723,7 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(TwoServersNonBlocking)) // See: https://github.com/gazebosim/gz-sim/issues/1544 TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(TwoServersMixedBlocking)) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfString(TestWorldSansPhysics::World()); gazebo::Server server1(serverConfig); @@ -756,7 +756,7 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(TwoServersMixedBlocking)) ///////////////////////////////////////////////// TEST_P(ServerFixture, AddSystemWhileRunning) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -804,7 +804,7 @@ TEST_P(ServerFixture, AddSystemWhileRunning) ///////////////////////////////////////////////// TEST_P(ServerFixture, AddSystemAfterLoad) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -867,18 +867,18 @@ TEST_P(ServerFixture, AddSystemAfterLoad) ///////////////////////////////////////////////// TEST_P(ServerFixture, Seed) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; EXPECT_EQ(0u, serverConfig.Seed()); unsigned int mySeed = 12345u; serverConfig.SetSeed(mySeed); EXPECT_EQ(mySeed, serverConfig.Seed()); - EXPECT_EQ(mySeed, ignition::math::Rand::Seed()); + EXPECT_EQ(mySeed, math::Rand::Seed()); } ///////////////////////////////////////////////// TEST_P(ServerFixture, ResourcePath) { - ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", + common::setenv("IGN_GAZEBO_RESOURCE_PATH", (std::string(PROJECT_SOURCE_PATH) + "/test/worlds:" + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/models").c_str()); @@ -937,7 +937,7 @@ TEST_P(ServerFixture, ResourcePath) // Check physics system loaded meshes and got their BB correct eachCount = 0; _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::AxisAlignedBox *_box)->bool { auto box = _box->Data(); @@ -966,7 +966,7 @@ TEST_P(ServerFixture, ResourcePath) ///////////////////////////////////////////////// TEST_P(ServerFixture, GetResourcePaths) { - ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", + common::setenv("IGN_GAZEBO_RESOURCE_PATH", "/tmp/some/path:/home/user/another_path"); ServerConfig serverConfig; @@ -998,7 +998,7 @@ TEST_P(ServerFixture, CachedFuelWorld) { auto cachedWorldPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds"); - ignition::common::setenv("IGN_FUEL_CACHE_PATH", cachedWorldPath.c_str()); + common::setenv("IGN_FUEL_CACHE_PATH", cachedWorldPath.c_str()); ServerConfig serverConfig; auto fuelWorldURL = @@ -1022,10 +1022,10 @@ TEST_P(ServerFixture, CachedFuelWorld) ///////////////////////////////////////////////// TEST_P(ServerFixture, AddResourcePaths) { - ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", + common::setenv("IGN_GAZEBO_RESOURCE_PATH", "/tmp/some/path:/home/user/another_path"); - ignition::common::setenv("SDF_PATH", ""); - ignition::common::setenv("IGN_FILE_PATH", ""); + common::setenv("SDF_PATH", ""); + common::setenv("IGN_FILE_PATH", ""); ServerConfig serverConfig; gazebo::Server server(serverConfig); diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 9d06d9fce4..ebd33f7c51 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -175,7 +175,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, { ignmsg << "No systems loaded from SDF, loading defaults" << std::endl; bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); - auto plugins = ignition::gazebo::loadPluginInfo(isPlayback); + auto plugins = gazebo::loadPluginInfo(isPlayback); this->LoadServerPlugins(plugins); } @@ -381,14 +381,14 @@ void SimulationRunner::PublishStats() IGN_PROFILE("SimulationRunner::PublishStats"); // Create the world statistics message. - ignition::msgs::WorldStatistics msg; + msgs::WorldStatistics msg; msg.set_real_time_factor(this->realTimeFactor); auto realTimeSecNsec = - ignition::math::durationToSecNsec(this->currentInfo.realTime); + math::durationToSecNsec(this->currentInfo.realTime); auto simTimeSecNsec = - ignition::math::durationToSecNsec(this->currentInfo.simTime); + math::durationToSecNsec(this->currentInfo.simTime); msg.mutable_real_time()->set_sec(realTimeSecNsec.first); msg.mutable_real_time()->set_nsec(realTimeSecNsec.second); @@ -408,7 +408,7 @@ void SimulationRunner::PublishStats() // Create and publish the clock message. The clock message is not // throttled. - ignition::msgs::Clock clockMsg; + msgs::Clock clockMsg; clockMsg.mutable_real()->set_sec(realTimeSecNsec.first); clockMsg.mutable_real()->set_nsec(realTimeSecNsec.second); clockMsg.mutable_sim()->set_sec(simTimeSecNsec.first); @@ -543,7 +543,7 @@ void SimulationRunner::UpdateSystems() { IGN_PROFILE("SimulationRunner::UpdateSystems"); // \todo(nkoenig) Systems used to be updated in parallel using - // an ignition::common::WorkerPool. There is overhead associated with + // an common::WorkerPool. There is overhead associated with // this, most notably the creation and destruction of WorkOrders (see // WorkerPool.cc). We could turn on parallel updates in the future, and/or // turn it on if there are sufficient systems. More testing is required. @@ -650,14 +650,14 @@ bool SimulationRunner::Run(const uint64_t _iterations) { transport::AdvertiseMessageOptions advertOpts; advertOpts.SetMsgsPerSec(5); - this->statsPub = this->node->Advertise( + this->statsPub = this->node->Advertise( "stats", advertOpts); } if (!this->rootStatsPub.Valid()) { // Check for the existence of other publishers on `/stats` - std::vector publishers; + std::vector publishers; this->node->TopicInfo("/stats", publishers); if (!publishers.empty()) @@ -684,13 +684,13 @@ bool SimulationRunner::Run(const uint64_t _iterations) // Create the clock publisher. if (!this->clockPub.Valid()) - this->clockPub = this->node->Advertise("clock"); + this->clockPub = this->node->Advertise("clock"); // Create the global clock publisher. if (!this->rootClockPub.Valid()) { // Check for the existence of other publishers on `/clock` - std::vector publishers; + std::vector publishers; this->node->TopicInfo("/clock", publishers); if (!publishers.empty()) @@ -710,7 +710,7 @@ bool SimulationRunner::Run(const uint64_t _iterations) { ignmsg << "Found no publishers on /clock, adding root clock topic" << std::endl; - this->rootClockPub = this->node->Advertise( + this->rootClockPub = this->node->Advertise( "/clock"); } } @@ -1187,13 +1187,13 @@ SimulationRunner::UpdatePeriod() const } ///////////////////////////////////////////////// -const ignition::math::clock::duration &SimulationRunner::StepSize() const +const math::clock::duration &SimulationRunner::StepSize() const { return this->stepSize; } ///////////////////////////////////////////////// -void SimulationRunner::SetStepSize(const ignition::math::clock::duration &_step) +void SimulationRunner::SetStepSize(const math::clock::duration &_step) { this->stepSize = _step; } @@ -1269,7 +1269,7 @@ bool SimulationRunner::RequestRemoveEntity(const Entity _entity, } ////////////////////////////////////////////////// -bool SimulationRunner::GuiInfoService(ignition::msgs::GUI &_res) +bool SimulationRunner::GuiInfoService(msgs::GUI &_res) { _res.Clear(); diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 3a691ea6e2..49e3cbeb27 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -68,11 +68,11 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { -using IntComponent = components::Component; +using IntComponent = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.IntComponent", IntComponent) -using DoubleComponent = components::Component; +using DoubleComponent = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.DoubleComponent", DoubleComponent) } @@ -118,31 +118,31 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check component types EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::World::typeId)); + World::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Model::typeId)); + Model::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::CanonicalLink::typeId)); + CanonicalLink::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Link::typeId)); + Link::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Collision::typeId)); + Collision::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Visual::typeId)); + Visual::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Light::typeId)); + Light::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Name::typeId)); + Name::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::ParentEntity::typeId)); + ParentEntity::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Geometry::typeId)); + Geometry::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( components::Material::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Inertial::typeId)); + Inertial::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Wind::typeId)); + Wind::typeId)); // Check entities // 1 x world + 1 x (default) level + 1 x wind + 3 x model + 3 x link + 3 x @@ -152,10 +152,10 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check worlds unsigned int worldCount{0}; Entity worldEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::World *_world, + const World *_world, const components::Name *_name)->bool { EXPECT_NE(nullptr, _world); @@ -181,14 +181,14 @@ TEST_P(SimulationRunnerTest, CreateEntities) Entity boxModelEntity = kNullEntity; Entity cylModelEntity = kNullEntity; Entity sphModelEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::Model *_model, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Model *_model, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _model); @@ -201,21 +201,21 @@ TEST_P(SimulationRunnerTest, CreateEntities) EXPECT_EQ(worldEntity, _parent->Data()); if (modelCount == 1) { - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 1), + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1), _pose->Data()); EXPECT_EQ("box", _name->Data()); boxModelEntity = _entity; } else if (modelCount == 2) { - EXPECT_EQ(ignition::math::Pose3d(-1, -2, -3, 0, 0, 1), + EXPECT_EQ(math::Pose3d(-1, -2, -3, 0, 0, 1), _pose->Data()); EXPECT_EQ("cylinder", _name->Data()); cylModelEntity = _entity; } else if (modelCount == 3) { - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 1), + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 0, 1), _pose->Data()); EXPECT_EQ("sphere", _name->Data()); sphModelEntity = _entity; @@ -233,14 +233,14 @@ TEST_P(SimulationRunnerTest, CreateEntities) Entity boxLinkEntity = kNullEntity; Entity cylLinkEntity = kNullEntity; Entity sphLinkEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::Link *_link, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Link *_link, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _link); @@ -252,7 +252,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) if (linkCount == 1) { - EXPECT_EQ(ignition::math::Pose3d(0.1, 0.1, 0.1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.1, 0.1, 0.1, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_link", _name->Data()); EXPECT_EQ(boxModelEntity, _parent->Data()); @@ -260,7 +260,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (linkCount == 2) { - EXPECT_EQ(ignition::math::Pose3d(0.2, 0.2, 0.2, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.2, 0.2, 0.2, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_link", _name->Data()); EXPECT_EQ(cylModelEntity, _parent->Data()); @@ -268,7 +268,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (linkCount == 3) { - EXPECT_EQ(ignition::math::Pose3d(0.3, 0.3, 0.3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.3, 0.3, 0.3, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_link", _name->Data()); EXPECT_EQ(sphModelEntity, _parent->Data()); @@ -284,10 +284,10 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check inertials unsigned int inertialCount{0}; - runner.EntityCompMgr().Each( + runner.EntityCompMgr().Each( [&](const Entity & _entity, - const components::Link *_link, - const components::Inertial *_inertial)->bool + const Link *_link, + const Inertial *_inertial)->bool { EXPECT_NE(nullptr, _link); EXPECT_NE(nullptr, _inertial); @@ -319,16 +319,16 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check collisions unsigned int collisionCount{0}; - runner.EntityCompMgr().Each( [&](const Entity &/*_entity*/, - const components::Collision *_collision, - const components::Geometry *_geometry, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Collision *_collision, + const Geometry *_geometry, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _collision); @@ -341,7 +341,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) if (collisionCount == 1) { - EXPECT_EQ(ignition::math::Pose3d(0.11, 0.11, 0.11, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.11, 0.11, 0.11, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_collision", _name->Data()); @@ -355,7 +355,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (collisionCount == 2) { - EXPECT_EQ(ignition::math::Pose3d(0.21, 0.21, 0.21, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.21, 0.21, 0.21, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_collision", _name->Data()); @@ -369,7 +369,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (collisionCount == 3) { - EXPECT_EQ(ignition::math::Pose3d(0.31, 0.31, 0.31, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.31, 0.31, 0.31, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_collision", _name->Data()); @@ -387,18 +387,18 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check visuals unsigned int visualCount{0}; - runner.EntityCompMgr().Each( [&](const Entity &/*_entity*/, - const components::Visual *_visual, - const components::Geometry *_geometry, + const Visual *_visual, + const Geometry *_geometry, const components::Material *_material, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _visual); @@ -412,7 +412,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) if (visualCount == 1) { - EXPECT_EQ(ignition::math::Pose3d(0.12, 0.12, 0.12, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.12, 0.12, 0.12, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_visual", _name->Data()); @@ -431,7 +431,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (visualCount == 2) { - EXPECT_EQ(ignition::math::Pose3d(0.22, 0.22, 0.22, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.22, 0.22, 0.22, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_visual", _name->Data()); @@ -450,7 +450,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (visualCount == 3) { - EXPECT_EQ(ignition::math::Pose3d(0.32, 0.32, 0.32, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.32, 0.32, 0.32, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_visual", _name->Data()); @@ -473,14 +473,14 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check lights unsigned int lightCount{0}; - runner.EntityCompMgr().Each( [&](const Entity &/*_entity*/, - const components::Light *_light, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Light *_light, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _light); @@ -490,7 +490,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) lightCount++; - EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 10, 0, 0, 0), _pose->Data()); EXPECT_EQ("sun", _name->Data()); @@ -499,19 +499,19 @@ TEST_P(SimulationRunnerTest, CreateEntities) EXPECT_EQ("sun", _light->Data().Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, _light->Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 10, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ("", _light->Data().PoseRelativeTo()); EXPECT_TRUE(_light->Data().CastShadows()); - EXPECT_EQ(ignition::math::Color(0.8f, 0.8f, 0.8f, 1), + EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1), _light->Data().Diffuse()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(1000, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.9, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(-0.5, 0.1, -0.9), + EXPECT_EQ(math::Vector3d(-0.5, 0.1, -0.9), _light->Data().Direction()); return true; }); @@ -541,10 +541,10 @@ TEST_P(SimulationRunnerTest, CreateLights) // Check worlds unsigned int worldCount{0}; Entity worldEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::World *_world, + const World *_world, const components::Name *_name)->bool { EXPECT_NE(nullptr, _world); @@ -564,14 +564,14 @@ TEST_P(SimulationRunnerTest, CreateLights) // Check model unsigned int modelCount{0}; Entity sphModelEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::Model *_model, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Model *_model, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _model); @@ -582,7 +582,7 @@ TEST_P(SimulationRunnerTest, CreateLights) modelCount++; EXPECT_EQ(worldEntity, _parent->Data()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere", _name->Data()); sphModelEntity = _entity; @@ -596,14 +596,14 @@ TEST_P(SimulationRunnerTest, CreateLights) // Check link unsigned int linkCount{0}; Entity sphLinkEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::Link *_link, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Link *_link, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _link); @@ -613,7 +613,7 @@ TEST_P(SimulationRunnerTest, CreateLights) linkCount++; - EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_link", _name->Data()); EXPECT_EQ(sphModelEntity, _parent->Data()); @@ -627,18 +627,18 @@ TEST_P(SimulationRunnerTest, CreateLights) // Check visuals unsigned int visualCount{0}; - runner.EntityCompMgr().Each( [&](const Entity &/*_entity*/, - const components::Visual *_visual, - const components::Geometry *_geometry, + const Visual *_visual, + const Geometry *_geometry, const components::Material *_material, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _visual); @@ -650,7 +650,7 @@ TEST_P(SimulationRunnerTest, CreateLights) visualCount++; - EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_visual", _name->Data()); @@ -671,14 +671,14 @@ TEST_P(SimulationRunnerTest, CreateLights) // Check lights unsigned int lightCount{0}; - runner.EntityCompMgr().Each( [&](const Entity &/*_entity*/, - const components::Light *_light, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Light *_light, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _light); @@ -691,19 +691,19 @@ TEST_P(SimulationRunnerTest, CreateLights) // light attached to link if (lightCount == 1u) { - EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("link_light_point", _name->Data()); EXPECT_EQ(sphLinkEntity, _parent->Data()); EXPECT_EQ("link_light_point", _light->Data().Name()); EXPECT_EQ(sdf::LightType::POINT, _light->Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 1, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(ignition::math::Color(0.0f, 0.0f, 1.0f, 1), + EXPECT_EQ(math::Color(0.0f, 0.0f, 1.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(ignition::math::Color(0.1f, 0.1f, 0.1f, 1), + EXPECT_EQ(math::Color(0.1f, 0.1f, 0.1f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(2, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.05, _light->Data().ConstantAttenuationFactor()); @@ -713,43 +713,43 @@ TEST_P(SimulationRunnerTest, CreateLights) // directional light in the world else if (lightCount == 2u) { - EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 10, 0, 0, 0), _pose->Data()); EXPECT_EQ("directional", _name->Data()); EXPECT_EQ(worldEntity, _parent->Data()); EXPECT_EQ("directional", _light->Data().Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, _light->Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(0, 0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 10, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_TRUE(_light->Data().CastShadows()); - EXPECT_EQ(ignition::math::Color(0.8f, 0.8f, 0.8f, 1), + EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1), _light->Data().Diffuse()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(100, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.9, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(0.5, 0.2, -0.9), + EXPECT_EQ(math::Vector3d(0.5, 0.2, -0.9), _light->Data().Direction()); } // point light in the world else if (lightCount == 3u) { - EXPECT_EQ(ignition::math::Pose3d(0.0, -1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, -1.5, 3, 0, 0, 0), _pose->Data()); EXPECT_EQ("point", _name->Data()); EXPECT_EQ(worldEntity, _parent->Data()); EXPECT_EQ("point", _light->Data().Name()); EXPECT_EQ(sdf::LightType::POINT, _light->Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(0, -1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, -1.5, 3, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(ignition::math::Color(1.0f, 0.0f, 0.0f, 1), + EXPECT_EQ(math::Color(1.0f, 0.0f, 0.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(ignition::math::Color(0.1f, 0.1f, 0.1f, 1), + EXPECT_EQ(math::Color(0.1f, 0.1f, 0.1f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(4, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.2, _light->Data().ConstantAttenuationFactor()); @@ -759,25 +759,25 @@ TEST_P(SimulationRunnerTest, CreateLights) // spot light in the world else if (lightCount == 4u) { - EXPECT_EQ(ignition::math::Pose3d(0.0, 1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 1.5, 3, 0, 0, 0), _pose->Data()); EXPECT_EQ("spot", _name->Data()); EXPECT_EQ(worldEntity, _parent->Data()); EXPECT_EQ("spot", _light->Data().Name()); EXPECT_EQ(sdf::LightType::SPOT, _light->Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(0, 1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 1.5, 3, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(ignition::math::Color(0.0f, 1.0f, 0.0f, 1), + EXPECT_EQ(math::Color(0.0f, 1.0f, 0.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(5, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.3, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.4, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(ignition::math::Vector3d(0.0, 0.0, -1.0), + EXPECT_EQ(math::Vector3d(0.0, 0.0, -1.0), _light->Data().Direction()); EXPECT_DOUBLE_EQ(0.1, _light->Data().SpotInnerAngle().Radian()); EXPECT_DOUBLE_EQ(0.5, _light->Data().SpotOuterAngle().Radian()); @@ -805,34 +805,34 @@ TEST_P(SimulationRunnerTest, CreateJointEntities) // Check component types EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::World::typeId)); + World::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::CanonicalLink::typeId)); + CanonicalLink::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Link::typeId)); + Link::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Joint::typeId)); + Joint::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::JointAxis::typeId)); + JointAxis::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::JointType::typeId)); + JointType::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::ChildLinkName::typeId)); + ChildLinkName::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::ParentLinkName::typeId)); + ParentLinkName::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::ParentEntity::typeId)); + ParentEntity::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Pose::typeId)); + Pose::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Name::typeId)); + Name::typeId)); const sdf::Model *model = root.WorldByIndex(0)->ModelByIndex(1); // Check canonical links unsigned int canonicalLinkCount{0}; - runner.EntityCompMgr().Each( - [&](const Entity &, const components::CanonicalLink *)->bool + runner.EntityCompMgr().Each( + [&](const Entity &, const CanonicalLink *)->bool { canonicalLinkCount++; return true; @@ -852,11 +852,11 @@ TEST_P(SimulationRunnerTest, CreateJointEntities) }; auto testJoint = [&testAxis](const sdf::Joint *_joint, - const components::JointAxis *_axis, - const components::JointAxis2 *_axis2, - const components::ParentLinkName *_parentLinkName, - const components::ChildLinkName *_childLinkName, - const components::Pose *_pose, + const JointAxis *_axis, + const JointAxis2 *_axis2, + const ParentLinkName *_parentLinkName, + const ChildLinkName *_childLinkName, + const Pose *_pose, const components::Name *_name, bool _checkAxis = true, bool _checkAxis2 = false) @@ -891,25 +891,25 @@ TEST_P(SimulationRunnerTest, CreateJointEntities) }; std::set jointTypes; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::Joint * /*_joint*/, - const components::JointType *_jointType, - const components::ParentLinkName *_parentLinkName, - const components::ChildLinkName *_childLinkName, - const components::Pose *_pose, + const Joint * /*_joint*/, + const JointType *_jointType, + const ParentLinkName *_parentLinkName, + const ChildLinkName *_childLinkName, + const Pose *_pose, const components::Name *_name)->bool { jointTypes.insert(_jointType->Data()); auto axis = - runner.EntityCompMgr().Component(_entity); + runner.EntityCompMgr().Component(_entity); auto axis2 = - runner.EntityCompMgr().Component(_entity); + runner.EntityCompMgr().Component(_entity); const sdf::Joint *joint = model->JointByName(_name->Data()); @@ -1068,9 +1068,9 @@ TEST_P(SimulationRunnerTest, LoadPlugins) // Get world entity Entity worldId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::World *_world)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const World *_world)->bool { EXPECT_NE(nullptr, _world); worldId = _entity; @@ -1080,9 +1080,9 @@ TEST_P(SimulationRunnerTest, LoadPlugins) // Get model entity Entity modelId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::Model *_model)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const Model *_model)->bool { EXPECT_NE(nullptr, _model); modelId = _entity; @@ -1092,9 +1092,9 @@ TEST_P(SimulationRunnerTest, LoadPlugins) // Get sensor entity Entity sensorId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::Sensor *_sensor)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const Sensor *_sensor)->bool { EXPECT_NE(nullptr, _sensor); sensorId = _entity; @@ -1104,7 +1104,7 @@ TEST_P(SimulationRunnerTest, LoadPlugins) // Check component registered by world plugin std::string worldComponentName{"WorldPluginComponent"}; - auto worldComponentId = ignition::common::hash64(worldComponentName); + auto worldComponentId = common::hash64(worldComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(worldComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(worldId, @@ -1112,7 +1112,7 @@ TEST_P(SimulationRunnerTest, LoadPlugins) // Check component registered by model plugin std::string modelComponentName{"ModelPluginComponent"}; - auto modelComponentId = ignition::common::hash64(modelComponentName); + auto modelComponentId = common::hash64(modelComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(modelComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(modelId, @@ -1120,7 +1120,7 @@ TEST_P(SimulationRunnerTest, LoadPlugins) // Check component registered by sensor plugin std::string sensorComponentName{"SensorPluginComponent"}; - auto sensorComponentId = ignition::common::hash64(sensorComponentName); + auto sensorComponentId = common::hash64(sensorComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(sensorComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sensorId, @@ -1133,9 +1133,9 @@ TEST_P(SimulationRunnerTest, LoadPlugins) // reproduce? Maybe we need to test unloading plugins, but we have no API for // it yet. #if defined (__clang__) - components::Factory::Instance()->Unregister(worldComponentId); - components::Factory::Instance()->Unregister(modelComponentId); - components::Factory::Instance()->Unregister(sensorComponentId); + Factory::Instance()->Unregister(worldComponentId); + Factory::Instance()->Unregister(modelComponentId); + Factory::Instance()->Unregister(sensorComponentId); #endif } @@ -1150,7 +1150,7 @@ TEST_P(SimulationRunnerTest, LoadServerNoPlugins) // ServerConfig will fall back to environment variable auto config = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "server_valid2.config"); - ASSERT_EQ(true, common::setenv(gazebo::kServerConfigPathEnv, config)); + ASSERT_EQ(true, common::setenv(kServerConfigPathEnv, config)); ServerConfig serverConfig; // Create simulation runner @@ -1190,9 +1190,9 @@ TEST_P(SimulationRunnerTest, LoadServerConfigPlugins) // Get world entity Entity worldId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::World *_world)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const World *_world)->bool { EXPECT_NE(nullptr, _world); worldId = _entity; @@ -1202,9 +1202,9 @@ TEST_P(SimulationRunnerTest, LoadServerConfigPlugins) // Get model entity Entity modelId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::Model *_model)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const Model *_model)->bool { EXPECT_NE(nullptr, _model); modelId = _entity; @@ -1214,9 +1214,9 @@ TEST_P(SimulationRunnerTest, LoadServerConfigPlugins) // Get sensor entity Entity sensorId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::Sensor *_sensor)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const Sensor *_sensor)->bool { EXPECT_NE(nullptr, _sensor); sensorId = _entity; @@ -1226,7 +1226,7 @@ TEST_P(SimulationRunnerTest, LoadServerConfigPlugins) // Check component registered by world plugin std::string worldComponentName{"WorldPluginComponent"}; - auto worldComponentId = ignition::common::hash64(worldComponentName); + auto worldComponentId = common::hash64(worldComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(worldComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(worldId, @@ -1234,7 +1234,7 @@ TEST_P(SimulationRunnerTest, LoadServerConfigPlugins) // Check component registered by model plugin std::string modelComponentName{"ModelPluginComponent"}; - auto modelComponentId = ignition::common::hash64(modelComponentName); + auto modelComponentId = common::hash64(modelComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(modelComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(modelId, @@ -1242,7 +1242,7 @@ TEST_P(SimulationRunnerTest, LoadServerConfigPlugins) // Check component registered by sensor plugin std::string sensorComponentName{"SensorPluginComponent"}; - auto sensorComponentId = ignition::common::hash64(sensorComponentName); + auto sensorComponentId = common::hash64(sensorComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(sensorComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sensorId, @@ -1255,9 +1255,9 @@ TEST_P(SimulationRunnerTest, LoadServerConfigPlugins) // reproduce? Maybe we need to test unloading plugins, but we have no API for // it yet. #if defined (__clang__) - components::Factory::Instance()->Unregister(worldComponentId); - components::Factory::Instance()->Unregister(modelComponentId); - components::Factory::Instance()->Unregister(sensorComponentId); + Factory::Instance()->Unregister(worldComponentId); + Factory::Instance()->Unregister(modelComponentId); + Factory::Instance()->Unregister(sensorComponentId); #endif } @@ -1273,13 +1273,13 @@ TEST_P(SimulationRunnerTest, LoadPluginsDefault) // The user may have modified their local config. auto config = common::joinPaths(PROJECT_SOURCE_PATH, "include", "ignition", "gazebo", "server.config"); - ASSERT_TRUE(common::setenv(gazebo::kServerConfigPathEnv, config)); + ASSERT_TRUE(common::setenv(kServerConfigPathEnv, config)); // Create simulation runner auto systemLoader = std::make_shared(); SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader); ASSERT_EQ(3u, runner.SystemCount()); - common::unsetenv(gazebo::kServerConfigPathEnv); + common::unsetenv(kServerConfigPathEnv); } ///////////////////////////////////////////////// @@ -1298,24 +1298,24 @@ TEST_P(SimulationRunnerTest, LoadPluginsEvent) // Get model entities auto boxEntity = runner.EntityCompMgr().EntityByComponents( - ignition::gazebo::components::Model(), - ignition::gazebo::components::Name("box")); + Model(), + components::Name("box")); EXPECT_NE(kNullEntity, boxEntity); auto sphereEntity = runner.EntityCompMgr().EntityByComponents( - ignition::gazebo::components::Model(), - ignition::gazebo::components::Name("sphere")); + Model(), + components::Name("sphere")); EXPECT_NE(kNullEntity, sphereEntity); auto cylinderEntity = runner.EntityCompMgr().EntityByComponents( - ignition::gazebo::components::Model(), - ignition::gazebo::components::Name("cylinder")); + Model(), + components::Name("cylinder")); EXPECT_NE(kNullEntity, cylinderEntity); // We can't access the type registered by the plugin unless we link against // it, but we know its name to check std::string componentName{"ModelPluginComponent"}; - auto componentId = ignition::common::hash64(componentName); + auto componentId = common::hash64(componentName); // Check there's no double component EXPECT_FALSE(runner.EntityCompMgr().HasComponentType(componentId)); diff --git a/src/SystemLoader_TEST.cc b/src/SystemLoader_TEST.cc index 2a06a7daac..ce4c7845b7 100644 --- a/src/SystemLoader_TEST.cc +++ b/src/SystemLoader_TEST.cc @@ -34,7 +34,7 @@ TEST(SystemLoader, Constructor) gazebo::SystemLoader sm; // Add test plugin to path (referenced in config) - auto testBuildPath = ignition::common::joinPaths( + auto testBuildPath = common::joinPaths( std::string(PROJECT_BINARY_PATH), "lib"); sm.AddSystemPluginPath(testBuildPath); diff --git a/src/TestFixture.cc b/src/TestFixture.cc index 8f1dffd9f5..e3a9479749 100644 --- a/src/TestFixture.cc +++ b/src/TestFixture.cc @@ -113,7 +113,7 @@ class ignition::gazebo::TestFixturePrivate public: void Init(const ServerConfig &_config); /// \brief Pointer to underlying server - public: std::shared_ptr server{nullptr}; + public: std::shared_ptr server{nullptr}; /// \brief Pointer to underlying Helper interface public: std::shared_ptr helperSystem{nullptr}; @@ -149,7 +149,7 @@ TestFixture::~TestFixture() void TestFixturePrivate::Init(const ServerConfig &_config) { this->helperSystem = std::make_shared(); - this->server = std::make_shared(_config); + this->server = std::make_shared(_config); } ////////////////////////////////////////////////// @@ -208,7 +208,7 @@ TestFixture &TestFixture::OnPostUpdate(std::function TestFixture::Server() const +std::shared_ptr TestFixture::Server() const { return this->dataPtr->server; } diff --git a/src/gui/GuiFileHandler.cc b/src/gui/GuiFileHandler.cc index 8572af744a..643ef2dd7b 100644 --- a/src/gui/GuiFileHandler.cc +++ b/src/gui/GuiFileHandler.cc @@ -45,7 +45,7 @@ void GuiFileHandler::SaveWorldAs(const QString &_fileUrl, std::string localPath = url.toLocalFile().toStdString() + suffix; std::string service{"/gazebo/worlds"}; - ignition::msgs::StringMsg_V worldsMsg; + msgs::StringMsg_V worldsMsg; bool result{false}; unsigned int timeout{5000}; diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index a023c9205d..6077221e94 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -75,7 +75,7 @@ void GuiRunner::RequestState() std::string reqSrv = this->node.Options().NameSpace() + "/" + id + "/state_async"; this->node.Advertise(reqSrv, &GuiRunner::OnStateAsyncService, this); - ignition::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(reqSrv); // send async state request diff --git a/src/gui/Gui_TEST.cc b/src/gui/Gui_TEST.cc index 24f9cd620a..5c51810e75 100644 --- a/src/gui/Gui_TEST.cc +++ b/src/gui/Gui_TEST.cc @@ -44,10 +44,10 @@ TEST(GuiTest, IGN_UTILS_TEST_DISABLED_ON_MAC(PathManager)) common::Console::SetVerbosity(4); igndbg << "Start test" << std::endl; - ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", + common::setenv("IGN_GAZEBO_RESOURCE_PATH", "/from_env:/tmp/more_env"); - ignition::common::setenv("SDF_PATH", ""); - ignition::common::setenv("IGN_FILE_PATH", ""); + common::setenv("SDF_PATH", ""); + common::setenv("IGN_FILE_PATH", ""); igndbg << "Environment set" << std::endl; transport::Node node; @@ -87,7 +87,7 @@ TEST(GuiTest, IGN_UTILS_TEST_DISABLED_ON_MAC(PathManager)) node.Advertise("/gazebo/resource_paths/get", pathsCb); igndbg << "Paths advertised" << std::endl; - auto app = ignition::gazebo::gui::createGui(gg_argc, gg_argv, nullptr); + auto app = createGui(gg_argc, gg_argv, nullptr); EXPECT_NE(nullptr, app); igndbg << "GUI created" << std::endl; diff --git a/src/gui/TmpIface.cc b/src/gui/TmpIface.cc index 2cf80304de..3711e892bc 100644 --- a/src/gui/TmpIface.cc +++ b/src/gui/TmpIface.cc @@ -40,8 +40,8 @@ bool TmpIface::OnServerControl(const msgs::ServerControl &/*_req*/, ///////////////////////////////////////////////// void TmpIface::OnNewWorld() { - std::function cb = - [](const ignition::msgs::Boolean &_res, const bool _result) + std::function cb = + [](const msgs::Boolean &_res, const bool _result) { igndbg << "NewWorld callback: result: " << _result << std::endl; igndbg << "NewWorld callback: response: " << _res.DebugString() @@ -60,8 +60,8 @@ void TmpIface::OnLoadWorld(const QString &_path) if (localPath.isEmpty()) localPath = _path; - std::function cb = - [](const ignition::msgs::Boolean &_res, const bool _result) + std::function cb = + [](const msgs::Boolean &_res, const bool _result) { igndbg << "LoadWorld callback: result: " << _result << std::endl; igndbg << "LoadWorld callback: response: " << _res.DebugString() @@ -80,8 +80,8 @@ void TmpIface::OnSaveWorldAs(const QString &_path) if (localPath.isEmpty()) localPath = _path; - std::function cb = - [](const ignition::msgs::Boolean &_res, const bool _result) + std::function cb = + [](const msgs::Boolean &_res, const bool _result) { igndbg << "SaveWorldAs callback: result: " << _result << std::endl; igndbg << "SaveWorldAs callback: response: " << _res.DebugString() diff --git a/src/gui/plugins/align_tool/AlignTool.cc b/src/gui/plugins/align_tool/AlignTool.cc index 8a720e44bc..43b649840d 100644 --- a/src/gui/plugins/align_tool/AlignTool.cc +++ b/src/gui/plugins/align_tool/AlignTool.cc @@ -326,14 +326,14 @@ void AlignTool::Align() this->dataPtr->scene = rendering::sceneFromFirstRenderEngine(); // Get current list of selected entities - std::vector selectedList; - ignition::rendering::VisualPtr relativeVisual; + std::vector selectedList; + rendering::VisualPtr relativeVisual; for (const auto &entityId : this->dataPtr->selectedEntities) { for (auto i = 0u; i < this->dataPtr->scene->VisualCount(); ++i) { - ignition::rendering::VisualPtr vis = + rendering::VisualPtr vis = this->dataPtr->scene->VisualByIndex(i); if (!vis) continue; @@ -356,8 +356,8 @@ void AlignTool::Align() (relativeVisual = selectedList.back()); // Callback function for ignition node request - std::function cb = - [](const ignition::msgs::Boolean &/* _rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/* _rep*/, const bool _result) { if (!_result) ignerr << "Error setting pose" << std::endl; @@ -371,7 +371,7 @@ void AlignTool::Align() } int axisIndex = static_cast(this->dataPtr->axis); - ignition::msgs::Pose req; + msgs::Pose req; // Index math to avoid iterating through the selected node for (unsigned int i = this->dataPtr->first; @@ -483,7 +483,7 @@ rendering::NodePtr AlignTool::TopLevelNode(rendering::ScenePtr &_scene, ///////////////////////////////////////////////// bool AlignTool::eventFilter(QObject *_obj, QEvent *_event) { - if (_event->type() == ignition::gazebo::gui::events::Render::kType) + if (_event->type() == gui::events::Render::kType) { // This event is called in Scene3d's RenderThread, so it's safe to make // rendering calls here @@ -498,7 +498,7 @@ bool AlignTool::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::EntitiesSelected::kType) + gui::events::EntitiesSelected::kType) { auto selectedEvent = reinterpret_cast(_event); @@ -520,7 +520,7 @@ bool AlignTool::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::DeselectAllEntities::kType) + gui::events::DeselectAllEntities::kType) { this->dataPtr->selectedEntities.clear(); } @@ -528,5 +528,5 @@ bool AlignTool::eventFilter(QObject *_obj, QEvent *_event) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::AlignTool, +IGNITION_ADD_PLUGIN(AlignTool, ignition::gui::Plugin) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 8350d79f77..3d44e8138a 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -110,7 +110,7 @@ using namespace gazebo; ////////////////////////////////////////////////// template<> -void ignition::gazebo::setData(QStandardItem *_item, +void gazebo::setData(QStandardItem *_item, const math::Vector3d &_data) { if (nullptr == _item) @@ -127,7 +127,7 @@ void ignition::gazebo::setData(QStandardItem *_item, ////////////////////////////////////////////////// template<> -void ignition::gazebo::setData(QStandardItem *_item, const std::string &_data) +void gazebo::setData(QStandardItem *_item, const std::string &_data) { if (nullptr == _item) return; @@ -140,7 +140,7 @@ void ignition::gazebo::setData(QStandardItem *_item, const std::string &_data) ////////////////////////////////////////////////// template<> -void ignition::gazebo::setData(QStandardItem *_item, +void gazebo::setData(QStandardItem *_item, const std::ostringstream &_data) { if (nullptr == _item) @@ -154,7 +154,7 @@ void ignition::gazebo::setData(QStandardItem *_item, ////////////////////////////////////////////////// template<> -void ignition::gazebo::setData(QStandardItem *_item, const bool &_data) +void gazebo::setData(QStandardItem *_item, const bool &_data) { if (nullptr == _item) return; @@ -166,7 +166,7 @@ void ignition::gazebo::setData(QStandardItem *_item, const bool &_data) ////////////////////////////////////////////////// template<> -void ignition::gazebo::setData(QStandardItem *_item, const int &_data) +void gazebo::setData(QStandardItem *_item, const int &_data) { if (nullptr == _item) return; @@ -178,14 +178,14 @@ void ignition::gazebo::setData(QStandardItem *_item, const int &_data) ////////////////////////////////////////////////// template<> -void ignition::gazebo::setData(QStandardItem *_item, const Entity &_data) +void gazebo::setData(QStandardItem *_item, const Entity &_data) { setData(_item, static_cast(_data)); } ////////////////////////////////////////////////// template<> -void ignition::gazebo::setData(QStandardItem *_item, const double &_data) +void gazebo::setData(QStandardItem *_item, const double &_data) { if (nullptr == _item) return; @@ -197,7 +197,7 @@ void ignition::gazebo::setData(QStandardItem *_item, const double &_data) ////////////////////////////////////////////////// template<> -void ignition::gazebo::setData(QStandardItem *_item, const sdf::Physics &_data) +void gazebo::setData(QStandardItem *_item, const sdf::Physics &_data) { if (nullptr == _item) return; @@ -211,7 +211,7 @@ void ignition::gazebo::setData(QStandardItem *_item, const sdf::Physics &_data) } ////////////////////////////////////////////////// -void ignition::gazebo::setUnit(QStandardItem *_item, const std::string &_unit) +void gazebo::setUnit(QStandardItem *_item, const std::string &_unit) { if (nullptr == _item) return; @@ -240,7 +240,7 @@ ComponentsModel::ComponentsModel() : QStandardItemModel() ///////////////////////////////////////////////// QStandardItem *ComponentsModel::AddComponentType( - ignition::gazebo::ComponentTypeId _typeId) + ComponentTypeId _typeId) { IGN_PROFILE_THREAD_NAME("Qt thread"); IGN_PROFILE("ComponentsModel::AddComponentType"); @@ -271,7 +271,7 @@ QStandardItem *ComponentsModel::AddComponentType( ///////////////////////////////////////////////// void ComponentsModel::RemoveComponentType( - ignition::gazebo::ComponentTypeId _typeId) + ComponentTypeId _typeId) { IGN_PROFILE_THREAD_NAME("Qt thread"); IGN_PROFILE("ComponentsModel::RemoveComponentType"); @@ -307,7 +307,7 @@ QHash ComponentsModel::RoleNames() ComponentInspector::ComponentInspector() : GuiSystem(), dataPtr(std::make_unique()) { - qRegisterMetaType(); + qRegisterMetaType(); qRegisterMetaType("Entity"); } @@ -693,7 +693,7 @@ void ComponentInspector::Update(const UpdateInfo &, QMetaObject::invokeMethod(&this->dataPtr->componentsModel, "RemoveComponentType", Qt::QueuedConnection, - Q_ARG(ignition::gazebo::ComponentTypeId, typeId)); + Q_ARG(ComponentTypeId, typeId)); } } } @@ -710,7 +710,7 @@ bool ComponentInspector::eventFilter(QObject *_obj, QEvent *_event) { if (!this->dataPtr->locked) { - if (_event->type() == gazebo::gui::events::EntitiesSelected::kType) + if (_event->type() == gui::events::EntitiesSelected::kType) { auto event = reinterpret_cast(_event); if (event && !event->Data().empty()) @@ -719,7 +719,7 @@ bool ComponentInspector::eventFilter(QObject *_obj, QEvent *_event) } } - if (_event->type() == gazebo::gui::events::DeselectAllEntities::kType) + if (_event->type() == gui::events::DeselectAllEntities::kType) { auto event = reinterpret_cast( _event); @@ -797,14 +797,14 @@ void ComponentInspector::SetPaused(bool _paused) ///////////////////////////////////////////////// void ComponentInspector::OnPhysics(double _stepSize, double _realTimeFactor) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error setting physics parameters" << std::endl; }; - ignition::msgs::Physics req; + msgs::Physics req; req.set_max_step_size(_stepSize); req.set_real_time_factor(_realTimeFactor); auto physicsCmdService = "/world/" + this->dataPtr->worldName @@ -837,5 +837,5 @@ transport::Node &ComponentInspector::TransportNode() } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::ComponentInspector, +IGNITION_ADD_PLUGIN(ComponentInspector, ignition::gui::Plugin) diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 79de5dfb7f..7eac482f15 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -400,7 +400,7 @@ void EntityTree::DeselectAllEntities() ///////////////////////////////////////////////// bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) { - if (_event->type() == ignition::gazebo::gui::events::EntitiesSelected::kType) + if (_event->type() == gui::events::EntitiesSelected::kType) { auto selectedEvent = reinterpret_cast(_event); @@ -418,7 +418,7 @@ bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::DeselectAllEntities::kType) + gui::events::DeselectAllEntities::kType) { auto deselectAllEvent = reinterpret_cast(_event); @@ -434,5 +434,5 @@ bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::EntityTree, +IGNITION_ADD_PLUGIN(EntityTree, ignition::gui::Plugin) diff --git a/src/gui/plugins/grid_config/GridConfig.cc b/src/gui/plugins/grid_config/GridConfig.cc index 740ee7b299..28adc956d6 100644 --- a/src/gui/plugins/grid_config/GridConfig.cc +++ b/src/gui/plugins/grid_config/GridConfig.cc @@ -87,7 +87,7 @@ void GridConfig::LoadConfig(const tinyxml2::XMLElement *) ///////////////////////////////////////////////// bool GridConfig::eventFilter(QObject *_obj, QEvent *_event) { - if (_event->type() == ignition::gazebo::gui::events::Render::kType) + if (_event->type() == gui::events::Render::kType) { // This event is called in Scene3d's RenderThread, so it's safe to make // rendering calls here @@ -255,5 +255,5 @@ void GridConfig::OnShow(bool _checked) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::GridConfig, +IGNITION_ADD_PLUGIN(GridConfig, ignition::gui::Plugin) diff --git a/src/gui/plugins/joint_position_controller/JointPositionController.cc b/src/gui/plugins/joint_position_controller/JointPositionController.cc index 132d7b3488..20723b2755 100644 --- a/src/gui/plugins/joint_position_controller/JointPositionController.cc +++ b/src/gui/plugins/joint_position_controller/JointPositionController.cc @@ -375,7 +375,7 @@ void JointPositionController::OnCommand(const QString &_jointName, double _pos) { std::string jointName = _jointName.toStdString(); - ignition::msgs::Double msg; + msgs::Double msg; msg.set_data(_pos); auto topic = transport::TopicUtils::AsValidTopic("/model/" + this->dataPtr->modelName.toStdString() + "/joint/" + jointName + @@ -388,7 +388,7 @@ void JointPositionController::OnCommand(const QString &_jointName, double _pos) return; } - auto pub = this->dataPtr->node.Advertise(topic); + auto pub = this->dataPtr->node.Advertise(topic); pub.Publish(msg); } @@ -405,7 +405,7 @@ void JointPositionController::OnReset() continue; } - ignition::msgs::Double msg; + msgs::Double msg; msg.set_data(0); auto topic = transport::TopicUtils::AsValidTopic("/model/" + this->dataPtr->modelName.toStdString() + "/joint/" + jointName + @@ -418,11 +418,11 @@ void JointPositionController::OnReset() return; } - auto pub = this->dataPtr->node.Advertise(topic); + auto pub = this->dataPtr->node.Advertise(topic); pub.Publish(msg); } } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::gui::JointPositionController, +IGNITION_ADD_PLUGIN(JointPositionController, ignition::gui::Plugin) diff --git a/src/gui/plugins/modules/EntityContextMenu.cc b/src/gui/plugins/modules/EntityContextMenu.cc index e76cbd8cf1..28f177504c 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -62,7 +62,7 @@ using namespace gazebo; void IgnGazeboPlugin::registerTypes(const char *_uri) { // Register our 'EntityContextMenuItem' in qml engine - qmlRegisterType(_uri, 1, 0, + qmlRegisterType(_uri, 1, 0, "EntityContextMenuItem"); } @@ -115,14 +115,14 @@ void EntityContextMenu::OnRemove( "/world/" + this->dataPtr->worldName + "/remove"; } - std::function cb = - [](const ignition::msgs::Boolean &_rep, const bool _result) + std::function cb = + [](const msgs::Boolean &_rep, const bool _result) { if (!_result || !_rep.data()) ignerr << "Error sending remove request" << std::endl; }; - ignition::msgs::Entity req; + msgs::Entity req; req.set_name(_data.toStdString()); req.set_type(convert(_type.toStdString())); @@ -132,8 +132,8 @@ void EntityContextMenu::OnRemove( ///////////////////////////////////////////////// void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error sending move to request" << std::endl; @@ -142,19 +142,19 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) std::string request = _request.toStdString(); if (request == "move_to") { - ignition::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->moveToService, req, cb); } else if (request == "follow") { - ignition::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->followService, req, cb); } else if (request == "view_collisions") { - ignition::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->viewCollisionsService, req, cb); } diff --git a/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc b/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc index 9f4606091b..cd62531d73 100644 --- a/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc +++ b/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc @@ -252,5 +252,5 @@ void PlaybackScrubber::OnDrop(double _value) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::PlaybackScrubber, +IGNITION_ADD_PLUGIN(PlaybackScrubber, ignition::gui::Plugin) diff --git a/src/gui/plugins/plot_3d/Plot3D.cc b/src/gui/plugins/plot_3d/Plot3D.cc index 45565bbc75..b2e2657a3c 100644 --- a/src/gui/plugins/plot_3d/Plot3D.cc +++ b/src/gui/plugins/plot_3d/Plot3D.cc @@ -165,7 +165,7 @@ void Plot3D::ClearPlot() // Clear previous plot if (this->dataPtr->markerMsg.point().size() > 0) { - this->dataPtr->markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER); + this->dataPtr->markerMsg.set_action(msgs::Marker::DELETE_MARKER); this->dataPtr->node.Request("/marker", this->dataPtr->markerMsg); } } @@ -245,7 +245,7 @@ void Plot3D::Update(const UpdateInfo &, EntityComponentManager &_ecm) return; this->dataPtr->prevPos = point; - ignition::msgs::Set(this->dataPtr->markerMsg.add_point(), point); + msgs::Set(this->dataPtr->markerMsg.add_point(), point); // Reduce message array if (this->dataPtr->markerMsg.point_size() > this->dataPtr->maxPoints) @@ -401,5 +401,5 @@ void Plot3D::SetMaxPoints(int _maxPoints) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::gui::Plot3D, +IGNITION_ADD_PLUGIN(Plot3D, ignition::gui::Plugin) diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index 8d7a2b1b61..47767e49ac 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -216,7 +216,7 @@ ResourceSpawner::ResourceSpawner() ignition::gui::App()->Engine()->rootContext()->setContextProperty( "OwnerList", &this->dataPtr->ownerModel); this->dataPtr->fuelClient = - std::make_unique(); + std::make_unique(); } ///////////////////////////////////////////////// @@ -481,7 +481,7 @@ void ResourceSpawner::OnDownloadFuelResource(const QString &_path, // Set the waiting cursor while the resource downloads QGuiApplication::setOverrideCursor(Qt::WaitCursor); if (this->dataPtr->fuelClient->DownloadModel( - ignition::common::URI(_path.toStdString()), localPath)) + common::URI(_path.toStdString()), localPath)) { // Successful download, set thumbnail std::string thumbnailPath = common::joinPaths(localPath, "thumbnails"); @@ -563,7 +563,7 @@ void ResourceSpawner::LoadConfig(const tinyxml2::XMLElement *) std::set ownerSet; for (auto const &server : servers) { - std::vector models; + std::vector models; for (auto iter = this->dataPtr->fuelClient->Models(server); iter; ++iter) { models.push_back(iter->Identification()); @@ -583,10 +583,10 @@ void ResourceSpawner::LoadConfig(const tinyxml2::XMLElement *) // If the resource is cached, we can go ahead and populate the // respective information if (this->dataPtr->fuelClient->CachedModel( - ignition::common::URI(id.UniqueName()), path)) + common::URI(id.UniqueName()), path)) { resource.isDownloaded = true; - resource.sdfPath = ignition::common::joinPaths(path, "model.sdf"); + resource.sdfPath = common::joinPaths(path, "model.sdf"); std::string thumbnailPath = common::joinPaths(path, "thumbnails"); this->SetThumbnail(thumbnailPath, resource); } @@ -632,5 +632,5 @@ void ResourceSpawner::OnResourceSpawn(const QString &_sdfPath) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::ResourceSpawner, +IGNITION_ADD_PLUGIN(ResourceSpawner, ignition::gui::Plugin) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 429294f7d4..c0a265af29 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -533,7 +533,7 @@ void IgnRenderer::Render() std::chrono::steady_clock::duration dt; dt = t - this->dataPtr->recordStartTime; int64_t sec, nsec; - std::tie(sec, nsec) = ignition::math::durationToSecNsec(dt); + std::tie(sec, nsec) = math::durationToSecNsec(dt); msgs::Time msg; msg.set_sec(sec); msg.set_nsec(nsec); @@ -778,13 +778,13 @@ void IgnRenderer::Render() if (ignition::gui::App()) { - ignition::gui::events::Render event; + gui::events::Render event; ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &event); // This will be deprecated on v5 and removed on v6 - ignition::gazebo::gui::events::Render oldEvent; + gui::events::Render oldEvent; ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &oldEvent); @@ -811,7 +811,7 @@ bool IgnRenderer::GeneratePreview(const sdf::Root &_sdf) // Only preview first model sdf::Model model = *(_sdf.ModelByIndex(0)); this->dataPtr->spawnPreviewPose = model.RawPose(); - model.SetName(ignition::common::Uuid().String()); + model.SetName(common::Uuid().String()); Entity modelId = this->UniqueId(); if (!modelId) { @@ -827,7 +827,7 @@ bool IgnRenderer::GeneratePreview(const sdf::Root &_sdf) for (auto j = 0u; j < model.LinkCount(); j++) { sdf::Link link = *(model.LinkByIndex(j)); - link.SetName(ignition::common::Uuid().String()); + link.SetName(common::Uuid().String()); Entity linkId = this->UniqueId(); if (!linkId) { @@ -840,7 +840,7 @@ bool IgnRenderer::GeneratePreview(const sdf::Root &_sdf) for (auto k = 0u; k < link.VisualCount(); k++) { sdf::Visual visual = *(link.VisualByIndex(k)); - visual.SetName(ignition::common::Uuid().String()); + visual.SetName(common::Uuid().String()); Entity visualId = this->UniqueId(); if (!visualId) { @@ -1120,8 +1120,8 @@ void IgnRenderer::HandleModelPlacement() this->TerminateSpawnPreview(); math::Pose3d modelPose = this->dataPtr->spawnPreviewPose; - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error creating model" << std::endl; @@ -1226,7 +1226,7 @@ double IgnRenderer::SnapValue( ///////////////////////////////////////////////// void IgnRenderer::SnapPoint( - ignition::math::Vector3d &_point, math::Vector3d &_snapVals, + math::Vector3d &_point, math::Vector3d &_snapVals, double _sensitivity) const { if (_snapVals.X() <= 0 || _snapVals.Y() <= 0 || _snapVals.Z() <= 0) @@ -1333,7 +1333,7 @@ void IgnRenderer::HandleMouseTransformControl() // check if the visual is an axis in the gizmo visual math::Vector3d axis = this->dataPtr->transformControl.AxisById(visual->Id()); - if (axis != ignition::math::Vector3d::Zero) + if (axis != math::Vector3d::Zero) { // start the transform process this->dataPtr->transformControl.SetActiveAxis(axis); @@ -1351,14 +1351,14 @@ void IgnRenderer::HandleMouseTransformControl() { if (this->dataPtr->transformControl.Node()) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error setting pose" << std::endl; }; rendering::NodePtr node = this->dataPtr->transformControl.Node(); - ignition::msgs::Pose req; + msgs::Pose req; req.set_name(node->Name()); msgs::Set(req.mutable_position(), node->WorldPosition()); msgs::Set(req.mutable_orientation(), node->WorldRotation()); @@ -1393,7 +1393,7 @@ void IgnRenderer::HandleMouseTransformControl() // check if the visual is an axis in the gizmo visual math::Vector3d axis = this->dataPtr->transformControl.AxisById(visual->Id()); - if (axis == ignition::math::Vector3d::Zero) + if (axis == math::Vector3d::Zero) { auto topVis = this->dataPtr->renderUtil.SceneManager().TopLevelVisual(visual); @@ -1461,7 +1461,7 @@ void IgnRenderer::HandleMouseTransformControl() this->dataPtr->startWorldPos = target->WorldPosition(); } - ignition::math::Vector3d worldPos = + math::Vector3d worldPos = target->WorldPosition(); math::Vector3d distance = this->dataPtr->transformControl.TranslationFrom2d(axis, start, end); @@ -1599,7 +1599,7 @@ void IgnRenderer::HandleMouseViewControl() // unset the target on release (by setting to inf) else if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE) { - this->dataPtr->target = ignition::math::INF_D; + this->dataPtr->target = math::INF_D; } // Pan with left button @@ -2021,7 +2021,7 @@ math::Vector3d IgnRenderer::ScreenToPlane( this->dataPtr->rayQuery->SetFromCamera( this->dataPtr->camera, math::Vector2d(nx, ny)); - ignition::math::Planed plane(ignition::math::Vector3d(0, 0, 1), 0); + math::Planed plane(math::Vector3d(0, 0, 1), 0); math::Vector3d origin = this->dataPtr->rayQuery->Origin(); math::Vector3d direction = this->dataPtr->rayQuery->Direction(); @@ -2807,8 +2807,8 @@ void Scene3D::OnDropped(const QString &_drop, int _mouseX, int _mouseY) return; } - std::function cb = - [](const ignition::msgs::Boolean &_res, const bool _result) + std::function cb = + [](const msgs::Boolean &_res, const bool _result) { if (!_result || !_res.data()) ignerr << "Error creating dropped entity." << std::endl; @@ -2950,7 +2950,7 @@ bool Scene3D::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::EntitiesSelected::kType) + gui::events::EntitiesSelected::kType) { auto selectedEvent = reinterpret_cast(_event); @@ -2983,7 +2983,7 @@ bool Scene3D::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::DeselectAllEntities::kType) + gui::events::DeselectAllEntities::kType) { auto deselectEvent = reinterpret_cast(_event); @@ -2996,7 +2996,7 @@ bool Scene3D::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::SnapIntervals::kType) + gui::events::SnapIntervals::kType) { auto snapEvent = reinterpret_cast(_event); if (snapEvent) @@ -3008,7 +3008,7 @@ bool Scene3D::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::SpawnPreviewModel::kType) + gui::events::SpawnPreviewModel::kType) { auto spawnPreviewEvent = reinterpret_cast(_event); @@ -3019,7 +3019,7 @@ bool Scene3D::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::SpawnPreviewPath::kType) + gui::events::SpawnPreviewPath::kType) { auto spawnPreviewPathEvent = reinterpret_cast(_event); @@ -3194,7 +3194,7 @@ void RenderWindowItem::SetRecordVideoBitrate(unsigned int _bitrate) } ///////////////////////////////////////////////// -void RenderWindowItem::OnHovered(const ignition::math::Vector2i &_hoverPos) +void RenderWindowItem::OnHovered(const math::Vector2i &_hoverPos) { this->dataPtr->renderThread->ignRenderer.NewHoverEvent(_hoverPos); } @@ -3313,5 +3313,5 @@ void RenderWindowItem::HandleKeyRelease(QKeyEvent *_e) // // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::Scene3D, +IGNITION_ADD_PLUGIN(gazebo::Scene3D, ignition::gui::Plugin) diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index be68df3105..dbb134ac13 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -198,5 +198,5 @@ void Shapes::OnMode(const QString &_mode) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::Shapes, +IGNITION_ADD_PLUGIN(Shapes, ignition::gui::Plugin) diff --git a/src/gui/plugins/tape_measure/TapeMeasure.cc b/src/gui/plugins/tape_measure/TapeMeasure.cc index 882ce7e9c3..d08189fdf7 100644 --- a/src/gui/plugins/tape_measure/TapeMeasure.cc +++ b/src/gui/plugins/tape_measure/TapeMeasure.cc @@ -150,8 +150,8 @@ void TapeMeasure::Reset() this->DeleteMarker(this->dataPtr->kLineId); this->dataPtr->currentId = this->dataPtr->kStartPointId; - this->dataPtr->startPoint = ignition::math::Vector3d::Zero; - this->dataPtr->endPoint = ignition::math::Vector3d::Zero; + this->dataPtr->startPoint = math::Vector3d::Zero; + this->dataPtr->endPoint = math::Vector3d::Zero; this->dataPtr->distance = 0.0; this->dataPtr->measure = false; this->newDistance(); @@ -179,51 +179,51 @@ void TapeMeasure::DeleteMarker(int _id) return; // Delete the previously created marker - ignition::msgs::Marker markerMsg; + msgs::Marker markerMsg; markerMsg.set_ns(this->dataPtr->ns); markerMsg.set_id(_id); - markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER); + markerMsg.set_action(msgs::Marker::DELETE_MARKER); this->dataPtr->node.Request("/marker", markerMsg); this->dataPtr->placedMarkers.erase(_id); } ///////////////////////////////////////////////// void TapeMeasure::DrawPoint(int _id, - ignition::math::Vector3d &_point, ignition::math::Color &_color) + math::Vector3d &_point, math::Color &_color) { this->DeleteMarker(_id); - ignition::msgs::Marker markerMsg; + msgs::Marker markerMsg; markerMsg.set_ns(this->dataPtr->ns); markerMsg.set_id(_id); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::SPHERE); - ignition::msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color); - ignition::msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(0.1, 0.1, 0.1)); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(_point.X(), _point.Y(), _point.Z(), 0, 0, 0)); + markerMsg.set_action(msgs::Marker::ADD_MODIFY); + markerMsg.set_type(msgs::Marker::SPHERE); + msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color); + msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color); + msgs::Set(markerMsg.mutable_scale(), + math::Vector3d(0.1, 0.1, 0.1)); + msgs::Set(markerMsg.mutable_pose(), + math::Pose3d(_point.X(), _point.Y(), _point.Z(), 0, 0, 0)); this->dataPtr->node.Request("/marker", markerMsg); this->dataPtr->placedMarkers.insert(_id); } ///////////////////////////////////////////////// -void TapeMeasure::DrawLine(int _id, ignition::math::Vector3d &_startPoint, - ignition::math::Vector3d &_endPoint, ignition::math::Color &_color) +void TapeMeasure::DrawLine(int _id, math::Vector3d &_startPoint, + math::Vector3d &_endPoint, math::Color &_color) { this->DeleteMarker(_id); - ignition::msgs::Marker markerMsg; + msgs::Marker markerMsg; markerMsg.set_ns(this->dataPtr->ns); markerMsg.set_id(_id); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::LINE_LIST); - ignition::msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color); - ignition::msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color); - ignition::msgs::Set(markerMsg.add_point(), _startPoint); - ignition::msgs::Set(markerMsg.add_point(), _endPoint); + markerMsg.set_action(msgs::Marker::ADD_MODIFY); + markerMsg.set_type(msgs::Marker::LINE_LIST); + msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color); + msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color); + msgs::Set(markerMsg.add_point(), _startPoint); + msgs::Set(markerMsg.add_point(), _endPoint); this->dataPtr->node.Request("/marker", markerMsg); this->dataPtr->placedMarkers.insert(_id); @@ -241,7 +241,7 @@ bool TapeMeasure::eventFilter(QObject *_obj, QEvent *_event) // rendering calls here if (this->dataPtr->measure && hoverToSceneEvent) { - ignition::math::Vector3d point = hoverToSceneEvent->Point(); + math::Vector3d point = hoverToSceneEvent->Point(); this->DrawPoint(this->dataPtr->currentId, point, this->dataPtr->hoverColor); @@ -265,7 +265,7 @@ bool TapeMeasure::eventFilter(QObject *_obj, QEvent *_event) // rendering calls here if (this->dataPtr->measure && leftClickToSceneEvent) { - ignition::math::Vector3d point = leftClickToSceneEvent->Point(); + math::Vector3d point = leftClickToSceneEvent->Point(); this->DrawPoint(this->dataPtr->currentId, point, this->dataPtr->drawColor); // If the user is placing the start point, update its position @@ -329,5 +329,5 @@ bool TapeMeasure::eventFilter(QObject *_obj, QEvent *_event) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::TapeMeasure, +IGNITION_ADD_PLUGIN(TapeMeasure, ignition::gui::Plugin) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 8c5835469d..e30d2e956e 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -120,14 +120,14 @@ void TransformControl::OnSnapUpdate( ///////////////////////////////////////////////// void TransformControl::OnMode(const QString &_mode) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error setting transform mode" << std::endl; }; - ignition::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_mode.toStdString()); this->dataPtr->node.Request(this->dataPtr->service, req, cb); } @@ -186,7 +186,7 @@ void TransformControl::LoadGrid() ///////////////////////////////////////////////// bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) { - if (_event->type() == ignition::gazebo::gui::events::Render::kType) + if (_event->type() == gui::events::Render::kType) { // This event is called in Scene3d's RenderThread, so it's safe to make // rendering calls here @@ -275,5 +275,5 @@ double TransformControl::scaleZSnap() } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::TransformControl, +IGNITION_ADD_PLUGIN(TransformControl, ignition::gui::Plugin) diff --git a/src/gui/plugins/video_recorder/VideoRecorder.cc b/src/gui/plugins/video_recorder/VideoRecorder.cc index 84406d718f..5249fa2c28 100644 --- a/src/gui/plugins/video_recorder/VideoRecorder.cc +++ b/src/gui/plugins/video_recorder/VideoRecorder.cc @@ -70,8 +70,8 @@ void VideoRecorder::LoadConfig(const tinyxml2::XMLElement *) ///////////////////////////////////////////////// void VideoRecorder::OnStart(const QString &_format) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error sending video record start request" << std::endl; @@ -80,7 +80,7 @@ void VideoRecorder::OnStart(const QString &_format) std::string format = _format.toStdString(); this->dataPtr->filename = "ign_recording." + format; - ignition::msgs::VideoRecord req; + msgs::VideoRecord req; req.set_start(true); req.set_format(format); req.set_save_filename(this->dataPtr->filename); @@ -90,14 +90,14 @@ void VideoRecorder::OnStart(const QString &_format) ///////////////////////////////////////////////// void VideoRecorder::OnStop() { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error sending video record stop request" << std::endl; }; - ignition::msgs::VideoRecord req; + msgs::VideoRecord req; req.set_stop(true); this->dataPtr->node.Request(this->dataPtr->service, req, cb); } @@ -143,5 +143,5 @@ void VideoRecorder::OnCancel() } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::VideoRecorder, +IGNITION_ADD_PLUGIN(VideoRecorder, ignition::gui::Plugin) diff --git a/src/gui/plugins/view_angle/ViewAngle.cc b/src/gui/plugins/view_angle/ViewAngle.cc index 9de7fddf4f..84bb9c7b9b 100644 --- a/src/gui/plugins/view_angle/ViewAngle.cc +++ b/src/gui/plugins/view_angle/ViewAngle.cc @@ -82,14 +82,14 @@ void ViewAngle::LoadConfig(const tinyxml2::XMLElement *) ///////////////////////////////////////////////// void ViewAngle::OnAngleMode(int _x, int _y, int _z) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error setting view angle mode" << std::endl; }; - ignition::msgs::Vector3d req; + msgs::Vector3d req; req.set_x(_x); req.set_y(_y); req.set_z(_z); @@ -116,14 +116,14 @@ void ViewAngle::SetCamPose(double _x, double _y, double _z, { this->dataPtr->camPose.Set(_x, _y, _z, _roll, _pitch, _yaw); - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error sending move camera to pose request" << std::endl; }; - ignition::msgs::GUICamera req; + msgs::GUICamera req; msgs::Set(req.mutable_pose(), this->dataPtr->camPose); this->dataPtr->node.Request(this->dataPtr->moveToPoseService, req, cb); @@ -143,5 +143,5 @@ void ViewAngle::CamPoseCb(const msgs::Pose &_msg) } // Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::ViewAngle, +IGNITION_ADD_PLUGIN(ViewAngle, ignition::gui::Plugin) diff --git a/src/ign.cc b/src/ign.cc index 298e4ba045..7d581a4c0a 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -56,7 +56,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE void cmdVerbosity( const char *_verbosity) { int verbosity = std::atoi(_verbosity); - ignition::common::Console::SetVerbosity(verbosity); + common::Console::SetVerbosity(verbosity); // SDFormat only has 2 levels: quiet / loud. Let sim users suppress all SDF // console output with zero verbosity. @@ -78,17 +78,17 @@ extern "C" IGNITION_GAZEBO_VISIBLE const char *findFuelResource( { std::string path; std::string worldPath; - ignition::fuel_tools::FuelClient fuelClient; + fuel_tools::FuelClient fuelClient; // Attempt to find cached copy, and then attempt download - if (fuelClient.CachedWorld(ignition::common::URI(_pathToResource), path)) + if (fuelClient.CachedWorld(common::URI(_pathToResource), path)) { ignmsg << "Cached world found." << std::endl; worldPath = path; } // cppcheck-suppress syntaxError - else if (ignition::fuel_tools::Result result = - fuelClient.DownloadWorld(ignition::common::URI(_pathToResource), path); + else if (fuel_tools::Result result = + fuelClient.DownloadWorld(common::URI(_pathToResource), path); result) { ignmsg << "Successfully downloaded world from fuel." << std::endl; @@ -101,20 +101,20 @@ extern "C" IGNITION_GAZEBO_VISIBLE const char *findFuelResource( return ""; } - if (!ignition::common::exists(worldPath)) + if (!common::exists(worldPath)) return ""; // Find the first sdf file in the world path for now, the later intention is // to load an optional world config file first and if that does not exist, // continue to load the first sdf file found as done below - for (ignition::common::DirIter file(worldPath); - file != ignition::common::DirIter(); ++file) + for (common::DirIter file(worldPath); + file != common::DirIter(); ++file) { std::string current(*file); - if (ignition::common::isFile(current)) + if (common::isFile(current)) { - std::string fileName = ignition::common::basename(current); + std::string fileName = common::basename(current); std::string::size_type fileExtensionIndex = fileName.rfind("."); std::string fileExtension = fileName.substr(fileExtensionIndex + 1); @@ -137,7 +137,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, const char *_file, const char *_recordTopics, int _waitGui) { std::string startingWorldPath{""}; - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; // Lock until the starting world is received from Gui if (_waitGui == 1) @@ -172,7 +172,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, // Path for compressed log, used to check for duplicates std::string cmpPath = std::string(recordPathMod); - if (!std::string(1, cmpPath.back()).compare(ignition::common::separator(""))) + if (!std::string(1, cmpPath.back()).compare(common::separator(""))) { // Remove the separator at end of path cmpPath = cmpPath.substr(0, cmpPath.length() - 1); @@ -200,7 +200,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, // Update compressed file path to name of recording directory path cmpPath = std::string(recordPathMod); - if (!std::string(1, cmpPath.back()).compare(ignition::common::separator( + if (!std::string(1, cmpPath.back()).compare(common::separator( ""))) { // Remove the separator at end of path @@ -209,8 +209,8 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, cmpPath += ".zip"; // Check if path or compressed file with same prefix exists - if (ignition::common::exists(recordPathMod) || - ignition::common::exists(cmpPath)) + if (common::exists(recordPathMod) || + common::exists(cmpPath)) { // Overwrite if flag specified if (_logOverwrite > 0) @@ -218,15 +218,15 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, bool recordMsg = false; bool cmpMsg = false; // Remove files before initializing console log files on top of them - if (ignition::common::exists(recordPathMod)) + if (common::exists(recordPathMod)) { recordMsg = true; - ignition::common::removeAll(recordPathMod); + common::removeAll(recordPathMod); } - if (ignition::common::exists(cmpPath)) + if (common::exists(cmpPath)) { cmpMsg = true; - ignition::common::removeFile(cmpPath); + common::removeFile(cmpPath); } // Create log file before printing any messages so they can be logged @@ -253,7 +253,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, { // Remove the separator at end of path if (!std::string(1, recordPathMod.back()).compare( - ignition::common::separator(""))) + common::separator(""))) { recordPathMod = recordPathMod.substr(0, recordPathMod.length() - 1); @@ -264,8 +264,8 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, // Keep renaming until path does not exist for both directory and // compressed file - while (ignition::common::exists(recordPathMod) || - ignition::common::exists(cmpPath)) + while (common::exists(recordPathMod) || + common::exists(cmpPath)) { recordPathMod = recordOrigPrefix + "(" + std::to_string(count++) + ")"; @@ -273,7 +273,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, cmpPath = std::string(recordPathMod); // Remove the separator at end of path if (!std::string(1, cmpPath.back()).compare( - ignition::common::separator(""))) + common::separator(""))) { cmpPath = cmpPath.substr(0, cmpPath.length() - 1); } @@ -314,7 +314,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, serverConfig.SetLogRecordPath(recordPathMod); } - std::vector topics = ignition::common::split( + std::vector topics = common::split( _recordTopics, ":"); for (const std::string &topic : topics) { @@ -381,7 +381,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, else { ignmsg << "Playing back states" << _playback << std::endl; - serverConfig.SetLogPlaybackPath(ignition::common::absPath( + serverConfig.SetLogPlaybackPath(common::absPath( std::string(_playback))); } } @@ -402,7 +402,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, } // Create the Gazebo server - ignition::gazebo::Server server(serverConfig); + gazebo::Server server(serverConfig); // Run the server server.Run(true, _iterations, _run == 0); @@ -426,6 +426,6 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runGui( // be converted to a const char *. The const cast is here to prevent a warning // since we do need to pass a char* to runGui char *argv = const_cast("ign-gazebo-gui"); - return ignition::gazebo::gui::runGui( + return gazebo::gui::runGui( argc, &argv, _guiConfig, _file, _waitGui); } diff --git a/src/network/PeerInfo.cc b/src/network/PeerInfo.cc index e749caff66..b345fde017 100644 --- a/src/network/PeerInfo.cc +++ b/src/network/PeerInfo.cc @@ -24,8 +24,8 @@ using namespace gazebo; ///////////////////////////////////////////////// PeerInfo::PeerInfo(const NetworkRole &_role): - id(ignition::common::Uuid().String()), - hostname(ignition::transport::hostname()), + id(common::Uuid().String()), + hostname(transport::hostname()), role(_role) { } @@ -42,36 +42,36 @@ std::string PeerInfo::Namespace() const } ///////////////////////////////////////////////// -ignition::gazebo::private_msgs::PeerInfo ignition::gazebo::toProto( +private_msgs::PeerInfo gazebo::toProto( const PeerInfo &_info) { - ignition::gazebo::private_msgs::PeerInfo proto; + private_msgs::PeerInfo proto; proto.set_id(_info.id); proto.set_hostname(_info.hostname); switch (_info.role) { case NetworkRole::ReadOnly: - proto.set_role(ignition::gazebo::private_msgs::PeerInfo::READ_ONLY); + proto.set_role(private_msgs::PeerInfo::READ_ONLY); break; case NetworkRole::SimulationPrimary: proto.set_role( - ignition::gazebo::private_msgs::PeerInfo::SIMULATION_PRIMARY); + private_msgs::PeerInfo::SIMULATION_PRIMARY); break; case NetworkRole::SimulationSecondary: proto.set_role( - ignition::gazebo::private_msgs::PeerInfo::SIMULATION_SECONDARY); + private_msgs::PeerInfo::SIMULATION_SECONDARY); break; case NetworkRole::None: default: - proto.set_role(ignition::gazebo::private_msgs::PeerInfo::NONE); + proto.set_role(private_msgs::PeerInfo::NONE); } return proto; } ///////////////////////////////////////////////// -PeerInfo ignition::gazebo::fromProto( - const ignition::gazebo::private_msgs::PeerInfo& _proto) +PeerInfo gazebo::fromProto( + const private_msgs::PeerInfo& _proto) { PeerInfo info; info.id = _proto.id(); @@ -79,16 +79,16 @@ PeerInfo ignition::gazebo::fromProto( switch (_proto.role()) { - case ignition::gazebo::private_msgs::PeerInfo::READ_ONLY: + case private_msgs::PeerInfo::READ_ONLY: info.role = NetworkRole::ReadOnly; break; - case ignition::gazebo::private_msgs::PeerInfo::SIMULATION_PRIMARY: + case private_msgs::PeerInfo::SIMULATION_PRIMARY: info.role = NetworkRole::SimulationPrimary; break; - case ignition::gazebo::private_msgs::PeerInfo::SIMULATION_SECONDARY: + case private_msgs::PeerInfo::SIMULATION_SECONDARY: info.role = NetworkRole::SimulationSecondary; break; - case ignition::gazebo::private_msgs::PeerInfo::NONE: + case private_msgs::PeerInfo::NONE: default: info.role = NetworkRole::None; break; diff --git a/src/network/PeerTracker.cc b/src/network/PeerTracker.cc index f4271fac98..e10a785e84 100644 --- a/src/network/PeerTracker.cc +++ b/src/network/PeerTracker.cc @@ -26,7 +26,7 @@ using namespace gazebo; PeerTracker::PeerTracker( PeerInfo _info, EventManager *_eventMgr, - const ignition::transport::NodeOptions &_options): + const transport::NodeOptions &_options): info(std::move(_info)), eventMgr(_eventMgr), node(_options) diff --git a/src/rendering/MarkerManager.cc b/src/rendering/MarkerManager.cc index 1bf4404112..bdd8f082a9 100644 --- a/src/rendering/MarkerManager.cc +++ b/src/rendering/MarkerManager.cc @@ -42,50 +42,50 @@ class ignition::gazebo::MarkerManagerPrivate /// \brief Processes a marker message. /// \param[in] _msg The message data. /// \return True if the marker was processed successfully. - public: bool ProcessMarkerMsg(const ignition::msgs::Marker &_msg); + public: bool ProcessMarkerMsg(const msgs::Marker &_msg); /// \brief Converts an ignition msg render type to ignition rendering /// \param[in] _msg The message data /// \return Converted rendering type, if any. - public: ignition::rendering::MarkerType MsgToType( - const ignition::msgs::Marker &_msg); + public: rendering::MarkerType MsgToType( + const msgs::Marker &_msg); /// \brief Converts an ignition msg material to ignition rendering // material. // \param[in] _msg The message data. // \return Converted rendering material, if any. public: rendering::MaterialPtr MsgToMaterial( - const ignition::msgs::Marker &_msg); + const msgs::Marker &_msg); /// \brief Updates the markers. public: void Update(); /// \brief Callback that receives marker messages. /// \param[in] _req The marker message. - public: void OnMarkerMsg(const ignition::msgs::Marker &_req); + public: void OnMarkerMsg(const msgs::Marker &_req); /// \brief Callback that receives multiple marker messages. /// \param[in] _req The vector of marker messages /// \param[in] _res Response data /// \return True if the request is received - public: bool OnMarkerMsgArray(const ignition::msgs::Marker_V &_req, - ignition::msgs::Boolean &_res); + public: bool OnMarkerMsgArray(const msgs::Marker_V &_req, + msgs::Boolean &_res); /// \brief Services callback that returns a list of markers. /// \param[out] _rep Service reply /// \return True on success. - public: bool OnList(ignition::msgs::Marker_V &_rep); + public: bool OnList(msgs::Marker_V &_rep); /// \brief Sets Marker from marker message. /// \param[in] _msg The message data. /// \param[out] _markerPtr The message pointer to set. - public: void SetMarker(const ignition::msgs::Marker &_msg, + public: void SetMarker(const msgs::Marker &_msg, const rendering::MarkerPtr &_markerPtr); /// \brief Sets Visual from marker message. /// \param[in] _msg The message data. /// \param[out] _visualPtr The visual pointer to set. - public: void SetVisual(const ignition::msgs::Marker &_msg, + public: void SetVisual(const msgs::Marker &_msg, const rendering::VisualPtr &_visualPtr); /// \brief Sets sim time from time. @@ -100,22 +100,22 @@ class ignition::gazebo::MarkerManagerPrivate /// \brief Map of visuals public: std::map> visuals; + std::map> visuals; /// \brief List of marker message to process. - public: std::list markerMsgs; + public: std::list markerMsgs; /// \brief Pointer to the scene public: rendering::ScenePtr scene; /// \brief Ignition node - public: ignition::transport::Node node; + public: transport::Node node; /// \brief Sim time according to UpdateInfo in RenderUtil public: std::chrono::steady_clock::duration simTime; /// \brief The last marker message received - public: ignition::msgs::Marker msg; + public: msgs::Marker msg; /// \brief Topic name for the marker service public: std::string topicName = "/marker"; @@ -149,7 +149,7 @@ void MarkerManager::Update() } ///////////////////////////////////////////////// -bool MarkerManager::Init(const ignition::rendering::ScenePtr &_scene) +bool MarkerManager::Init(const rendering::ScenePtr &_scene) { if (!_scene) { @@ -229,8 +229,8 @@ void MarkerManagerPrivate::Update() if (it->second->GeometryCount() == 0u) continue; - ignition::rendering::MarkerPtr markerPtr = - std::dynamic_pointer_cast + rendering::MarkerPtr markerPtr = + std::dynamic_pointer_cast (it->second->GeometryByIndex(0u)); if (markerPtr != nullptr) { @@ -263,7 +263,7 @@ void MarkerManagerPrivate::SetSimTime( } ///////////////////////////////////////////////// -void MarkerManagerPrivate::SetVisual(const ignition::msgs::Marker &_msg, +void MarkerManagerPrivate::SetVisual(const msgs::Marker &_msg, const rendering::VisualPtr &_visualPtr) { // Set Visual Scale @@ -302,7 +302,7 @@ void MarkerManagerPrivate::SetVisual(const ignition::msgs::Marker &_msg, } ///////////////////////////////////////////////// -void MarkerManagerPrivate::SetMarker(const ignition::msgs::Marker &_msg, +void MarkerManagerPrivate::SetMarker(const msgs::Marker &_msg, const rendering::MarkerPtr &_markerPtr) { _markerPtr->SetLayer(_msg.layer()); @@ -319,7 +319,7 @@ void MarkerManagerPrivate::SetMarker(const ignition::msgs::Marker &_msg, _markerPtr->SetLifetime(std::chrono::seconds(0)); } // Set Marker Render Type - ignition::rendering::MarkerType markerType = MsgToType(_msg); + rendering::MarkerType markerType = MsgToType(_msg); _markerPtr->SetType(markerType); // Set Marker Material @@ -357,47 +357,47 @@ void MarkerManagerPrivate::SetMarker(const ignition::msgs::Marker &_msg, } ///////////////////////////////////////////////// -ignition::rendering::MarkerType MarkerManagerPrivate::MsgToType( - const ignition::msgs::Marker &_msg) +rendering::MarkerType MarkerManagerPrivate::MsgToType( + const msgs::Marker &_msg) { - ignition::msgs::Marker_Type marker = this->msg.type(); - if (marker != _msg.type() && _msg.type() != ignition::msgs::Marker::NONE) + msgs::Marker_Type marker = this->msg.type(); + if (marker != _msg.type() && _msg.type() != msgs::Marker::NONE) { marker = _msg.type(); this->msg.set_type(_msg.type()); } switch (marker) { - case ignition::msgs::Marker::BOX: - return ignition::rendering::MarkerType::MT_BOX; - case ignition::msgs::Marker::CYLINDER: - return ignition::rendering::MarkerType::MT_CYLINDER; - case ignition::msgs::Marker::LINE_STRIP: - return ignition::rendering::MarkerType::MT_LINE_STRIP; - case ignition::msgs::Marker::LINE_LIST: - return ignition::rendering::MarkerType::MT_LINE_LIST; - case ignition::msgs::Marker::POINTS: - return ignition::rendering::MarkerType::MT_POINTS; - case ignition::msgs::Marker::SPHERE: - return ignition::rendering::MarkerType::MT_SPHERE; - case ignition::msgs::Marker::TEXT: - return ignition::rendering::MarkerType::MT_TEXT; - case ignition::msgs::Marker::TRIANGLE_FAN: - return ignition::rendering::MarkerType::MT_TRIANGLE_FAN; - case ignition::msgs::Marker::TRIANGLE_LIST: - return ignition::rendering::MarkerType::MT_TRIANGLE_LIST; - case ignition::msgs::Marker::TRIANGLE_STRIP: - return ignition::rendering::MarkerType::MT_TRIANGLE_STRIP; + case msgs::Marker::BOX: + return rendering::MarkerType::MT_BOX; + case msgs::Marker::CYLINDER: + return rendering::MarkerType::MT_CYLINDER; + case msgs::Marker::LINE_STRIP: + return rendering::MarkerType::MT_LINE_STRIP; + case msgs::Marker::LINE_LIST: + return rendering::MarkerType::MT_LINE_LIST; + case msgs::Marker::POINTS: + return rendering::MarkerType::MT_POINTS; + case msgs::Marker::SPHERE: + return rendering::MarkerType::MT_SPHERE; + case msgs::Marker::TEXT: + return rendering::MarkerType::MT_TEXT; + case msgs::Marker::TRIANGLE_FAN: + return rendering::MarkerType::MT_TRIANGLE_FAN; + case msgs::Marker::TRIANGLE_LIST: + return rendering::MarkerType::MT_TRIANGLE_LIST; + case msgs::Marker::TRIANGLE_STRIP: + return rendering::MarkerType::MT_TRIANGLE_STRIP; default: ignerr << "Unable to create marker of type[" << _msg.type() << "]\n"; break; } - return ignition::rendering::MarkerType::MT_NONE; + return rendering::MarkerType::MT_NONE; } ///////////////////////////////////////////////// rendering::MaterialPtr MarkerManagerPrivate::MsgToMaterial( - const ignition::msgs::Marker &_msg) + const msgs::Marker &_msg) { rendering::MaterialPtr material = this->scene->CreateMaterial(); @@ -431,7 +431,7 @@ rendering::MaterialPtr MarkerManagerPrivate::MsgToMaterial( } ////////////////////////////////////////////////// -bool MarkerManagerPrivate::ProcessMarkerMsg(const ignition::msgs::Marker &_msg) +bool MarkerManagerPrivate::ProcessMarkerMsg(const msgs::Marker &_msg) { // Get the namespace, if it exists. Otherwise, use the global namespace std::string ns; @@ -451,14 +451,14 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const ignition::msgs::Marker &_msg) // Otherwise generate unique id else { - id = ignition::math::Rand::IntUniform(0, ignition::math::MAX_I32); + id = math::Rand::IntUniform(0, math::MAX_I32); // Make sure it's unique if namespace is given if (nsIter != this->visuals.end()) { while (nsIter->second.find(id) != nsIter->second.end()) - id = ignition::math::Rand::IntUniform(ignition::math::MIN_UI32, - ignition::math::MAX_UI32); + id = math::Rand::IntUniform(math::MIN_UI32, + math::MAX_UI32); } } @@ -468,7 +468,7 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const ignition::msgs::Marker &_msg) visualIter = nsIter->second.find(id); // Add/modify a marker - if (_msg.action() == ignition::msgs::Marker::ADD_MODIFY) + if (_msg.action() == msgs::Marker::ADD_MODIFY) { // Modify an existing marker, identified by namespace and id if (nsIter != this->visuals.end() && @@ -478,8 +478,8 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const ignition::msgs::Marker &_msg) { // TODO(anyone): Update so that multiple markers can // be attached to one visual - ignition::rendering::MarkerPtr markerPtr = - std::dynamic_pointer_cast + rendering::MarkerPtr markerPtr = + std::dynamic_pointer_cast (visualIter->second->GeometryByIndex(0)); visualIter->second->RemoveGeometryByIndex(0); @@ -526,7 +526,7 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const ignition::msgs::Marker &_msg) } } // Remove a single marker - else if (_msg.action() == ignition::msgs::Marker::DELETE_MARKER) + else if (_msg.action() == msgs::Marker::DELETE_MARKER) { // Remove the marker if it can be found. if (nsIter != this->visuals.end() && @@ -547,7 +547,7 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const ignition::msgs::Marker &_msg) } } // Remove all markers, or all markers in a namespace - else if (_msg.action() == ignition::msgs::Marker::DELETE_ALL) + else if (_msg.action() == msgs::Marker::DELETE_ALL) { // If given namespace doesn't exist if (!ns.empty() && nsIter == this->visuals.end()) @@ -591,7 +591,7 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const ignition::msgs::Marker &_msg) ///////////////////////////////////////////////// -bool MarkerManagerPrivate::OnList(ignition::msgs::Marker_V &_rep) +bool MarkerManagerPrivate::OnList(msgs::Marker_V &_rep) { std::lock_guard lock(this->mutex); _rep.clear_marker(); @@ -601,7 +601,7 @@ bool MarkerManagerPrivate::OnList(ignition::msgs::Marker_V &_rep) { for (auto iter : mIter.second) { - ignition::msgs::Marker *markerMsg = _rep.add_marker(); + msgs::Marker *markerMsg = _rep.add_marker(); markerMsg->set_ns(mIter.first); markerMsg->set_id(iter.first); } @@ -611,7 +611,7 @@ bool MarkerManagerPrivate::OnList(ignition::msgs::Marker_V &_rep) } ///////////////////////////////////////////////// -void MarkerManagerPrivate::OnMarkerMsg(const ignition::msgs::Marker &_req) +void MarkerManagerPrivate::OnMarkerMsg(const msgs::Marker &_req) { std::lock_guard lock(this->mutex); this->markerMsgs.push_back(_req); @@ -619,7 +619,7 @@ void MarkerManagerPrivate::OnMarkerMsg(const ignition::msgs::Marker &_req) ///////////////////////////////////////////////// bool MarkerManagerPrivate::OnMarkerMsgArray( - const ignition::msgs::Marker_V&_req, ignition::msgs::Boolean &_res) + const msgs::Marker_V&_req, msgs::Boolean &_res) { std::lock_guard lock(this->mutex); std::copy(_req.marker().begin(), _req.marker().end(), diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 6e347d01bc..94f25f4be4 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -121,7 +121,7 @@ class ignition::gazebo::RenderUtilPrivate public: MarkerManager markerManager; /// \brief Pointer to rendering engine. - public: ignition::rendering::RenderEngine *engine{nullptr}; + public: rendering::RenderEngine *engine{nullptr}; /// \brief rendering scene to be managed by the scene manager and used to /// generate sensor data @@ -1190,7 +1190,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( ///////////////////////////////////////////////// void RenderUtil::Init() { - ignition::common::SystemPaths pluginPath; + common::SystemPaths pluginPath; pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); rendering::setPluginPaths(pluginPath.PluginPaths()); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index a38ed7ffe8..1276d63183 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -446,8 +446,8 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, descriptor.subMeshName = _geom.MeshShape()->Submesh(); descriptor.centerSubMesh = _geom.MeshShape()->CenterSubmesh(); - ignition::common::MeshManager *meshManager = - ignition::common::MeshManager::Instance(); + common::MeshManager *meshManager = + common::MeshManager::Instance(); descriptor.mesh = meshManager->Load(descriptor.meshName); geom = this->dataPtr->scene->CreateMesh(descriptor); scale = _geom.MeshShape()->Scale(); @@ -462,7 +462,7 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, std::string name("POLYLINE_" + common::Uuid().String()); - auto meshManager = ignition::common::MeshManager::Instance(); + auto meshManager = common::MeshManager::Instance(); meshManager->CreateExtrudedPolyline(name, vertices, _geom.PolylineShape()[0].Height()); diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index d2dc5f39e7..34d22ab1a7 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -58,21 +58,21 @@ class ignition::gazebo::systems::AckermannSteeringPrivate { /// \brief Callback for velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const ignition::msgs::Twist &_msg); + public: void OnCmdVel(const msgs::Twist &_msg); /// \brief Update odometry and publish an odometry message. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); + public: void UpdateOdometry(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); + public: void UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Ignition communication node. public: transport::Node node; @@ -165,10 +165,10 @@ class ignition::gazebo::systems::AckermannSteeringPrivate public: std::chrono::steady_clock::duration lastOdomTime{0}; /// \brief Linear velocity limiter. - public: std::unique_ptr limiterLin; + public: std::unique_ptr limiterLin; /// \brief Angular velocity limiter. - public: std::unique_ptr limiterAng; + public: std::unique_ptr limiterAng; /// \brief Previous control command. public: Commands last0Cmd; @@ -260,8 +260,8 @@ void AckermannSteering::Configure(const Entity &_entity, this->dataPtr->wheelRadius).first; // Instantiate the speed limiters. - this->dataPtr->limiterLin = std::make_unique(); - this->dataPtr->limiterAng = std::make_unique(); + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); // Parse speed limiter parameters. if (_sdf->HasElement("min_velocity")) @@ -375,8 +375,8 @@ void AckermannSteering::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void AckermannSteering::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void AckermannSteering::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("AckermannSteering::PreUpdate"); @@ -587,8 +587,8 @@ void AckermannSteering::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void AckermannSteeringPrivate::UpdateOdometry( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { IGN_PROFILE("AckermannSteering::UpdateOdometry"); // Initialize, if not already initialized. @@ -690,7 +690,7 @@ void AckermannSteeringPrivate::UpdateOdometry( // Construct the Pose_V/tf message and publish it. msgs::Pose_V tfMsg; - ignition::msgs::Pose *tfMsgPose = tfMsg.add_pose(); + msgs::Pose *tfMsgPose = tfMsg.add_pose(); tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); @@ -702,8 +702,8 @@ void AckermannSteeringPrivate::UpdateOdometry( ////////////////////////////////////////////////// void AckermannSteeringPrivate::UpdateVelocity( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { IGN_PROFILE("AckermannSteering::UpdateVelocity"); @@ -788,7 +788,7 @@ void AckermannSteeringPrivate::OnCmdVel(const msgs::Twist &_msg) } IGNITION_ADD_PLUGIN(AckermannSteering, - ignition::gazebo::System, + System, AckermannSteering::ISystemConfigure, AckermannSteering::ISystemPreUpdate, AckermannSteering::ISystemPostUpdate) diff --git a/src/systems/apply_joint_force/ApplyJointForce.cc b/src/systems/apply_joint_force/ApplyJointForce.cc index 582d46e101..58e136d540 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.cc +++ b/src/systems/apply_joint_force/ApplyJointForce.cc @@ -34,7 +34,7 @@ class ignition::gazebo::systems::ApplyJointForcePrivate { /// \brief Callback for joint force subscription /// \param[in] _msg Joint force message - public: void OnCmdForce(const ignition::msgs::Double &_msg); + public: void OnCmdForce(const msgs::Double &_msg); /// \brief Ignition communication node. public: transport::Node node; @@ -103,8 +103,8 @@ void ApplyJointForce::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void ApplyJointForce::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void ApplyJointForce::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("ApplyJointForce::PreUpdate"); @@ -156,7 +156,7 @@ void ApplyJointForcePrivate::OnCmdForce(const msgs::Double &_msg) } IGNITION_ADD_PLUGIN(ApplyJointForce, - ignition::gazebo::System, + System, ApplyJointForce::ISystemConfigure, ApplyJointForce::ISystemPreUpdate) diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 47530eff67..0be91f91f5 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -63,11 +63,11 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate /// \brief Callback executed to start recharging. /// \param[in] _req This value should be true. - public: void OnEnableRecharge(const ignition::msgs::Boolean &_req); + public: void OnEnableRecharge(const msgs::Boolean &_req); /// \brief Callback executed to stop recharging. /// \param[in] _req This value should be true. - public: void OnDisableRecharge(const ignition::msgs::Boolean &_req); + public: void OnDisableRecharge(const msgs::Boolean &_req); /// \brief Name of model, only used for printing warning when battery drains. public: std::string modelName; @@ -360,7 +360,7 @@ double LinearBatteryPluginPrivate::StateOfCharge() const ////////////////////////////////////////////////// void LinearBatteryPluginPrivate::OnEnableRecharge( - const ignition::msgs::Boolean &/*_req*/) + const msgs::Boolean &/*_req*/) { igndbg << "Request for start charging received" << std::endl; this->startCharging = true; @@ -368,7 +368,7 @@ void LinearBatteryPluginPrivate::OnEnableRecharge( ////////////////////////////////////////////////// void LinearBatteryPluginPrivate::OnDisableRecharge( - const ignition::msgs::Boolean &/*_req*/) + const msgs::Boolean &/*_req*/) { igndbg << "Request for stop charging received" << std::endl; this->startCharging = false; @@ -376,8 +376,8 @@ void LinearBatteryPluginPrivate::OnDisableRecharge( ////////////////////////////////////////////////// void LinearBatteryPlugin::PreUpdate( - const ignition::gazebo::UpdateInfo &/*_info*/, - ignition::gazebo::EntityComponentManager &_ecm) + const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) { IGN_PROFILE("LinearBatteryPlugin::PreUpdate"); this->dataPtr->startDraining = false; @@ -608,7 +608,7 @@ double LinearBatteryPlugin::OnUpdateVoltage( } IGNITION_ADD_PLUGIN(LinearBatteryPlugin, - ignition::gazebo::System, + System, LinearBatteryPlugin::ISystemConfigure, LinearBatteryPlugin::ISystemPreUpdate, LinearBatteryPlugin::ISystemUpdate, diff --git a/src/systems/breadcrumbs/Breadcrumbs.cc b/src/systems/breadcrumbs/Breadcrumbs.cc index a06789d845..60b0b976ef 100644 --- a/src/systems/breadcrumbs/Breadcrumbs.cc +++ b/src/systems/breadcrumbs/Breadcrumbs.cc @@ -152,8 +152,8 @@ void Breadcrumbs::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void Breadcrumbs::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void Breadcrumbs::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("Breadcrumbs::PreUpdate"); @@ -390,7 +390,7 @@ void Breadcrumbs::OnDeploy(const msgs::Empty &) } IGNITION_ADD_PLUGIN(Breadcrumbs, - ignition::gazebo::System, + System, Breadcrumbs::ISystemConfigure, Breadcrumbs::ISystemPreUpdate) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index 0e7a286583..47b2bc0349 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -117,8 +117,8 @@ void Buoyancy::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void Buoyancy::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("Buoyancy::PreUpdate"); const components::Gravity *gravity = _ecm.Component( @@ -151,8 +151,8 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, _entity, components::Collision()); double volumeSum = 0; - ignition::math::Vector3d weightedPosInLinkSum = - ignition::math::Vector3d::Zero; + math::Vector3d weightedPosInLinkSum = + math::Vector3d::Zero; // Compute the volume of the link by iterating over all the collision // elements and storing each geometry's volume. @@ -266,7 +266,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } IGNITION_ADD_PLUGIN(Buoyancy, - ignition::gazebo::System, + System, Buoyancy::ISystemConfigure, Buoyancy::ISystemPreUpdate) diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index 6918036299..013a255a93 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -70,7 +70,7 @@ class ignition::gazebo::systems::CameraVideoRecorderPrivate public: std::mutex updateMutex; /// \brief Connection to the post-render event. - public: ignition::common::ConnectionPtr postRenderConn; + public: common::ConnectionPtr postRenderConn; /// \brief Pointer to the event manager public: EventManager *eventMgr = nullptr; @@ -356,7 +356,7 @@ void CameraVideoRecorderPrivate::OnPostRender() std::chrono::steady_clock::duration dt; dt = t - this->recordStartTime; int64_t sec, nsec; - std::tie(sec, nsec) = ignition::math::durationToSecNsec(dt); + std::tie(sec, nsec) = math::durationToSecNsec(dt); msgs::Time msg; msg.set_sec(sec); msg.set_nsec(nsec); @@ -461,7 +461,7 @@ void CameraVideoRecorder::PostUpdate(const UpdateInfo &_info, } IGNITION_ADD_PLUGIN(CameraVideoRecorder, - ignition::gazebo::System, + System, CameraVideoRecorder::ISystemConfigure, CameraVideoRecorder::ISystemPostUpdate) diff --git a/src/systems/collada_world_exporter/ColladaWorldExporter.cc b/src/systems/collada_world_exporter/ColladaWorldExporter.cc index 0ab619f899..50c2abf4c5 100644 --- a/src/systems/collada_world_exporter/ColladaWorldExporter.cc +++ b/src/systems/collada_world_exporter/ColladaWorldExporter.cc @@ -83,7 +83,7 @@ class ignition::gazebo::systems::ColladaWorldExporterPrivate components::Name, components::Geometry, components::Transparency>( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::Visual *, const components::Name *_name, const components::Geometry *_geom, @@ -105,12 +105,12 @@ class ignition::gazebo::systems::ColladaWorldExporterPrivate } mat->SetTransparency(_transparency->Data()); - const ignition::common::Mesh *mesh; - std::weak_ptr subm; + const common::Mesh *mesh; + std::weak_ptr subm; math::Vector3d scale; math::Matrix4d matrix(worldPose); - ignition::common::MeshManager *meshManager = - ignition::common::MeshManager::Instance(); + common::MeshManager *meshManager = + common::MeshManager::Instance(); auto addSubmeshFunc = [&](int i) { subm = worldMesh.AddSubMesh( diff --git a/src/systems/contact/Contact.cc b/src/systems/contact/Contact.cc index afd1122245..ac5bf96914 100644 --- a/src/systems/contact/Contact.cc +++ b/src/systems/contact/Contact.cc @@ -123,7 +123,7 @@ void ContactSensor::Load(const sdf::ElementPtr &_sdf, const std::string &_topic, } ignmsg << "Contact system publishing on " << this->topic << std::endl; - this->pub = this->node.Advertise(this->topic); + this->pub = this->node.Advertise(this->topic); } ////////////////////////////////////////////////// diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 1ed2125a1f..aedaff0002 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -106,8 +106,8 @@ void DetachableJoint::Configure(const Entity &_entity, ////////////////////////////////////////////////// void DetachableJoint::PreUpdate( - const ignition::gazebo::UpdateInfo &/*_info*/, - ignition::gazebo::EntityComponentManager &_ecm) + const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) { IGN_PROFILE("DetachableJoint::PreUpdate"); if (this->validConfig && !this->initialized) @@ -182,7 +182,7 @@ void DetachableJoint::OnDetachRequest(const msgs::Empty &) } IGNITION_ADD_PLUGIN(DetachableJoint, - ignition::gazebo::System, + System, DetachableJoint::ISystemConfigure, DetachableJoint::ISystemPreUpdate) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 11828b72a0..d0935f5224 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -57,25 +57,25 @@ class ignition::gazebo::systems::DiffDrivePrivate { /// \brief Callback for velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const ignition::msgs::Twist &_msg); + public: void OnCmdVel(const msgs::Twist &_msg); /// \brief Callback for enable/disable subscription /// \param[in] _msg Boolean message - public: void OnEnable(const ignition::msgs::Boolean &_msg); + public: void OnEnable(const msgs::Boolean &_msg); /// \brief Update odometry and publish an odometry message. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); + public: void UpdateOdometry(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); + public: void UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Ignition communication node. public: transport::Node node; @@ -126,10 +126,10 @@ class ignition::gazebo::systems::DiffDrivePrivate public: transport::Node::Publisher tfPub; /// \brief Linear velocity limiter. - public: std::unique_ptr limiterLin; + public: std::unique_ptr limiterLin; /// \brief Angular velocity limiter. - public: std::unique_ptr limiterAng; + public: std::unique_ptr limiterAng; /// \brief Previous control command. public: Commands last0Cmd; @@ -204,8 +204,8 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->wheelRadius).first; // Instantiate the speed limiters. - this->dataPtr->limiterLin = std::make_unique(); - this->dataPtr->limiterAng = std::make_unique(); + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); // Parse speed limiter parameters. if (_sdf->HasElement("min_velocity")) @@ -297,8 +297,8 @@ void DiffDrive::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void DiffDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void DiffDrive::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("DiffDrive::PreUpdate"); @@ -409,8 +409,8 @@ void DiffDrive::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) +void DiffDrivePrivate::UpdateOdometry(const UpdateInfo &_info, + const EntityComponentManager &_ecm) { IGN_PROFILE("DiffDrive::UpdateOdometry"); // Initialize, if not already initialized. @@ -490,7 +490,7 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, // Construct the Pose_V/tf message and publish it. msgs::Pose_V tfMsg; - ignition::msgs::Pose *tfMsgPose = tfMsg.add_pose(); + msgs::Pose *tfMsgPose = tfMsg.add_pose(); tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); @@ -501,8 +501,8 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, } ////////////////////////////////////////////////// -void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &/*_ecm*/) +void DiffDrivePrivate::UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &/*_ecm*/) { IGN_PROFILE("DiffDrive::UpdateVelocity"); @@ -556,7 +556,7 @@ void DiffDrivePrivate::OnEnable(const msgs::Boolean &_msg) } IGNITION_ADD_PLUGIN(DiffDrive, - ignition::gazebo::System, + System, DiffDrive::ISystemConfigure, DiffDrive::ISystemPreUpdate, DiffDrive::ISystemPostUpdate) diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index a6a3ecf7d9..ad62354f57 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -52,7 +52,7 @@ class ignition::gazebo::systems::ImuPrivate { /// \brief A map of IMU entity to its IMU sensor. public: std::unordered_map> entitySensorMap; + std::unique_ptr> entitySensorMap; /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index a8880ec6c6..d7dc14fe67 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -39,7 +39,7 @@ class ignition::gazebo::systems::JointControllerPrivate { /// \brief Callback for velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const ignition::msgs::Double &_msg); + public: void OnCmdVel(const msgs::Double &_msg); /// \brief Ignition communication node. public: transport::Node node; @@ -64,7 +64,7 @@ class ignition::gazebo::systems::JointControllerPrivate public: bool useForceCommands{false}; /// \brief Velocity PID controller. - public: ignition::math::PID velPid; + public: math::PID velPid; }; ////////////////////////////////////////////////// @@ -152,8 +152,8 @@ void JointController::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void JointController::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void JointController::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("JointController::PreUpdate"); @@ -242,7 +242,7 @@ void JointControllerPrivate::OnCmdVel(const msgs::Double &_msg) } IGNITION_ADD_PLUGIN(JointController, - ignition::gazebo::System, + System, JointController::ISystemConfigure, JointController::ISystemPreUpdate) diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 9530256eff..d43b44e6e2 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -39,7 +39,7 @@ class ignition::gazebo::systems::JointPositionControllerPrivate { /// \brief Callback for position subscription /// \param[in] _msg Position message - public: void OnCmdPos(const ignition::msgs::Double &_msg); + public: void OnCmdPos(const msgs::Double &_msg); /// \brief Ignition communication node. public: transport::Node node; @@ -60,7 +60,7 @@ class ignition::gazebo::systems::JointPositionControllerPrivate public: Model model{kNullEntity}; /// \brief Position PID controller. - public: ignition::math::PID posPid; + public: math::PID posPid; /// \brief Joint index to be used. public: unsigned int jointIndex = 0u; @@ -172,8 +172,8 @@ void JointPositionController::Configure(const Entity &_entity, ////////////////////////////////////////////////// void JointPositionController::PreUpdate( - const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) + const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("JointPositionController::PreUpdate"); @@ -269,7 +269,7 @@ void JointPositionControllerPrivate::OnCmdPos(const msgs::Double &_msg) } IGNITION_ADD_PLUGIN(JointPositionController, - ignition::gazebo::System, + System, JointPositionController::ISystemConfigure, JointPositionController::ISystemPreUpdate) diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index ce0eb759a0..65991f1933 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.cc +++ b/src/systems/joint_state_publisher/JointStatePublisher.cc @@ -68,7 +68,7 @@ void JointStatePublisher::Configure( while (elem) { std::string jointName = elem->Get(); - gazebo::Entity jointEntity = this->model.JointByName(_ecm, jointName); + Entity jointEntity = this->model.JointByName(_ecm, jointName); if (jointEntity != kNullEntity) { this->CreateComponents(_ecm, jointEntity); @@ -96,7 +96,7 @@ void JointStatePublisher::Configure( ////////////////////////////////////////////////// void JointStatePublisher::CreateComponents(EntityComponentManager &_ecm, - gazebo::Entity _joint) + Entity _joint) { if (this->joints.find(_joint) != this->joints.end()) { @@ -287,7 +287,7 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, } IGNITION_ADD_PLUGIN(JointStatePublisher, - ignition::gazebo::System, + System, JointStatePublisher::ISystemConfigure, JointStatePublisher::ISystemPostUpdate) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index a9e63d1764..cccdfff3fb 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -105,16 +105,16 @@ class ignition::gazebo::systems::LiftDragPrivate /// \brief center of pressure in link local coordinates with respect to the /// link's center of mass - public: ignition::math::Vector3d cp = math::Vector3d::Zero; + public: math::Vector3d cp = math::Vector3d::Zero; /// \brief Normally, this is taken as a direction parallel to the chord /// of the airfoil in zero angle of attack forward flight. - public: ignition::math::Vector3d forward = math::Vector3d::UnitX; + public: math::Vector3d forward = math::Vector3d::UnitX; /// \brief A vector in the lift/drag plane, perpendicular to the forward /// vector. Inflow velocity orthogonal to forward and upward vectors /// is considered flow in the wing sweep direction. - public: ignition::math::Vector3d upward = math::Vector3d::UnitZ; + public: math::Vector3d upward = math::Vector3d::UnitZ; /// \brief how much to change CL per radian of control surface joint /// value. @@ -153,15 +153,15 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm, this->radialSymmetry).first; this->area = _sdf->Get("area", this->area).first; this->alpha0 = _sdf->Get("a0", this->alpha0).first; - this->cp = _sdf->Get("cp", this->cp).first; + this->cp = _sdf->Get("cp", this->cp).first; // blade forward (-drag) direction in link frame this->forward = - _sdf->Get("forward", this->forward).first; + _sdf->Get("forward", this->forward).first; this->forward.Normalize(); // blade upward (+lift) direction in link frame - this->upward = _sdf->Get( + this->upward = _sdf->Get( "upward", this->upward) .first; this->upward.Normalize(); @@ -246,12 +246,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // rotate forward and upward vectors into world frame const auto forwardI = pose.Rot().RotateVector(this->forward); - ignition::math::Vector3d upwardI; + math::Vector3d upwardI; if (this->radialSymmetry) { // use inflow velocity to determine upward direction // which is the component of inflow perpendicular to forward direction. - ignition::math::Vector3d tmp = forwardI.Cross(velI); + math::Vector3d tmp = forwardI.Cross(velI); upwardI = forwardI.Cross(tmp).Normalize(); } else @@ -265,7 +265,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const double minRatio = -1.0; const double maxRatio = 1.0; // check sweep (angle between velI and lift-drag-plane) - double sinSweepAngle = ignition::math::clamp( + double sinSweepAngle = math::clamp( spanwiseI.Dot(velI), minRatio, maxRatio); // get cos from trig identity @@ -305,7 +305,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // given upwardI and liftI are both unit vectors, we can drop the denominator // cos(theta) = a.Dot(b) const double cosAlpha = - ignition::math::clamp(liftI.Dot(upwardI), minRatio, maxRatio); + math::clamp(liftI.Dot(upwardI), minRatio, maxRatio); // Is alpha positive or negative? Test: // forwardI points toward zero alpha @@ -354,7 +354,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) } // compute lift force at cp - ignition::math::Vector3d lift = cl * q * this->area * liftI; + math::Vector3d lift = cl * q * this->area * liftI; // compute cd at cp, check for stall, correct for sweep double cd; @@ -377,7 +377,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) cd = std::fabs(cd); // drag at cp - ignition::math::Vector3d drag = cd * q * this->area * dragDirection; + math::Vector3d drag = cd * q * this->area * dragDirection; // compute cm at cp, check for stall, correct for sweep double cm; @@ -406,12 +406,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // compute moment (torque) at cp // spanwiseI used to be momentDirection - ignition::math::Vector3d moment = cm * q * this->area * spanwiseI; + math::Vector3d moment = cm * q * this->area * spanwiseI; // force and torque about cg in world frame - ignition::math::Vector3d force = lift + drag; - ignition::math::Vector3d torque = moment; + math::Vector3d force = lift + drag; + math::Vector3d torque = moment; // Correct for nan or inf force.Correct(); this->cp.Correct(); @@ -523,7 +523,7 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) } IGNITION_ADD_PLUGIN(LiftDrag, - ignition::gazebo::System, + System, LiftDrag::ISystemConfigure, LiftDrag::ISystemPreUpdate) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index 0303677ec8..811e059f3c 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -655,10 +655,10 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) } } -IGNITION_ADD_PLUGIN(ignition::gazebo::systems::LogPlayback, - ignition::gazebo::System, +IGNITION_ADD_PLUGIN(LogPlayback, + System, LogPlayback::ISystemConfigure, LogPlayback::ISystemUpdate) -IGNITION_ADD_PLUGIN_ALIAS(ignition::gazebo::systems::LogPlayback, +IGNITION_ADD_PLUGIN_ALIAS(LogPlayback, "ignition::gazebo::systems::LogPlayback") diff --git a/src/systems/log/LogRecord.cc b/src/systems/log/LogRecord.cc index 9cc417088f..57ac7e7c3d 100644 --- a/src/systems/log/LogRecord.cc +++ b/src/systems/log/LogRecord.cc @@ -344,7 +344,7 @@ bool LogRecordPrivate::Start(const std::string &_logPath, // This calls Log::Open() and loads sql schema if (this->recorder.Start(dbPath) == - ignition::transport::log::RecorderError::SUCCESS) + transport::log::RecorderError::SUCCESS) { this->instStarted = true; return true; @@ -694,11 +694,11 @@ void LogRecord::PostUpdate(const UpdateInfo &_info, this->dataPtr->LogModelResources(_ecm); } -IGNITION_ADD_PLUGIN(ignition::gazebo::systems::LogRecord, - ignition::gazebo::System, +IGNITION_ADD_PLUGIN(LogRecord, + gazebo::System, LogRecord::ISystemConfigure, LogRecord::ISystemPreUpdate, LogRecord::ISystemPostUpdate) -IGNITION_ADD_PLUGIN_ALIAS(ignition::gazebo::systems::LogRecord, +IGNITION_ADD_PLUGIN_ALIAS(LogRecord, "ignition::gazebo::systems::LogRecord") diff --git a/src/systems/log_video_recorder/LogVideoRecorder.cc b/src/systems/log_video_recorder/LogVideoRecorder.cc index 29416a3a0e..8fe62313cd 100644 --- a/src/systems/log_video_recorder/LogVideoRecorder.cc +++ b/src/systems/log_video_recorder/LogVideoRecorder.cc @@ -372,8 +372,8 @@ void LogVideoRecorder::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void LogVideoRecorderPrivate::Rewind() { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error sending rewind request" << std::endl; @@ -399,14 +399,14 @@ void LogVideoRecorderPrivate::Play() ////////////////////////////////////////////////// void LogVideoRecorderPrivate::Follow(const std::string &_entity) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error sending follow request" << std::endl; }; - ignition::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_entity); if (this->node.Request(this->followService, req, cb)) { @@ -417,14 +417,14 @@ void LogVideoRecorderPrivate::Follow(const std::string &_entity) ////////////////////////////////////////////////// void LogVideoRecorderPrivate::Record(bool _record) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) ignerr << "Error sending record request" << std::endl; }; - ignition::msgs::VideoRecord req; + msgs::VideoRecord req; if (_record) { @@ -445,7 +445,7 @@ void LogVideoRecorderPrivate::Record(bool _record) } IGNITION_ADD_PLUGIN(LogVideoRecorder, - ignition::gazebo::System, + System, LogVideoRecorder::ISystemConfigure, LogVideoRecorder::ISystemPostUpdate) diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc index 33b9310b2b..8844dd2773 100644 --- a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc @@ -78,7 +78,7 @@ class ignition::gazebo::systems::LogicalAudioSensorPluginPrivate const logical_audio::SourcePlayInfo &_sourcePlayInfo); /// \brief Node used to create publishers and services - public: ignition::transport::Node node; + public: transport::Node node; /// \brief A flag used to initialize a source's playing information /// before starting simulation. @@ -97,7 +97,7 @@ class ignition::gazebo::systems::LogicalAudioSensorPluginPrivate /// (an entity can have multiple microphones attached to it). /// The value is the microphone's detection publisher. public: std::unordered_map micEntities; + transport::Node::Publisher> micEntities; /// \brief A mutex used to ensure that the play source service call does /// not interfere with the source's state in the PreUpdate step. @@ -240,7 +240,7 @@ void LogicalAudioSensorPlugin::PostUpdate(const UpdateInfo &_info, // publish the source that the microphone heard, along with the // volume level the microphone detected. The detected source's // ID is embedded in the message's header - ignition::msgs::Double msg; + msgs::Double msg; auto header = msg.mutable_header(); auto timeStamp = header->mutable_stamp(); timeStamp->set_sec(simSeconds.count()); @@ -284,7 +284,7 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( } _ids.insert(id); - ignition::math::Pose3d pose; + math::Pose3d pose; if (!_elem->HasElement("pose")) { ignwarn << "Audio source is missing a pose. " @@ -293,7 +293,7 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( } else { - pose = _elem->Get("pose"); + pose = _elem->Get("pose"); } if (!_elem->HasElement("attenuation_function")) @@ -385,16 +385,16 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( components::LogicalAudioSourcePlayInfo(playInfo)); // create service callbacks that allow this source to be played/stopped - std::function playSrvCb = - [this, entity](ignition::msgs::Boolean &_resp) + std::function playSrvCb = + [this, entity](msgs::Boolean &_resp) { std::lock_guard lock(this->playSourceMutex); this->sourceEntities[entity].first = true; _resp.set_data(true); return true; }; - std::function stopSrvCb = - [this, entity](ignition::msgs::Boolean &_resp) + std::function stopSrvCb = + [this, entity](msgs::Boolean &_resp) { std::lock_guard lock(this->stopSourceMutex); this->sourceEntities[entity].second = true; @@ -447,7 +447,7 @@ void LogicalAudioSensorPluginPrivate::CreateMicrophone( } _ids.insert(id); - ignition::math::Pose3d pose; + math::Pose3d pose; if (!_elem->HasElement("pose")) { ignwarn << "Microphone is missing a pose. " @@ -456,7 +456,7 @@ void LogicalAudioSensorPluginPrivate::CreateMicrophone( } else { - pose = _elem->Get("pose"); + pose = _elem->Get("pose"); } double volumeDetectionThreshold; @@ -494,7 +494,7 @@ void LogicalAudioSensorPluginPrivate::CreateMicrophone( components::Pose(pose)); // create the detection publisher for this microphone - auto pub = this->node.Advertise( + auto pub = this->node.Advertise( scopedName(entity, _ecm) + "/detection"); if (!pub) { @@ -519,7 +519,7 @@ bool LogicalAudioSensorPluginPrivate::DurationExceeded( } IGNITION_ADD_PLUGIN(LogicalAudioSensorPlugin, - ignition::gazebo::System, + System, LogicalAudioSensorPlugin::ISystemConfigure, LogicalAudioSensorPlugin::ISystemPreUpdate, LogicalAudioSensorPlugin::ISystemPostUpdate) diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index bf974caa82..02b1155a6b 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -52,7 +52,7 @@ class ignition::gazebo::systems::MagnetometerPrivate { /// \brief A map of magnetometer entity to its sensor. public: std::unordered_map> entitySensorMap; + std::unique_ptr> entitySensorMap; /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.cc b/src/systems/multicopter_control/MulticopterVelocityControl.cc index edeed38318..e82a327321 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.cc +++ b/src/systems/multicopter_control/MulticopterVelocityControl.cc @@ -299,8 +299,8 @@ void MulticopterVelocityControl::Configure(const Entity &_entity, ////////////////////////////////////////////////// void MulticopterVelocityControl::PreUpdate( - const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) + const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("MulticopterVelocityControl::PreUpdate"); @@ -391,7 +391,7 @@ void MulticopterVelocityControl::OnEnable( ////////////////////////////////////////////////// void MulticopterVelocityControl::PublishRotorVelocities( - ignition::gazebo::EntityComponentManager &_ecm, + EntityComponentManager &_ecm, const Eigen::VectorXd &_vels) { if (_vels.size() != this->rotorVelocitiesMsg.velocity_size()) @@ -428,7 +428,7 @@ void MulticopterVelocityControl::PublishRotorVelocities( } IGNITION_ADD_PLUGIN(MulticopterVelocityControl, - ignition::gazebo::System, + System, MulticopterVelocityControl::ISystemConfigure, MulticopterVelocityControl::ISystemPreUpdate) diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index 730103d407..40c0bda6d5 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -118,7 +118,7 @@ enum class MotorType { class ignition::gazebo::systems::MulticopterMotorModelPrivate { /// \brief Callback for actuator commands. - public: void OnActuatorMsg(const ignition::msgs::Actuators &_msg); + public: void OnActuatorMsg(const msgs::Actuators &_msg); /// \brief Apply link forces and moments based on propeller state. public: void UpdateForcesAndMoments(EntityComponentManager &_ecm); @@ -364,8 +364,8 @@ void MulticopterMotorModel::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void MulticopterMotorModel::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void MulticopterMotorModel::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("MulticopterMotorModel::PreUpdate"); @@ -463,7 +463,7 @@ void MulticopterMotorModel::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ////////////////////////////////////////////////// void MulticopterMotorModelPrivate::OnActuatorMsg( - const ignition::msgs::Actuators &_msg) + const msgs::Actuators &_msg) { std::lock_guard lock(this->recvdActuatorsMsgMutex); this->recvdActuatorsMsg = _msg; @@ -553,8 +553,8 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( realMotorVelocity * realMotorVelocity * this->motorConstant; - using Pose = ignition::math::Pose3d; - using Vector3 = ignition::math::Vector3d; + using Pose = math::Pose3d; + using Vector3 = math::Vector3d; Link link(this->linkEntity); const auto worldPose = link.WorldPose(_ecm); @@ -665,7 +665,7 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( } IGNITION_ADD_PLUGIN(MulticopterMotorModel, - ignition::gazebo::System, + System, MulticopterMotorModel::ISystemConfigure, MulticopterMotorModel::ISystemPreUpdate) diff --git a/src/systems/performer_detector/PerformerDetector.cc b/src/systems/performer_detector/PerformerDetector.cc index 9aa1dc3a45..2c9ccd89c7 100644 --- a/src/systems/performer_detector/PerformerDetector.cc +++ b/src/systems/performer_detector/PerformerDetector.cc @@ -130,8 +130,8 @@ void PerformerDetector::Configure(const Entity &_entity, ////////////////////////////////////////////////// void PerformerDetector::PostUpdate( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { IGN_PROFILE("PerformerDetector::PostUpdate"); @@ -255,7 +255,7 @@ void PerformerDetector::Publish( } IGNITION_ADD_PLUGIN(PerformerDetector, - ignition::gazebo::System, + System, PerformerDetector::ISystemConfigure, PerformerDetector::ISystemPostUpdate) diff --git a/src/systems/physics/EntityFeatureMap_TEST.cc b/src/systems/physics/EntityFeatureMap_TEST.cc index 79c5314a87..258dc19ac0 100644 --- a/src/systems/physics/EntityFeatureMap_TEST.cc +++ b/src/systems/physics/EntityFeatureMap_TEST.cc @@ -63,7 +63,7 @@ class EntityFeatureMapFixture: public InternalFixture<::testing::Test> << "Failed to find plugin [" << pluginLib << "]"; // Load engine plugin - ignition::plugin::Loader pluginLoader; + plugin::Loader pluginLoader; auto plugins = pluginLoader.LoadLib(pathToLib); ASSERT_FALSE(plugins.empty()) << "Unable to load the [" << pathToLib << "] library."; @@ -81,7 +81,7 @@ class EntityFeatureMapFixture: public InternalFixture<::testing::Test> << pathToLib << "]"; this->engine = - ignition::physics::RequestEngine::From(plugin); ASSERT_NE(nullptr, this->engine); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 1ec9b9c43d..636f48a9b8 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -143,37 +143,37 @@ class ignition::gazebo::systems::PhysicsPrivate /// implement to be supported by this system. /// New features can't be added to this list in minor / patch releases, in /// order to maintain backwards compatibility with downstream physics plugins. - public: struct MinimumFeatureList : ignition::physics::FeatureList< - ignition::physics::FindFreeGroupFeature, - ignition::physics::SetFreeGroupWorldPose, - ignition::physics::FreeGroupFrameSemantics, - ignition::physics::LinkFrameSemantics, - ignition::physics::ForwardStep, - ignition::physics::RemoveEntities, - ignition::physics::sdf::ConstructSdfLink, - ignition::physics::sdf::ConstructSdfModel, - ignition::physics::sdf::ConstructSdfWorld + public: struct MinimumFeatureList : physics::FeatureList< + physics::FindFreeGroupFeature, + physics::SetFreeGroupWorldPose, + physics::FreeGroupFrameSemantics, + physics::LinkFrameSemantics, + physics::ForwardStep, + physics::RemoveEntities, + physics::sdf::ConstructSdfLink, + physics::sdf::ConstructSdfModel, + physics::sdf::ConstructSdfWorld >{}; /// \brief Engine type with just the minimum features. - public: using EnginePtrType = ignition::physics::EnginePtr< - ignition::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using EnginePtrType = physics::EnginePtr< + physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief World type with just the minimum features. - public: using WorldPtrType = ignition::physics::WorldPtr< - ignition::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using WorldPtrType = physics::WorldPtr< + physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Model type with just the minimum features. - public: using ModelPtrType = ignition::physics::ModelPtr< - ignition::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using ModelPtrType = physics::ModelPtr< + physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Link type with just the minimum features. - public: using LinkPtrType = ignition::physics::LinkPtr< - ignition::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using LinkPtrType = physics::LinkPtr< + physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Free group type with just the minimum features. - public: using FreeGroupPtrType = ignition::physics::FreeGroupPtr< - ignition::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using FreeGroupPtrType = physics::FreeGroupPtr< + physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Create physics entities /// \param[in] _ecm Constant reference to ECM. @@ -236,7 +236,7 @@ class ignition::gazebo::systems::PhysicsPrivate /// \param[in] _from An ancestor of the _to entity. /// \param[in] _to A descendant of the _from entity. /// \return Pose transform between the two entities - public: ignition::math::Pose3d RelativePose(const Entity &_from, + public: math::Pose3d RelativePose(const Entity &_from, const Entity &_to, const EntityComponentManager &_ecm) const; /// \brief Enable contact surface customization for the given world. @@ -312,18 +312,18 @@ class ignition::gazebo::systems::PhysicsPrivate public: struct FrictionPyramidSlipComplianceFeatureList : physics::FeatureList< MinimumFeatureList, - ignition::physics::GetShapeFrictionPyramidSlipCompliance, - ignition::physics::SetShapeFrictionPyramidSlipCompliance>{}; + physics::GetShapeFrictionPyramidSlipCompliance, + physics::SetShapeFrictionPyramidSlipCompliance>{}; ////////////////////////////////////////////////// // Joints /// \brief Feature list to handle joints. - public: struct JointFeatureList : ignition::physics::FeatureList< + public: struct JointFeatureList : physics::FeatureList< MinimumFeatureList, - ignition::physics::GetBasicJointProperties, - ignition::physics::GetBasicJointState, - ignition::physics::SetBasicJointState, - ignition::physics::sdf::ConstructSdfJoint>{}; + physics::GetBasicJointProperties, + physics::GetBasicJointState, + physics::SetBasicJointState, + physics::sdf::ConstructSdfJoint>{}; ////////////////////////////////////////////////// @@ -340,51 +340,51 @@ class ignition::gazebo::systems::PhysicsPrivate // Collisions /// \brief Feature list to handle collisions. - public: struct CollisionFeatureList : ignition::physics::FeatureList< + public: struct CollisionFeatureList : physics::FeatureList< MinimumFeatureList, - ignition::physics::GetContactsFromLastStepFeature, - ignition::physics::sdf::ConstructSdfCollision>{}; + physics::GetContactsFromLastStepFeature, + physics::sdf::ConstructSdfCollision>{}; /// \brief Feature list to handle contacts information. - public: struct ContactFeatureList : ignition::physics::FeatureList< + public: struct ContactFeatureList : physics::FeatureList< CollisionFeatureList, - ignition::physics::GetContactsFromLastStepFeature>{}; + physics::GetContactsFromLastStepFeature>{}; /// \brief Feature list to change contacts before they are applied to physics. public: struct SetContactPropertiesCallbackFeatureList : - ignition::physics::FeatureList< + physics::FeatureList< ContactFeatureList, - ignition::physics::SetContactPropertiesCallbackFeature>{}; + physics::SetContactPropertiesCallbackFeature>{}; /// \brief Collision type with collision features. - public: using ShapePtrType = ignition::physics::ShapePtr< - ignition::physics::FeaturePolicy3d, CollisionFeatureList>; + public: using ShapePtrType = physics::ShapePtr< + physics::FeaturePolicy3d, CollisionFeatureList>; /// \brief World type with just the minimum features. Non-pointer. - public: using WorldShapeType = ignition::physics::World< - ignition::physics::FeaturePolicy3d, CollisionFeatureList>; + public: using WorldShapeType = physics::World< + physics::FeaturePolicy3d, CollisionFeatureList>; ////////////////////////////////////////////////// // Collision filtering with bitmasks /// \brief Feature list to filter collisions with bitmasks. - public: struct CollisionMaskFeatureList : ignition::physics::FeatureList< + public: struct CollisionMaskFeatureList : physics::FeatureList< CollisionFeatureList, - ignition::physics::CollisionFilterMaskFeature>{}; + physics::CollisionFilterMaskFeature>{}; ////////////////////////////////////////////////// // Link force /// \brief Feature list for applying forces to links. - public: struct LinkForceFeatureList : ignition::physics::FeatureList< - ignition::physics::AddLinkExternalForceTorque>{}; + public: struct LinkForceFeatureList : physics::FeatureList< + physics::AddLinkExternalForceTorque>{}; ////////////////////////////////////////////////// // Bounding box /// \brief Feature list for model bounding box. - public: struct BoundingBoxFeatureList : ignition::physics::FeatureList< + public: struct BoundingBoxFeatureList : physics::FeatureList< MinimumFeatureList, - ignition::physics::GetModelBoundingBox>{}; + physics::GetModelBoundingBox>{}; ////////////////////////////////////////////////// @@ -418,8 +418,8 @@ class ignition::gazebo::systems::PhysicsPrivate ////////////////////////////////////////////////// // World velocity command public: struct WorldVelocityCommandFeatureList : - ignition::physics::FeatureList< - ignition::physics::SetFreeGroupWorldVelocity>{}; + physics::FeatureList< + physics::SetFreeGroupWorldVelocity>{}; ////////////////////////////////////////////////// @@ -436,9 +436,9 @@ class ignition::gazebo::systems::PhysicsPrivate // Nested Models /// \brief Feature list to construct nested models - public: struct NestedModelFeatureList : ignition::physics::FeatureList< + public: struct NestedModelFeatureList : physics::FeatureList< MinimumFeatureList, - ignition::physics::sdf::ConstructSdfNestedModel>{}; + physics::sdf::ConstructSdfNestedModel>{}; ////////////////////////////////////////////////// /// \brief World EntityFeatureMap @@ -589,7 +589,7 @@ void Physics::Configure(const Entity &_entity, } // Load engine plugin - ignition::plugin::Loader pluginLoader; + plugin::Loader pluginLoader; auto plugins = pluginLoader.LoadLib(pathToLib); if (plugins.empty()) { @@ -619,8 +619,8 @@ void Physics::Configure(const Entity &_entity, continue; } - this->dataPtr->engine = ignition::physics::RequestEngine< - ignition::physics::FeaturePolicy3d, + this->dataPtr->engine = physics::RequestEngine< + physics::FeaturePolicy3d, PhysicsPrivate::MinimumFeatureList>::From(plugin); if (nullptr != this->dataPtr->engine) @@ -630,8 +630,8 @@ void Physics::Configure(const Entity &_entity, break; } - auto missingFeatures = ignition::physics::RequestEngine< - ignition::physics::FeaturePolicy3d, + auto missingFeatures = physics::RequestEngine< + physics::FeaturePolicy3d, PhysicsPrivate::MinimumFeatureList>::MissingFeatureNames(plugin); std::stringstream msg; @@ -970,7 +970,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) return true; } - auto &meshManager = *ignition::common::MeshManager::Instance(); + auto &meshManager = *common::MeshManager::Instance(); auto fullPath = asFullPath(meshSdf->Uri(), meshSdf->FilePath()); auto *mesh = meshManager.Load(fullPath); if (nullptr == mesh) @@ -1018,7 +1018,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) } std::string name("POLYLINE_" + common::Uuid().String()); - auto meshManager = ignition::common::MeshManager::Instance(); + auto meshManager = common::MeshManager::Instance(); meshManager->CreateExtrudedPolyline(name, vertices, _geom->Data().PolylineShape()[0].Height()); @@ -2080,9 +2080,9 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) void PhysicsPrivate::Step(const std::chrono::steady_clock::duration &_dt) { IGN_PROFILE("PhysicsPrivate::Step"); - ignition::physics::ForwardStep::Input input; - ignition::physics::ForwardStep::State state; - ignition::physics::ForwardStep::Output output; + physics::ForwardStep::Input input; + physics::ForwardStep::State state; + physics::ForwardStep::Output output; input.Get() = _dt; @@ -2093,7 +2093,7 @@ void PhysicsPrivate::Step(const std::chrono::steady_clock::duration &_dt) } ////////////////////////////////////////////////// -ignition::math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, +math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, const Entity &_to, const EntityComponentManager &_ecm) const { math::Pose3d transform; @@ -2413,7 +2413,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); - ignition::math::Vector3d entityWorldAngularVel = + math::Vector3d entityWorldAngularVel = math::eigen3::convert(entityFrameData.angularVelocity); auto entityBodyAngularVel = @@ -2438,7 +2438,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); - ignition::math::Vector3d entityWorldLinearAcc = + math::Vector3d entityWorldLinearAcc = math::eigen3::convert(entityFrameData.linearAcceleration); auto entityBodyLinearAcc = @@ -2808,7 +2808,7 @@ void PhysicsPrivate::DisableContactSurfaceCustomization(const Entity &_world) } IGNITION_ADD_PLUGIN(Physics, - ignition::gazebo::System, + gazebo::System, Physics::ISystemConfigure, Physics::ISystemUpdate) diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 6a699b6b1f..25e428b7b6 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -154,12 +154,12 @@ class ignition::gazebo::systems::PosePublisherPrivate /// \brief A variable that gets populated with poses. This also here as a /// member variable to avoid repeated memory allocations and improve /// performance. - public: ignition::msgs::Pose poseMsg; + public: msgs::Pose poseMsg; /// \brief A variable that gets populated with poses. This also here as a /// member variable to avoid repeated memory allocations and improve /// performance. - public: ignition::msgs::Pose_V poseVMsg; + public: msgs::Pose_V poseVMsg; /// \brief True to publish a vector of poses. False to publish individual pose /// msgs. @@ -248,23 +248,23 @@ void PosePublisher::Configure(const Entity &_entity, if (this->dataPtr->usePoseV) { this->dataPtr->posePub = - this->dataPtr->node.Advertise(poseTopic); + this->dataPtr->node.Advertise(poseTopic); if (this->dataPtr->staticPosePublisher) { this->dataPtr->poseStaticPub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( staticPoseTopic); } } else { this->dataPtr->posePub = - this->dataPtr->node.Advertise(poseTopic); + this->dataPtr->node.Advertise(poseTopic); if (this->dataPtr->staticPosePublisher) { this->dataPtr->poseStaticPub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( staticPoseTopic); } } @@ -498,7 +498,7 @@ void PosePublisherPrivate::PublishPoses( IGN_PROFILE("PosePublisher::PublishPoses"); // publish poses - ignition::msgs::Pose *msg = nullptr; + msgs::Pose *msg = nullptr; if (this->usePoseV) this->poseVMsg.Clear(); diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 3cf1980d17..67942f74d5 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -83,21 +83,21 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate /// \brief Callback for scene info service. /// \param[out] _res Response containing the latest scene message. /// \return True if successful. - public: bool SceneInfoService(ignition::msgs::Scene &_res); + public: bool SceneInfoService(msgs::Scene &_res); /// \brief Callback for scene graph service. /// \param[out] _res Response containing the the scene graph in DOT format. /// \return True if successful. - public: bool SceneGraphService(ignition::msgs::StringMsg &_res); + public: bool SceneGraphService(msgs::StringMsg &_res); /// \brief Callback for state service. /// \param[out] _res Response containing the latest full state. /// \return True if successful. - public: bool StateService(ignition::msgs::SerializedStepMap &_res); + public: bool StateService(msgs::SerializedStepMap &_res); /// \brief Callback for state service - non blocking. /// \param[out] _res Response containing the last available full state. - public: void StateAsyncService(const ignition::msgs::StringMsg &_req); + public: void StateAsyncService(const msgs::StringMsg &_req); /// \brief Updates the scene graph when entities are added /// \param[in] _manager The entity component manager @@ -523,7 +523,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) // Scene info topic std::string sceneTopic{"/world/" + _worldName + "/scene/info"}; - this->scenePub = this->node->Advertise(sceneTopic); + this->scenePub = this->node->Advertise(sceneTopic); ignmsg << "Publishing scene information on [" << sceneTopic << "]" << std::endl; @@ -532,7 +532,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) std::string deletionTopic{"/world/" + _worldName + "/scene/deletion"}; this->deletionPub = - this->node->Advertise(deletionTopic); + this->node->Advertise(deletionTopic); ignmsg << "Publishing entity deletions on [" << deletionTopic << "]" << std::endl; @@ -541,7 +541,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) std::string stateTopic{"/world/" + _worldName + "/state"}; this->statePub = - this->node->Advertise(stateTopic); + this->node->Advertise(stateTopic); ignmsg << "Publishing state changes on [" << stateTopic << "]" << std::endl; @@ -570,7 +570,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) } ////////////////////////////////////////////////// -bool SceneBroadcasterPrivate::SceneInfoService(ignition::msgs::Scene &_res) +bool SceneBroadcasterPrivate::SceneInfoService(msgs::Scene &_res) { std::lock_guard lock(this->graphMutex); @@ -589,7 +589,7 @@ bool SceneBroadcasterPrivate::SceneInfoService(ignition::msgs::Scene &_res) ////////////////////////////////////////////////// void SceneBroadcasterPrivate::StateAsyncService( - const ignition::msgs::StringMsg &_req) + const msgs::StringMsg &_req) { std::unique_lock lock(this->stateMutex); this->stateServiceRequest = true; @@ -598,7 +598,7 @@ void SceneBroadcasterPrivate::StateAsyncService( ////////////////////////////////////////////////// bool SceneBroadcasterPrivate::StateService( - ignition::msgs::SerializedStepMap &_res) + msgs::SerializedStepMap &_res) { _res.Clear(); @@ -620,7 +620,7 @@ bool SceneBroadcasterPrivate::StateService( } ////////////////////////////////////////////////// -bool SceneBroadcasterPrivate::SceneGraphService(ignition::msgs::StringMsg &_res) +bool SceneBroadcasterPrivate::SceneGraphService(msgs::StringMsg &_res) { std::lock_guard lock(this->graphMutex); @@ -890,22 +890,22 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( msgs::IMUSensor * imuMsg = sensorMsg->mutable_imu(); const auto * imu = imuComp->Data().ImuSensor(); - ignition::gazebo::set( + set( imuMsg->mutable_linear_acceleration()->mutable_x_noise(), imu->LinearAccelerationXNoise()); - ignition::gazebo::set( + set( imuMsg->mutable_linear_acceleration()->mutable_y_noise(), imu->LinearAccelerationYNoise()); - ignition::gazebo::set( + set( imuMsg->mutable_linear_acceleration()->mutable_z_noise(), imu->LinearAccelerationZNoise()); - ignition::gazebo::set( + set( imuMsg->mutable_angular_velocity()->mutable_x_noise(), imu->AngularVelocityXNoise()); - ignition::gazebo::set( + set( imuMsg->mutable_angular_velocity()->mutable_y_noise(), imu->AngularVelocityYNoise()); - ignition::gazebo::set( + set( imuMsg->mutable_angular_velocity()->mutable_z_noise(), imu->AngularVelocityZNoise()); } @@ -1148,7 +1148,7 @@ void SceneBroadcasterPrivate::RemoveFromGraph(const Entity _entity, IGNITION_ADD_PLUGIN(SceneBroadcaster, - ignition::gazebo::System, + System, SceneBroadcaster::ISystemConfigure, SceneBroadcaster::ISystemPostUpdate) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 217f57829c..592e02d5e3 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -108,10 +108,10 @@ class ignition::gazebo::systems::SensorsPrivate public: std::condition_variable renderCv; /// \brief Connection to events::Stop event, used to stop thread - public: ignition::common::ConnectionPtr stopConn; + public: common::ConnectionPtr stopConn; /// \brief Update time for the next rendering iteration - public: ignition::common::Time updateTime; + public: common::Time updateTime; /// \brief Sensors to include in the next rendering iteration public: std::vector activeSensors; @@ -120,7 +120,7 @@ class ignition::gazebo::systems::SensorsPrivate public: std::mutex sensorMaskMutex; /// \brief Mask sensor updates for sensors currently being rendered - public: std::map sensorMask; + public: std::map sensorMask; /// \brief Pointer to the event manager public: EventManager *eventManager{nullptr}; @@ -242,7 +242,7 @@ void SensorsPrivate::RunOnce() for (const auto & sensor : this->activeSensors) { // 90% of update delta (1/UpdateRate()); - ignition::common::Time delta(0.9 / sensor->UpdateRate()); + common::Time delta(0.9 / sensor->UpdateRate()); this->sensorMask[sensor->Id()] = this->updateTime + delta; } this->sensorMaskMutex.unlock(); diff --git a/src/systems/thermal/Thermal.cc b/src/systems/thermal/Thermal.cc index 2a0f346797..0a5652ec50 100644 --- a/src/systems/thermal/Thermal.cc +++ b/src/systems/thermal/Thermal.cc @@ -45,8 +45,8 @@ Thermal::~Thermal() = default; ////////////////////////////////////////////////// void Thermal::Configure(const Entity &_entity, const std::shared_ptr &_sdf, - gazebo::EntityComponentManager &_ecm, - gazebo::EventManager & /*_eventMgr*/) + EntityComponentManager &_ecm, + EventManager & /*_eventMgr*/) { if (!_sdf->HasElement("temperature")) { diff --git a/src/systems/touch_plugin/TouchPlugin.cc b/src/systems/touch_plugin/TouchPlugin.cc index daa1098aff..3628c1876c 100644 --- a/src/systems/touch_plugin/TouchPlugin.cc +++ b/src/systems/touch_plugin/TouchPlugin.cc @@ -402,7 +402,7 @@ void TouchPlugin::PostUpdate(const UpdateInfo &_info, } IGNITION_ADD_PLUGIN(TouchPlugin, - ignition::gazebo::System, + System, TouchPlugin::ISystemConfigure, TouchPlugin::ISystemPreUpdate, TouchPlugin::ISystemPostUpdate) diff --git a/src/systems/track_controller/TrackController.cc b/src/systems/track_controller/TrackController.cc index 116d3cb787..fdf8d1d51a 100644 --- a/src/systems/track_controller/TrackController.cc +++ b/src/systems/track_controller/TrackController.cc @@ -91,8 +91,8 @@ class ignition::gazebo::systems::TrackControllerPrivate /// \param[in] _frictionDirection First friction direction (in world coords). /// \return The computed contact surface speed. public: double ComputeSurfaceMotion( - double _beltSpeed, const ignition::math::Vector3d &_beltDirection, - const ignition::math::Vector3d &_frictionDirection); + double _beltSpeed, const math::Vector3d &_beltDirection, + const math::Vector3d &_frictionDirection); /// \brief Compute the first friction direction of the contact surface. /// \param[in] _centerOfRotation The point around which the track circles ( @@ -100,11 +100,11 @@ class ignition::gazebo::systems::TrackControllerPrivate /// \param[in] _contactWorldPosition Position of the contact point. /// \param[in] _contactNormal Normal of the contact surface (in world coords). /// \param[in] _beltDirection Direction of the belt (in world coords). - public: ignition::math::Vector3d ComputeFrictionDirection( - const ignition::math::Vector3d &_centerOfRotation, - const ignition::math::Vector3d &_contactWorldPosition, - const ignition::math::Vector3d &_contactNormal, - const ignition::math::Vector3d &_beltDirection); + public: math::Vector3d ComputeFrictionDirection( + const math::Vector3d &_centerOfRotation, + const math::Vector3d &_contactWorldPosition, + const math::Vector3d &_contactNormal, + const math::Vector3d &_beltDirection); /// \brief Name of the link to which the track is attached. public: std::string linkName; @@ -342,24 +342,24 @@ void TrackController::Configure(const Entity &_entity, if (this->dataPtr->debug) { this->dataPtr->debugMarker.set_ns(this->dataPtr->linkName + "/friction"); - this->dataPtr->debugMarker.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->debugMarker.set_type(ignition::msgs::Marker::BOX); - this->dataPtr->debugMarker.set_visibility(ignition::msgs::Marker::GUI); + this->dataPtr->debugMarker.set_action(msgs::Marker::ADD_MODIFY); + this->dataPtr->debugMarker.set_type(msgs::Marker::BOX); + this->dataPtr->debugMarker.set_visibility(msgs::Marker::GUI); this->dataPtr->debugMarker.mutable_lifetime()->set_sec(0); this->dataPtr->debugMarker.mutable_lifetime()->set_nsec(4000000); // Set material properties - ignition::msgs::Set( + msgs::Set( this->dataPtr->debugMarker.mutable_material()->mutable_ambient(), - ignition::math::Color(0, 0, 1, 1)); - ignition::msgs::Set( + math::Color(0, 0, 1, 1)); + msgs::Set( this->dataPtr->debugMarker.mutable_material()->mutable_diffuse(), - ignition::math::Color(0, 0, 1, 1)); + math::Color(0, 0, 1, 1)); // Set marker scale - ignition::msgs::Set( + msgs::Set( this->dataPtr->debugMarker.mutable_scale(), - ignition::math::Vector3d(0.3, 0.03, 0.03)); + math::Vector3d(0.3, 0.03, 0.03)); } } @@ -530,7 +530,7 @@ void TrackControllerPrivate::ComputeSurfaceProperties( p += rot.RotateVector( math::Vector3d::UnitX * this->debugMarker.scale().x() / 2); - ignition::msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( + msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( p.X(), p.Y(), p.Z(), rot.Roll(), rot.Pitch(), rot.Yaw())); this->debugMarker.mutable_material()->mutable_diffuse()->set_r( surfaceMotion >= 0 ? 0 : 1); @@ -541,8 +541,8 @@ void TrackControllerPrivate::ComputeSurfaceProperties( ////////////////////////////////////////////////// double TrackControllerPrivate::ComputeSurfaceMotion( - const double _beltSpeed, const ignition::math::Vector3d &_beltDirection, - const ignition::math::Vector3d &_frictionDirection) + const double _beltSpeed, const math::Vector3d &_beltDirection, + const math::Vector3d &_frictionDirection) { // the dot product is the cosine of the angle they // form (because both are unit vectors) @@ -553,11 +553,11 @@ double TrackControllerPrivate::ComputeSurfaceMotion( } ////////////////////////////////////////////////// -ignition::math::Vector3d TrackControllerPrivate::ComputeFrictionDirection( - const ignition::math::Vector3d &_centerOfRotation, - const ignition::math::Vector3d &_contactWorldPosition, - const ignition::math::Vector3d &_contactNormal, - const ignition::math::Vector3d &_beltDirection) +math::Vector3d TrackControllerPrivate::ComputeFrictionDirection( + const math::Vector3d &_centerOfRotation, + const math::Vector3d &_contactWorldPosition, + const math::Vector3d &_contactNormal, + const math::Vector3d &_beltDirection) { if (_centerOfRotation.IsFinite()) { @@ -614,7 +614,7 @@ void TrackControllerPrivate::OnCenterOfRotation(const msgs::Vector3d& _msg) } IGNITION_ADD_PLUGIN(TrackController, - ignition::gazebo::System, + System, TrackController::ISystemConfigure, TrackController::ISystemPreUpdate) diff --git a/src/systems/tracked_vehicle/TrackedVehicle.cc b/src/systems/tracked_vehicle/TrackedVehicle.cc index e175d44329..4b3ecc64e6 100644 --- a/src/systems/tracked_vehicle/TrackedVehicle.cc +++ b/src/systems/tracked_vehicle/TrackedVehicle.cc @@ -60,25 +60,25 @@ class ignition::gazebo::systems::TrackedVehiclePrivate { /// \brief Callback for velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const ignition::msgs::Twist &_msg); + public: void OnCmdVel(const msgs::Twist &_msg); /// \brief Callback for steering efficiency subscription /// \param[in] _msg Steering efficiency message - public: void OnSteeringEfficiency(const ignition::msgs::Double &_msg); + public: void OnSteeringEfficiency(const msgs::Double &_msg); /// \brief Update odometry and publish an odometry message. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); + public: void UpdateOdometry(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); + public: void UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Ignition communication node. public: transport::Node node; @@ -423,31 +423,31 @@ void TrackedVehicle::Configure(const Entity &_entity, { this->dataPtr->debugMarker.set_ns( this->dataPtr->model.Name(_ecm) + "/cor"); - this->dataPtr->debugMarker.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->debugMarker.set_type(ignition::msgs::Marker::SPHERE); - this->dataPtr->debugMarker.set_visibility(ignition::msgs::Marker::GUI); + this->dataPtr->debugMarker.set_action(msgs::Marker::ADD_MODIFY); + this->dataPtr->debugMarker.set_type(msgs::Marker::SPHERE); + this->dataPtr->debugMarker.set_visibility(msgs::Marker::GUI); this->dataPtr->debugMarker.mutable_lifetime()->set_sec(0); this->dataPtr->debugMarker.mutable_lifetime()->set_nsec(4000000); this->dataPtr->debugMarker.set_id(1); // Set material properties - ignition::msgs::Set( + msgs::Set( this->dataPtr->debugMarker.mutable_material()->mutable_ambient(), - ignition::math::Color(0, 0, 1, 1)); - ignition::msgs::Set( + math::Color(0, 0, 1, 1)); + msgs::Set( this->dataPtr->debugMarker.mutable_material()->mutable_diffuse(), - ignition::math::Color(0, 0, 1, 1)); + math::Color(0, 0, 1, 1)); // Set marker scale - ignition::msgs::Set( + msgs::Set( this->dataPtr->debugMarker.mutable_scale(), - ignition::math::Vector3d(0.1, 0.1, 0.1)); + math::Vector3d(0.1, 0.1, 0.1)); } } ////////////////////////////////////////////////// -void TrackedVehicle::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void TrackedVehicle::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("TrackedVehicle::PreUpdate"); @@ -546,8 +546,8 @@ void TrackedVehicle::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void TrackedVehiclePrivate::UpdateOdometry( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { IGN_PROFILE("TrackedVehicle::UpdateOdometry"); // Initialize, if not already initialized. @@ -629,8 +629,8 @@ void TrackedVehiclePrivate::UpdateOdometry( ////////////////////////////////////////////////// void TrackedVehiclePrivate::UpdateVelocity( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { IGN_PROFILE("TrackedVehicle::UpdateVelocity"); @@ -702,7 +702,7 @@ void TrackedVehiclePrivate::UpdateVelocity( const auto bodyPose = worldPose(this->bodyLink, _ecm); const auto bodyYAxisGlobal = - bodyPose.Rot().RotateVector(ignition::math::Vector3d(0, 1, 0)); + bodyPose.Rot().RotateVector(math::Vector3d(0, 1, 0)); // centerOfRotation may be +inf this->centerOfRotation = (bodyYAxisGlobal * desiredRotationRadiusSigned) + bodyPose.Pos(); @@ -742,7 +742,7 @@ void TrackedVehiclePrivate::UpdateVelocity( << ", right v=" << this->rightSpeed << (sendCommandsToTracks ? " (sent to tracks)" : "") << std::endl; - ignition::msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( + msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( this->centerOfRotation.X(), this->centerOfRotation.Y(), this->centerOfRotation.Z(), @@ -761,7 +761,7 @@ void TrackedVehiclePrivate::OnCmdVel(const msgs::Twist &_msg) ////////////////////////////////////////////////// void TrackedVehiclePrivate::OnSteeringEfficiency( - const ignition::msgs::Double& _msg) + const msgs::Double& _msg) { std::lock_guard lock(this->mutex); this->steeringEfficiency = _msg.data(); @@ -769,7 +769,7 @@ void TrackedVehiclePrivate::OnSteeringEfficiency( } IGNITION_ADD_PLUGIN(TrackedVehicle, - ignition::gazebo::System, + System, TrackedVehicle::ISystemConfigure, TrackedVehicle::ISystemPreUpdate, TrackedVehicle::ISystemPostUpdate) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 316177f2d4..f9e79b14fe 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -664,7 +664,7 @@ bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) } IGNITION_ADD_PLUGIN(TriggeredPublisher, - ignition::gazebo::System, + System, TriggeredPublisher::ISystemConfigure) IGNITION_ADD_PLUGIN_ALIAS(TriggeredPublisher, diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 2e8941cce4..b1b12a329d 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -40,26 +40,26 @@ class ignition::gazebo::systems::VelocityControlPrivate { /// \brief Callback for model velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const ignition::msgs::Twist &_msg); + public: void OnCmdVel(const msgs::Twist &_msg); /// \brief Callback for link velocity subscription /// \param[in] _msg Velocity message - public: void OnLinkCmdVel(const ignition::msgs::Twist &_msg, - const ignition::transport::MessageInfo &_info); + public: void OnLinkCmdVel(const msgs::Twist &_msg, + const transport::MessageInfo &_info); /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); + public: void UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Update link velocity. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateLinkVelocity(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); + public: void UpdateLinkVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Ignition communication node. public: transport::Node node; @@ -177,8 +177,8 @@ void VelocityControl::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void VelocityControl::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { IGN_PROFILE("VelocityControl::PreUpdate"); @@ -321,8 +321,8 @@ void VelocityControl::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void VelocityControlPrivate::UpdateVelocity( - const ignition::gazebo::UpdateInfo &/*_info*/, - const ignition::gazebo::EntityComponentManager &/*_ecm*/) + const UpdateInfo &/*_info*/, + const EntityComponentManager &/*_ecm*/) { IGN_PROFILE("VeocityControl::UpdateVelocity"); @@ -333,8 +333,8 @@ void VelocityControlPrivate::UpdateVelocity( ////////////////////////////////////////////////// void VelocityControlPrivate::UpdateLinkVelocity( - const ignition::gazebo::UpdateInfo &/*_info*/, - const ignition::gazebo::EntityComponentManager &/*_ecm*/) + const UpdateInfo &/*_info*/, + const EntityComponentManager &/*_ecm*/) { IGN_PROFILE("VelocityControl::UpdateLinkVelocity"); @@ -372,7 +372,7 @@ void VelocityControlPrivate::OnLinkCmdVel(const msgs::Twist &_msg, } IGNITION_ADD_PLUGIN(VelocityControl, - ignition::gazebo::System, + System, VelocityControl::ISystemConfigure, VelocityControl::ISystemPreUpdate, VelocityControl::ISystemPostUpdate) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index df1dd2c5dd..41826fdb63 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -360,7 +360,7 @@ void WheelSlip::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) } IGNITION_ADD_PLUGIN(WheelSlip, - ignition::gazebo::System, + System, WheelSlip::ISystemConfigure, WheelSlip::ISystemPreUpdate) diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index 215bc57ac4..1a419b6bed 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/src/systems/wind_effects/WindEffects.cc @@ -374,7 +374,7 @@ void WindEffectsPrivate::UpdateWindVelocity(const UpdateInfo &_info, direction = this->noiseDirection->Apply(direction); // Apply wind velocity - ignition::math::Vector3d windVel; + math::Vector3d windVel; windVel.X(magnitude * std::cos(IGN_DTOR(direction))); windVel.Y(magnitude * std::sin(IGN_DTOR(direction))); diff --git a/test/integration/ackermann_steering_system.cc b/test/integration/ackermann_steering_system.cc index a98bf26c0a..6ead175dd1 100644 --- a/test/integration/ackermann_steering_system.cc +++ b/test/integration/ackermann_steering_system.cc @@ -62,8 +62,8 @@ class AckermannSteeringTest test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), @@ -123,8 +123,8 @@ class AckermannSteeringTest const double desiredLinVel = 10.5; const double desiredAngVel = 0.1; velocityRamp.OnPreUpdate( - [&](const gazebo::UpdateInfo &/*_info*/, - const gazebo::EntityComponentManager &) + [&](const UpdateInfo &/*_info*/, + const EntityComponentManager &) { msgs::Set(msg.mutable_linear(), math::Vector3d(desiredLinVel, 0, 0)); @@ -228,8 +228,8 @@ TEST_P(AckermannSteeringTest, SkidPublishCmd) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), @@ -326,8 +326,8 @@ TEST_P(AckermannSteeringTest, TfPublishes) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), diff --git a/test/integration/air_pressure_system.cc b/test/integration/air_pressure_system.cc index 52ad96ab50..367392b08a 100644 --- a/test/integration/air_pressure_system.cc +++ b/test/integration/air_pressure_system.cc @@ -62,11 +62,11 @@ TEST_F(AirPressureTest, AirPressure) // Create a system that checks sensor topic test::Relay testSystem; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::AirPressureSensor *, const components::Name *_name) -> bool { diff --git a/test/integration/altimeter_system.cc b/test/integration/altimeter_system.cc index 42bae54c3e..f5f53fa28b 100644 --- a/test/integration/altimeter_system.cc +++ b/test/integration/altimeter_system.cc @@ -82,13 +82,13 @@ TEST_F(AltimeterTest, IGN_UTILS_TEST_DISABLED_ON_MAC(ModelFalling)) test::Relay testSystem; std::vector poses; std::vector velocities; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::Altimeter *, const components::Name *_name, const components::WorldPose *_worldPose, diff --git a/test/integration/apply_joint_force_system.cc b/test/integration/apply_joint_force_system.cc index c8423e88ae..a2cfcb5ae5 100644 --- a/test/integration/apply_joint_force_system.cc +++ b/test/integration/apply_joint_force_system.cc @@ -69,7 +69,7 @@ TEST_F(ApplyJointForceTestFixture, JointVelocityCommand) test::Relay testSystem; std::vector jointForceCmd; testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto joint = _ecm.EntityByComponents(components::Joint(), components::Name(jointName)); diff --git a/test/integration/battery_plugin.cc b/test/integration/battery_plugin.cc index 53f09d2153..336ef6cab1 100644 --- a/test/integration/battery_plugin.cc +++ b/test/integration/battery_plugin.cc @@ -57,15 +57,15 @@ class BatteryPluginTest : public InternalFixture<::testing::Test> EXPECT_TRUE(plugin.has_value()); this->systemPtr = plugin.value(); - this->mockSystem = static_cast( - systemPtr->QueryInterface()); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); EXPECT_NE(nullptr, this->mockSystem); } - public: ignition::gazebo::SystemPluginPtr systemPtr; - public: gazebo::MockSystem *mockSystem; + public: SystemPluginPtr systemPtr; + public: MockSystem *mockSystem; - private: gazebo::SystemLoader sm; + private: SystemLoader sm; }; @@ -83,9 +83,9 @@ TEST_F(BatteryPluginTest, SingleBattery) serverConfig.SetSdfFile(sdfPath); // A pointer to the ecm. This will be valid once we run the mock system - gazebo::EntityComponentManager *ecm = nullptr; + EntityComponentManager *ecm = nullptr; this->mockSystem->preUpdateCallback = - [&ecm](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&ecm](const UpdateInfo &, EntityComponentManager &_ecm) { ecm = &_ecm; diff --git a/test/integration/breadcrumbs.cc b/test/integration/breadcrumbs.cc index ed79d6eef6..947c2dbde8 100644 --- a/test/integration/breadcrumbs.cc +++ b/test/integration/breadcrumbs.cc @@ -145,8 +145,8 @@ TEST_F(BreadcrumbsTest, DeployAtOffset) node.Advertise("/model/vehicle_blue/breadcrumbs/B1/deploy"); std::size_t iterTestStart = 1000; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Start moving the vehicle // After 1000 iterations, stop the vehicle, spawn a breadcrumb @@ -210,8 +210,8 @@ TEST_F(BreadcrumbsTest, MaxDeployments) node.Advertise("/model/vehicle_blue/breadcrumbs/B1/deploy"); std::size_t iterTestStart = 1000; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Start moving the vehicle // Every 1000 iterations, deploy @@ -268,8 +268,8 @@ TEST_F(BreadcrumbsTest, FuelDeploy) const std::size_t nIters = iterTestStart + 2500; const std::size_t maxDeployments = 5; std::size_t deployCount = 0; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Start moving the vehicle // Every 500 iterations, deploy @@ -322,8 +322,8 @@ TEST_F(BreadcrumbsTest, Performer) const std::size_t nIters = iterTestStart + 10000; std::optional initialPose; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Deploy a performer breadcrumb on a tile that's on a level, and ensure // that it keeps the tile from being unloaded. @@ -395,8 +395,8 @@ TEST_F(BreadcrumbsTest, PerformerSetVolume) const std::size_t nIters = iterTestStart + 2000; std::optional initialPose; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Deploy a performer breadcrumb on a tile that's on the default a level, // and check that it causes tile_1 to be loaded since the performer's volume @@ -448,8 +448,8 @@ TEST_F(BreadcrumbsTest, DeployDisablePhysics) node.Advertise("/model/vehicle_blue/breadcrumbs/B2/deploy"); std::size_t iterTestStart = 1000; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Start moving the vehicle // After 1000 iterations, stop the vehicle, spawn a breadcrumb @@ -549,7 +549,7 @@ TEST_F(BreadcrumbsTest, AllowRenaming) ///////////////////////////////////////////////// /// Return a list of model entities whose names match the given regex std::vector ModelsByNameRegex( - const gazebo::EntityComponentManager &_ecm, const std::regex &_re) + const EntityComponentManager &_ecm, const std::regex &_re) { std::vector entities; _ecm.Each( @@ -584,8 +584,8 @@ TEST_F(BreadcrumbsTest, LevelLoadUnload) std::regex reTile1{"tile_1"}; std::regex reBreadcrumb{"B1_.*"}; testSystem.OnPostUpdate( - [&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Ensure that tile_1 is loaded at the start, deploy a breadcrumb if (_info.iterations == iterTestStart) diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 7acfee7a5e..8d49f83bc8 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -63,8 +63,8 @@ TEST_F(BuoyancyTest, Movement) bool finished = false; test::Relay testSystem; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Check pose Entity submarine = _ecm.EntityByComponents( @@ -99,7 +99,7 @@ TEST_F(BuoyancyTest, Movement) auto submarineCenterOfVolume = _ecm.Component(submarineLink); ASSERT_NE(submarineCenterOfVolume, nullptr); - EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), + EXPECT_EQ(math::Vector3d(0, 0, 0), submarineCenterOfVolume->Data()); // Get the submarine buoyant link @@ -117,7 +117,7 @@ TEST_F(BuoyancyTest, Movement) auto submarineBuoyantCenterOfVolume = _ecm.Component(submarineBuoyantLink); ASSERT_NE(submarineBuoyantCenterOfVolume, nullptr); - EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), + EXPECT_EQ(math::Vector3d(0, 0, 0), submarineBuoyantCenterOfVolume->Data()); // Get the submarine sinking link @@ -135,7 +135,7 @@ TEST_F(BuoyancyTest, Movement) auto submarineSinkingCenterOfVolume = _ecm.Component(submarineSinkingLink); ASSERT_NE(submarineSinkingCenterOfVolume, nullptr); - EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), + EXPECT_EQ(math::Vector3d(0, 0, 0), submarineSinkingCenterOfVolume->Data()); // Get the duck link @@ -151,7 +151,7 @@ TEST_F(BuoyancyTest, Movement) auto duckCenterOfVolume = _ecm.Component(duckLink); ASSERT_NE(duckCenterOfVolume, nullptr); - EXPECT_EQ(ignition::math::Vector3d(0, 0, -0.4), + EXPECT_EQ(math::Vector3d(0, 0, -0.4), duckCenterOfVolume->Data()); auto submarinePose = _ecm.Component(submarine); @@ -205,8 +205,8 @@ TEST_F(BuoyancyTest, OffsetAndRotation) std::size_t iterations{0}; fixture.OnPostUpdate([&]( - const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + const UpdateInfo &, + const EntityComponentManager &_ecm) { // Get links auto noOffsets = entitiesFromScopedName("no_offset::link", _ecm); diff --git a/test/integration/camera_video_record_system.cc b/test/integration/camera_video_record_system.cc index e21b71fc98..13396cb3df 100644 --- a/test/integration/camera_video_record_system.cc +++ b/test/integration/camera_video_record_system.cc @@ -51,7 +51,7 @@ TEST_F(CameraVideoRecorderTest, IGN_UTILS_TEST_DISABLED_ON_MAC(RecordVideo)) // Run server server.Run(true, 1, false); - ignition::transport::Node node; + transport::Node node; std::vector services; bool hasService = false; @@ -75,8 +75,8 @@ TEST_F(CameraVideoRecorderTest, IGN_UTILS_TEST_DISABLED_ON_MAC(RecordVideo)) } EXPECT_TRUE(hasService); - ignition::msgs::VideoRecord videoRecordMsg; - ignition::msgs::Boolean res; + msgs::VideoRecord videoRecordMsg; + msgs::Boolean res; bool result = false; unsigned int timeout = 5000; diff --git a/test/integration/components.cc b/test/integration/components.cc index 8e0bc599dd..a5892d1682 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -109,7 +109,7 @@ TEST_F(ComponentsTest, Actor) comp3.Deserialize(istr); EXPECT_EQ("abc", comp3.Data().Name()); EXPECT_EQ("def", comp3.Data().SkinFilename()); - EXPECT_EQ(ignition::math::Pose3d(3, 2, 1, 0, 0, 0), comp3.Data().RawPose()); + EXPECT_EQ(math::Pose3d(3, 2, 1, 0, 0, 0), comp3.Data().RawPose()); } ///////////////////////////////////////////////// @@ -118,7 +118,7 @@ TEST_F(ComponentsTest, AirPressureSensor) sdf::Sensor data1; data1.SetName("abc"); data1.SetType(sdf::SensorType::AIR_PRESSURE); - data1.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + data1.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); sdf::AirPressure airPressure1; data1.SetAirPressureSensor(airPressure1); @@ -146,7 +146,7 @@ TEST_F(ComponentsTest, AirPressureSensor) comp3.Deserialize(istr); EXPECT_EQ("abc", comp3.Data().Name()); EXPECT_EQ(sdf::SensorType::AIR_PRESSURE, comp3.Data().Type()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); } ///////////////////////////////////////////////// @@ -409,7 +409,7 @@ TEST_F(ComponentsTest, Imu) data1.SetType(sdf::SensorType::IMU); data1.SetUpdateRate(100); data1.SetTopic("imu_data"); - data1.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + data1.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); sdf::Imu imu1; data1.SetImuSensor(imu1); @@ -440,7 +440,7 @@ TEST_F(ComponentsTest, Imu) EXPECT_EQ(sdf::SensorType::IMU, comp3.Data().Type()); EXPECT_EQ("imu_data", comp3.Data().Topic()); EXPECT_DOUBLE_EQ(100, comp3.Data().UpdateRate()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); } ///////////////////////////////////////////////// @@ -1033,7 +1033,7 @@ TEST_F(ComponentsTest, Magnetometer) data1.SetType(sdf::SensorType::MAGNETOMETER); data1.SetUpdateRate(12.4); data1.SetTopic("grape"); - data1.SetRawPose(ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + data1.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); sdf::Magnetometer mag1; data1.SetMagnetometerSensor(mag1); @@ -1063,7 +1063,7 @@ TEST_F(ComponentsTest, Magnetometer) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, comp3.Data().Type()); EXPECT_EQ("grape", comp3.Data().Topic()); EXPECT_DOUBLE_EQ(12.4, comp3.Data().UpdateRate()); - EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); } ///////////////////////////////////////////////// diff --git a/test/integration/depth_camera.cc b/test/integration/depth_camera.cc index b05d0db4ec..3e36c0013f 100644 --- a/test/integration/depth_camera.cc +++ b/test/integration/depth_camera.cc @@ -104,9 +104,9 @@ TEST_F(DepthCameraTest, IGN_UTILS_TEST_DISABLED_ON_MAC(DepthCameraBox)) // Lock access to buffer and don't release it mutex.lock(); - EXPECT_DOUBLE_EQ(depthBuffer[left], ignition::math::INF_D); + EXPECT_DOUBLE_EQ(depthBuffer[left], math::INF_D); EXPECT_NEAR(depthBuffer[mid], expectedRangeAtMidPointBox1, DEPTH_TOL); - EXPECT_DOUBLE_EQ(depthBuffer[right], ignition::math::INF_D); + EXPECT_DOUBLE_EQ(depthBuffer[right], math::INF_D); delete[] depthBuffer; } diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index bbbe91c66f..90feb230aa 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -69,8 +69,8 @@ TEST_F(DetachableJointTest, StartConnected) auto poseRecorder = [](const std::string &_modelName, std::vector &_poses) { - return [&, _modelName](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + return [&, _modelName](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( [&](const Entity &_entity, const components::Model *, @@ -147,8 +147,8 @@ TEST_F(DetachableJointTest, LinksInSameModel) auto poseRecorder = [](const std::string &_linkName, std::vector &_poses) { - return [&, _linkName](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + return [&, _linkName](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( [&](const Entity &_entity, const components::Link *, diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 683eb1d40b..f073fe7ee8 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -65,8 +65,8 @@ class DiffDriveTest : public InternalFixture<::testing::TestWithParam> test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), @@ -128,8 +128,8 @@ class DiffDriveTest : public InternalFixture<::testing::TestWithParam> double desiredLinVel = movementDirection * 10.5; double desiredAngVel = 0.2; velocityRamp.OnPreUpdate( - [&](const gazebo::UpdateInfo &/*_info*/, - const gazebo::EntityComponentManager &) + [&](const UpdateInfo &/*_info*/, + const EntityComponentManager &) { msgs::Set(msg.mutable_linear(), math::Vector3d(desiredLinVel, 0, 0)); @@ -247,8 +247,8 @@ TEST_P(DiffDriveTest, SkidPublishCmd) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), @@ -349,8 +349,8 @@ TEST_P(DiffDriveTest, EnableDisableCmd) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), diff --git a/test/integration/each_new_removed.cc b/test/integration/each_new_removed.cc index 780e210250..f252d1a4fd 100644 --- a/test/integration/each_new_removed.cc +++ b/test/integration/each_new_removed.cc @@ -46,7 +46,7 @@ class EachNewRemovedFixture : public InternalFixture<::testing::Test> ///////////////////////////////////////////////// TEST_F(EachNewRemovedFixture, EachNewEachRemovedInSystem) { - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; gazebo::Server server; diff --git a/test/integration/elevator_system.cc b/test/integration/elevator_system.cc index ab5aa80304..5889143bb5 100644 --- a/test/integration/elevator_system.cc +++ b/test/integration/elevator_system.cc @@ -40,7 +40,7 @@ class ElevatorTestFixture : public ::testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + common::Console::SetVerbosity(4); setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); } diff --git a/test/integration/entity_erase.cc b/test/integration/entity_erase.cc index f75da52514..8590c45fcf 100644 --- a/test/integration/entity_erase.cc +++ b/test/integration/entity_erase.cc @@ -32,12 +32,12 @@ class PhysicsSystemFixture : public InternalFixture<::testing::Test> ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, CreatePhysicsWorld) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); - gazebo::Server server(serverConfig); + Server server(serverConfig); EXPECT_TRUE(server.HasEntity("box")); EXPECT_TRUE(server.HasEntity("cylinder")); EXPECT_TRUE(server.HasEntity("sphere")); diff --git a/test/integration/examples_build.cc b/test/integration/examples_build.cc index bf40f70614..f6ff1db421 100644 --- a/test/integration/examples_build.cc +++ b/test/integration/examples_build.cc @@ -138,14 +138,14 @@ void ExamplesBuild::Build(const std::string &_type) // Path to examples of the given type auto examplesDir = std::string(PROJECT_SOURCE_PATH) + "/examples/" + _type; - ASSERT_TRUE(ignition::common::exists(examplesDir)); + ASSERT_TRUE(common::exists(examplesDir)); // Iterate over directory - ignition::common::DirIter endIter; - for (ignition::common::DirIter dirIter(examplesDir); + common::DirIter endIter; + for (common::DirIter dirIter(examplesDir); dirIter != endIter; ++dirIter) { - auto base = ignition::common::basename(*dirIter); + auto base = common::basename(*dirIter); math::SemanticVersion cmakeVersion{std::string(CMAKE_VERSION)}; if (base == "gtest_setup" && cmakeVersion < math::SemanticVersion(3, 11, 0)) @@ -158,13 +158,13 @@ void ExamplesBuild::Build(const std::string &_type) // Source directory for this example auto sourceDir = examplesDir; sourceDir += "/" + base; - ASSERT_TRUE(ignition::common::exists(sourceDir)); + ASSERT_TRUE(common::exists(sourceDir)); igndbg << "Source: " << sourceDir << std::endl; // Create a temp build directory std::string tmpBuildDir; ASSERT_TRUE(createAndSwitchToTempDir(tmpBuildDir)); - EXPECT_TRUE(ignition::common::exists(tmpBuildDir)); + EXPECT_TRUE(common::exists(tmpBuildDir)); igndbg << "Build directory: " << tmpBuildDir<< std::endl; char cmd[1024]; diff --git a/test/integration/gpu_lidar.cc b/test/integration/gpu_lidar.cc index a1f3368a43..8597db0348 100644 --- a/test/integration/gpu_lidar.cc +++ b/test/integration/gpu_lidar.cc @@ -100,9 +100,9 @@ TEST_F(GpuLidarTest, IGN_UTILS_TEST_DISABLED_ON_MAC(GpuLidarBox)) double expectedRangeAtMidPointBox1 = 0.45; // Sensor 1 should see TestBox1 - EXPECT_DOUBLE_EQ(lastMsg.ranges(0), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(lastMsg.ranges(0), math::INF_D); EXPECT_NEAR(lastMsg.ranges(mid), expectedRangeAtMidPointBox1, LASER_TOL); - EXPECT_DOUBLE_EQ(lastMsg.ranges(last), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(lastMsg.ranges(last), math::INF_D); EXPECT_EQ("gpu_lidar::gpu_lidar_link::gpu_lidar", lastMsg.frame()); } diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index c38c3cf6c7..d8addf7cb1 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -89,15 +89,15 @@ TEST_F(ImuTest, ModelFalling) std::vector poses; std::vector accelerations; std::vector angularVelocities; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::Imu *, const components::Name *_name, const components::WorldPose *_worldPose, @@ -128,7 +128,7 @@ TEST_F(ImuTest, ModelFalling) }); _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::Gravity *_gravity) -> bool { // gtest is having a hard time with ASSERTs inside nested lambdas diff --git a/test/integration/joint_controller_system.cc b/test/integration/joint_controller_system.cc index eedb002305..bfbd1cc6d2 100644 --- a/test/integration/joint_controller_system.cc +++ b/test/integration/joint_controller_system.cc @@ -67,7 +67,7 @@ TEST_F(JointControllerTestFixture, JointVelocityCommand) test::Relay testSystem; std::vector angularVelocities; testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto link = _ecm.EntityByComponents(components::Link(), components::Name(linkName)); @@ -79,12 +79,12 @@ TEST_F(JointControllerTestFixture, JointVelocityCommand) } }); - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::Link *, const components::Name *_name, const components::AngularVelocity *_angularVel) -> bool @@ -164,7 +164,7 @@ TEST_F(JointControllerTestFixture, JointVelocityCommandWithForce) test::Relay testSystem; math::Vector3d angularVelocity; testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto link = _ecm.EntityByComponents(components::Link(), components::Name(linkName)); @@ -176,12 +176,12 @@ TEST_F(JointControllerTestFixture, JointVelocityCommandWithForce) } }); - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::Link *, const components::Name *_name, const components::AngularVelocity *_angularVel) -> bool diff --git a/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index 679293f582..80bd5c71e1 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/test/integration/joint_position_controller_system.cc @@ -68,7 +68,7 @@ TEST_F(JointPositionControllerTestFixture, JointPositionCommand) test::Relay testSystem; std::vector currentPosition; testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto joint = _ecm.EntityByComponents(components::Joint(), components::Name(jointName)); @@ -80,12 +80,12 @@ TEST_F(JointPositionControllerTestFixture, JointPositionCommand) } }); - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_position) -> bool diff --git a/test/integration/level_manager.cc b/test/integration/level_manager.cc index 703f0dc6cd..d0a6536b02 100644 --- a/test/integration/level_manager.cc +++ b/test/integration/level_manager.cc @@ -73,15 +73,15 @@ class ModelMover: public test::Relay poseCmd = std::move(_pose); } - public: gazebo::Entity Id() const + public: Entity Id() const { return entity; } /// \brief Sets the pose component of the entity to the commanded pose. This /// function meant to be called in the preupdate phase - private: void MoveModel(const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + private: void MoveModel(const UpdateInfo &, + EntityComponentManager &_ecm) { if (this->poseCmd) { @@ -93,7 +93,7 @@ class ModelMover: public test::Relay /// \brief Entity to move - private: gazebo::Entity entity; + private: Entity entity; /// \brief Pose command private: std::optional poseCmd; }; @@ -106,7 +106,7 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> { InternalFixture::SetUp(); - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; // Except tile_0, which is on the default level, every tile belongs to a // level. The name of the level corresponds to the tile in its suffix, i.e., @@ -116,12 +116,12 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> serverConfig.SetUseLevels(true); EXPECT_EQ(nullptr, this->server); - this->server = std::make_unique(serverConfig); + this->server = std::make_unique(serverConfig); test::Relay testSystem; // Check entities loaded on the default level - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( [&](const Entity &, const components::Model *, @@ -170,7 +170,7 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> this->server->Run(true, 1, false); } - public: std::unique_ptr server; + public: std::unique_ptr server; public: std::vector loadedModels; public: std::vector unloadedModels; public: std::vector loadedLights; @@ -186,8 +186,8 @@ TEST_F(LevelManagerFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(DefaultLevel)) test::Relay recorder; // Check entities loaded on the default level - recorder.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + recorder.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( [&](const Entity &, const components::DefaultLevel *, diff --git a/test/integration/level_manager_runtime_performers.cc b/test/integration/level_manager_runtime_performers.cc index cb1ffedeb6..902b98cdfa 100644 --- a/test/integration/level_manager_runtime_performers.cc +++ b/test/integration/level_manager_runtime_performers.cc @@ -69,15 +69,15 @@ class ModelMover: public test::Relay poseCmd = std::move(_pose); } - public: gazebo::Entity Id() const + public: Entity Id() const { return entity; } /// \brief Sets the pose component of the entity to the commanded pose. This /// function meant to be called in the preupdate phase - private: void MoveModel(const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + private: void MoveModel(const UpdateInfo &, + EntityComponentManager &_ecm) { if (this->poseCmd) { @@ -89,7 +89,7 @@ class ModelMover: public test::Relay /// \brief Entity to move - private: gazebo::Entity entity; + private: Entity entity; /// \brief Pose command private: std::optional poseCmd; }; @@ -102,7 +102,7 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> { InternalFixture::SetUp(); - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; // Except tile_0, which is on the default level, every tile belongs to a // level. The name of the level corresponds to the tile in its suffix, i.e., @@ -111,7 +111,7 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> "/test/worlds/levels_no_performers.sdf"); serverConfig.SetUseLevels(true); - server = std::make_unique(serverConfig); + server = std::make_unique(serverConfig); // Add in the "box" performer using a service call transport::Node node; @@ -144,8 +144,8 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> test::Relay testSystem; // Check entities loaded on the default level - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { Entity sphere = _ecm.EntityByComponents(components::Name("sphere")); EXPECT_EQ(1u, @@ -197,7 +197,7 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> this->server->Run(true, 1, false); } - public: std::unique_ptr server; + public: std::unique_ptr server; public: std::vector loadedModels; public: std::vector unloadedModels; public: std::vector loadedLights; @@ -212,8 +212,8 @@ TEST_F(LevelManagerFixture, DefaultLevel) test::Relay recorder; // Check entities loaded on the default level - recorder.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + recorder.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( [&](const Entity &, const components::DefaultLevel *, diff --git a/test/integration/lift_drag_system.cc b/test/integration/lift_drag_system.cc index a03100f9cb..65e3b3fe57 100644 --- a/test/integration/lift_drag_system.cc +++ b/test/integration/lift_drag_system.cc @@ -78,7 +78,7 @@ TEST_F(LiftDragTestFixture, VerifyVerticalForce) std::vector linearVelocities; std::vector forces; testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { // Create velocity and acceleration components if they dont't exist. // This signals physics system to populate the component @@ -111,7 +111,7 @@ TEST_F(LiftDragTestFixture, VerifyVerticalForce) const double kp = 100.0; // Set a constant velocity to the prismatic joint testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto joint = _ecm.EntityByComponents(components::Joint(), components::Name(jointName)); @@ -142,8 +142,8 @@ TEST_F(LiftDragTestFixture, VerifyVerticalForce) // drag system. This is needed to capture the wrench set by the lift drag // system. This assumption may not hold when systems are run in parallel. test::Relay wrenchRecorder; - wrenchRecorder.OnPreUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + wrenchRecorder.OnPreUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto bladeLink = _ecm.EntityByComponents(components::Link(), components::Name(bladeName)); diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index 09ca215c93..78a2064eba 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -331,7 +331,7 @@ TEST_F(LogSystemTest, LogDefaults) std::string homeOrig; common::env(IGN_HOMEDIR, homeOrig); std::string homeFake = common::joinPaths(this->logsDir, "default"); - EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeFake.c_str())); + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, homeFake.c_str())); // Test case 1: // No path specified on command line. This does not go through @@ -415,7 +415,7 @@ TEST_F(LogSystemTest, LogDefaults) #endif // Revert environment variable after test is done - EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeOrig.c_str())); + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, homeOrig.c_str())); } ///////////////////////////////////////////////// @@ -471,7 +471,7 @@ TEST_F(LogSystemTest, LogPaths) std::string homeOrig; common::env(IGN_HOMEDIR, homeOrig); std::string homeFake = common::joinPaths(this->logsDir, "default"); - EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeFake.c_str())); + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, homeFake.c_str())); // Store number of files before running auto logPath = common::joinPaths(homeFake.c_str(), ".ignition", "gazebo", @@ -693,7 +693,7 @@ TEST_F(LogSystemTest, LogPaths) #endif // Revert environment variable after test is done - EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeOrig.c_str())); + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, homeOrig.c_str())); this->RemoveLogsDir(); } @@ -1159,7 +1159,7 @@ TEST_F(LogSystemTest, LogOverwrite) std::string homeOrig; common::env(IGN_HOMEDIR, homeOrig); std::string homeFake = common::joinPaths(this->logsDir, "default"); - EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeFake.c_str())); + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, homeFake.c_str())); // Store number of files before running auto logPath = common::joinPaths(homeFake.c_str(), ".ignition", "gazebo", @@ -1222,7 +1222,7 @@ TEST_F(LogSystemTest, LogOverwrite) common::removeAll(timestampPath); // Revert environment variable after test is done - EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeOrig.c_str())); + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, homeOrig.c_str())); // Test case 4: // Path exists, command line --log-overwrite, should overwrite by @@ -1713,7 +1713,7 @@ TEST_F(LogSystemTest, LogResources) std::string homeOrig; common::env(IGN_HOMEDIR, homeOrig); std::string homeFake = common::joinPaths(this->logsDir, "default"); - EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeFake.c_str())); + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, homeFake.c_str())); const std::string recordPath = this->logDir; std::string statePath = common::joinPaths(recordPath, "state.tlog"); @@ -1778,7 +1778,7 @@ TEST_F(LogSystemTest, LogResources) "models", "x2 config 1"))); // Revert environment variable after test is done - EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeOrig.c_str())); + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, homeOrig.c_str())); // Remove artifacts this->RemoveLogsDir(); @@ -1800,7 +1800,7 @@ TEST_F(LogSystemTest, LogTopics) std::string homeOrig; common::env(IGN_HOMEDIR, homeOrig); std::string homeFake = common::joinPaths(this->logsDir, "default"); - EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeFake.c_str())); + EXPECT_TRUE(common::setenv(IGN_HOMEDIR, homeFake.c_str())); const std::string recordPath = this->logDir; std::string statePath = common::joinPaths(recordPath, "state.tlog"); diff --git a/test/integration/logical_audio_sensor_plugin.cc b/test/integration/logical_audio_sensor_plugin.cc index 9af6e220aa..7fed016462 100644 --- a/test/integration/logical_audio_sensor_plugin.cc +++ b/test/integration/logical_audio_sensor_plugin.cc @@ -63,10 +63,10 @@ TEST_F(LogicalAudioTest, LogicalAudioDetections) EXPECT_FALSE(*server.Running(0)); // helper variables for checking the validity of the ECM - const ignition::math::Pose3d sourcePose(0, 0, 0, 0, 0, 0); + const math::Pose3d sourcePose(0, 0, 0, 0, 0, 0); const auto zeroSeconds = std::chrono::seconds(0); - const ignition::math::Pose3d micClosePose(0.5, 0, 0, 0, 0, 0); - const ignition::math::Pose3d micFarPose(0, 0, 0, 0, 0, 0); + const math::Pose3d micClosePose(0.5, 0, 0, 0, 0, 0); + const math::Pose3d micFarPose(0, 0, 0, 0, 0, 0); std::chrono::steady_clock::duration sourceStartTime; bool firstTime{true}; diff --git a/test/integration/logical_camera_system.cc b/test/integration/logical_camera_system.cc index 4abcf00700..f285b1f132 100644 --- a/test/integration/logical_camera_system.cc +++ b/test/integration/logical_camera_system.cc @@ -91,11 +91,11 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) // Create a system that checks sensor topics test::Relay testSystem; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::LogicalCamera *, const components::Name *_name) -> bool { @@ -175,23 +175,23 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) math::Pose3d boxPose(1, 0, 0.5, 0, 0, 0); math::Pose3d sensor1Pose(0.05, 0.05, 0.55, 0, 0, 0); mutex.lock(); - ignition::msgs::LogicalCameraImage img1 = logicalCamera1Msgs.back(); - EXPECT_EQ(sensor1Pose, ignition::msgs::Convert(img1.pose())); + msgs::LogicalCameraImage img1 = logicalCamera1Msgs.back(); + EXPECT_EQ(sensor1Pose, msgs::Convert(img1.pose())); EXPECT_EQ(1, img1.model().size()); EXPECT_EQ(boxName, img1.model(0).name()); - ignition::math::Pose3d boxPoseCamera1Frame = boxPose - sensor1Pose; - EXPECT_EQ(boxPoseCamera1Frame, ignition::msgs::Convert(img1.model(0).pose())); + math::Pose3d boxPoseCamera1Frame = boxPose - sensor1Pose; + EXPECT_EQ(boxPoseCamera1Frame, msgs::Convert(img1.model(0).pose())); mutex.unlock(); // Sensor 2 should see box too - note different sensor pose. math::Pose3d sensor2Pose(0.05, -0.45, 0.55, 0, 0, 0); mutex.lock(); - ignition::msgs::LogicalCameraImage img2 = logicalCamera2Msgs.back(); - EXPECT_EQ(sensor2Pose, ignition::msgs::Convert(img2.pose())); + msgs::LogicalCameraImage img2 = logicalCamera2Msgs.back(); + EXPECT_EQ(sensor2Pose, msgs::Convert(img2.pose())); EXPECT_EQ(1, img2.model().size()); EXPECT_EQ(boxName, img2.model(0).name()); - ignition::math::Pose3d boxPoseCamera2Frame = boxPose - sensor2Pose; - EXPECT_EQ(boxPoseCamera2Frame, ignition::msgs::Convert(img2.model(0).pose())); + math::Pose3d boxPoseCamera2Frame = boxPose - sensor2Pose; + EXPECT_EQ(boxPoseCamera2Frame, msgs::Convert(img2.model(0).pose())); mutex.unlock(); } diff --git a/test/integration/magnetometer_system.cc b/test/integration/magnetometer_system.cc index e17be9b9f7..b057caab4f 100644 --- a/test/integration/magnetometer_system.cc +++ b/test/integration/magnetometer_system.cc @@ -81,13 +81,13 @@ TEST_F(MagnetometerTest, RotatedMagnetometer) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::Magnetometer *, const components::Name *_name, const components::WorldPose *_worldPose) -> bool @@ -128,7 +128,7 @@ TEST_F(MagnetometerTest, RotatedMagnetometer) // Hardcoded SDF values math::Vector3d worldMagneticField(0.94, 0.76, -0.12); - ignition::math::Vector3d field = poses.back().Rot().Inverse().RotateVector( + math::Vector3d field = poses.back().Rot().Inverse().RotateVector( worldMagneticField); mutex.lock(); EXPECT_NEAR(magnetometerMsgs.back().mutable_field_tesla()->x(), diff --git a/test/integration/multicopter.cc b/test/integration/multicopter.cc index f88789a531..8f732edb0a 100644 --- a/test/integration/multicopter.cc +++ b/test/integration/multicopter.cc @@ -79,7 +79,7 @@ TEST_F(MulticopterTest, CommandedMotorSpeed) const std::size_t iterTestStart{100}; const std::size_t nIters{500}; testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, EntityComponentManager &_ecm) { // Create components, if the don't exist, on the first iteration if (_info.iterations == 1) @@ -95,8 +95,8 @@ TEST_F(MulticopterTest, CommandedMotorSpeed) }); testSystem.OnPostUpdate( - [&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Command a motor speed // After nIters iterations, check angular velocity of each of the rotors @@ -144,7 +144,7 @@ TEST_F(MulticopterTest, MulticopterVelocityControl) const std::size_t nIters{2000}; testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, EntityComponentManager &_ecm) { // Create components, if the don't exist, on the first iteration if (_info.iterations == 1) @@ -182,8 +182,8 @@ TEST_F(MulticopterTest, MulticopterVelocityControl) }; testSystem.OnPostUpdate( - [&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { if (!iterTestStart.has_value()) { @@ -251,7 +251,7 @@ TEST_F(MulticopterTest, ModelAndVelocityControlInteraction) auto cmdVel = node.Advertise("/X3/gazebo/command/twist"); testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, EntityComponentManager &_ecm) { // Create components, if the don't exist, on the first iteration if (_info.iterations == 1) @@ -285,8 +285,8 @@ TEST_F(MulticopterTest, ModelAndVelocityControlInteraction) node.Advertise("/X3/gazebo/command/motor_speed"); testSystem.OnPostUpdate( - [&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, + const EntityComponentManager &_ecm) { // Publish a motor speed command { diff --git a/test/integration/network_handshake.cc b/test/integration/network_handshake.cc index c0dbdddb09..3dc0dc9a68 100644 --- a/test/integration/network_handshake.cc +++ b/test/integration/network_handshake.cc @@ -44,8 +44,8 @@ uint64_t testPaused(bool _paused) bool paused = !_paused; uint64_t iterations = 0; - std::function cb = - [&](const ignition::msgs::WorldStatistics &_msg) + std::function cb = + [&](const msgs::WorldStatistics &_msg) { std::unique_lock lock(mutex); paused = _msg.paused(); @@ -173,8 +173,8 @@ TEST_F(NetworkHandshake, IGN_UTILS_TEST_DISABLED_ON_MAC(Updates)) // Subscribe to pose updates, which should come from the primary transport::Node node; std::vector zPos; - std::function cb = - [&](const ignition::msgs::Pose_V &_msg) + std::function cb = + [&](const msgs::Pose_V &_msg) { for (int i = 0; i < _msg.pose().size(); ++i) { diff --git a/test/integration/performer_detector.cc b/test/integration/performer_detector.cc index fc74cc6e3c..de569a7d27 100644 --- a/test/integration/performer_detector.cc +++ b/test/integration/performer_detector.cc @@ -187,8 +187,8 @@ TEST_F(PerformerDetectorTest, HandlesRemovedParentModel) auto server = this->StartServer("/test/worlds/performer_detector.sdf", true); test::Relay testSystem; - testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &_info, - gazebo::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &_info, + EntityComponentManager &_ecm) { Entity vehicle = _ecm.EntityByComponents( components::Model(), components::Name("vehicle_blue")); diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index c9a519084b..1b96ce5d64 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -92,12 +92,12 @@ class PhysicsSystemFixtureWithDart6_10 : public PhysicsSystemFixture ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, CreatePhysicsWorld) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); @@ -113,7 +113,7 @@ TEST_F(PhysicsSystemFixture, CreatePhysicsWorld) ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, FallingObject) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/falling.sdf"; @@ -124,22 +124,22 @@ TEST_F(PhysicsSystemFixture, FallingObject) const sdf::World *world = root.WorldByIndex(0); const sdf::Model *model = world->ModelByIndex(0); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1us); const std::string modelName = "sphere"; - std::vector spherePoses; + std::vector spherePoses; // Create a system that records the poses of the sphere test::Relay testSystem; testSystem.OnPostUpdate( - [modelName, &spherePoses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + [modelName, &spherePoses](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, const components::Model *, + [&](const Entity &, const components::Model *, const components::Name *_name, const components::Pose *_pose)->bool { if (_name->Data() == modelName) { @@ -182,7 +182,7 @@ TEST_F(PhysicsSystemFixture, FallingObject) // must be correct. TEST_F(PhysicsSystemFixture, CanonicalLink) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/canonical.sdf"; @@ -194,7 +194,7 @@ TEST_F(PhysicsSystemFixture, CanonicalLink) serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1us); @@ -203,7 +203,7 @@ TEST_F(PhysicsSystemFixture, CanonicalLink) const sdf::Model *model = world->ModelByIndex(0); - std::unordered_map expectedLinPoses; + std::unordered_map expectedLinPoses; for (auto &linkName : linksToCheck) expectedLinPoses[linkName] = model->LinkByName(linkName)->RawPose(); ASSERT_EQ(3u, expectedLinPoses.size()); @@ -211,14 +211,14 @@ TEST_F(PhysicsSystemFixture, CanonicalLink) // Create a system that records the poses of the links after physics test::Relay testSystem; - std::unordered_map postUpLinkPoses; + std::unordered_map postUpLinkPoses; testSystem.OnPostUpdate( - [&modelName, &postUpLinkPoses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + [&modelName, &postUpLinkPoses](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, const components::Link *, + [&](const Entity &, const components::Link *, const components::Name *_name, const components::Pose *_pose, const components::ParentEntity *_parent)->bool { @@ -253,7 +253,7 @@ TEST_F(PhysicsSystemFixture, CanonicalLink) // Same as the CanonicalLink test, but with a non-default canonical link TEST_F(PhysicsSystemFixture, NonDefaultCanonicalLink) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/nondefault_canonical.sdf"; @@ -265,7 +265,7 @@ TEST_F(PhysicsSystemFixture, NonDefaultCanonicalLink) serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); @@ -274,13 +274,13 @@ TEST_F(PhysicsSystemFixture, NonDefaultCanonicalLink) // Create a system that records the pose of the model. test::Relay testSystem; - std::vector modelPoses; + std::vector modelPoses; testSystem.OnPostUpdate( - [&modelName, &modelPoses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + [&modelName, &modelPoses](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, const components::Model *, + [&](const Entity &, const components::Model *, const components::Name *_name, const components::Pose *_pose)->bool { if (_name->Data() == modelName) @@ -308,7 +308,7 @@ TEST_F(PhysicsSystemFixture, NonDefaultCanonicalLink) // Test physics integration with revolute joints TEST_F(PhysicsSystemFixture, RevoluteJoint) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint.sdf"; @@ -320,7 +320,7 @@ TEST_F(PhysicsSystemFixture, RevoluteJoint) serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1us); @@ -337,11 +337,11 @@ TEST_F(PhysicsSystemFixture, RevoluteJoint) // arm is in its initial position. The minimum distance is when the arm is in // line with the support arm. testSystem.OnPostUpdate( - [&rotatingLinkName, &armDistances](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + [&rotatingLinkName, &armDistances](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, const components::Link *, + [&](const Entity &, const components::Link *, const components::Name *_name, const components::Pose *_pose)->bool { if (rotatingLinkName == _name->Data()) @@ -387,8 +387,8 @@ TEST_F(PhysicsSystemFixture, RevoluteJoint) ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, CreateRuntime) { - ignition::gazebo::ServerConfig serverConfig; - gazebo::Server server(serverConfig); + ServerConfig serverConfig; + Server server(serverConfig); server.SetPaused(false); // Create a system just to get the ECM @@ -398,8 +398,8 @@ TEST_F(PhysicsSystemFixture, CreateRuntime) // shared pointer owned by the SimulationRunner. EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -469,31 +469,31 @@ TEST_F(PhysicsSystemFixture, CreateRuntime) ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, SetFrictionCoefficient) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/friction.sdf"; serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); std::map boxParams{ {"box1", 0.01}, {"box2", 0.1}, {"box3", 1.0}}; - std::map> poses; + std::map> poses; // Create a system that records the poses of the 3 boxes test::Relay testSystem; double dt = 0.0; testSystem.OnPostUpdate( - [&boxParams, &poses, &dt](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + [&boxParams, &poses, &dt](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { dt = _info.dt.count(); _ecm.Each( - [&](const ignition::gazebo::Entity &, const components::Model *, + [&](const Entity &, const components::Model *, const components::Name *_name, const components::Pose *_pose)->bool { if (boxParams.find(_name->Data()) != boxParams.end()) { @@ -556,23 +556,23 @@ TEST_F(PhysicsSystemFixture, SetFrictionCoefficient) /// Test that joint position reported by the physics system include all axes TEST_F(PhysicsSystemFixture, MultiAxisJointPosition) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/demo_joint_types.sdf"; serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(0ns); test::Relay testSystem; // Create JointPosition components if they don't already exist testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, components::Joint *) -> bool { auto posComp = _ecm.Component(_entity); @@ -587,10 +587,10 @@ TEST_F(PhysicsSystemFixture, MultiAxisJointPosition) std::map jointPosDof; testSystem.OnPostUpdate( - [&](const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_jointPos) -> bool @@ -635,7 +635,7 @@ TEST_F(PhysicsSystemFixture, MultiAxisJointPosition) /// Test joint position reset component TEST_F(PhysicsSystemFixture, ResetPositionComponent) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint.sdf"; @@ -647,7 +647,7 @@ TEST_F(PhysicsSystemFixture, ResetPositionComponent) serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ms); @@ -661,10 +661,10 @@ TEST_F(PhysicsSystemFixture, ResetPositionComponent) bool firstRun = true; testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::Joint *, components::Name *_name) -> bool { EXPECT_NE(nullptr, _name); @@ -699,11 +699,11 @@ TEST_F(PhysicsSystemFixture, ResetPositionComponent) std::vector positions; testSystem.OnPostUpdate([&]( - const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) + const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_pos) @@ -734,7 +734,7 @@ TEST_F(PhysicsSystemFixture, ResetPositionComponent) /// Test joint veocity reset component TEST_F(PhysicsSystemFixture, ResetVelocityComponent) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint.sdf"; @@ -746,7 +746,7 @@ TEST_F(PhysicsSystemFixture, ResetVelocityComponent) serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ms); @@ -760,10 +760,10 @@ TEST_F(PhysicsSystemFixture, ResetVelocityComponent) bool firstRun = true; testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::Joint *, components::Name *_name) -> bool { if (_name->Data() == rotatingJointName) @@ -796,12 +796,12 @@ TEST_F(PhysicsSystemFixture, ResetVelocityComponent) std::vector velocities; testSystem.OnPostUpdate([&]( - const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) + const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointVelocity *_vel) @@ -832,7 +832,7 @@ TEST_F(PhysicsSystemFixture, ResetVelocityComponent) /// Test joint position limit command component TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint.sdf"; @@ -844,7 +844,7 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ms); @@ -865,10 +865,10 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) // commands do not break the positional limit. testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::Joint *, components::Name *_name) -> bool { if (_name->Data() == rotatingJointName) @@ -921,12 +921,12 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) std::vector positions; testSystem.OnPostUpdate([&]( - const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) + const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_pos) @@ -955,7 +955,7 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) /// Test joint velocity limit command component TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint.sdf"; @@ -967,7 +967,7 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ms); @@ -988,10 +988,10 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) // commands do not break the velocity limit. testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::Joint *, components::Name *_name) -> bool { if (_name->Data() == rotatingJointName) @@ -1044,12 +1044,12 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) std::vector velocities; testSystem.OnPostUpdate([&]( - const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) + const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointVelocity *_vel) @@ -1079,7 +1079,7 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) /// Test joint effort limit command component TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint_equilibrium.sdf"; @@ -1091,7 +1091,7 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ms); @@ -1112,10 +1112,10 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) // commands do not break the effort limit. testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, + [&](const Entity &_entity, const components::Joint *, components::Name *_name) -> bool { if (_name->Data() == rotatingJointName) @@ -1173,12 +1173,12 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) std::vector positions; testSystem.OnPostUpdate([&]( - const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) + const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_pos) @@ -1206,28 +1206,28 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, GetBoundingBox) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/contact.sdf"; serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); // a map of model name to its axis aligned box - std::map bbox; + std::map bbox; // Create a system that records the bounding box of a model test::Relay testSystem; testSystem.OnPreUpdate( - [&](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, + EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, const components::Model *, + [&](const Entity &_entity, const components::Model *, const components::Name *_name, const components::Static *)->bool { // create axis aligned box to be filled by physics @@ -1245,13 +1245,13 @@ TEST_F(PhysicsSystemFixture, GetBoundingBox) }); testSystem.OnPostUpdate( - [&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + [&](const UpdateInfo &, + const EntityComponentManager &_ecm) { // store models that have axis aligned box computed _ecm.Each( - [&](const ignition::gazebo::Entity &, const components::Model *, + [&](const Entity &, const components::Model *, const components::Name *_name, const components::Static *, const components::AxisAlignedBox *_aabb)->bool { @@ -1266,9 +1266,9 @@ TEST_F(PhysicsSystemFixture, GetBoundingBox) EXPECT_EQ(1u, bbox.size()); EXPECT_EQ("box1", bbox.begin()->first); - EXPECT_EQ(ignition::math::AxisAlignedBox( - ignition::math::Vector3d(-1.25, -2, 0), - ignition::math::Vector3d(-0.25, 2, 1)), + EXPECT_EQ(math::AxisAlignedBox( + math::Vector3d(-1.25, -2, 0), + math::Vector3d(-0.25, 2, 1)), bbox.begin()->second); } @@ -1277,7 +1277,7 @@ TEST_F(PhysicsSystemFixture, GetBoundingBox) // This tests whether nested models can be loaded correctly TEST_F(PhysicsSystemFixture, NestedModel) { - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/nested_model.sdf"; @@ -1289,22 +1289,22 @@ TEST_F(PhysicsSystemFixture, NestedModel) serverConfig.SetSdfFile(sdfFile); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1us); // Create a system that records the poses of the links after physics test::Relay testSystem; - std::unordered_map postUpModelPoses; - std::unordered_map postUpLinkPoses; + std::unordered_map postUpModelPoses; + std::unordered_map postUpLinkPoses; std::unordered_map parents; testSystem.OnPostUpdate( - [&postUpModelPoses, &postUpLinkPoses, &parents](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + [&postUpModelPoses, &postUpLinkPoses, &parents](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_entity, const components::Model *, + [&](const Entity &_entity, const components::Model *, const components::Name *_name, const components::Pose *_pose)->bool { // store model pose @@ -1323,7 +1323,7 @@ TEST_F(PhysicsSystemFixture, NestedModel) _ecm.Each( - [&](const ignition::gazebo::Entity &, const components::Link *, + [&](const Entity &, const components::Link *, const components::Name *_name, const components::Pose *_pose, const components::ParentEntity *_parent)->bool { diff --git a/test/integration/play_pause.cc b/test/integration/play_pause.cc index 92a9e6d398..ed7b2241f3 100644 --- a/test/integration/play_pause.cc +++ b/test/integration/play_pause.cc @@ -36,13 +36,13 @@ uint64_t kIterations; // Send a world control message. void worldControl(bool _paused, uint64_t _steps) { - std::function cb = - [&](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [&](const msgs::Boolean &/*_rep*/, const bool _result) { EXPECT_TRUE(_result); }; - ignition::msgs::WorldControl req; + msgs::WorldControl req; req.set_pause(_paused); req.set_multi_step(_steps); transport::Node node; @@ -58,8 +58,8 @@ void testPaused(bool _paused) transport::Node node; bool paused = !_paused; - std::function cb = - [&](const ignition::msgs::WorldStatistics &_msg) + std::function cb = + [&](const msgs::WorldStatistics &_msg) { std::unique_lock lock(mutex); paused = _msg.paused(); @@ -81,8 +81,8 @@ uint64_t iterations() transport::Node node; uint64_t iterations = 0; - std::function cb = - [&](const ignition::msgs::WorldStatistics &_msg) + std::function cb = + [&](const msgs::WorldStatistics &_msg) { std::unique_lock lock(mutex); iterations = _msg.iterations(); diff --git a/test/integration/pose_publisher_system.cc b/test/integration/pose_publisher_system.cc index 39a5f84792..5c4ff5df51 100644 --- a/test/integration/pose_publisher_system.cc +++ b/test/integration/pose_publisher_system.cc @@ -142,8 +142,8 @@ TEST_F(PosePublisherTest, PublishCmd) [&modelName, &baseName, &lowerLinkName, &upperLinkName, &sensorName, &poses, &basePoses, &lowerLinkPoses, &upperLinkPoses, &sensorPoses, ×tamps]( - const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // get our double pendulum model auto id = _ecm.EntityByComponents( @@ -190,7 +190,7 @@ TEST_F(PosePublisherTest, PublishCmd) // timestamps auto simTimeSecNsec = - ignition::math::durationToSecNsec(_info.simTime); + math::durationToSecNsec(_info.simTime); timestamps.push_back( common::Time(simTimeSecNsec.first, simTimeSecNsec.second)); }); @@ -237,7 +237,7 @@ TEST_F(PosePublisherTest, PublishCmd) // sort the pose msgs according to timestamp std::sort(poseMsgs.begin(), poseMsgs.end(), []( - const ignition::msgs::Pose &_l, const ignition::msgs::Pose &_r) + const msgs::Pose &_l, const msgs::Pose &_r) { common::Time lt(_l.header().stamp().sec(), _l.header().stamp().nsec()); common::Time rt(_r.header().stamp().sec(), _r.header().stamp().nsec()); @@ -427,8 +427,8 @@ TEST_F(PosePublisherTest, StaticPosePublisher) [&modelName, &baseName, &lowerLinkName, &upperLinkName, &sensorName, &poses, &basePoses, &lowerLinkPoses, &upperLinkPoses, &sensorPoses, ×tamps, &staticPoseTimestamps]( - const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // get our double pendulum model auto id = _ecm.EntityByComponents( @@ -475,7 +475,7 @@ TEST_F(PosePublisherTest, StaticPosePublisher) // timestamps auto simTimeSecNsec = - ignition::math::durationToSecNsec(_info.simTime); + math::durationToSecNsec(_info.simTime); timestamps.push_back( common::Time(simTimeSecNsec.first, simTimeSecNsec.second)); staticPoseTimestamps.push_back( diff --git a/test/integration/rgbd_camera.cc b/test/integration/rgbd_camera.cc index 2bd7a8e0f8..6194e3b564 100644 --- a/test/integration/rgbd_camera.cc +++ b/test/integration/rgbd_camera.cc @@ -104,9 +104,9 @@ TEST_F(RgbdCameraTest, IGN_UTILS_TEST_DISABLED_ON_MAC(RgbdCameraBox)) // Lock access to buffer and don't release it mutex.lock(); - EXPECT_DOUBLE_EQ(depthBuffer[left], ignition::math::INF_D); + EXPECT_DOUBLE_EQ(depthBuffer[left], math::INF_D); EXPECT_NEAR(depthBuffer[mid], expectedRangeAtMidPointBox1, DEPTH_TOL); - EXPECT_DOUBLE_EQ(depthBuffer[right], ignition::math::INF_D); + EXPECT_DOUBLE_EQ(depthBuffer[right], math::INF_D); delete[] depthBuffer; } diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 00e5717c2c..c7900ef847 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -41,7 +41,7 @@ class SceneBroadcasterTest TEST_P(SceneBroadcasterTest, PoseInfo) { // Start server - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -91,7 +91,7 @@ TEST_P(SceneBroadcasterTest, PoseInfo) TEST_P(SceneBroadcasterTest, SceneInfo) { // Start server - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -108,7 +108,7 @@ TEST_P(SceneBroadcasterTest, SceneInfo) bool result{false}; unsigned int timeout{5000}; - ignition::msgs::Scene res; + msgs::Scene res; EXPECT_TRUE(node.Request("/world/default/scene/info", timeout, res, result)); EXPECT_TRUE(result); @@ -126,7 +126,7 @@ TEST_P(SceneBroadcasterTest, SceneInfo) } // Repeat the request to make sure the same information is returned - ignition::msgs::Scene res2; + msgs::Scene res2; EXPECT_TRUE(node.Request("/world/default/scene/info", timeout, res2, result)); EXPECT_TRUE(result); @@ -137,7 +137,7 @@ TEST_P(SceneBroadcasterTest, SceneInfo) TEST_P(SceneBroadcasterTest, SceneGraph) { // Start server - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -154,7 +154,7 @@ TEST_P(SceneBroadcasterTest, SceneGraph) bool result{false}; unsigned int timeout{5000}; - ignition::msgs::StringMsg res; + msgs::StringMsg res; EXPECT_TRUE(node.Request("/world/default/scene/graph", timeout, res, result)); EXPECT_TRUE(result); @@ -177,7 +177,7 @@ TEST_P(SceneBroadcasterTest, SceneGraph) TEST_P(SceneBroadcasterTest, SceneTopic) { // Start server - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -209,7 +209,7 @@ TEST_P(SceneBroadcasterTest, SceneTopic) bool result{false}; unsigned int timeout{5000}; - ignition::msgs::Scene msg; + msgs::Scene msg; EXPECT_TRUE(node.Request("/world/default/scene/info", timeout, msg, result)); EXPECT_TRUE(result); @@ -221,7 +221,7 @@ TEST_P(SceneBroadcasterTest, SceneTopic) TEST_P(SceneBroadcasterTest, SceneTopicSensors) { // Start server - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/altimeter_with_pose.sdf"); @@ -253,7 +253,7 @@ TEST_P(SceneBroadcasterTest, SceneTopicSensors) bool result{false}; unsigned int timeout{5000}; - ignition::msgs::Scene msg; + msgs::Scene msg; EXPECT_TRUE(node.Request("/world/altimeter_sensor/scene/info", timeout, msg, result)); @@ -272,7 +272,7 @@ TEST_P(SceneBroadcasterTest, SceneTopicSensors) TEST_P(SceneBroadcasterTest, DeletedTopic) { // Start server - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -333,7 +333,7 @@ TEST_P(SceneBroadcasterTest, DeletedTopic) TEST_P(SceneBroadcasterTest, SpawnedModel) { // Start server - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -379,8 +379,8 @@ TEST_P(SceneBroadcasterTest, SpawnedModel) // Check that the model is in the scene/infor response { - ignition::msgs::Empty req; - ignition::msgs::Scene rep; + msgs::Empty req; + msgs::Scene rep; bool result; unsigned int timeout = 2000; EXPECT_TRUE(node.Request("/world/default/scene/info", req, timeout, @@ -403,7 +403,7 @@ TEST_P(SceneBroadcasterTest, SpawnedModel) TEST_P(SceneBroadcasterTest, State) { // Start server - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -494,7 +494,7 @@ TEST_P(SceneBroadcasterTest, State) std::string reqSrv = "/state_async_callback_test"; node.Advertise(reqSrv, cbAsync); - ignition::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(reqSrv); node.Request("/world/default/state_async", req); @@ -514,7 +514,7 @@ TEST_P(SceneBroadcasterTest, State) TEST_P(SceneBroadcasterTest, StateStatic) { // Start server - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/empty.sdf"); diff --git a/test/integration/sdf_frame_semantics.cc b/test/integration/sdf_frame_semantics.cc index 9ca7890669..d8ac4ccdf0 100644 --- a/test/integration/sdf_frame_semantics.cc +++ b/test/integration/sdf_frame_semantics.cc @@ -60,7 +60,7 @@ class SdfFrameSemanticsTest : public InternalFixture<::testing::Test> EXPECT_FALSE(*this->server->Running(0)); // A pointer to the ecm. This will be valid once we run the mock system relay->OnPreUpdate( - [this](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [this](const UpdateInfo &, EntityComponentManager &_ecm) { this->ecm = &_ecm; }); @@ -88,9 +88,9 @@ class SdfFrameSemanticsTest : public InternalFixture<::testing::Test> this->creator->SetParent(modelEntity, worldEntity); } - public: gazebo::Model GetModel(const std::string &_name) + public: Model GetModel(const std::string &_name) { - return gazebo::Model(this->ecm->EntityByComponents( + return Model(this->ecm->EntityByComponents( components::Model(), components::Name(_name))); } @@ -145,8 +145,8 @@ TEST_F(SdfFrameSemanticsTest, LinkRelativeTo) EXPECT_NE(link2, kNullEntity); // Expect the pose of L2 relative to model to be 0 0 1 0 0 pi - ignition::math::Pose3d expRelPose(0, 0, 1, 0, 0, IGN_PI); - ignition::math::Pose3d expWorldPose(0, 0, 3, 0, 0, IGN_PI); + math::Pose3d expRelPose(0, 0, 1, 0, 0, IGN_PI); + math::Pose3d expWorldPose(0, 0, 3, 0, 0, IGN_PI); EXPECT_EQ(expRelPose, this->GetPose(link2)); @@ -212,7 +212,7 @@ TEST_F(SdfFrameSemanticsTest, JointRelativeTo) // Expect the pose of J1 relative to model to be the same as L2 (default // behavior) - ignition::math::Pose3d expWorldPose(1, 0, 2, 0, 0, 0); + math::Pose3d expWorldPose(1, 0, 2, 0, 0, 0); // Expect the pose of J2 relative to model to be the same as L2 (non default // behavior due to "relative_to='L2'") @@ -267,7 +267,7 @@ TEST_F(SdfFrameSemanticsTest, VisualCollisionRelativeTo) // Expect the pose of v1 and relative to L2 (their parent link) to be the same // as the pose of L1 relative to L2 - ignition::math::Pose3d expPose(0, 0, -1, 0, 0, 0); + math::Pose3d expPose(0, 0, -1, 0, 0, 0); EXPECT_EQ(expPose, this->GetPose(visual)); EXPECT_EQ(expPose, this->GetPose(collision)); @@ -315,13 +315,13 @@ TEST_F(SdfFrameSemanticsTest, ExplicitFramesWithLinks) // Expect the pose of L1 and relative to M to be the same // as the pose of F1 relative to M - ignition::math::Pose3d link1ExpRelativePose(0, 0, 1, 0, 0, 0); + math::Pose3d link1ExpRelativePose(0, 0, 1, 0, 0, 0); EXPECT_EQ(link1ExpRelativePose, this->GetPose(link1)); // Expect the pose of L2 and relative to M to be the same // as the pose of F2, which is at the origin of M - ignition::math::Pose3d link2ExpRelativePose = ignition::math::Pose3d::Zero; + math::Pose3d link2ExpRelativePose = math::Pose3d::Zero; EXPECT_EQ(link2ExpRelativePose, this->GetPose(link2)); @@ -370,7 +370,7 @@ TEST_F(SdfFrameSemanticsTest, ExplicitFramesWithJoints) this->server->Run(true, 1, false); // Expect the pose of J1 relative to model to be the same as F1 in world - ignition::math::Pose3d expWorldPose(1, 0, 2, 0, 0, 0); + math::Pose3d expWorldPose(1, 0, 2, 0, 0, 0); // TODO(anyone) Enable the following expectation when a joint's WorldPose // can be computed by ign-physics. // EXPECT_EQ(expWorldPose, this->GetWorldPose(joint1)); @@ -421,7 +421,7 @@ TEST_F(SdfFrameSemanticsTest, ExplicitFramesWithVisualAndCollision) // Expect the pose of v1 and relative to L1 (their parent link) to be the same // as the pose of F1 relative to L1 - ignition::math::Pose3d expPose(0, 0, 1, 0, 0, 0); + math::Pose3d expPose(0, 0, 1, 0, 0, 0); EXPECT_EQ(expPose, this->GetPose(visual)); EXPECT_EQ(expPose, this->GetPose(collision)); diff --git a/test/integration/sdf_include.cc b/test/integration/sdf_include.cc index 7a19d30b5f..37b4c86b57 100644 --- a/test/integration/sdf_include.cc +++ b/test/integration/sdf_include.cc @@ -37,11 +37,11 @@ TEST_F(SdfInclude, DownloadFromFuel) std::string path = common::cwd() + "/test_cache"; // Configure the gazebo server, which will cause a model to be downloaded. - gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetResourceCache(path); serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/include.sdf"); - gazebo::Server server(serverConfig); + Server server(serverConfig); EXPECT_TRUE(common::exists(path + "/fuel.ignitionrobotics.org/openrobotics/models/ground plane" + diff --git a/test/integration/sensors_system.cc b/test/integration/sensors_system.cc index 6165ee9620..29cb445733 100644 --- a/test/integration/sensors_system.cc +++ b/test/integration/sensors_system.cc @@ -62,7 +62,7 @@ class SensorsFixture : public InternalFixture> systemPtr->QueryInterface()); } - public: ignition::gazebo::SystemPluginPtr systemPtr; + public: gazebo::SystemPluginPtr systemPtr; public: gazebo::MockSystem *mockSystem; private: gazebo::SystemLoader sm; @@ -113,7 +113,7 @@ void testDefaultTopics() /// are removed and then added back TEST_F(SensorsFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleRemovedEntities)) { - ignition::gazebo::ServerConfig serverConfig; + gazebo::ServerConfig serverConfig; const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/sensor.sdf"; diff --git a/test/integration/thermal_system.cc b/test/integration/thermal_system.cc index 3dac35cc2c..f7d5f0a0df 100644 --- a/test/integration/thermal_system.cc +++ b/test/integration/thermal_system.cc @@ -57,11 +57,11 @@ TEST_F(ThermalTest, TemperatureComponent) test::Relay testSystem; std::map entityTemp; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const ignition::gazebo::Entity &_id, + [&](const Entity &_id, const components::Temperature *_temp, const components::Name *_name) -> bool { diff --git a/test/integration/tracked_vehicle_system.cc b/test/integration/tracked_vehicle_system.cc index ea53742d3b..2677d265d8 100644 --- a/test/integration/tracked_vehicle_system.cc +++ b/test/integration/tracked_vehicle_system.cc @@ -89,7 +89,7 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> << "Failed to find plugin [" << *pluginLib << "]"; // Load engine plugin - ignition::plugin::Loader pluginLoader; + plugin::Loader pluginLoader; auto plugins = pluginLoader.LoadLib(pathToLib); ASSERT_FALSE(plugins.empty()) << "Unable to load the [" << pathToLib << "] library"; @@ -132,8 +132,8 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> // Create a system that records the vehicle poses test::Relay ecmGetterSystem; EntityComponentManager* ecm {nullptr}; - ecmGetterSystem.OnPreUpdate([&ecm](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + ecmGetterSystem.OnPreUpdate([&ecm](const UpdateInfo &, + EntityComponentManager &_ecm) { if (ecm == nullptr) ecm = &_ecm; @@ -155,8 +155,8 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> test::Relay testSystem; Entity modelEntity {kNullEntity}; std::vector poses; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { modelEntity = _ecm.EntityByComponents( components::Model(), @@ -252,7 +252,7 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> poses.clear(); - gazebo::Model model(modelEntity); + Model model(modelEntity); // Move the robot somewhere to free space without obstacles. model.SetWorldPoseCmd(*ecm, math::Pose3d(10, 10, 0.1, 0, 0, 0)); @@ -454,8 +454,8 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> // Create a system that records the vehicle poses test::Relay ecmGetterSystem; EntityComponentManager* ecm {nullptr}; - ecmGetterSystem.OnPreUpdate([&ecm](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + ecmGetterSystem.OnPreUpdate([&ecm](const UpdateInfo &, + EntityComponentManager &_ecm) { if (ecm == nullptr) ecm = &_ecm; @@ -477,8 +477,8 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> test::Relay testSystem; Entity boxEntity {kNullEntity}; std::vector poses; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { boxEntity = _ecm.EntityByComponents( components::Model(), diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 63dee21695..2bbab80fc2 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -66,8 +66,8 @@ TEST_F(UserCommandsTest, Create) // shared pointer owned by the SimulationRunner. EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -328,8 +328,8 @@ TEST_F(UserCommandsTest, Remove) // shared pointer owned by the SimulationRunner. EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -512,8 +512,8 @@ TEST_F(UserCommandsTest, Pose) // Create a system just to get the ECM EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -686,8 +686,8 @@ TEST_F(UserCommandsTest, PoseVector) // Create a system just to get the ECM EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -760,8 +760,8 @@ TEST_F(UserCommandsTest, Physics) // Create a system just to get the ECM EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); diff --git a/test/integration/velocity_control_system.cc b/test/integration/velocity_control_system.cc index 1b67309190..1482d261ca 100644 --- a/test/integration/velocity_control_system.cc +++ b/test/integration/velocity_control_system.cc @@ -58,8 +58,8 @@ class VelocityControlTest test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), @@ -145,8 +145,8 @@ class VelocityControlTest std::vector modelPoses; std::vector linkPoses; testSystem.OnPostUpdate([&linkPoses, &modelPoses]( - const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + const UpdateInfo &, + const EntityComponentManager &_ecm) { auto modelId = _ecm.EntityByComponents( components::Model(), diff --git a/test/integration/wheel_slip.cc b/test/integration/wheel_slip.cc index 6e931b02ea..831d3be5a1 100644 --- a/test/integration/wheel_slip.cc +++ b/test/integration/wheel_slip.cc @@ -81,7 +81,7 @@ class WheelSlipTest : public InternalFixture<::testing::Test> public: double drumSpeed = 0.0; /// \brief Steer angle to apply. - public: ignition::math::Angle steer; + public: math::Angle steer; /// \brief Suspension force to apply in N. public: double suspForce = 0.0; @@ -118,10 +118,10 @@ TEST_F(WheelSlipTest, TireDrum) EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); - gazebo::EntityComponentManager *ecm = nullptr; + EntityComponentManager *ecm = nullptr; test::Relay testSystem; - testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -139,7 +139,7 @@ TEST_F(WheelSlipTest, TireDrum) Entity worldEntity = ecm->EntityByComponents(components::World()); - EXPECT_NE(gazebo::kNullEntity, worldEntity); + EXPECT_NE(kNullEntity, worldEntity); // Get both models Entity tireEntity = @@ -150,8 +150,8 @@ TEST_F(WheelSlipTest, TireDrum) ecm->EntityByComponents(components::Model(), components::Name("drum")); - EXPECT_NE(gazebo::kNullEntity, tireEntity); - EXPECT_NE(gazebo::kNullEntity, drumEntity); + EXPECT_NE(kNullEntity, tireEntity); + EXPECT_NE(kNullEntity, drumEntity); Entity wheelLinkEntity = ecm->EntityByComponents( components::ParentEntity(tireEntity), @@ -163,8 +163,8 @@ TEST_F(WheelSlipTest, TireDrum) components::Name("link"), components::Link()); - EXPECT_NE(gazebo::kNullEntity, wheelLinkEntity); - EXPECT_NE(gazebo::kNullEntity, drumLinkEntity); + EXPECT_NE(kNullEntity, wheelLinkEntity); + EXPECT_NE(kNullEntity, drumLinkEntity); auto wheelInertialComp = ecm->Component(wheelLinkEntity); @@ -191,8 +191,8 @@ TEST_F(WheelSlipTest, TireDrum) components::Name("collision"), components::Collision()); - EXPECT_NE(gazebo::kNullEntity, collisionWheelLinkEntity); - EXPECT_NE(gazebo::kNullEntity, collisionDrumLinkEntity); + EXPECT_NE(kNullEntity, collisionWheelLinkEntity); + EXPECT_NE(kNullEntity, collisionDrumLinkEntity); auto wheelCollisionComp = ecm->Component(collisionWheelLinkEntity); @@ -245,7 +245,7 @@ TEST_F(WheelSlipTest, TireDrum) components::ParentEntity(tireEntity), components::Name(linkName), components::Link()); - EXPECT_NE(gazebo::kNullEntity, linkEntity); + EXPECT_NE(kNullEntity, linkEntity); auto inertialComp = ecm->Component(linkEntity); EXPECT_NE(nullptr, inertialComp); @@ -259,14 +259,14 @@ TEST_F(WheelSlipTest, TireDrum) components::Name("axle_wheel"), components::Joint()); - ASSERT_NE(gazebo::kNullEntity, wheelAxleJointEntity); + ASSERT_NE(kNullEntity, wheelAxleJointEntity); Entity wheelSteerJointEntity = ecm->EntityByComponents( components::ParentEntity(tireEntity), components::Name("steer"), components::Joint()); - ASSERT_NE(gazebo::kNullEntity, wheelSteerJointEntity); + ASSERT_NE(kNullEntity, wheelSteerJointEntity); const double wheelSpeed = -25.0 * metersPerMile / secondsPerHour / wheelRadius; @@ -380,10 +380,10 @@ TEST_F(WheelSlipTest, TricyclesUphill) EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); - gazebo::EntityComponentManager *ecm = nullptr; + EntityComponentManager *ecm = nullptr; test::Relay testSystem; - testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -401,7 +401,7 @@ TEST_F(WheelSlipTest, TricyclesUphill) Entity worldEntity = ecm->EntityByComponents(components::World()); - EXPECT_NE(gazebo::kNullEntity, worldEntity); + EXPECT_NE(kNullEntity, worldEntity); auto gravity = ecm->Component(worldEntity); @@ -413,14 +413,14 @@ TEST_F(WheelSlipTest, TricyclesUphill) ecm->EntityByComponents(components::Model(), components::Name("trisphere_cycle0")); - EXPECT_NE(gazebo::kNullEntity, trisphereCycle0Entity); + EXPECT_NE(kNullEntity, trisphereCycle0Entity); Entity trisphereCycle1Entity = ecm->EntityByComponents(components::Model(), components::Name("trisphere_cycle1")); - EXPECT_NE(gazebo::kNullEntity, trisphereCycle1Entity); + EXPECT_NE(kNullEntity, trisphereCycle1Entity); // Check rear left wheel of first model Entity wheelRearLeftEntity = ecm->EntityByComponents( @@ -428,7 +428,7 @@ TEST_F(WheelSlipTest, TricyclesUphill) components::Name("wheel_rear_left"), components::Link()); - EXPECT_NE(gazebo::kNullEntity, wheelRearLeftEntity); + EXPECT_NE(kNullEntity, wheelRearLeftEntity); Entity wheelRearLeftCollisionEntity = ecm->EntityByComponents( components::ParentEntity(wheelRearLeftEntity), @@ -448,28 +448,28 @@ TEST_F(WheelSlipTest, TricyclesUphill) components::Name("wheel_rear_left_spin"), components::Joint()); - EXPECT_NE(gazebo::kNullEntity, wheelRearLeftSpin0Entity); + EXPECT_NE(kNullEntity, wheelRearLeftSpin0Entity); Entity wheelRearRightSpin0Entity = ecm->EntityByComponents( components::ParentEntity(trisphereCycle0Entity), components::Name("wheel_rear_right_spin"), components::Joint()); - EXPECT_NE(gazebo::kNullEntity, wheelRearRightSpin0Entity); + EXPECT_NE(kNullEntity, wheelRearRightSpin0Entity); Entity wheelRearLeftSpin1Entity = ecm->EntityByComponents( components::ParentEntity(trisphereCycle1Entity), components::Name("wheel_rear_left_spin"), components::Joint()); - EXPECT_NE(gazebo::kNullEntity, wheelRearLeftSpin1Entity); + EXPECT_NE(kNullEntity, wheelRearLeftSpin1Entity); Entity wheelRearRightSpin1Entity = ecm->EntityByComponents( components::ParentEntity(trisphereCycle1Entity), components::Name("wheel_rear_right_spin"), components::Joint()); - EXPECT_NE(gazebo::kNullEntity, wheelRearRightSpin1Entity); + EXPECT_NE(kNullEntity, wheelRearRightSpin1Entity); // Set speed of both models const double angularSpeed = 6.0; @@ -524,8 +524,8 @@ TEST_F(WheelSlipTest, TricyclesUphill) } test::Relay testSlipSystem; - testSlipSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, - gazebo::EntityComponentManager &) + testSlipSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &) { auto wheelRearLeftVelocity0Cmd = ecm->Component(wheelRearLeftSpin0Entity); diff --git a/test/integration/wind_effects.cc b/test/integration/wind_effects.cc index 1bbee6e6f0..2190994474 100644 --- a/test/integration/wind_effects.cc +++ b/test/integration/wind_effects.cc @@ -82,7 +82,7 @@ class LinkComponentRecorder if (_createComp) { this->mockSystem->preUpdateCallback = - [this](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + [this](const UpdateInfo &, EntityComponentManager &_ecm) { auto linkEntity = _ecm.EntityByComponents( components::Link(), components::Name(this->linkName)); @@ -98,8 +98,8 @@ class LinkComponentRecorder } this->mockSystem->postUpdateCallback = - [this](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) + [this](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto linkEntity = _ecm.EntityByComponents( components::Link(), components::Name(this->linkName)); diff --git a/test/performance/level_manager.cc b/test/performance/level_manager.cc index 172c70db95..2a55f3ffd2 100644 --- a/test/performance/level_manager.cc +++ b/test/performance/level_manager.cc @@ -36,10 +36,10 @@ TEST(LevelManagerPerfrormance, LevelVsNoLevel) common::Console::SetVerbosity(4); - ignition::common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", (std::string(PROJECT_BINARY_PATH) + "/lib").c_str()); - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/level_performance.sdf"); math::Stopwatch watch; @@ -51,7 +51,7 @@ TEST(LevelManagerPerfrormance, LevelVsNoLevel) // measuring time differences between levels and no levels. { serverConfig.SetUseLevels(true); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); server.Run(true, 1, false); @@ -60,7 +60,7 @@ TEST(LevelManagerPerfrormance, LevelVsNoLevel) // Server with levels { serverConfig.SetUseLevels(true); - gazebo::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); watch.Start(true); @@ -72,7 +72,7 @@ TEST(LevelManagerPerfrormance, LevelVsNoLevel) // Server without levels { serverConfig.SetUseLevels(false); - gazebo::Server serverNoLevels(serverConfig); + Server serverNoLevels(serverConfig); serverNoLevels.SetUpdatePeriod(1ns); watch.Start(true); diff --git a/test/performance/sdf_runner.cc b/test/performance/sdf_runner.cc index 10a48a8bcf..5262971cb7 100644 --- a/test/performance/sdf_runner.cc +++ b/test/performance/sdf_runner.cc @@ -33,7 +33,7 @@ using namespace gazebo; ////////////////////////////////////////////////// int main(int _argc, char** _argv) { - ignition::common::Console::SetVerbosity(4); + common::Console::SetVerbosity(4); std::string sdfFile{""}; if (_argc >= 2) @@ -56,7 +56,7 @@ int main(int _argc, char** _argv) } igndbg << "Update rate: " << updateRate << std::endl; - ignition::gazebo::ServerConfig serverConfig; + ServerConfig serverConfig; if (!serverConfig.SetSdfFile(sdfFile)) { ignerr << "Failed to set SDF file [" << sdfFile << "]" << std::endl; @@ -68,23 +68,23 @@ int main(int _argc, char** _argv) serverConfig.SetUpdateRate(updateRate); // Create the Gazebo server - ignition::gazebo::Server server(serverConfig); + Server server(serverConfig); - ignition::transport::Node node; + transport::Node node; - std::vector msgs; + std::vector msgs; msgs.reserve(iterations); - std::function cb = - [&](const ignition::msgs::Clock &_msg) + std::function cb = + [&](const msgs::Clock &_msg) { msgs.push_back(_msg); }; double progress = 0; - std::function cb2 = - [&](const ignition::msgs::WorldStatistics &_msg) + std::function cb2 = + [&](const msgs::WorldStatistics &_msg) { double nIters = _msg.iterations(); nIters = nIters / iterations * 100; diff --git a/test/plugins/TestSystem.cc b/test/plugins/TestSystem.cc index 94896f5185..2c362db0b8 100644 --- a/test/plugins/TestSystem.cc +++ b/test/plugins/TestSystem.cc @@ -32,6 +32,6 @@ TestSystem::TestSystem() TestSystem::~TestSystem() = default; // Register this plugin -IGNITION_ADD_PLUGIN(TestSystem, ignition::gazebo::System) +IGNITION_ADD_PLUGIN(TestSystem, System) IGNITION_ADD_PLUGIN_ALIAS(TestSystem, "ignition::gazebo::TestSystem") From 873b43bd6ae4f1f571e53d0e790815ce92463aca Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Aug 2022 12:42:03 -0700 Subject: [PATCH 003/160] =?UTF-8?q?=F0=9F=8E=88=203.14.0=20(#1657)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- Changelog.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f638d5d30a..09d5bbd93a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ignition-cmake2 2.8.0 REQUIRED) #============================================================================ # Configure the project #============================================================================ -ign_configure_project(VERSION_SUFFIX pre1) +ign_configure_project(VERSION_SUFFIX) #============================================================================ # Set project-specific options diff --git a/Changelog.md b/Changelog.md index 23d7336e66..71ea0b3f4d 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,6 @@ ## Gazebo Sim 3.x -### Gazebo Sim 3.14.0 (2022-08-XX) +### Gazebo Sim 3.14.0 (2022-08-17) 1. Change `CODEOWNERS` and maintainer to Michael * [Pull request #1644](https://github.com/gazebosim/gz-sim/pull/1644) From 3ff727e57144bd2f5bfd0411186a74b638244e13 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Fri, 26 Aug 2022 19:32:20 -0700 Subject: [PATCH 004/160] readd namespaces for Q_ARGS (#1670) --- src/gui/plugins/component_inspector/ComponentInspector.cc | 2 +- .../joint_position_controller/JointPositionController.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 3d44e8138a..84cfcf7cff 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -693,7 +693,7 @@ void ComponentInspector::Update(const UpdateInfo &, QMetaObject::invokeMethod(&this->dataPtr->componentsModel, "RemoveComponentType", Qt::QueuedConnection, - Q_ARG(ComponentTypeId, typeId)); + Q_ARG(ignition::gazebo::ComponentTypeId, typeId)); } } } diff --git a/src/gui/plugins/joint_position_controller/JointPositionController.cc b/src/gui/plugins/joint_position_controller/JointPositionController.cc index 20723b2755..10a1a13383 100644 --- a/src/gui/plugins/joint_position_controller/JointPositionController.cc +++ b/src/gui/plugins/joint_position_controller/JointPositionController.cc @@ -292,7 +292,7 @@ void JointPositionController::Update(const UpdateInfo &, QMetaObject::invokeMethod(&this->dataPtr->jointsModel, "RemoveJoint", Qt::QueuedConnection, - Q_ARG(Entity, jointEntity)); + Q_ARG(ignition::gazebo::Entity, jointEntity)); } } } From e5f22e3b3a9617d55160d585b5b2fc479325470a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 31 Aug 2022 00:24:17 +0200 Subject: [PATCH 005/160] Add topic parameter to thrust plugin (#1681) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add topic parameter. Signed-off-by: Carlos Agüero --- src/systems/thruster/Thruster.cc | 21 +++++++++++++++++++-- src/systems/thruster/Thruster.hh | 4 +++- test/integration/thruster.cc | 26 +++++++++++++++++--------- test/worlds/thruster_battery.sdf | 1 + 4 files changed, 40 insertions(+), 12 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 92a74bb3a7..b775cf26e4 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -104,6 +104,9 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief Diameter of propeller in m, default: 0.02 public: double propellerDiameter = 0.02; + /// \brief Topic name used to control thrust. + public: std::string topic = ""; + /// \brief Callback for handling thrust update public: void OnCmdThrust(const msgs::Double &_msg); @@ -171,6 +174,13 @@ void Thruster::Configure( this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); } + // Get a custom topic. + if (_sdf->HasElement("topic")) + { + this->dataPtr->topic = transport::TopicUtils::AsValidTopic( + _sdf->Get("topic")); + } + this->dataPtr->jointEntity = model.JointByName(_ecm, jointName); if (kNullEntity == this->dataPtr->jointEntity) { @@ -191,14 +201,21 @@ void Thruster::Configure( std::string thrusterTopicOld = ignition::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/cmd_pos"); + ignwarn << thrusterTopicOld << " topic is deprecated" << std::endl; + this->dataPtr->node.Subscribe( thrusterTopicOld, &ThrusterPrivateData::OnCmdThrust, this->dataPtr.get()); // Subscribe to force commands - std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic( - "/model/" + ns + "/joint/" + jointName + "/cmd_thrust"); + std::string thrusterTopic = + "/model/" + ns + "/joint/" + jointName + "/cmd_thrust"; + + if (!this->dataPtr->topic.empty()) + thrusterTopic = ns + "/" + this->dataPtr->topic; + + thrusterTopic = transport::TopicUtils::AsValidTopic(thrusterTopic); this->dataPtr->node.Subscribe( thrusterTopic, diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 6380eb7972..415fa8b660 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -40,8 +40,10 @@ namespace systems /// /// ## System Parameters /// - - The namespace in which the robot exists. The plugin will - /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust`. + /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust` + /// or on {namespace}/{topic} if {topic} is set. /// [Optional] + /// - - The topic for receiving thrust commands. [Optional] /// - - This is the joint in the model which corresponds to the /// propeller. [Required] /// - - The fluid density of the liquid in which the thruster diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 1d28df8d5d..1e147bfe69 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -43,19 +43,20 @@ class ThrusterTest : public InternalFixture<::testing::Test> /// \brief Test a world file /// \param[in] _world Path to world file /// \param[in] _namespace Namespace for topic + /// \param[in] _topic Thrust topic /// \param[in] _coefficient Thrust coefficient /// \param[in] _density Fluid density /// \param[in] _diameter Propeller diameter /// \param[in] _baseTol Base tolerance for most quantities public: void TestWorld(const std::string &_world, - const std::string &_namespace, double _coefficient, double _density, - double _diameter, double _baseTol); + const std::string &_namespace, const std::string &_topic, + double _coefficient, double _density, double _diameter, double _baseTol); }; ////////////////////////////////////////////////// void ThrusterTest::TestWorld(const std::string &_world, - const std::string &_namespace, double _coefficient, double _density, - double _diameter, double _baseTol) + const std::string &_namespace, const std::string &_topic, + double _coefficient, double _density, double _diameter, double _baseTol) { // Start server ServerConfig serverConfig; @@ -122,8 +123,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // Publish command and check that vehicle moved transport::Node node; - auto pub = node.Advertise( - "/model/" + _namespace + "/joint/propeller_joint/cmd_thrust"); + auto pub = node.Advertise(_topic); int sleep{0}; int maxSleep{30}; @@ -234,31 +234,39 @@ void ThrusterTest::TestWorld(const std::string &_world, // See https://github.com/ignitionrobotics/ign-gazebo/issues/1175 TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PIDControl)) { + const std::string ns{"sub"}; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_pid.sdf"); // Tolerance could be lower (1e-6) if the joint pose had a precise 180 // rotation - this->TestWorld(world, "sub", 0.004, 1000, 0.2, 1e-4); + this->TestWorld(world, ns, topic, 0.004, 1000, 0.2, 1e-4); } ///////////////////////////////////////////////// TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(VelocityControl)) { + const std::string ns = "custom"; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_vel_cmd.sdf"); // Tolerance is high because the joint command disturbs the vehicle body - this->TestWorld(world, "custom", 0.005, 950, 0.25, 1e-2); + this->TestWorld(world, ns, topic, 0.005, 950, 0.25, 1e-2); } ///////////////////////////////////////////////// TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration)) { + const std::string ns = "lowbattery"; + const std::string topic = ns + "/thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_battery.sdf"); // Tolerance is high because the joint command disturbs the vehicle body - this->TestWorld(world, "lowbattery", 0.005, 950, 0.25, 1e-2); + this->TestWorld(world, ns, topic, 0.005, 950, 0.25, 1e-2); } diff --git a/test/worlds/thruster_battery.sdf b/test/worlds/thruster_battery.sdf index 3cf5a3495e..0a76710429 100644 --- a/test/worlds/thruster_battery.sdf +++ b/test/worlds/thruster_battery.sdf @@ -105,6 +105,7 @@ true 300 0 + thrust Date: Tue, 30 Aug 2022 19:16:32 -0700 Subject: [PATCH 006/160] =?UTF-8?q?=F0=9F=8E=88=206.12.0:=20bumped=20minor?= =?UTF-8?q?=20and=20updated=20changelog=20(#1682)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * bumped minor and updated changelog Signed-off-by: Dharini Dutia * fixed changelog as per feedback and updated migration guide Signed-off-by: Dharini Dutia Signed-off-by: Dharini Dutia --- CMakeLists.txt | 2 +- Changelog.md | 14 ++++++++++++++ Migration.md | 6 ++++++ 3 files changed, 21 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5efa74bd84..897ea591a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo6 VERSION 6.11.0) +project(ignition-gazebo6 VERSION 6.12.0) set (GZ_DISTRIBUTION "Fortress") #============================================================================ diff --git a/Changelog.md b/Changelog.md index d9bb90509e..2487f3b1ce 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,19 @@ ## Ignition Gazebo 6.x +### Gazebo Sim 6.12.0 (2022-08-30) + +1. Add topic parameter to thrust plugin + * [Pull request #1681](https://github.com/gazebosim/gz-sim/pull/1681) + +1. Add information about `` system parameter + * [Pull request #1671](https://github.com/gazebosim/gz-sim/pull/1671) + +1. Adding tests for hydrodynamics + * [Pull request #1617](https://github.com/gazebosim/gz-sim/pull/1617) + +1. Fix Windows and Doxygen + * [Pull request #1643](https://github.com/gazebosim/gz-sim/pull/1643) + ### Gazebo Sim 6.11.0 (2022-08-17) 1. Add support for specifying log record period diff --git a/Migration.md b/Migration.md index 58a4e5c815..ea3bdeea7c 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,12 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Ignition Gazebo 6.11.X to 6.12.X + + * **Modified**: + + In the Hydrodynamics plugin, inverted the added mass contribution to make it + act in the correct direction. + ## Ignition Gazebo 6.1 to 6.2 * If no `` is given to the `Thruster` plugin, the namespace now From 545b36467c315a2ff45c337d3afce748ee109eff Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 1 Sep 2022 16:29:57 +0900 Subject: [PATCH 007/160] Fix reference link in ackermann steering (#1683) Signed-off-by: Kenji Brameld --- src/systems/ackermann_steering/AckermannSteering.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index 11aacd9159..19b3f8d18c 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -111,7 +111,7 @@ namespace systems /// right_steering_joint /// /// References: - /// https://github.com/ignitionrobotics/ign-gazebo/tree/main/src/systems/diff_drive + /// https://github.com/gazebosim/gz-sim/tree/main/src/systems/ackermann_steering /// https://www.auto.tuwien.ac.at/bib/pdf_TR/TR0183.pdf /// https://github.com/froohoo/ackermansteer/blob/master/ackermansteer/ From 69e77571904c4f1888bbeb5dad0eb6637123dbae Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 2 Sep 2022 20:38:37 +0200 Subject: [PATCH 008/160] Fix installation instructions on Ubuntu 22.04 (#1686) Signed-off-by: Silvio Traversaro --- tutorials/install.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/install.md b/tutorials/install.md index 3260f10154..1b1694f402 100644 --- a/tutorials/install.md +++ b/tutorials/install.md @@ -61,7 +61,7 @@ feature which hasn't been released yet. 1. Install tools ``` - sudo apt install -y build-essential cmake g++-8 git gnupg lsb-release wget + sudo apt install -y build-essential cmake git gnupg lsb-release wget ``` 2. Enable the Ignition software repositories: From 0cbe6e7d23c74f481c3a1758b4c62a2559137bc2 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Wed, 7 Sep 2022 09:23:55 -0700 Subject: [PATCH 009/160] Add a service to trigger functionality (#1611) * initial commit to allow plugin to call a service Signed-off-by: Liam Han * adding tutorial and modifying the world sdf Signed-off-by: Liam Han * added test for single input and single service output Signed-off-by: Liam Han * added test for single input and multiple service output Signed-off-by: Liam Han * added test for invalid matching service name => timeout Signed-off-by: Liam Han * modified variables the camelCase Signed-off-by: Liam Han * fixed typo, indentation, grammar, lines that exceeded 80 char Signed-off-by: Liam Han * fixing ubuntu bionic ci issue Signed-off-by: Liam Han * silly syntax mistake on expect_eq Signed-off-by: Liam Han * added three more test cases that addesses incorrect response type, incorrect request type and false result Signed-off-by: Liam Han * WIP: major restructuring and currently working. Requires more cleanup and test Signed-off-by: Liam Han * WIP: fixed preprocessor define bug Signed-off-by: Liam Han * WIP: working but extremely convoluted Signed-off-by: Liam Han * WIP major modification but a lot of errors and tests failed Signed-off-by: Liam Han * stable version: had to revert back to previous work. all tests passed Signed-off-by: Liam Han * modified to use blocking Request method as well as reduce a service worker thread to just one thread with the publisher. all tests passed Signed-off-by: Liam Han * stable version: had to revert back to previous work. all tests passed Signed-off-by: Liam Han * successfully reverted and tested Signed-off-by: Liam Han * fixing PR suggestions Signed-off-by: Liam Han * changed string with 'serv' to 'srv' and included to the header Signed-off-by: Liam Han * fixed indentation and removed rep.set_data since it's unused on the client service Signed-off-by: Liam Han * getting rid of the id Signed-off-by: Liam Han * fixed race condition resulting seldom test failure Signed-off-by: Liam Han * changed from triggerSrv to serviceCount. This compensates for the two threads running at different rate Signed-off-by: Liam Han * braces indentation Signed-off-by: Mabel Zhang * addressing gnu c compiler (gcc) warnings Signed-off-by: Liam Han Signed-off-by: Liam Han Signed-off-by: Mabel Zhang Co-authored-by: Mabel Zhang --- examples/worlds/triggered_publisher.sdf | 35 ++- .../triggered_publisher/TriggeredPublisher.cc | 158 ++++++++-- .../triggered_publisher/TriggeredPublisher.hh | 57 +++- test/integration/triggered_publisher.cc | 269 ++++++++++++++++++ test/worlds/triggered_publisher.sdf | 161 +++++++++-- tutorials/triggered_publisher.md | 39 ++- 6 files changed, 664 insertions(+), 55 deletions(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 303f78beca..98b7c52c64 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -387,13 +387,17 @@ start falling. true - + body box1 box_body /box1/detach - + body box2 box_body @@ -448,19 +452,40 @@ start falling. - + linear: {x: 3} - + + + + linear: {x: 0} + + + + + data: true - + -7.5 diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 94a63b7ac8..cc7a4f88ad 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -252,7 +252,9 @@ FullMatcher::FullMatcher(const std::string &_msgType, bool _logicType, : InputMatcher(_msgType), logicType(_logicType) { if (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()) + { return; + } this->valid = google::protobuf::TextFormat::ParseFromString( _matchString, this->matchMsg.get()); @@ -273,7 +275,9 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, fieldName(_fieldName) { if (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()) + { return; + } transport::ProtoMsg *matcherSubMsg{nullptr}; if (!FindFieldSubMessage(this->matchMsg.get(), _fieldName, @@ -294,7 +298,9 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, } if (nullptr == matcherSubMsg) + { return; + } bool result = google::protobuf::TextFormat::ParseFieldValueFromString( _fieldString, this->fieldDescMatcher.back(), matcherSubMsg); @@ -548,7 +554,9 @@ void TriggeredPublisher::Configure(const Entity &, { int ms = sdfClone->Get("delay_ms"); if (ms > 0) + { this->delay = std::chrono::milliseconds(ms); + } } if (sdfClone->HasElement("output")) @@ -595,9 +603,52 @@ void TriggeredPublisher::Configure(const Entity &, } } } - else + + if (sdfClone->HasElement("service")) { - ignerr << "No ouptut specified" << std::endl; + for (auto serviceElem = sdfClone->GetElement("service"); serviceElem; + serviceElem = serviceElem->GetNextElement("service")) + { + SrvOutputInfo serviceInfo; + serviceInfo.srvName = serviceElem->Get("name"); + if (serviceInfo.srvName.empty()) + { + ignerr << "Service name cannot be empty\n"; + return; + } + serviceInfo.reqType = serviceElem->Get("reqType"); + if (serviceInfo.reqType.empty()) + { + ignerr << "Service request type cannot be empty\n"; + return; + } + serviceInfo.repType = serviceElem->Get("repType"); + if (serviceInfo.repType.empty()) + { + ignerr << "Service reply type cannot be empty\n"; + return; + } + serviceInfo.reqMsg = serviceElem->Get("reqMsg"); + if (serviceInfo.reqMsg.empty()) + { + ignerr << "Service request message cannot be empty\n"; + return; + } + std::string timeoutInfo = serviceElem->Get("timeout"); + if (timeoutInfo.empty()) + { + ignerr << "Timeout value cannot be empty\n"; + return; + } + + serviceInfo.timeout = std::stoi(timeoutInfo); + this->srvOutputInfo.push_back(std::move(serviceInfo)); + } + } + if (!sdfClone->HasElement("service") && !sdfClone->HasElement("output")) + { + ignerr << "No output and service specified. Make sure to specify at least" + "one of them." << std::endl; return; } @@ -606,23 +657,27 @@ void TriggeredPublisher::Configure(const Entity &, { if (this->MatchInput(_msg)) { + if (this->delay > 0ms) + { + std::lock_guard lock(this->publishQueueMutex); + this->publishQueue.push_back(this->delay); + } + else { - if (this->delay > 0ms) - { - std::lock_guard lock(this->publishQueueMutex); - this->publishQueue.push_back(this->delay); - } - else { - { - std::lock_guard lock(this->publishCountMutex); - ++this->publishCount; - } - this->newMatchSignal.notify_one(); + std::lock_guard lock(this->publishCountMutex); + ++this->publishCount; } + this->newMatchSignal.notify_one(); + } + if (this->srvOutputInfo.size() > 0) + { + std::lock_guard lock(this->triggerSrvMutex); + ++this->serviceCount; } } }); + if (!this->node.Subscribe(this->inputTopic, msgCb)) { ignerr << "Input subscriber could not be created for topic [" @@ -645,11 +700,69 @@ void TriggeredPublisher::Configure(const Entity &, std::thread(std::bind(&TriggeredPublisher::DoWork, this)); } +////////////////////////////////////////////////// +void TriggeredPublisher::PublishMsg(std::size_t pending) +{ + for (auto &info : this->outputInfo) + { + for (std::size_t i = 0; i < pending; ++i) + { + info.pub.Publish(*info.msgData); + } + } +} + +////////////////////////////////////////////////// +void TriggeredPublisher::CallService(std::size_t pendingSrv) +{ + for (auto &serviceInfo : this->srvOutputInfo) + { + for (std::size_t i = 0; i < pendingSrv; ++i) + { + bool result; + auto req = msgs::Factory::New(serviceInfo.reqType, serviceInfo.reqMsg); + if (!req) + { + ignerr << "Unable to create request for type [" + << serviceInfo.reqType << "].\n"; + return; + } + + auto rep = msgs::Factory::New(serviceInfo.repType); + if (!rep) + { + ignerr << "Unable to create response for type [" + << serviceInfo.repType << "].\n"; + return; + } + + bool executed = this->node.Request(serviceInfo.srvName, *req, + serviceInfo.timeout, *rep, result); + if (executed) + { + if (!result) + { + ignerr << "Service call [" << serviceInfo.srvName << "] failed\n"; + } + else + { + ignmsg << "Service call [" << serviceInfo.srvName << "] succeeded\n"; + } + } + else + { + ignerr << "Service call [" << serviceInfo.srvName << "] timed out\n"; + } + } + } +} + ////////////////////////////////////////////////// void TriggeredPublisher::DoWork() { while (!this->done) { + // check whether to publish a msg by checking publishCount std::size_t pending{0}; { using namespace std::chrono_literals; @@ -661,18 +774,25 @@ void TriggeredPublisher::DoWork() }); if (this->publishCount == 0 || this->done) + { continue; - + } std::swap(pending, this->publishCount); } - for (auto &info : this->outputInfo) + PublishMsg(pending); + + // check whether to call a service by checking serviceCount + std::size_t pendingSrv{0}; { - for (std::size_t i = 0; i < pending; ++i) - { - info.pub.Publish(*info.msgData); + std::lock_guard lock(this->triggerSrvMutex); + if (this->serviceCount == 0 || this->done){ + continue; } + std::swap(pendingSrv, this->serviceCount); } + + CallService(pendingSrv); } } @@ -712,7 +832,9 @@ void TriggeredPublisher::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } if (notify) + { this->newMatchSignal.notify_one(); + } } ////////////////////////////////////////////////// diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index f524b119ab..2560e67722 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -20,7 +20,7 @@ #include #include #include - +#include #include #include "ignition/gazebo/System.hh" @@ -37,8 +37,10 @@ namespace systems /// \brief The triggered publisher system publishes a user specified message /// on an output topic in response to an input message that matches user - /// specified criteria. An optional simulation time delay can be used - /// delay message publication. + /// specified criteria. It can also call a user specified service as an + /// output in response to an input message. It currently supports blocking + /// service call. An optional simulation time delay can be used delay message + /// publication. /// /// ## System Parameters /// @@ -75,9 +77,19 @@ namespace systems /// the human-readable representation of a protobuf message as used by /// `ign topic` for publishing messages /// - /// ``: Integer number of milliseconds, in simulation time, to + /// - ``: Integer number of milliseconds, in simulation time, to /// delay publication. /// + /// - ``: Contains configuration for service to call: Multiple + /// `` tags are possible. A service will be called for each input + /// that matches. + /// * Attributes: + /// * `name`: Service name (eg. `/world/triggered_publisher/set_pose`) + /// * `timeout`: Service timeout + /// * `reqType`: Service request message type (eg. ignition.msgs.Pose) + /// * `repType`: Service response message type (eg. ignition.msgs.Empty) + /// * `reqMsg`: String used to construct the service protobuf message. + /// /// Examples: /// 1. Any receipt of a Boolean messages on the input topic triggers an output /// \code{.xml} @@ -182,6 +194,12 @@ namespace systems /// \brief Thread that handles publishing output messages public: void DoWork(); + /// \brief Method that calls a service + private: void CallService(std::size_t pendingSrv); + + /// \brief Method that publishes a message + private: void PublishMsg(std::size_t pending); + /// \brief Helper function that calls Match on every InputMatcher available /// \param[in] _inputMsg Input message /// \return True if all of the matchers return true @@ -210,21 +228,49 @@ namespace systems transport::Node::Publisher pub; }; + /// \brief Class that holds necessary bits for each specified service output + private: struct SrvOutputInfo + { + /// \brief Service name + std::string srvName; + + /// \brief Service request type + std::string reqType; + + /// \brief Service reply type + std::string repType; + + /// \brief Service request message + std::string reqMsg; + + /// \brief Serivce timeout + int timeout; + }; + /// \brief List of InputMatchers private: std::vector> matchers; /// \brief List of outputs private: std::vector outputInfo; + /// \brief List of service outputs + private: std::vector srvOutputInfo; + /// \brief Ignition communication node. private: transport::Node node; + /// \brief Counter that tells how manny times to call the service + private: std::size_t serviceCount{0}; + /// \brief Counter that tells the publisher how many times to publish private: std::size_t publishCount{0}; /// \brief Mutex to synchronize access to publishCount private: std::mutex publishCountMutex; + /// \brief Mutex to synchronize access to serviceCount + private: std::mutex triggerSrvMutex; + /// \brief Condition variable to signal that new matches have occured private: std::condition_variable newMatchSignal; @@ -244,9 +290,8 @@ namespace systems /// \brief Mutex to synchronize access to publishQueue private: std::mutex publishQueueMutex; }; - } } } } - +} #endif diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 9590a7c421..3ae1070d12 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -637,3 +637,272 @@ TEST_F(TriggeredPublisherTest, EXPECT_EQ(0u, recvCount); } + +///////////////////////////////////////////////// +/// Test for invalid service name. A service, `/srv-dummy-test` is advertised +/// and the callback is also provided in this test. Everytime an input msg is +/// published to `/in_14` topic, triggered_publisher plugin will call the +/// service `srv-test`, specified in the test/worlds/triggered_publisher.sdf. +/// However, since the two service names do not match, the callback provided in +/// this test will not be invoked. Therefore, the pubCount, which is set to 10, +/// will not equal to recvCount. The recvCount will be 0, since the callback +/// isn't invoked. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(InvalidServiceName)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_14"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-dummy-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return recvCount == 0u;}); + EXPECT_EQ(recvCount, 0u); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call in response to an input msg. A service, +/// `srv-test` is advertised and the callback is also provided in this test. +/// Everytime an input msg is published to `/in_14` topic, triggered_publisher +/// plugin will call the service `/srv-test`, specified in the test/worlds/ +/// triggered_publisher.sdf. This time, the name of the services match. By +/// publishing input msg 10 times, the service callback will also be invoked 10 +/// times. The `pubCount` is set to 10 and recvCount is increased everytime +/// request data matches the string "test" inside the service callback. For a +/// successful test, the pubCount will equal to recvCount. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_14"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +/// Test for triggering multiple services (in sequence) in response to an input +/// msg by publishing 10 times. Two services, `srv-test-0` and `srv-test-1` are +/// specified in the test/worlds/triggered_publisher.sdf. Everytime an input msg +/// is published, triggered_publisher will call the service and invoke the +/// callback. std::vector is passed as a reference and will be populated with +/// the request message, which will be a boolean value of `true`. If successful, +/// `recvMsg0` and `recvMsg1` vectors should both have a size of 10 with all +/// true boolean values. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_15"); + std::mutex recvMsgMutex; + std::vector recvMsgs0; + std::vector recvMsgs1; + auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) + { + return std::function( + [&_msgVector, &recvMsgMutex](const auto &req, auto &) + { + std::lock_guard lock(recvMsgMutex); + if (req.data()) + { + _msgVector.push_back(req.data()); + return true; + } + return false; + }); + }; + + auto msgCb0 = cbCreator(recvMsgs0); + auto msgCb1 = cbCreator(recvMsgs1); + + // Advertise two dummy services + node.Advertise("/srv-test-0", msgCb0); + node.Advertise("/srv-test-1", msgCb1); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&] + { + std::lock_guard lock(recvMsgMutex); + return static_cast(pubCount) == recvMsgs0.size() && + static_cast(pubCount) == recvMsgs1.size(); + }); + + EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); + EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); + + // The plugin has two outputs. We expect 10 messages in each output topic + EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), true)); + EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), true)); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call with incorrect request type or reply +/// type specified in test/worlds/triggered_publisher.sdf. `InvalidReqType` and +/// `InvalidRepType` do not exist. Hence, the callback will never be invoked and +/// the recvCount will be 0. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongRequestOrResponseType)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_16"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return recvCount == 0u;}); + EXPECT_EQ(0u, recvCount); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call with different request (Boolean) and +/// reply type (StringMsg). Check `InputMessagesTriggerServices` test for more +/// details on how the test works. This test is very similar except that it has +/// different request and reply type. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(BooleanReqStringMsgRep)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_18"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), true); + if (req.data() == true) + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-diff-type-0"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call with different request (StringMsg) and +/// reply type (Boolean). Check `InputMessagesTriggerServices` test for more +/// details on how the test works. This test is very similar except that it has +/// different request and reply type. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(StringMsgReqBooleanRep)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_19"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-diff-type-1"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(recvCount, recvCount); +} diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 7c9f21c568..2ce7ab2394 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -5,19 +5,25 @@ 0 - + - + data: true - + data: false @@ -27,7 +33,9 @@ - + data: true @@ -36,7 +44,9 @@ - + data: 0 @@ -45,7 +55,9 @@ - + data: 0 @@ -54,7 +66,9 @@ - + data: -5 @@ -75,7 +89,9 @@ - + 1.0 2.0 @@ -83,7 +99,9 @@ - + 1.0 2.0 @@ -91,7 +109,9 @@ - + { @@ -102,7 +122,9 @@ - + { @@ -120,53 +142,152 @@ - + data: 0, data: 1 - + data: 0.5 - + 0.5 - + "value1" - + 1000 - - + + data: 0, data: 1 - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index d07c74c318..ff4759c3ef 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -1,17 +1,20 @@ \page triggeredpublisher Triggered Publisher The `TriggeredPublisher` system publishes a user specified message on an output -topic in response to an input message that matches user specified criteria. The -system works by checking the input against a set of Matchers. Matchers -contain string representations of protobuf messages which are compared for -equality or containment with the input message. Matchers can match the whole -input message or only a specific field inside the message. +topic in response to an input message that matches user specified criteria. It +can also call a user specified service in response to an input +message. The system works by checking the input against a set of Matchers. +Matchers contain string representations of protobuf messages which are compared +for equality or containment with the input message. Matchers can match the +whole input message or only a specific field inside the message. + This tutorial describes how the Triggered Publisher system can be used to cause a box to fall from its initial position by detaching a detachable joint in response to the motion of a vehicle. The tutorial also covers how Triggered Publisher systems can be chained together by showing how the falling of the box -can trigger another box to fall. The finished world SDFormat file for this +can trigger another box to fall. Last, it covers how a service call can be +triggered to reset the robot pose. The finished world SDFormat file for this tutorial can be found in [examples/worlds/triggered_publisher.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/triggered_publisher.sdf) @@ -263,3 +266,27 @@ and publish the start message ``` ign topic -t "/start" -m ignition.msgs.Empty -p " " ``` + +Once both boxes have fallen, we can publish a message to invoke a service call +to reset the robot position as well as set the speed to 0. As shown below, the +`` sets the linear x speed to 0, and the `` tag contains +metadata to invoke a service call to `/world/triggered_publisher/set_pose`. The +`reqMsg` is expressed in the human-readable form of Google Protobuf meesages. +Multiple `` tags can be used as well as with the `` tag. + +```xml + + + + linear: {x: 0} + + + + +``` From 721bc356a827b26b4edd893110e065f2f74c0963 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 9 Sep 2022 06:30:23 -0700 Subject: [PATCH 010/160] Fix loading render engine plugins in GUI (#1694) Signed-off-by: Ian Chen --- include/ignition/gazebo/rendering/RenderUtil.hh | 4 ++++ src/gui/plugins/scene_manager/GzSceneManager.cc | 9 +++++++++ src/rendering/RenderUtil.cc | 12 +++++++++--- 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index a757ec5378..444d9c0daa 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -201,6 +201,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Clears the set of selected entities and lowlights all of them. public: void DeselectAllEntities(); + /// \brief Init render engine plugins paths. This lets gz-rendering know + /// paths to find render engine plugins + public: void InitRenderEnginePluginPaths(); + /// \brief Helper function to get all child links of a model entity. /// \param[in] _entity Entity to find child links /// \return Vector of child links found for the parent entity diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index f04c61d07a..9d0c6b1338 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -59,6 +59,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Rendering utility public: RenderUtil renderUtil; + /// \brief True if render engine plugins paths are initialized + public: bool renderEnginePluginPathsInit{false}; + /// \brief List of new entities from a gui event public: std::set newEntities; @@ -123,6 +126,12 @@ void GzSceneManager::Update(const UpdateInfo &_info, IGN_PROFILE("GzSceneManager::Update"); + if (!this->dataPtr->renderEnginePluginPathsInit) + { + this->dataPtr->renderUtil.InitRenderEnginePluginPaths(); + this->dataPtr->renderEnginePluginPathsInit = true; + } + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 415908ff9c..6f4ef07941 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -2496,6 +2496,14 @@ bool RenderUtil::HeadlessRendering() const return this->dataPtr->isHeadlessRendering; } +///////////////////////////////////////////////// +void RenderUtil::InitRenderEnginePluginPaths() +{ + ignition::common::SystemPaths pluginPath; + pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); + rendering::setPluginPaths(pluginPath.PluginPaths()); +} + ///////////////////////////////////////////////// void RenderUtil::Init() { @@ -2503,9 +2511,7 @@ void RenderUtil::Init() if (nullptr != this->dataPtr->scene) return; - ignition::common::SystemPaths pluginPath; - pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); - rendering::setPluginPaths(pluginPath.PluginPaths()); + this->InitRenderEnginePluginPaths(); std::map params; if (this->dataPtr->useCurrentGLContext) From f16b35ea04416232200658703f24262a8368cb0b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 14 Sep 2022 14:01:50 +0800 Subject: [PATCH 011/160] Remove actors from screen when they are supposed to (#1699) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # 🦟 Bug fix Supercedes #1697. Note: When forward porting we will have to update the hashmaps to erase the new hashmaps created. Fixes # ## Summary Found that when actors are De-spawned the actor visuals are not destroyed. This commit addresses this bug by adding the missing remove logic in RenderUtils. ## Before ![bug](https://user-images.githubusercontent.com/542272/189558600-196d98c5-1dcf-4d6c-93d6-7493df38c0e4.gif) ## After ![no_bug](https://user-images.githubusercontent.com/542272/189558924-3f2e3c5d-68f3-4d80-aee4-3dc3ce6742a1.gif) ## Notes: Theres a lot of hashmaps being populated in RenderUtils whenever a new actor is spawned. I hope I've caught them all. Also while I need these working in garden (as all the projects Im working on use garden), should I backport these changes? Signed-off-by: Arjo Chakravarty --- src/rendering/RenderUtil.cc | 8 ++++++++ src/rendering/SceneManager.cc | 22 ++++++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 94f25f4be4..0756702774 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1104,6 +1104,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( const EntityComponentManager &_ecm, const UpdateInfo &_info) { IGN_PROFILE("RenderUtilPrivate::RemoveRenderingEntities"); + _ecm.EachRemoved( + [&](const Entity &_entity, const components::Actor *)->bool + { + this->removeEntities[_entity] = _info.iterations; + this->entityPoses.erase(_entity); + this->actorTransforms.erase(_entity); + return true; + }); _ecm.EachRemoved( [&](const Entity &_entity, const components::Model *)->bool { diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 1276d63183..aca843d96a 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -1126,6 +1126,28 @@ void SceneManager::RemoveEntity(Entity _id) if (!this->dataPtr->scene) return; + { + auto it = this->dataPtr->actors.find(_id); + if (it != this->dataPtr->actors.end()) + { + this->dataPtr->actors.erase(it); + } + } + { + auto it = this->dataPtr->actorTrajectories.find(_id); + if (it != this->dataPtr->actorTrajectories.end()) + { + this->dataPtr->actorTrajectories.erase(it); + } + } + { + auto it = this->dataPtr->actorSkeletons.find(_id); + if (it != this->dataPtr->actorSkeletons.end()) + { + this->dataPtr->actorSkeletons.erase(it); + } + } + { auto it = this->dataPtr->visuals.find(_id); if (it != this->dataPtr->visuals.end()) From 6b48eea5e74636ddde6445a167478663cc0f49c2 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Thu, 22 Sep 2022 09:48:40 -0400 Subject: [PATCH 012/160] Enable inherited model topic name. (#1689) Allows for inheriting model name for robotNamespace when SDF element is not set and provides a debug message showing the topics it subscribes to. Signed-off-by: Benjamin Perseghetti Co-authored-by: Nate Koenig --- .../multicopter_motor_model/MulticopterMotorModel.cc | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index dbb5102d00..db6552555b 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -6,6 +6,7 @@ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland * Copyright 2016 Geoffrey Hunter * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2022 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -49,6 +50,7 @@ #include "ignition/gazebo/components/Wind.hh" #include "ignition/gazebo/Link.hh" #include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" // from rotors_gazebo_plugins/include/rotors_gazebo_plugins/common.h /// \brief This class can be used to apply a first order filter on a signal. @@ -250,7 +252,8 @@ void MulticopterMotorModel::Configure(const Entity &_entity, } else { - ignerr << "Please specify a robotNamespace.\n"; + ignwarn << "No robotNamespace set using entity name.\n"; + this->dataPtr->robotNamespace = this->dataPtr->model.Name(_ecm); } // Get params from SDF @@ -365,6 +368,10 @@ void MulticopterMotorModel::Configure(const Entity &_entity, << "]" << std::endl; return; } + else + { + igndbg << "Listening to topic: " << topic << std::endl; + } this->dataPtr->node.Subscribe(topic, &MulticopterMotorModelPrivate::OnActuatorMsg, this->dataPtr.get()); } From 96b282b872a0a96d54589643e3d8e03d5160d0a3 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Fri, 23 Sep 2022 14:14:41 +0200 Subject: [PATCH 013/160] Add ResourceSpawner example file (#1701) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add an example file for the ResourceSpawner plugin. I'm using this to link from https://github.com/gazebosim/docs/blob/master/garden/Model_insertion_fuel.md. To improve gazebosim/garden-tutorial-party#1991. Signed-off-by: Jose Luis Rivero Co-authored-by: Alejandro Hernández Cordero --- examples/worlds/resource_spawner.sdf | 167 +++++++++++++++++++++++++++ 1 file changed, 167 insertions(+) create mode 100644 examples/worlds/resource_spawner.sdf diff --git a/examples/worlds/resource_spawner.sdf b/examples/worlds/resource_spawner.sdf new file mode 100644 index 0000000000..ec9eb90392 --- /dev/null +++ b/examples/worlds/resource_spawner.sdf @@ -0,0 +1,167 @@ + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + + + + 3D View + false + docked + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + floating + + + + + + true + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + docked + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 20 20 0.1 + + + + + + + 20 20 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + From 5330437e4eb31dfa1432a7b989ac2e5a83259808 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 26 Sep 2022 06:07:14 -0700 Subject: [PATCH 014/160] Update triggered_publisher.sdf (#1737) found a silly typo that was pushed back in PR (https://github.com/gazebosim/gz-sim/pull/1611) --- examples/worlds/triggered_publisher.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 98b7c52c64..07c7eb7396 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -472,7 +472,7 @@ start falling. reqType="ignition.msgs.Pose" repType="ignition.msgs.Boolean" timeout="3000" - reqMsg="name: 'blue_vehicle', position: {x: -3, z: 0.325}"> + reqMsg="name: 'vehicle_blue', position: {x: -3, z: 0.325}"> Date: Wed, 28 Sep 2022 08:50:05 -0700 Subject: [PATCH 015/160] Adds sky cubemap URI to the sky.proto's header (#1739) * Adds sky cubemap URI to the sky.proto's header Signed-off-by: Nate Koenig * require sdf 12.6 Signed-off-by: Nate Koenig Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- src/Conversions.cc | 17 +++++++++++++++++ src/Conversions_TEST.cc | 6 ++++++ 3 files changed, 24 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 897ea591a0..38008583c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,7 +60,7 @@ set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # as protobuf could be find transitively by any dependency set(protobuf_MODULE_COMPATIBLE TRUE) -ign_find_package(sdformat12 REQUIRED VERSION 12.4) +ign_find_package(sdformat12 REQUIRED VERSION 12.6) set(SDF_VER ${sdformat12_VERSION_MAJOR}) #-------------------------------------- diff --git a/src/Conversions.cc b/src/Conversions.cc index ac2bda406a..c5db3be557 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -863,6 +863,13 @@ msgs::Scene ignition::gazebo::convert(const sdf::Scene &_in) skyMsg->set_mean_cloud_size(_in.Sky()->CloudMeanSize()); msgs::Set(skyMsg->mutable_cloud_ambient(), _in.Sky()->CloudAmbient()); + + if (!_in.Sky()->CubemapUri().empty()) + { + auto header = skyMsg->mutable_header()->add_data(); + header->set_key("cubemap_uri"); + header->add_value(_in.Sky()->CubemapUri()); + } } return out; @@ -893,6 +900,16 @@ sdf::Scene ignition::gazebo::convert(const msgs::Scene &_in) sky.SetCloudHumidity(_in.sky().humidity()); sky.SetCloudMeanSize(_in.sky().mean_cloud_size()); sky.SetCloudAmbient(msgs::Convert(_in.sky().cloud_ambient())); + + for (int i = 0; i < _in.sky().header().data_size(); ++i) + { + auto data = _in.sky().header().data(i); + if (data.key() == "cubemap_uri" && data.value_size() > 0) + { + sky.SetCubemapUri(data.value(0)); + } + } + out.SetSky(sky); } return out; diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index f1876943b4..9033949c9d 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -710,6 +710,7 @@ TEST(Conversions, Scene) sky.SetCloudHumidity(0.11); sky.SetCloudMeanSize(0.88); sky.SetCloudAmbient(math::Color::Red); + sky.SetCubemapUri("test.dds"); scene.SetSky(sky); auto sceneSkyMsg = convert(scene); @@ -723,6 +724,10 @@ TEST(Conversions, Scene) EXPECT_DOUBLE_EQ(0.88, sceneSkyMsg.sky().mean_cloud_size()); EXPECT_EQ(math::Color::Red, msgs::Convert(sceneSkyMsg.sky().cloud_ambient())); + ASSERT_GT(sceneSkyMsg.sky().header().data_size(), 0); + auto header = sceneSkyMsg.sky().header().data(0); + EXPECT_EQ("cubemap_uri", header.key()); + EXPECT_EQ("test.dds", header.value(0)); auto newSceneSky = convert(sceneSkyMsg); ASSERT_NE(nullptr, newSceneSky.Sky()); @@ -734,6 +739,7 @@ TEST(Conversions, Scene) EXPECT_DOUBLE_EQ(0.11, newSceneSky.Sky()->CloudHumidity()); EXPECT_DOUBLE_EQ(0.88, newSceneSky.Sky()->CloudMeanSize()); EXPECT_EQ(math::Color::Red, newSceneSky.Sky()->CloudAmbient()); + EXPECT_EQ("test.dds", newSceneSky.Sky()->CubemapUri()); } ///////////////////////////////////////////////// From aff92493203bdd72de7a85fa2f1c114f3d23c834 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 28 Sep 2022 08:50:26 -0700 Subject: [PATCH 016/160] Return absolute path when finding a resource (#1741) * Adds sky cubemap URI to the sky.proto's header Signed-off-by: Nate Koenig * Return absolute path when finding a resource Signed-off-by: Nate Koenig Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- src/ServerPrivate.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 0381c96660..6f1929b82e 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -490,9 +490,9 @@ bool ServerPrivate::ResourcePathsResolveService( std::string req = _req.data(); // Handle the case where the request is already a valid path - if (common::exists(req)) + if (common::exists(common::absPath(req))) { - _res.set_data(req); + _res.set_data(common::absPath(req)); return true; } From cdc9dc291c6d71aa9f2bdf24a45d86fae23c9007 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 3 Oct 2022 11:59:15 -0700 Subject: [PATCH 017/160] Restore Add System GUI plugin (#1685) * cherry pick aef3020cf6d982b483553fd56cf2acea4a3d3f5f Signed-off-by: Ian Chen --- include/ignition/gazebo/SystemLoader.hh | 8 +- src/SystemLoader.cc | 43 ++++-- src/SystemLoader_TEST.cc | 36 +++++ src/SystemManager.cc | 60 +++++++- src/SystemManager.hh | 8 ++ .../component_inspector/ComponentInspector.cc | 129 +++++++++++++++++- .../component_inspector/ComponentInspector.hh | 30 ++++ .../ComponentInspector.qml | 128 ++++++++++++++++- test/integration/entity_system.cc | 48 +++++++ test/worlds/diff_drive_no_plugin.sdf | 4 + 10 files changed, 477 insertions(+), 17 deletions(-) diff --git a/include/ignition/gazebo/SystemLoader.hh b/include/ignition/gazebo/SystemLoader.hh index 592027394a..2ba237bcd3 100644 --- a/include/ignition/gazebo/SystemLoader.hh +++ b/include/ignition/gazebo/SystemLoader.hh @@ -17,6 +17,7 @@ #ifndef IGNITION_GAZEBO_SYSTEMLOADER_HH_ #define IGNITION_GAZEBO_SYSTEMLOADER_HH_ +#include #include #include #include @@ -61,7 +62,8 @@ namespace ignition /// \brief Load and instantiate system plugin from name/filename. /// \param[in] _filename Shared library filename to load plugin from. - /// \param[in] _name Class name to be instantiated. + /// \param[in] _name Class name to be instantiated. If empty, the first + /// plugin in the shared library will be loaded. /// \param[in] _sdf SDF Element describing plugin instance to be loaded. /// \returns Shared pointer to system instance or nullptr. /// \note This will be deprecated in Gazebo 7 (Garden), please the use @@ -81,6 +83,10 @@ namespace ignition /// \returns A pretty string public: std::string PrettyStr() const; + /// \brief Get the plugin search paths used for loading system plugins + /// \return Paths to search for plugins + public: std::list PluginPaths() const; + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index fbf7ae020e..fd8556cb7f 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -41,8 +41,7 @@ class ignition::gazebo::SystemLoaderPrivate public: explicit SystemLoaderPrivate() = default; ////////////////////////////////////////////////// - public: bool InstantiateSystemPlugin(const sdf::Plugin &_sdfPlugin, - ignition::plugin::PluginPtr &_gzPlugin) + public: std::list PluginPaths() const { ignition::common::SystemPaths systemPaths; systemPaths.SetPluginPathEnv(pluginPathEnv); @@ -52,9 +51,24 @@ class ignition::gazebo::SystemLoaderPrivate std::string homePath; ignition::common::env(IGN_HOMEDIR, homePath); - systemPaths.AddPluginPaths(homePath + "/.ignition/gazebo/plugins"); + systemPaths.AddPluginPaths(common::joinPaths( + homePath, ".ignition", "gazebo", "plugins")); systemPaths.AddPluginPaths(IGN_GAZEBO_PLUGIN_INSTALL_DIR); + return systemPaths.PluginPaths(); + } + + ////////////////////////////////////////////////// + public: bool InstantiateSystemPlugin(const sdf::Plugin &_sdfPlugin, + ignition::plugin::PluginPtr &_gzPlugin) + { + std::list paths = this->PluginPaths(); + common::SystemPaths systemPaths; + for (const auto &p : paths) + { + systemPaths.AddPluginPaths(p); + } + auto pathToLib = systemPaths.FindSharedLibrary(_sdfPlugin.Filename()); if (pathToLib.empty()) { @@ -85,7 +99,11 @@ class ignition::gazebo::SystemLoaderPrivate return false; } - _gzPlugin = this->loader.Instantiate(_sdfPlugin.Name()); + // use the first plugin name in the library if not specified + std::string pluginToInstantiate = _sdfPlugin.Name().empty() ? + pluginName : _sdfPlugin.Name(); + + _gzPlugin = this->loader.Instantiate(pluginToInstantiate); if (!_gzPlugin) { ignerr << "Failed to load system plugin [" << _sdfPlugin.Name() << @@ -129,6 +147,12 @@ SystemLoader::SystemLoader() ////////////////////////////////////////////////// SystemLoader::~SystemLoader() = default; +////////////////////////////////////////////////// +std::list SystemLoader::PluginPaths() const +{ + return this->dataPtr->PluginPaths(); +} + ////////////////////////////////////////////////// void SystemLoader::AddSystemPluginPath(const std::string &_path) { @@ -141,11 +165,10 @@ std::optional SystemLoader::LoadPlugin( const std::string &_name, const sdf::ElementPtr &_sdf) { - if (_filename == "" || _name == "") + if (_filename == "") { ignerr << "Failed to instantiate system plugin: empty argument " - "[(filename): " << _filename << "] " << - "[(name): " << _name << "]." << std::endl; + "[(filename): " << _filename << "] " << std::endl; return {}; } @@ -175,18 +198,16 @@ std::optional SystemLoader::LoadPlugin( { ignition::plugin::PluginPtr plugin; - if (_plugin.Filename() == "" || _plugin.Name() == "") + if (_plugin.Filename() == "") { ignerr << "Failed to instantiate system plugin: empty argument " - "[(filename): " << _plugin.Filename() << "] " << - "[(name): " << _plugin.Name() << "]." << std::endl; + "[(filename): " << _plugin.Filename() << "] " << std::endl; return {}; } auto ret = this->dataPtr->InstantiateSystemPlugin(_plugin, plugin); if (ret && plugin) return plugin; - return {}; } diff --git a/src/SystemLoader_TEST.cc b/src/SystemLoader_TEST.cc index 8ca53aff47..d1c8b38bd6 100644 --- a/src/SystemLoader_TEST.cc +++ b/src/SystemLoader_TEST.cc @@ -27,6 +27,7 @@ #include "ignition/gazebo/test_config.hh" // NOLINT(build/include) using namespace ignition; +using namespace gazebo; ///////////////////////////////////////////////// TEST(SystemLoader, Constructor) @@ -67,3 +68,38 @@ TEST(SystemLoader, EmptyNames) auto system = sm.LoadPlugin(plugin); ASSERT_FALSE(system.has_value()); } + +///////////////////////////////////////////////// +TEST(SystemLoader, PluginPaths) +{ + SystemLoader sm; + + // verify that there should exist some default paths + std::list paths = sm.PluginPaths(); + unsigned int pathCount = paths.size(); + EXPECT_LT(0u, pathCount); + + // Add test path and verify that the loader now contains this path + auto testBuildPath = common::joinPaths( + std::string(PROJECT_BINARY_PATH), "lib"); + sm.AddSystemPluginPath(testBuildPath); + paths = sm.PluginPaths(); + + // Number of paths should increase by 1 + EXPECT_EQ(pathCount + 1, paths.size()); + + // verify newly added paths exists + bool hasPath = false; + for (const auto &s : paths) + { + // the returned path string may not be exact match due to extra '/' + // appended at the end of the string. So use absPath to generate + // a path string that matches the format returned by joinPaths + if (common::absPath(s) == testBuildPath) + { + hasPath = true; + break; + } + } + EXPECT_TRUE(hasPath); +} diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 8440364fc8..ff3bc2f65b 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -15,6 +15,11 @@ * */ +#include +#include + +#include + #include "ignition/gazebo/components/SystemPluginInfo.hh" #include "ignition/gazebo/Conversions.hh" #include "SystemManager.hh" @@ -34,11 +39,15 @@ SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, transport::NodeOptions opts; opts.SetNameSpace(_namespace); this->node = std::make_unique(opts); - std::string entitySystemService{"entity/system/add"}; - this->node->Advertise(entitySystemService, + std::string entitySystemAddService{"entity/system/add"}; + this->node->Advertise(entitySystemAddService, &SystemManager::EntitySystemAddService, this); ignmsg << "Serving entity system service on [" - << "/" << entitySystemService << "]" << std::endl; + << "/" << entitySystemAddService << "]" << std::endl; + + std::string entitySystemInfoService{"system/info"}; + this->node->Advertise(entitySystemInfoService, + &SystemManager::EntitySystemInfoService, this); } ////////////////////////////////////////////////// @@ -215,6 +224,51 @@ bool SystemManager::EntitySystemAddService(const msgs::EntityPlugin_V &_req, return true; } +////////////////////////////////////////////////// +bool SystemManager::EntitySystemInfoService(const msgs::Empty &, + msgs::EntityPlugin_V &_res) +{ + // loop through all files in paths and populate the list of + // plugin libraries. + std::list paths = this->systemLoader->PluginPaths(); + std::set filenames; + for (const auto &p : paths) + { + if (common::exists(p)) + { + for (common::DirIter file(p); + file != common::DirIter(); ++file) + { + std::string current(*file); + std::string filename = common::basename(current); + if (common::isFile(current) && + (common::EndsWith(filename, ".so") || + common::EndsWith(filename, ".dll") || + common::EndsWith(filename, ".dylib"))) + { + // remove extension and lib prefix + size_t extensionIndex = filename.rfind("."); + std::string nameWithoutExtension = + filename.substr(0, extensionIndex); + if (common::StartsWith(nameWithoutExtension, "lib")) + { + nameWithoutExtension = nameWithoutExtension.substr(3); + } + filenames.insert(nameWithoutExtension); + } + } + } + } + + for (const auto &fn : filenames) + { + auto plugin = _res.add_plugins(); + plugin->set_filename(fn); + } + + return true; +} + ////////////////////////////////////////////////// void SystemManager::ProcessPendingEntitySystems() { diff --git a/src/SystemManager.hh b/src/SystemManager.hh index c23ee38cee..a7bf1484ee 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -142,6 +142,14 @@ namespace ignition private: bool EntitySystemAddService(const msgs::EntityPlugin_V &_req, msgs::Boolean &_res); + /// \brief Callback for entity info system service. + /// \param[in] _req Empty request message + /// \param[out] _res Response containing a list of plugin names + /// and filenames + /// \return True if request received. + private: bool EntitySystemInfoService(const msgs::Empty &_req, + msgs::EntityPlugin_V &_res); + /// \brief All the systems. private: std::vector systems; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index d235fce439..0c9fe29d50 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -68,6 +69,7 @@ #include "ignition/gazebo/components/Volume.hh" #include "ignition/gazebo/components/WindMode.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/config.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/GuiEvents.hh" @@ -118,9 +120,35 @@ namespace ignition::gazebo /// \brief Handles all system info components. public: std::unique_ptr systemInfo; + + /// \brief A list of system plugin human readable names. + public: QStringList systemNameList; + + /// \brief Maps plugin display names to their filenames. + public: std::unordered_map systemMap; }; } +// Helper to remove a prefix from a string if present +void removePrefix(const std::string &_prefix, std::string &_s) +{ + auto id = _s.find(_prefix); + if (id != std::string::npos) + { + _s = _s.substr(_prefix.length()); + } +} + +// Helper to remove a suffix from a string if present +void removeSuffix(const std::string &_suffix, std::string &_s) +{ + auto id = _s.find(_suffix); + if (id != std::string::npos && id + _suffix.length() == _s.length()) + { + _s.erase(id, _suffix.length()); + } +} + using namespace ignition; using namespace gazebo; @@ -895,7 +923,6 @@ void ComponentInspector::Update(const UpdateInfo &, auto comp = _ecm.Component(this->dataPtr->entity); if (comp) { - this->SetType("material"); setData(item, comp->Data()); } } @@ -1216,6 +1243,106 @@ transport::Node &ComponentInspector::TransportNode() return this->dataPtr->node; } +///////////////////////////////////////////////// +void ComponentInspector::QuerySystems() +{ + msgs::Empty req; + msgs::EntityPlugin_V res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/" + this->dataPtr->worldName + + "/system/info"}; + if (!this->dataPtr->node.Request(service, req, timeout, res, result)) + { + ignerr << "Unable to query available systems." << std::endl; + return; + } + + this->dataPtr->systemNameList.clear(); + this->dataPtr->systemMap.clear(); + for (const auto &plugin : res.plugins()) + { + if (plugin.filename().empty()) + { + ignerr << "Received empty plugin name. This shouldn't happen." + << std::endl; + continue; + } + + // Remove common prefixes and suffixes + auto humanReadable = plugin.filename(); + removePrefix("ignition-gazebo-", humanReadable); + removePrefix("ignition-gazebo" + + std::string(IGNITION_GAZEBO_MAJOR_VERSION_STR) + "-", humanReadable); + removeSuffix("-system", humanReadable); + removeSuffix("system", humanReadable); + removeSuffix("-plugin", humanReadable); + removeSuffix("plugin", humanReadable); + + // Replace - with space, capitalize + std::replace(humanReadable.begin(), humanReadable.end(), '-', ' '); + humanReadable[0] = std::toupper(humanReadable[0]); + + this->dataPtr->systemMap[humanReadable] = plugin.filename(); + this->dataPtr->systemNameList.push_back( + QString::fromStdString(humanReadable)); + } + this->dataPtr->systemNameList.sort(); + this->dataPtr->systemNameList.removeDuplicates(); + this->SystemNameListChanged(); +} + +///////////////////////////////////////////////// +QStringList ComponentInspector::SystemNameList() const +{ + return this->dataPtr->systemNameList; +} + +///////////////////////////////////////////////// +void ComponentInspector::SetSystemNameList(const QStringList &_list) +{ + this->dataPtr->systemNameList = _list; +} + +///////////////////////////////////////////////// +void ComponentInspector::OnAddSystem(const QString &_name, + const QString &_filename, const QString &_innerxml) +{ + auto filenameStr = _filename.toStdString(); + auto it = this->dataPtr->systemMap.find(filenameStr); + if (it == this->dataPtr->systemMap.end()) + { + ignerr << "Internal error: failed to find [" << filenameStr + << "] in system map." << std::endl; + return; + } + + msgs::EntityPlugin_V req; + auto ent = req.mutable_entity(); + ent->set_id(this->dataPtr->entity); + auto plugin = req.add_plugins(); + std::string name = _name.toStdString(); + std::string filename = this->dataPtr->systemMap[filenameStr]; + std::string innerxml = _innerxml.toStdString(); + plugin->set_name(name); + plugin->set_filename(filename); + plugin->set_innerxml(innerxml); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/" + this->dataPtr->worldName + + "/entity/system/add"}; + if (!this->dataPtr->node.Request(service, req, timeout, res, result)) + { + ignerr << "Error adding new system to entity: " + << this->dataPtr->entity << "\n" + << "Name: " << name << "\n" + << "Filename: " << filename << "\n" + << "Inner XML: " << innerxml << std::endl; + } +} + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gazebo::ComponentInspector, ignition::gui::Plugin) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index a4800795e2..9e8735a928 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -212,6 +212,14 @@ namespace gazebo NOTIFY NestedModelChanged ) + /// \brief System display name list + Q_PROPERTY( + QStringList systemNameList + READ SystemNameList + WRITE SetSystemNameList + NOTIFY SystemNameListChanged + ) + /// \brief Constructor public: ComponentInspector(); @@ -372,6 +380,28 @@ namespace gazebo /// \return Transport node public: transport::Node &TransportNode(); + /// \brief Query system plugin info. + public: Q_INVOKABLE void QuerySystems(); + + /// \brief Get the system plugin display name list + /// \return A list of names that are potentially system plugins + public: Q_INVOKABLE QStringList SystemNameList() const; + + /// \brief Set the system plugin display name list + /// \param[in] _systempFilenameList A list of system plugin display names + public: Q_INVOKABLE void SetSystemNameList( + const QStringList &_systemNameList); + + /// \brief Notify that system plugin display name list has changed + signals: void SystemNameListChanged(); + + /// \brief Callback when a new system is to be added to an entity + /// \param[in] _name Name of system + /// \param[in] _filename Filename of system + /// \param[in] _innerxml Inner XML content of the system + public: Q_INVOKABLE void OnAddSystem(const QString &_name, + const QString &_filename, const QString &_innerxml); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 21f43e9506..14bbee7153 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -15,9 +15,9 @@ * */ import QtQuick 2.9 -import QtQuick.Controls 1.4 import QtQuick.Controls 2.2 import QtQuick.Controls.Material 2.1 +import QtQuick.Dialogs 1.1 import QtQuick.Layouts 1.3 import QtQuick.Controls.Styles 1.4 import IgnGazebo 1.0 as IgnGazebo @@ -216,6 +216,27 @@ Rectangle { } } + ToolButton { + id: addSystemButton + checkable: false + text: "\u002B" + contentItem: Text { + text: addSystemButton.text + color: "#b5b5b5" + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + } + visible: (entityType == "model" || entityType == "visual" || + entityType == "sensor" || entityType == "world") + ToolTip.text: "Add a system to this entity" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + onClicked: { + addSystemDialog.open() + } + } + + Label { id: entityLabel text: 'Entity ' + ComponentInspector.entity @@ -227,6 +248,111 @@ Rectangle { } } + Dialog { + id: addSystemDialog + modal: false + focus: true + title: "Add System" + closePolicy: Popup.CloseOnEscape + width: parent.width + + ColumnLayout { + width: parent.width + GridLayout { + columns: 2 + columnSpacing: 30 + Text { + text: "Name" + Layout.row: 0 + Layout.column: 0 + } + + TextField { + id: nameField + selectByMouse: true + Layout.row: 0 + Layout.column: 1 + Layout.fillWidth: true + Layout.minimumWidth: 250 + onTextEdited: { + addSystemDialog.updateButtonState(); + } + placeholderText: "Leave empty to load first plugin" + } + + Text { + text: "Filename" + Layout.row: 1 + Layout.column: 0 + } + + ComboBox { + id: filenameCB + Layout.row: 1 + Layout.column: 1 + Layout.fillWidth: true + Layout.minimumWidth: 250 + model: ComponentInspector.systemNameList + currentIndex: 0 + onCurrentIndexChanged: { + if (currentIndex < 0) + return; + addSystemDialog.updateButtonState(); + } + ToolTip.visible: hovered + ToolTip.delay: tooltipDelay + ToolTip.text: currentText + } + } + + Text { + id: innerxmlLabel + text: "Inner XML" + } + + Flickable { + id: innerxmlFlickable + Layout.minimumHeight: 300 + Layout.fillWidth: true + Layout.fillHeight: true + + flickableDirection: Flickable.VerticalFlick + TextArea.flickable: TextArea { + id: innerxmlField + wrapMode: Text.WordWrap + selectByMouse: true + textFormat: TextEdit.PlainText + font.pointSize: 10 + } + ScrollBar.vertical: ScrollBar { + policy: ScrollBar.AlwaysOn + } + } + } + + footer: DialogButtonBox { + id: buttons + standardButtons: Dialog.Ok | Dialog.Cancel + } + + onOpened: { + ComponentInspector.QuerySystems(); + addSystemDialog.updateButtonState(); + } + + onAccepted: { + ComponentInspector.OnAddSystem(nameField.text.trim(), + filenameCB.currentText.trim(), innerxmlField.text.trim()) + } + + function updateButtonState() { + buttons.standardButton(Dialog.Ok).enabled = + (filenameCB.currentText.trim() != '') + } + } + + + ListView { anchors.top: header.bottom anchors.bottom: parent.bottom diff --git a/test/integration/entity_system.cc b/test/integration/entity_system.cc index d4ea7ce57c..338cf4e685 100644 --- a/test/integration/entity_system.cc +++ b/test/integration/entity_system.cc @@ -158,6 +158,54 @@ TEST_P(EntitySystemTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) "/model/vehicle/cmd_vel"); } +///////////////////////////////////////////////// +TEST_P(EntitySystemTest, SystemInfo) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/empty.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // get info on systems available + msgs::Empty req; + msgs::EntityPlugin_V res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/empty/system/info"}; + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + + // verify plugins are not empty + EXPECT_FALSE(res.plugins().empty()); + + // check for a few known plugins that we know should exist in gazebo + std::set knownPlugins; + knownPlugins.insert("user-commands-system"); + knownPlugins.insert("physics-system"); + knownPlugins.insert("scene-broadcaster-system"); + knownPlugins.insert("sensors-system"); + + for (const auto &plugin : res.plugins()) + { + for (const auto &kp : knownPlugins) + { + if (plugin.filename().find(kp) != std::string::npos) + { + knownPlugins.erase(kp); + break; + } + } + } + // verify all known plugins are found and removed from the set + EXPECT_TRUE(knownPlugins.empty()); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, EntitySystemTest, ::testing::Range(1, 2)); diff --git a/test/worlds/diff_drive_no_plugin.sdf b/test/worlds/diff_drive_no_plugin.sdf index 1792199086..1a70e4eff1 100644 --- a/test/worlds/diff_drive_no_plugin.sdf +++ b/test/worlds/diff_drive_no_plugin.sdf @@ -10,6 +10,10 @@ filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics"> + + true From 1a612696855727842e3ff1f501d98ad855990deb Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 6 Oct 2022 09:42:04 -0700 Subject: [PATCH 018/160] Update examples to use gazebosim.org Signed-off-by: Nate Koenig --- examples/standalone/joy_to_twist/README.md | 4 +- examples/standalone/joystick/README.md | 2 +- examples/worlds/actor.sdf | 6 +- examples/worlds/actors_population.sdf.erb | 4 +- examples/worlds/breadcrumbs.sdf | 2 +- examples/worlds/camera_sensor.sdf | 2 +- examples/worlds/depth_camera_sensor.sdf | 2 +- examples/worlds/elevator.sdf | 2 +- examples/worlds/fuel.sdf | 14 +- examples/worlds/fuel_textured_mesh.sdf | 4 +- examples/worlds/gpu_lidar_sensor.sdf | 2 +- examples/worlds/import_mesh.sdf | 2 +- examples/worlds/log_record_resources.sdf | 2 +- .../worlds/multicopter_velocity_control.sdf | 4 +- examples/worlds/plot_3d.sdf | 2 +- examples/worlds/quadcopter.sdf | 2 +- examples/worlds/sensors_demo.sdf | 2 +- examples/worlds/tracked_vehicle_simple.sdf | 2 +- examples/worlds/tunnel.sdf | 278 +++++++++--------- 19 files changed, 169 insertions(+), 169 deletions(-) diff --git a/examples/standalone/joy_to_twist/README.md b/examples/standalone/joy_to_twist/README.md index 893bdeed1d..1dbc40f6a4 100644 --- a/examples/standalone/joy_to_twist/README.md +++ b/examples/standalone/joy_to_twist/README.md @@ -1,9 +1,9 @@ # Joy to Twist Standalone program that subscribes to -[ignition::msgs::Joy](https://ignitionrobotics.org/api/msgs/5.6/classignition_1_1msgs_1_1Joy.html) +[ignition::msgs::Joy](https://gazebosim.org/api/msgs/5.6/classignition_1_1msgs_1_1Joy.html) messages and converts publishes -[ignition::msgs::Twist](https://ignitionrobotics.org/api/msgs/5.6/classignition_1_1msgs_1_1Twist.html) +[ignition::msgs::Twist](https://gazebosim.org/api/msgs/5.6/classignition_1_1msgs_1_1Twist.html) messages according to user-defined configuration. ## Build diff --git a/examples/standalone/joystick/README.md b/examples/standalone/joystick/README.md index 9e05e2e172..9292b6bd9e 100644 --- a/examples/standalone/joystick/README.md +++ b/examples/standalone/joystick/README.md @@ -1,7 +1,7 @@ # Joystick Standalone program that publishes -[ignition::msgs::Joy](https://ignitionrobotics.org/api/msgs/5.6/classignition_1_1msgs_1_1Joy.html) +[ignition::msgs::Joy](https://gazebosim.org/api/msgs/5.6/classignition_1_1msgs_1_1Joy.html) messages from a joystick device using Ignition Transport. The mapping of joystick buttons to fields in the message is the same as [this](http://wiki.ros.org/joy). diff --git a/examples/worlds/actor.sdf b/examples/worlds/actor.sdf index e69a66513f..b805eee94f 100644 --- a/examples/worlds/actor.sdf +++ b/examples/worlds/actor.sdf @@ -147,16 +147,16 @@ - https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor + https://fuel.gazebosim.org/1.0/Mingfei/models/actor - https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae 1.0 - https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae 0.055 true diff --git a/examples/worlds/actors_population.sdf.erb b/examples/worlds/actors_population.sdf.erb index f2bd07ece1..79d017e67d 100644 --- a/examples/worlds/actors_population.sdf.erb +++ b/examples/worlds/actors_population.sdf.erb @@ -160,11 +160,11 @@ - https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae 1.0 - https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae 0.055 true diff --git a/examples/worlds/breadcrumbs.sdf b/examples/worlds/breadcrumbs.sdf index b3ade84636..79cccff819 100644 --- a/examples/worlds/breadcrumbs.sdf +++ b/examples/worlds/breadcrumbs.sdf @@ -404,7 +404,7 @@ -2 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X2 Config 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/X2 Config 1 diff --git a/examples/worlds/camera_sensor.sdf b/examples/worlds/camera_sensor.sdf index b79e76b81b..cec60815ea 100644 --- a/examples/worlds/camera_sensor.sdf +++ b/examples/worlds/camera_sensor.sdf @@ -256,7 +256,7 @@ 0 1 3 0.0 0.0 1.57 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Construction Cone diff --git a/examples/worlds/depth_camera_sensor.sdf b/examples/worlds/depth_camera_sensor.sdf index ce693fe1b0..e9c3afd2d8 100644 --- a/examples/worlds/depth_camera_sensor.sdf +++ b/examples/worlds/depth_camera_sensor.sdf @@ -225,7 +225,7 @@ 0 1 3 0.0 0.0 1.57 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Barrel + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Construction Barrel diff --git a/examples/worlds/elevator.sdf b/examples/worlds/elevator.sdf index d2b1238e6b..77d01899cb 100644 --- a/examples/worlds/elevator.sdf +++ b/examples/worlds/elevator.sdf @@ -73,7 +73,7 @@ - https://fuel.ignitionrobotics.org/1.0/nlamprian/models/Elevator + https://fuel.gazebosim.org/1.0/nlamprian/models/Elevator diff --git a/examples/worlds/fuel.sdf b/examples/worlds/fuel.sdf index 2831408576..2fc59f83db 100644 --- a/examples/worlds/fuel.sdf +++ b/examples/worlds/fuel.sdf @@ -1,6 +1,6 @@ @@ -61,24 +61,24 @@ -3 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Double pendulum with base 3 -1 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack 2 5 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Gazebo - relative paths + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Gazebo - relative paths - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/actor - relative paths + https://fuel.gazebosim.org/1.0/OpenRobotics/models/actor - relative paths @@ -89,14 +89,14 @@ - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae diff --git a/examples/worlds/fuel_textured_mesh.sdf b/examples/worlds/fuel_textured_mesh.sdf index 7c2a39f95a..f69c8b5aa3 100644 --- a/examples/worlds/fuel_textured_mesh.sdf +++ b/examples/worlds/fuel_textured_mesh.sdf @@ -166,7 +166,7 @@ true Rescue Randy 0 0 0 0 0 1.57 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Rescue Randy + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Rescue Randy @@ -174,7 +174,7 @@ true Tube Light 1.5 0 3 0 0.78 0 - https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Tube Light + https://fuel.gazebosim.org/1.0/openrobotics/models/Tube Light diff --git a/examples/worlds/gpu_lidar_sensor.sdf b/examples/worlds/gpu_lidar_sensor.sdf index 07020dda9b..c8efaaefda 100644 --- a/examples/worlds/gpu_lidar_sensor.sdf +++ b/examples/worlds/gpu_lidar_sensor.sdf @@ -169,7 +169,7 @@ 0 0 0 0 0 1.57 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Playground + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Playground diff --git a/examples/worlds/import_mesh.sdf b/examples/worlds/import_mesh.sdf index 1309625e2d..b10e41ef5d 100644 --- a/examples/worlds/import_mesh.sdf +++ b/examples/worlds/import_mesh.sdf @@ -141,7 +141,7 @@ true Electrical Box 0 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Electrical Box + https://fuel.gazebosim.org/1.0/openrobotics/models/Electrical Box diff --git a/examples/worlds/log_record_resources.sdf b/examples/worlds/log_record_resources.sdf index 6bf9adb87c..d258b89e40 100644 --- a/examples/worlds/log_record_resources.sdf +++ b/examples/worlds/log_record_resources.sdf @@ -52,7 +52,7 @@ false staging_area 0 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X2 Config 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/X2 Config 1 diff --git a/examples/worlds/multicopter_velocity_control.sdf b/examples/worlds/multicopter_velocity_control.sdf index 57e5bed01f..75b98272cf 100644 --- a/examples/worlds/multicopter_velocity_control.sdf +++ b/examples/worlds/multicopter_velocity_control.sdf @@ -89,7 +89,7 @@ You can use the velocity controller and command linear velocity and yaw angular - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X3 UAV/4 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/X3 UAV/4 0 3 1 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X4 UAV Config 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/X4 UAV Config 1 Double_pendulum - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Double pendulum with base diff --git a/examples/worlds/quadcopter.sdf b/examples/worlds/quadcopter.sdf index 224a11cda9..89a507d1ff 100644 --- a/examples/worlds/quadcopter.sdf +++ b/examples/worlds/quadcopter.sdf @@ -78,7 +78,7 @@ - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X3 UAV/4 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/X3 UAV/4 0 1 3 0.0 0.0 1.57 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Construction Cone diff --git a/examples/worlds/tracked_vehicle_simple.sdf b/examples/worlds/tracked_vehicle_simple.sdf index cf9cd29acc..fbc77a7d0d 100644 --- a/examples/worlds/tracked_vehicle_simple.sdf +++ b/examples/worlds/tracked_vehicle_simple.sdf @@ -1719,7 +1719,7 @@ pallet 2 -2 0.1 0 0 0 - https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Euro pallet + https://fuel.gazebosim.org/1.0/openrobotics/models/Euro pallet diff --git a/examples/worlds/tunnel.sdf b/examples/worlds/tunnel.sdf index 484c80ea67..a61e9546ff 100644 --- a/examples/worlds/tunnel.sdf +++ b/examples/worlds/tunnel.sdf @@ -34,181 +34,181 @@ true BaseStation 0 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/subt_tunnel_staging_area + https://fuel.gazebosim.org/1.0/OpenRobotics/models/subt_tunnel_staging_area artifact_origin 2 4 0.5 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fiducial + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Fiducial tile_0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 240.000000 240.000000 -15.000000 0 0 0.000000 tile_1 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 260.000000 240.000000 -15.000000 0 0 1.570796 tile_2 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Constrained Tunnel Tile Tall + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Constrained Tunnel Tile Tall 280.000000 240.000000 -15.000000 0 0 1.570796 tile_3 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 300.000000 240.000000 -15.000000 0 0 4.712389 tile_4 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 160.000000 211.000000 -15.000000 0 0 0.000000 tile_5 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 240.000000 220.000000 -15.000000 0 0 0.000000 radio_1 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Radio 241.500000 220.000000 -15.000000 0 0 0.000000 tile_6 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 300.000000 220.000000 -15.000000 0 0 0.000000 tile_7 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 69.000000 200.000000 -15.000000 0 0 0.000000 tile_8 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 80.000000 200.000000 -15.000000 0 0 1.570796 tile_9 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 100.000000 200.000000 -15.000000 0 0 1.570796 tile_10 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 120.000000 200.000000 -15.000000 0 0 1.570796 tile_11 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 140.000000 200.000000 -15.000000 0 0 1.570796 tile_12 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 1 160.000000 200.000000 -15.000000 0 0 0.000000 tile_13 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 180.000000 200.000000 -15.000000 0 0 1.570796 tile_14 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 200.000000 200.000000 -15.000000 0 0 1.570796 tile_15 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 220.000000 200.000000 -15.000000 0 0 1.570796 tile_16 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 240.000000 200.000000 -15.000000 0 0 3.141593 tile_17 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 300.000000 200.000000 -15.000000 0 0 0.000000 tile_18 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 160.000000 180.000000 -15.000000 0 0 0.000000 backpack_1 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack 219.000000 202.000000 -15.000000 0 0 0.000000 tile_19 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 260.000000 180.000000 -15.000000 0 0 0.000000 tile_20 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 280.000000 180.000000 -15.000000 0 0 1.570796 tile_21 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 1 300.000000 180.000000 -15.000000 0 0 0.000000 tile_22 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 320.000000 180.000000 -15.000000 0 0 1.570796 tile_23 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 340.000000 180.000000 -15.000000 0 0 4.712389 tile_24 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Constrained Tunnel Tile Tall + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Constrained Tunnel Tile Tall 160.000000 160.000000 -15.000000 0 0 0.000000 @@ -216,509 +216,509 @@ phone_1 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Phone + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Phone 260.000000 160.000000 -14.996000 -1.570796 0 0.000000 tile_25 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 260.000000 160.000000 -15.000000 0 0 0.000000 tile_26 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 300.000000 169.000000 -15.000000 0 0 0.000000 tile_27 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 340.000000 160.000000 -15.000000 0 0 0.000000 toolbox_1 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Toolbox + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Toolbox 342.000000 160.000000 -15.000000 0 0 0.000000 tile_28 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 60.000000 131.000000 -15.000000 0 0 0.000000 tile_29 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 80.000000 131.000000 -15.000000 0 0 0.000000 tile_30 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 160.000000 140.000000 -15.000000 0 0 0.000000 extinguisher_1 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Extinguisher + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Extinguisher 158.000000 140.000000 -15.000000 0 0 0.000000 tile_31 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 260.000000 149.000000 -15.000000 0 0 0.000000 tile_32 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 340.000000 149.000000 -15.000000 0 0 0.000000 tile_33 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 60.000000 120.000000 -15.000000 0 0 1.570796 tile_34 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 1 80.000000 120.000000 -15.000000 0 0 0.000000 tile_35 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 100.000000 120.000000 -15.000000 0 0 1.570796 tile_36 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 120.000000 120.000000 -15.000000 0 0 1.570796 tile_37 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 140.000000 120.000000 -15.000000 0 0 1.570796 tile_38 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 160.000000 120.000000 -15.000000 0 0 3.141593 tile_39 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 80.000000 100.000000 -15.000000 0 0 0.000000 tile_40 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 200.000000 100.000000 -15.000000 0 0 0.000000 tile_41 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 220.000000 100.000000 -15.000000 0 0 1.570796 tile_42 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 240.000000 100.000000 -15.000000 0 0 1.570796 tile_43 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 260.000000 100.000000 -15.000000 0 0 4.712389 radio_2 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Radio 260.000000 82.300000 -15.000000 0 0 0.000000 tile_44 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 80.000000 80.000000 -15.000000 0 0 0.000000 tile_45 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 200.000000 80.000000 -15.000000 0 0 0.000000 tile_46 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 260.000000 80.000000 -15.000000 0 0 1.570796 tile_47 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 280.000000 80.000000 -15.000000 0 0 4.712389 tile_48 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 6 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 6 80.000000 60.000000 -15.000000 0 0 3.141593 tile_49 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 200.000000 60.000000 -15.000000 0 0 0.000000 tile_50 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 280.000000 60.000000 -15.000000 0 0 0.000000 toolbox_2 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Toolbox + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Toolbox 278.000000 60.000000 -15.000000 0 0 0.000000 tile_51 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 80.000000 40.000000 -10.000000 0 0 0.000000 tile_52 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 200.000000 40.000000 -15.000000 0 0 0.000000 tile_53 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 280.000000 40.000000 -15.000000 0 0 0.000000 tile_54 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 80.000000 20.000000 -10.000000 0 0 0.000000 tile_55 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 200.000000 20.000000 -15.000000 0 0 0.000000 phone_2 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Phone + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Phone 201.800000 20.000000 -14.996000 -1.570796 0 0.000000 tile_56 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 280.000000 20.000000 -15.000000 0 0 0.000000 tile_57 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 320.000000 20.000000 -20.000000 0 0 0.000000 tile_58 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 340.000000 20.000000 -20.000000 0 0 1.570796 tile_59 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 360.000000 20.000000 -20.000000 0 0 4.712389 electrical_box_1 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Electrical Box + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Electrical Box 400.000000 -1.000000 -20.000000 0 0 0.000000 tile_60 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 20.000000 0.000000 0.000000 0 0 1.570796 tile_61 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 6 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 6 40.000000 0.000000 -5.000000 0 0 1.570796 tile_62 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 6 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 6 60.000000 0.000000 -10.000000 0 0 1.570796 tile_63 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 1 80.000000 0.000000 -10.000000 0 0 0.000000 tile_64 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 100.000000 0.000000 -10.000000 0 0 1.570796 tile_65 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 120.000000 0.000000 -10.000000 0 0 1.570796 tile_66 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 140.000000 0.000000 -10.000000 0 0 1.570796 tile_67 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 6 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 6 160.000000 0.000000 -15.000000 0 0 1.570796 tile_68 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 180.000000 0.000000 -15.000000 0 0 1.570796 tile_69 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 1 200.000000 0.000000 -15.000000 0 0 0.000000 tile_70 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 211.000000 0.000000 -15.000000 0 0 0.000000 tile_71 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 269.000000 0.000000 -15.000000 0 0 0.000000 tile_72 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 1 280.000000 0.000000 -15.000000 0 0 0.000000 tile_73 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 6 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 6 300.000000 0.000000 -20.000000 0 0 1.570796 tile_74 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 1 320.000000 0.000000 -20.000000 0 0 0.000000 tile_75 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 340.000000 0.000000 -20.000000 0 0 1.570796 tile_76 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 1 360.000000 0.000000 -20.000000 0 0 0.000000 tile_77 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 380.000000 0.000000 -20.000000 0 0 1.570796 tile_78 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 400.000000 0.000000 -20.000000 0 0 1.570796 tile_79 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 411.000000 0.000000 -20.000000 0 0 0.000000 tile_80 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 7 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 7 80.000000 -20.000000 -10.000000 0 0 3.141593 tile_81 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 200.000000 -20.000000 -15.000000 0 0 0.000000 tile_82 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 280.000000 -20.000000 -15.000000 0 0 0.000000 tile_83 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 320.000000 -20.000000 -20.000000 0 0 1.570796 tile_84 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 340.000000 -20.000000 -20.000000 0 0 1.570796 tile_85 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 360.000000 -20.000000 -20.000000 0 0 3.141593 tile_86 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 80.000000 -40.000000 -5.000000 0 0 0.000000 tile_87 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 200.000000 -40.000000 -15.000000 0 0 0.000000 tile_88 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 280.000000 -40.000000 -15.000000 0 0 0.000000 backpack_2 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack 340.000000 -22.000000 -20.000000 0 0 0.000000 tile_89 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 80.000000 -60.000000 -5.000000 0 0 0.000000 tile_90 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 200.000000 -60.000000 -15.000000 0 0 1.570796 tile_91 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 220.000000 -60.000000 -15.000000 0 0 1.570796 tile_92 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 240.000000 -60.000000 -15.000000 0 0 1.570796 tile_93 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 260.000000 -60.000000 -15.000000 0 0 1.570796 tile_94 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 280.000000 -60.000000 -15.000000 0 0 3.141593 tile_95 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 80.000000 -80.000000 -5.000000 0 0 1.570796 tile_96 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 7 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 7 100.000000 -80.000000 -10.000000 0 0 1.570796 tile_97 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 120.000000 -80.000000 -10.000000 0 0 4.712389 phone_3 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Phone + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Phone 120.800000 -98.400000 -9.996000 1.570796 0 0.000000 @@ -726,180 +726,180 @@ valve_1 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Valve + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Valve 240.000000 -58.000000 -15.000000 0 0 0.000000 tile_98 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 120.000000 -100.000000 -10.000000 0 0 1.570796 tile_99 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 140.000000 -100.000000 -10.000000 0 0 4.712389 tile_100 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 140.000000 -120.000000 -10.000000 0 0 0.000000 valve_2 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Valve + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Valve 141.750000 -119.000000 -10.000000 0 0 0.000000 tile_101 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 140.000000 -140.000000 -10.000000 0 0 0.000000 tile_102 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Constrained Tunnel Tile Short + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Constrained Tunnel Tile Short 140.000000 -160.000000 -10.000000 0 0 0.000000 tile_103 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile Blocker + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile Blocker 280.000000 -169.000000 -10.000000 0 0 0.000000 electrical_box_2 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Electrical Box + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Electrical Box 138.000000 -180.000000 -10.000000 0 0 0.000000 tile_104 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 140.000000 -180.000000 -10.000000 0 0 0.000000 backpack_3 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack 180.000000 -198.000000 -10.000000 0 0 0.000000 tile_105 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 280.000000 -180.000000 -10.000000 0 0 0.000000 tile_106 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 140.000000 -200.000000 -10.000000 0 0 0.000000 tile_107 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 160.000000 -200.000000 -10.000000 0 0 0.000000 tile_108 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 180.000000 -200.000000 -10.000000 0 0 1.570796 tile_109 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 200.000000 -200.000000 -10.000000 0 0 1.570796 tile_110 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 220.000000 -200.000000 -10.000000 0 0 4.712389 tile_111 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 280.000000 -200.000000 -10.000000 0 0 0.000000 tile_112 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 140.000000 -220.000000 -10.000000 0 0 1.570796 tile_113 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 160.000000 -220.000000 -10.000000 0 0 3.141593 tile_114 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 220.000000 -220.000000 -10.000000 0 0 0.000000 tile_115 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 280.000000 -220.000000 -10.000000 0 0 0.000000 extinguisher_2 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Extinguisher + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Extinguisher 278.000000 -220.000000 -10.000000 0 0 0.000000 tile_116 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 220.000000 -240.000000 -10.000000 0 0 1.570796 tile_117 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 240.000000 -240.000000 -10.000000 0 0 1.570796 tile_118 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 5 260.000000 -240.000000 -10.000000 0 0 1.570796 tile_119 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Tunnel Tile 2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Tunnel Tile 2 280.000000 -240.000000 -10.000000 0 0 3.141593 x1 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X1 UGV + https://fuel.gazebosim.org/1.0/OpenRobotics/models/X1 UGV 0 0 .2 0 0 0 From 4dd1858d3a7f35c06f51535b4539690356a9da5f Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Fri, 7 Oct 2022 01:17:36 +0800 Subject: [PATCH 019/160] Adding thrust coefficient calculation (#1652) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * adding thrust coefficient calculation Signed-off-by: Marco A. Gutierrez * Update src/systems/thruster/Thruster.cc Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez * Update src/systems/thruster/Thruster.cc Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez * Update src/systems/thruster/Thruster.cc Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez * Update src/systems/thruster/Thruster.cc Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez * Update src/systems/thruster/Thruster.cc Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez * Update src/systems/thruster/Thruster.cc Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez * Update src/systems/thruster/Thruster.cc Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez * Update src/systems/thruster/Thruster.cc Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez * Update src/systems/thruster/Thruster.hh Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Marco A. Gutierrez * thrust coefficient test and behavior updates Signed-off-by: Marco A. Gutierrez * making float comparision more robust Signed-off-by: Marco A. Gutierrez * fix float comparision and lint Signed-off-by: Marco A. Gutierrez Signed-off-by: Marco A. Gutierrez Co-authored-by: Alejandro Hernández Cordero --- src/systems/thruster/Thruster.cc | 82 +++++++++++++++++++++ src/systems/thruster/Thruster.hh | 16 ++++ test/integration/thruster.cc | 64 ++++++++++++++-- test/worlds/thrust_coefficient.sdf | 114 +++++++++++++++++++++++++++++ 4 files changed, 270 insertions(+), 6 deletions(-) create mode 100644 test/worlds/thrust_coefficient.sdf diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index b775cf26e4..bae8bb04d5 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -16,6 +16,7 @@ */ #include #include +#include #include #include @@ -98,18 +99,39 @@ class ignition::gazebo::systems::ThrusterPrivateData /// thrust public: double thrustCoefficient = 1; + /// \brief True if the thrust coefficient was set by configuration. + public: bool thrustCoefficientSet = false; + + /// \brief Relative speed reduction between the water at the propeller vs + /// behind the vessel. + public: double wakeFraction = 0.2; + + /// \brief Constant given by the open water propeller diagram. Used in the + /// calculation of the thrust coefficient. + public: double alpha1 = 1; + + /// \brief Constant given by the open water propeller diagram. Used in the + /// calculation of the thrust coefficient. + public: double alpha2 = 0; + /// \brief Density of fluid in kgm^-3, default: 1000kgm^-3 public: double fluidDensity = 1000; /// \brief Diameter of propeller in m, default: 0.02 public: double propellerDiameter = 0.02; + /// \brief Linear velocity of the vehicle. + public: double linearVelocity = 0.0; + /// \brief Topic name used to control thrust. public: std::string topic = ""; /// \brief Callback for handling thrust update public: void OnCmdThrust(const msgs::Double &_msg); + /// \brief Recalculates and updates the thrust coefficient. + public: void UpdateThrustCoefficient(); + /// \brief Function which computes angular velocity from thrust /// \param[in] _thrust Thrust in N /// \return Angular velocity in rad/s @@ -160,6 +182,7 @@ void Thruster::Configure( if (_sdf->HasElement("thrust_coefficient")) { this->dataPtr->thrustCoefficient = _sdf->Get("thrust_coefficient"); + this->dataPtr->thrustCoefficientSet = true; } // Get propeller diameter @@ -174,6 +197,40 @@ void Thruster::Configure( this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); } + // Get wake fraction number, default 0.2 otherwise + if (_sdf->HasElement("wake_fraction")) + { + this->dataPtr->wakeFraction = _sdf->Get("wake_fraction"); + } + + // Get alpha_1, default to 1 othwewise + if (_sdf->HasElement("alpha_1")) + { + this->dataPtr->alpha1 = _sdf->Get("alpha_1"); + if (this->dataPtr->thrustCoefficientSet) + { + ignwarn << " The [alpha_2] value will be ignored as a " + << "[thrust_coefficient] was also defined through the SDF file." + << " If you want the system to use the alpha values to calculate" + << " and update the thrust coefficient please remove the " + << "[thrust_coefficient] value from the SDF file." << std::endl; + } + } + + // Get alpha_2, default to 1 othwewise + if (_sdf->HasElement("alpha_2")) + { + this->dataPtr->alpha2 = _sdf->Get("alpha_2"); + if (this->dataPtr->thrustCoefficientSet) + { + ignwarn << " The [alpha_2] value will be ignored as a " + << "[thrust_coefficient] was also defined through the SDF file." + << " If you want the system to use the alpha values to calculate" + << " and update the thrust coefficient please remove the " + << "[thrust_coefficient] value from the SDF file." << std::endl; + } + } + // Get a custom topic. if (_sdf->HasElement("topic")) { @@ -235,6 +292,8 @@ void Thruster::Configure( enableComponent(_ecm, this->dataPtr->linkEntity); enableComponent(_ecm, this->dataPtr->linkEntity); + enableComponent(_ecm, + this->dataPtr->linkEntity); double minThrustCmd = this->dataPtr->cmdMin; double maxThrustCmd = this->dataPtr->cmdMax; @@ -321,6 +380,14 @@ void ThrusterPrivateData::OnCmdThrust(const msgs::Double &_msg) ///////////////////////////////////////////////// double ThrusterPrivateData::ThrustToAngularVec(double _thrust) { + // Only update if the thrust coefficient was not set by configuration + // and angular velocity is not zero. Some velocity is needed to calculate + // the thrust coefficient otherwise it will never start moving. + if (!this->thrustCoefficientSet && + std::abs(this->propellerAngVel) > std::numeric_limits::epsilon()) + { + this->UpdateThrustCoefficient(); + } // Thrust is proportional to the Rotation Rate squared // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 auto propAngularVelocity = sqrt(abs( @@ -333,6 +400,14 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust) return propAngularVelocity; } +///////////////////////////////////////////////// +void ThrusterPrivateData::UpdateThrustCoefficient() +{ + this->thrustCoefficient = this->alpha1 + this->alpha2 * + (((1 - this->wakeFraction) * this->linearVelocity) + / (this->propellerAngVel * this->propellerDiameter)); +} + ///////////////////////////////////////////////// bool ThrusterPrivateData::HasSufficientBattery( const EntityComponentManager &_ecm) const @@ -384,6 +459,8 @@ void Thruster::PreUpdate( { std::lock_guard lock(this->dataPtr->mtx); desiredThrust = this->dataPtr->thrust; + this->dataPtr->propellerAngVel = + this->dataPtr->ThrustToAngularVec(this->dataPtr->thrust); desiredPropellerAngVel = this->dataPtr->propellerAngVel; } @@ -422,6 +499,11 @@ void Thruster::PreUpdate( _ecm, unitVector * desiredThrust, unitVector * torque); + + // Update the LinearVelocity of the vehicle + this->dataPtr->linearVelocity = + _ecm.Component( + this->dataPtr->linkEntity)->Data().Length(); } ///////////////////////////////////////////////// diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 415fa8b660..e40627d465 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -70,6 +70,22 @@ namespace systems /// defaults to 1000N] /// - - Minimum thrust command. [Optional, /// defaults to -1000N] + /// - - Relative speed reduction between the water + /// at the propeller (Va) vs behind the vessel. + /// [Optional, defults to 0.2] + /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: + /// + /// Va = (1 - wake_fraction) * advance_speed + /// + /// - - Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coefficient (Kt). + /// [Optional, defults to 1] + /// - - Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coefficient (Kt). + /// [Optional, defults to 0] + /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: + /// + /// Kt = alpha_1 * alpha_2 * (Va/(propeller_revolution * propeller_diameter)) /// /// ## Example /// An example configuration is installed with Gazebo. The example diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 1e147bfe69..92c60a7d89 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -32,6 +32,9 @@ #include "ignition/gazebo/Util.hh" #include "ignition/gazebo/World.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/Pose.hh" + #include "ignition/gazebo/test_config.hh" #include "../helpers/EnvTestFixture.hh" @@ -50,13 +53,17 @@ class ThrusterTest : public InternalFixture<::testing::Test> /// \param[in] _baseTol Base tolerance for most quantities public: void TestWorld(const std::string &_world, const std::string &_namespace, const std::string &_topic, - double _coefficient, double _density, double _diameter, double _baseTol); + double _thrustCoefficient, double _density, double _diameter, + double _baseTol, double _wakeFraction = 0.2, double _alpha_1 = 1, + double _alpha_2 = 0, bool _calculateCoefficient = false); }; ////////////////////////////////////////////////// void ThrusterTest::TestWorld(const std::string &_world, const std::string &_namespace, const std::string &_topic, - double _coefficient, double _density, double _diameter, double _baseTol) + double _thrustCoefficient, double _density, double _diameter, + double _baseTol, double _wakeFraction, double _alpha1, double _alpha2, + bool _calculateCoefficient) { // Start server ServerConfig serverConfig; @@ -68,6 +75,9 @@ void ThrusterTest::TestWorld(const std::string &_world, Link propeller; std::vector modelPoses; std::vector propellerAngVels; + std::vector propellerLinVels; + ignition::math::Pose3d jointPose, linkWorldPose; + ignition::math::Vector3d jointAxis; double dt{0.0}; fixture. OnConfigure( @@ -82,11 +92,19 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_NE(modelEntity, kNullEntity); model = Model(modelEntity); + auto jointEntity = model.JointByName(_ecm, "propeller_joint"); + jointAxis = + _ecm.Component( + jointEntity)->Data().Xyz(); + jointPose = _ecm.Component( + jointEntity)->Data(); + auto propellerEntity = model.LinkByName(_ecm, "propeller"); EXPECT_NE(propellerEntity, kNullEntity); propeller = Link(propellerEntity); propeller.EnableVelocityChecks(_ecm); + }). OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) @@ -99,6 +117,10 @@ void ThrusterTest::TestWorld(const std::string &_world, auto propellerAngVel = propeller.WorldAngularVelocity(_ecm); ASSERT_TRUE(propellerAngVel); propellerAngVels.push_back(propellerAngVel.value()); + + auto proellerLinVel = propeller.WorldLinearVelocity(_ecm); + ASSERT_TRUE(proellerLinVel); + propellerLinVels.push_back(proellerLinVel.value()); }). Finalize(); @@ -106,6 +128,7 @@ void ThrusterTest::TestWorld(const std::string &_world, fixture.Server()->Run(true, 100, false); EXPECT_EQ(100u, modelPoses.size()); EXPECT_EQ(100u, propellerAngVels.size()); + EXPECT_EQ(100u, propellerLinVels.size()); EXPECT_NE(model.Entity(), kNullEntity); EXPECT_NE(propeller.Entity(), kNullEntity); @@ -120,6 +143,7 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_EQ(math::Vector3d::Zero, vel); } propellerAngVels.clear(); + propellerLinVels.clear(); // Publish command and check that vehicle moved transport::Node node; @@ -146,8 +170,10 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X()); EXPECT_EQ(100u, modelPoses.size()); EXPECT_EQ(100u, propellerAngVels.size()); + EXPECT_EQ(100u, propellerLinVels.size()); modelPoses.clear(); propellerAngVels.clear(); + propellerLinVels.clear(); // input force cmd this should be capped to 300 forceCmd = 1000.0; @@ -168,6 +194,7 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_EQ(100u * sleep, modelPoses.size()); EXPECT_EQ(100u * sleep, propellerAngVels.size()); + EXPECT_EQ(100u * sleep, propellerLinVels.size()); } // max allowed force @@ -205,13 +232,26 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_NEAR(0.0, pose.Rot().Roll(), _baseTol); } - // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 - // omega = sqrt(thrust / - // (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) - auto omega = sqrt(force / (_density * _coefficient * pow(_diameter, 4))); + auto jointWorldPose = linkWorldPose * jointPose; + auto unitVector = jointWorldPose.Rot().RotateVector(jointAxis).Normalize(); + double omegaTol{1e-1}; for (unsigned int i = 0; i < propellerAngVels.size(); ++i) { + auto angularVelocity = propellerAngVels[i].Dot(unitVector); + // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246: + // thrust_coefficient = alpha_1 + alpha_2 * (((1-wake_fraction) * + // linear_velocity) / (angular_velocity * propeller_diameter)) + // omega = sqrt(thrust / + // (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) + if (_calculateCoefficient && angularVelocity != 0) + { + _thrustCoefficient = _alpha1 + _alpha2 * (((1 - _wakeFraction) * + propellerLinVels[i].Length()) / (angularVelocity * _diameter)); + } + auto omega = sqrt(abs(force / (_density * _thrustCoefficient * + pow(_diameter, 4)))); + auto angVel = propellerAngVels[i]; // It takes a few iterations to reach the speed if (i > 25) @@ -270,3 +310,15 @@ TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration)) this->TestWorld(world, ns, topic, 0.005, 950, 0.25, 1e-2); } +///////////////////////////////////////////////// +TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(ThrustCoefficient)) +{ + const std::string ns = "custom"; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_thrust"; + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "thrust_coefficient.sdf"); + + // Tolerance is high because the joint command disturbs the vehicle body + this->TestWorld(world, ns, topic, 1, 950, 0.25, 1e-2, 0.2, 0.9, 0.01, true); +} diff --git a/test/worlds/thrust_coefficient.sdf b/test/worlds/thrust_coefficient.sdf new file mode 100644 index 0000000000..f45c5cb1c7 --- /dev/null +++ b/test/worlds/thrust_coefficient.sdf @@ -0,0 +1,114 @@ + + + + + + + 0 + + + + 0 0 0 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.000354167 + 0 + 0 + 0.000021667 + 0 + 0.000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + custom + propeller_joint + 950 + 0.25 + true + 300 + 0 + 0.2 + 0.9 + 0.01 + + + + + From 25ddf2f2620819092e3c1c448ef5dd51208d56ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 10 Oct 2022 19:12:35 +0200 Subject: [PATCH 020/160] Citadel: Removed warnings (#1753) Signed-off-by: ahcorde --- src/Util_TEST.cc | 2 +- src/systems/altimeter/Altimeter.cc | 1 - src/systems/camera_video_recorder/CameraVideoRecorder.hh | 2 +- test/integration/wheel_slip.cc | 3 --- 4 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 3a051377e4..2385caf31a 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -722,7 +722,7 @@ TEST_F(UtilTest, EntityFromMsg) ecm.CreateComponent(actorDEntity, components::ParentEntity(worldEntity)); // Check entities - auto createMsg = [&ecm](Entity _id, const std::string &_name = "", + auto createMsg = [&](Entity _id, const std::string &_name = "", msgs::Entity::Type _type = msgs::Entity::NONE) -> msgs::Entity { msgs::Entity msg; diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index 1b34fc63c1..1a143e0d04 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -245,7 +245,6 @@ void AltimeterPrivate::UpdateAltimeters(const EntityComponentManager &_ecm) auto it = this->entitySensorMap.find(_entity); if (it != this->entitySensorMap.end()) { - math::Vector3d linearVel; math::Pose3d worldPose = _worldPose->Data(); it->second->SetPosition(worldPose.Pos().Z()); it->second->SetVerticalVelocity(_worldLinearVel->Data().Z()); diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.hh b/src/systems/camera_video_recorder/CameraVideoRecorder.hh index f5f3dc02c1..c905301036 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.hh +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.hh @@ -51,7 +51,7 @@ namespace systems /// /// Video recorder bitrate (bps). The default value is /// 2070000 bps, and the supported type is unsigned int. - class CameraVideoRecorder: + class CameraVideoRecorder final: public System, public ISystemConfigure, public ISystemPostUpdate diff --git a/test/integration/wheel_slip.cc b/test/integration/wheel_slip.cc index 831d3be5a1..4e3daeee72 100644 --- a/test/integration/wheel_slip.cc +++ b/test/integration/wheel_slip.cc @@ -238,7 +238,6 @@ TEST_F(WheelSlipTest, TireDrum) // const double kp = surfaceContactOde->GetElement("kp")->Get(); // ASSERT_EQ(kp, 250e3); - double modelMass = 0.0; for (const auto &linkName : linksToCheck) { Entity linkEntity = ecm->EntityByComponents( @@ -249,8 +248,6 @@ TEST_F(WheelSlipTest, TireDrum) auto inertialComp = ecm->Component(linkEntity); EXPECT_NE(nullptr, inertialComp); - - modelMass += inertialComp->Data().MassMatrix().Mass(); } // Get axle wheel and steer joint of wheel model From e502e9fd36544d4dd936ccde2308dc5938ec1950 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 11 Oct 2022 17:04:21 +0800 Subject: [PATCH 021/160] Enable/Disable individual hydrodynamic components. (#1692) This commit enables and disables individual components of the hydrodynamics. This is often useful for debugging odd behaviours of a hydrodynamic model. --- src/systems/hydrodynamics/Hydrodynamics.cc | 22 +- src/systems/hydrodynamics/Hydrodynamics.hh | 3 + test/integration/CMakeLists.txt | 1 + test/integration/hydrodynamics_flags.cc | 151 +++++++++ test/worlds/hydrodynamics_flags.sdf | 352 +++++++++++++++++++++ 5 files changed, 528 insertions(+), 1 deletion(-) create mode 100644 test/integration/hydrodynamics_flags.cc create mode 100644 test/worlds/hydrodynamics_flags.sdf diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index ac7f8fdf3d..1a59c06c11 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -103,6 +103,14 @@ class ignition::gazebo::systems::HydrodynamicsPrivateData /// \brief The ignition transport node public: transport::Node node; + /// \brief Plugin Parameter: Disable coriolis as part of equation. This is + /// occasionally useful for testing. + public: bool disableCoriolis = false; + + /// \brief Plugin Parameter: Disable added mass as part of equation. This is + /// occasionally useful for testing. + public: bool disableAddedMass = false; + /// \brief Ocean current experienced by this body public: math::Vector3d currentVector {0, 0, 0}; @@ -232,6 +240,9 @@ void Hydrodynamics::Configure( this->dataPtr->paramNr = SdfParamDouble(_sdf, "nR" , 20); this->dataPtr->paramNrr = SdfParamDouble(_sdf, "nRR" , 0); + _sdf->Get("disable_coriolis", this->dataPtr->disableCoriolis, false); + _sdf->Get("disable_added_mass", this->dataPtr->disableAddedMass, false); + // Create model object, to access convenient functions auto model = ignition::gazebo::Model(_entity); @@ -389,7 +400,16 @@ void Hydrodynamics::PreUpdate( const Eigen::VectorXd kDvec = Dmat * state; - const Eigen::VectorXd kTotalWrench = kAmassVec + kDvec + kCmatVec; + Eigen::VectorXd kTotalWrench = kDvec; + + if (!this->dataPtr->disableAddedMass) + { + kTotalWrench += kAmassVec; + } + if (!this->dataPtr->disableCoriolis) + { + kTotalWrench += kCmatVec; + } ignition::math::Vector3d totalForce(-kTotalWrench(0), -kTotalWrench(1), -kTotalWrench(2)); diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index f9f87cfe4c..4719703ac7 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -77,6 +77,9 @@ namespace systems /// listens on `/model/{namespace name}/ocean_current`.[String, Optional] /// * - A generic current. /// [vector3d m/s, optional, default = [0,0,0]m/s] + /// * - Disable Coriolis force [Boolean, Default: false] + /// * - Disable Added Mass [Boolean, Default: false]. + /// To be deprecated in Garden. /// /// # Example /// An example configuration is provided in the examples folder. The example diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index ad026ed661..d5776ad20f 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -25,6 +25,7 @@ set(tests fuel_cached_server.cc halt_motion.cc hydrodynamics.cc + hydrodynamics_flags.cc imu_system.cc joint_controller_system.cc joint_position_controller_system.cc diff --git a/test/integration/hydrodynamics_flags.cc b/test/integration/hydrodynamics_flags.cc new file mode 100644 index 0000000000..a997be5826 --- /dev/null +++ b/test/integration/hydrodynamics_flags.cc @@ -0,0 +1,151 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include +#include +#include +#include + +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/TestFixture.hh" +#include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/World.hh" + +#include "ignition/gazebo/test_config.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +class HydrodynamicsFlagsTest : public InternalFixture<::testing::Test> +{ + /// \brief Test a world file. + /// \param[in] _world Path to world file. + /// \param[in] _modelName Name of the model. + /// \param[in] _numsteps Number of steps to run the server. + /// \param[out] _linearVel Linear velocityies after each step. + /// \param[out] _angularVel Linear velocityies after each step. + public: void TestWorld(const std::string &_world, + const std::string &_modelName, const unsigned int &_numsteps, + std::vector &_linearVel, + std::vector &_angularVel); +}; + +////////////////////////////////////////////////// +void HydrodynamicsFlagsTest::TestWorld(const std::string &_world, + const std::string &_modelName, const unsigned int &_numsteps, + std::vector &_linearVel, + std::vector &_angularVel) +{ + // Maximum verbosity for debugging + ignition::common::Console::SetVerbosity(4); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link body; + std::vector bodyVels; + fixture. + OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, _modelName); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, "base_link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + // Forces in Y and Z are needed to make the coriolis + // force appear. + math::Vector3d force(0.0, 1000.0, 1000.0); + math::Vector3d torque(0.0, 0.0, 10.0); + body.AddWorldWrench(_ecm, force, torque); + + }). + OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + auto bodyVel = body.WorldLinearVelocity(_ecm); + ASSERT_TRUE(bodyVel); + _linearVel.push_back(bodyVel.value()); + auto bodyAngularVel = body.WorldAngularVelocity(_ecm); + ASSERT_TRUE(bodyAngularVel); + _angularVel.push_back(bodyAngularVel.value()); + }). + Finalize(); + + fixture.Server()->Run(true, _numsteps, false); + EXPECT_EQ(_numsteps, _linearVel.size()); + EXPECT_EQ(_numsteps, _angularVel.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(body.Entity(), kNullEntity); + +} + +///////////////////////////////////////////////// +/// This test makes sure that the linear velocity is reuduced +/// disbling the coriolis force and also when disabling the added mass. +TEST_F(HydrodynamicsFlagsTest, AddedMassCoriolisFlags) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "hydrodynamics_flags.sdf"); + + unsigned int numsteps = 1000; + + std::vector angularVels, angularCoriolisVels, + angularAddedMassVels; + std::vector linearVels, linearCoriolisVels, + linearAddedMassVels; + + this->TestWorld(world, "tethys", numsteps, linearVels, angularVels); + this->TestWorld(world, "triton", numsteps, linearCoriolisVels, + angularCoriolisVels); + this->TestWorld(world, "daphne", numsteps, linearAddedMassVels, + angularAddedMassVels); + + // Wait a couple of iterations for the body to move + for (unsigned int i = 4; i < numsteps; ++i) + { + // Angular and linear velocity should be lower + // with the produced coriolis and added mass + EXPECT_LT(angularCoriolisVels[i].Z(), angularVels[i].Z()); + EXPECT_LT(linearCoriolisVels[i].Z(), linearVels[i].Z()); + EXPECT_LT(angularAddedMassVels[i].Z(), angularVels[i].Z()); + EXPECT_LT(linearAddedMassVels[i].Z(), linearVels[i].Z()); + } +} diff --git a/test/worlds/hydrodynamics_flags.sdf b/test/worlds/hydrodynamics_flags.sdf new file mode 100644 index 0000000000..51ae7a93bd --- /dev/null +++ b/test/worlds/hydrodynamics_flags.sdf @@ -0,0 +1,352 @@ + + + + + + + + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + + + 0.001 + 1.0 + + + + + + + + + + + + 1000 + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + tethys + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + true + true + + + + + + 5 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + triton + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + triton + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + true + + + + + + -5 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + daphne + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + daphne + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + true + + + + + + 0 0 -1 0 0 3.1415926 + https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/ABCSign_5m + start_line + + + 0 -25 -1 0 0 3.1415926 + https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/ABCSign_5m + finish_line + + + + From f91d031184e417d10c680d16ab74ff7b0069dc3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 11 Oct 2022 13:48:48 +0200 Subject: [PATCH 022/160] Fortress: Removed warnings (#1754) * Fortress: Removed warnings --- .../optical_tactile_plugin/OpticalTactilePlugin.cc | 3 +-- src/systems/optical_tactile_plugin/Visualization.cc | 6 ++---- src/systems/optical_tactile_plugin/Visualization.hh | 8 +------- test/integration/thruster.cc | 3 ++- 4 files changed, 6 insertions(+), 14 deletions(-) diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 492666b0ec..3fc0458e08 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -615,8 +615,7 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) this->sensorSize, this->forceLength, this->cameraUpdateRate, - this->depthCameraOffset, - this->visualizationResolution); + this->depthCameraOffset); this->initialized = true; } diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index 22ba842ebb..d330e6a9d4 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -32,14 +32,12 @@ OpticalTactilePluginVisualization::OpticalTactilePluginVisualization( ignition::math::Vector3d &_sensorSize, double &_forceLength, float &_cameraUpdateRate, - ignition::math::Pose3f &_depthCameraOffset, - int &_visualizationResolution) : + ignition::math::Pose3f &_depthCameraOffset) : modelName(_modelName), sensorSize(_sensorSize), forceLength(_forceLength), cameraUpdateRate(_cameraUpdateRate), - depthCameraOffset(_depthCameraOffset), - visualizationResolution(_visualizationResolution) + depthCameraOffset(_depthCameraOffset) { } diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index a5e863e139..96c8b46cc0 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -52,15 +52,12 @@ namespace optical_tactile_sensor /// \param[in] _forceLength Value of the forceLength attribute /// \param[in] _cameraUpdateRate Value of the cameraUpdateRate attribute /// \param[in] _depthCameraOffset Value of the depthCameraOffset attribute - /// \param[in] _visualizationResolution Value of the - /// visualizationResolution attribute public: OpticalTactilePluginVisualization( std::string &_modelName, ignition::math::Vector3d &_sensorSize, double &_forceLength, float &_cameraUpdateRate, - ignition::math::Pose3f &_depthCameraOffset, - int &_visualizationResolution); + ignition::math::Pose3f &_depthCameraOffset); /// \brief Initialize the marker message representing the optical tactile /// sensor @@ -153,9 +150,6 @@ namespace optical_tactile_sensor /// \brief Offset between depth camera pose and model pose private: ignition::math::Pose3f depthCameraOffset; - /// \brief Resolution of the sensor in pixels to skip. - private: int visualizationResolution; - /// \brief Whether the normal forces messages are initialized or not private: bool normalForcesMsgsAreInitialized{false}; }; diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 92c60a7d89..d92b3f15c3 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -244,7 +245,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // linear_velocity) / (angular_velocity * propeller_diameter)) // omega = sqrt(thrust / // (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) - if (_calculateCoefficient && angularVelocity != 0) + if (_calculateCoefficient && gz::math::equal(angularVelocity, 0.0)) { _thrustCoefficient = _alpha1 + _alpha2 * (((1 - _wakeFraction) * propellerLinVels[i].Length()) / (angularVelocity * _diameter)); From 1ea9ce99a9ae6086d384d2c1629da375fc0889e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 12 Oct 2022 20:48:10 +0200 Subject: [PATCH 023/160] Added collection name to About Dialog (#1756) Signed-off-by: ahcorde --- src/gui/AboutDialogHandler.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/gui/AboutDialogHandler.cc b/src/gui/AboutDialogHandler.cc index 545f6fbb66..0b50dc5fb9 100644 --- a/src/gui/AboutDialogHandler.cc +++ b/src/gui/AboutDialogHandler.cc @@ -28,6 +28,7 @@ using namespace gazebo::gui; ///////////////////////////////////////////////// AboutDialogHandler::AboutDialogHandler() { + aboutText += "Gazebo " + std::string(GZ_DISTRIBUTION) + "
"; aboutText += std::string(IGNITION_GAZEBO_VERSION_HEADER); aboutText += "" "" From ac5df7f9969efe73336417fc76539e14937e6750 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 12 Oct 2022 13:18:58 -0700 Subject: [PATCH 024/160] Convert ignitionrobotics to gazebosim in tests directory (#1757) * Convert ignitionrobotics to gaazebosim in tests directory Signed-off-by: Nate Koenig * fix gz-gazebo Signed-off-by: Nate Koenig Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- test/integration/battery_plugin.cc | 2 +- test/integration/examples_build.cc | 2 +- test/integration/log_system.cc | 8 ++++---- test/integration/network_handshake.cc | 2 +- test/integration/save_world.cc | 2 +- test/integration/sdf_include.cc | 2 +- test/integration/touch_plugin.cc | 4 ++-- test/integration/tracked_vehicle_system.cc | 2 +- test/worlds/breadcrumbs.sdf | 2 +- test/worlds/include.sdf | 2 +- test/worlds/log_record_resources.sdf | 2 +- test/worlds/save_world.sdf | 6 +++--- 12 files changed, 18 insertions(+), 18 deletions(-) diff --git a/test/integration/battery_plugin.cc b/test/integration/battery_plugin.cc index 336ef6cab1..7b513cec3d 100644 --- a/test/integration/battery_plugin.cc +++ b/test/integration/battery_plugin.cc @@ -107,7 +107,7 @@ TEST_F(BatteryPluginTest, SingleBattery) // the LinearBatteryPlugin is not zero when created. If // components::BatterySoC is zero on start, then the Physics plugin // can disable a joint. This in turn can prevent the joint from - // rotating. See https://github.com/ignitionrobotics/ign-gazebo/issues/55 + // rotating. See https://github.com/gazebosim/gz-sim/issues/55 EXPECT_GT(batComp->Data(), 0); }; diff --git a/test/integration/examples_build.cc b/test/integration/examples_build.cc index f6ff1db421..a84e5bfe66 100644 --- a/test/integration/examples_build.cc +++ b/test/integration/examples_build.cc @@ -27,7 +27,7 @@ #include "../helpers/EnvTestFixture.hh" // File copied from -// https://github.com/ignitionrobotics/ign-gui/raw/ign-gui3/test/integration/ExamplesBuild_TEST.cc +// https://github.com/gazebosim/gz-gui/raw/ign-gui3/test/integration/ExamplesBuild_TEST.cc using namespace ignition; diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index 78a2064eba..700a4aeb0e 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -59,7 +59,7 @@ using namespace gazebo; static const std::string kBinPath(PROJECT_BINARY_PATH); // TODO(anyone) Support command line options for OSX, see -// https://github.com/ignitionrobotics/ign-gazebo/issues/25 +// https://github.com/gazebosim/gz-sim/issues/25 #ifndef __APPLE__ static const std::string kSdfFileOpt = // NOLINT(runtime/string) " "; @@ -1276,7 +1276,7 @@ TEST_F(LogSystemTest, LogOverwrite) // ign gazebo. server_main.cc is deprecated and does not have overwrite // renaming implemented. So will always overwrite. Will not test (#) type of // renaming on OS X until ign gazebo is fixed: - // https://github.com/ignitionrobotics/ign-gazebo/issues/25 + // https://github.com/gazebosim/gz-sim/issues/25 // New log files were created EXPECT_TRUE(common::exists(this->logDir + "(1)")); @@ -1739,7 +1739,7 @@ TEST_F(LogSystemTest, LogResources) // Recorded models should exist EXPECT_GT(entryCount(recordPath), 2); EXPECT_TRUE(common::exists(common::joinPaths(recordPath, homeFake, - ".ignition", "fuel", "fuel.ignitionrobotics.org", "openrobotics", + ".ignition", "fuel", "fuel.gazebosim.org", "openrobotics", "models", "x2 config 1"))); // Remove artifacts. Recreate new directory @@ -1774,7 +1774,7 @@ TEST_F(LogSystemTest, LogResources) EXPECT_GT(entryCount(recordPath), 1); #endif EXPECT_TRUE(common::exists(common::joinPaths(recordPath, homeFake, - ".ignition", "fuel", "fuel.ignitionrobotics.org", "openrobotics", + ".ignition", "fuel", "fuel.gazebosim.org", "openrobotics", "models", "x2 config 1"))); // Revert environment variable after test is done diff --git a/test/integration/network_handshake.cc b/test/integration/network_handshake.cc index 3dc0dc9a68..9f03ef9630 100644 --- a/test/integration/network_handshake.cc +++ b/test/integration/network_handshake.cc @@ -145,7 +145,7 @@ TEST_F(NetworkHandshake, IGN_UTILS_TEST_DISABLED_ON_MAC(Updates)) configPrimary.SetUseLevels(true); // Can only test one secondary running physics, because running 2 physics in // the same process causes a segfault, see - // https://github.com/ignitionrobotics/ign-gazebo/issues/18 + // https://github.com/gazebosim/gz-sim/issues/18 configPrimary.SetNetworkSecondaries(1); configPrimary.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/performers.sdf"); diff --git a/test/integration/save_world.cc b/test/integration/save_world.cc index 4142ed253c..f4792828aa 100644 --- a/test/integration/save_world.cc +++ b/test/integration/save_world.cc @@ -100,7 +100,7 @@ TEST_F(SdfGeneratorFixture, WorldWithModelsSpawnedAfterLoad) // This has to be different from the backpack in order to test SDFormat // generation for a Fuel URI that was not known when simulation started. const std::string groundPlaneUri = - "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/ground plane"; + "https://fuel.gazebosim.org/1.0/OpenRobotics/models/ground plane"; transport::Node node; { diff --git a/test/integration/sdf_include.cc b/test/integration/sdf_include.cc index 37b4c86b57..3e9eea8005 100644 --- a/test/integration/sdf_include.cc +++ b/test/integration/sdf_include.cc @@ -44,6 +44,6 @@ TEST_F(SdfInclude, DownloadFromFuel) Server server(serverConfig); EXPECT_TRUE(common::exists(path + - "/fuel.ignitionrobotics.org/openrobotics/models/ground plane" + + "/fuel.gazebosim.org/openrobotics/models/ground plane" + "/1/model.sdf")); } diff --git a/test/integration/touch_plugin.cc b/test/integration/touch_plugin.cc index a8abfa6ec1..f74e8972a9 100644 --- a/test/integration/touch_plugin.cc +++ b/test/integration/touch_plugin.cc @@ -99,7 +99,7 @@ TEST_F(TouchPluginTest, OneLink) std::this_thread::sleep_for(std::chrono::milliseconds(30)); } // Known to fail on OSX, see -// https://github.com/ignitionrobotics/ign-gazebo/issues/22 +// https://github.com/gazebosim/gz-sim/issues/22 #if !defined (__APPLE__) EXPECT_TRUE(whiteTouched); #endif @@ -179,7 +179,7 @@ TEST_F(TouchPluginTest, StartDisabled) std::this_thread::sleep_for(std::chrono::milliseconds(30)); } // Known to fail on OSX, see -// https://github.com/ignitionrobotics/ign-gazebo/issues/22 +// https://github.com/gazebosm/gz-sim/issues/22 #if !defined (__APPLE__) EXPECT_TRUE(blueTouched); #endif diff --git a/test/integration/tracked_vehicle_system.cc b/test/integration/tracked_vehicle_system.cc index 2677d265d8..5bfb76c2d9 100644 --- a/test/integration/tracked_vehicle_system.cc +++ b/test/integration/tracked_vehicle_system.cc @@ -69,7 +69,7 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> bool &_shouldSkip) { #if __APPLE__ - // until https://github.com/ignitionrobotics/ign-gazebo/issues/806 is fixed + // until https://github.com/gazebosim/gz-sim/issues/806 is fixed _shouldSkip = true; #else _shouldSkip = false; diff --git a/test/worlds/breadcrumbs.sdf b/test/worlds/breadcrumbs.sdf index a8b75f9899..bbc2bac8e6 100644 --- a/test/worlds/breadcrumbs.sdf +++ b/test/worlds/breadcrumbs.sdf @@ -481,7 +481,7 @@ -2.2 0 5 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cardboard box + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Cardboard box diff --git a/test/worlds/include.sdf b/test/worlds/include.sdf index ed6f427d9a..1d00bc2dc0 100644 --- a/test/worlds/include.sdf +++ b/test/worlds/include.sdf @@ -6,7 +6,7 @@ - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/ground plane/1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/ground plane/1 diff --git a/test/worlds/log_record_resources.sdf b/test/worlds/log_record_resources.sdf index c0e2761c55..2c596b9d9d 100644 --- a/test/worlds/log_record_resources.sdf +++ b/test/worlds/log_record_resources.sdf @@ -56,7 +56,7 @@ false staging_area 0 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X2 Config 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/X2 Config 1 diff --git a/test/worlds/save_world.sdf b/test/worlds/save_world.sdf index 2b2ee0b778..b66cf1de07 100644 --- a/test/worlds/save_world.sdf +++ b/test/worlds/save_world.sdf @@ -24,17 +24,17 @@ - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack backpack1 1 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack backpack2 1 2 3 0.1 0.2 0.3 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack/2 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack/2 backpack3 2 0 0 0.1 0.2 0.3 From 3276fbd053198d2514e093b9f00d27afa84c8479 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 12 Oct 2022 13:58:56 -0700 Subject: [PATCH 025/160] Convert ignitionrobotics to gazebosim in sources and includes (#1758) Signed-off-by: Nate Koenig Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/ignition/gazebo/Entity.hh | 3 +-- include/ignition/gazebo/ServerConfig.hh | 4 ++-- src/SdfGenerator_TEST.cc | 12 ++++++------ src/ServerConfig.cc | 2 +- src/Server_TEST.cc | 2 +- src/World.cc | 8 ++++---- src/cmd/cmdgazebo.rb.in | 4 ++-- src/gui/AboutDialogHandler.cc | 8 ++++---- src/gui/Gui_TEST.cc | 2 +- src/gui/plugins/scene3d/Scene3D.cc | 2 +- src/ign.hh | 2 +- src/ign_TEST.cc | 4 ++++ src/network/NetworkManagerPrimary.cc | 2 +- src/systems/ackermann_steering/AckermannSteering.hh | 2 +- src/systems/battery_plugin/LinearBatteryPlugin.hh | 2 +- 15 files changed, 31 insertions(+), 28 deletions(-) diff --git a/include/ignition/gazebo/Entity.hh b/include/ignition/gazebo/Entity.hh index 4ac16e82de..0db6365417 100644 --- a/include/ignition/gazebo/Entity.hh +++ b/include/ignition/gazebo/Entity.hh @@ -21,8 +21,7 @@ #include #include -/// \brief This library is part of the [Ignition -/// Robotics](https://ignitionrobotics.org) project. +/// \brief This library is part of the [Gazebo](https://gazebosim.org) project. namespace ignition { /// \brief Gazebo is a leading open source robotics simulator, that diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index d9b94c039e..185aed2d1e 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -308,14 +308,14 @@ namespace ignition UpdatePeriod() const; /// \brief Path to where simulation resources, such as models downloaded - /// from fuel.ignitionrobotics.org, should be stored. + /// from fuel.gazebosim.org, should be stored. /// \return Path to a location on disk. An empty string indicates that /// the default value will be used, which is currently /// ~/.ignition/fuel. public: const std::string &ResourceCache() const; /// \brief Set the path to where simulation resources, such as models - /// downloaded from fuel.ignitionrobotics.org, should be stored. + /// downloaded from fuel.gazebosim.org, should be stored. /// \param[in] _path Path to a location on disk. An empty string /// indicates that the default value will be used, which is currently /// ~/.ignition/fuel. diff --git a/src/SdfGenerator_TEST.cc b/src/SdfGenerator_TEST.cc index c4bbe58a31..995ae9f54c 100644 --- a/src/SdfGenerator_TEST.cc +++ b/src/SdfGenerator_TEST.cc @@ -544,17 +544,17 @@ TEST_F(ElementUpdateFixture, WorldWithModelsIncludedNotExpanded) TEST_F(ElementUpdateFixture, WorldWithModelsIncludedWithInvalidUris) { const std::string goodUri = - "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack/2"; + "https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack/2"; // These are URIs that are potentially problematic. const std::vector fuelUris = { // Thes following two URIs are valid, but have a trailing '/' - "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack/", - "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack/2/", + "https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack/", + "https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack/2/", // Thes following two URIs are invalid, and will not be saved - "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack/" + "https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack/" "notInt", - "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack/" + "https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack/" "notInt/", }; @@ -609,7 +609,7 @@ TEST_F(ElementUpdateFixture, WorldWithModelsIncludedWithInvalidUris) TEST_F(ElementUpdateFixture, WorldWithModelsIncludedWithNonFuelUris) { const std::vector includeUris = { - "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack", + "https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack", std::string("file://") + PROJECT_SOURCE_PATH + "/test/worlds/models/sphere"}; diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index db979e35fd..a8ea6c6731 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -270,7 +270,7 @@ class ignition::gazebo::ServerConfigPrivate public: std::string logRecordCompressPath = ""; /// \brief Path to where simulation resources, such as models downloaded - /// from fuel.ignitionrobotics.org, should be stored. + /// from fuel.gazebosim.org, should be stored. public: std::string resourceCache = ""; /// \brief File containing physics engine plugin. If empty, DART will be used. diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 7cff29395f..60f429123a 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -1002,7 +1002,7 @@ TEST_P(ServerFixture, CachedFuelWorld) ServerConfig serverConfig; auto fuelWorldURL = - "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/worlds/Test%20world"; + "https://fuel.gazebosim.org/1.0/OpenRobotics/worlds/Test%20world"; EXPECT_TRUE(serverConfig.SetSdfFile(fuelWorldURL)); EXPECT_EQ(fuelWorldURL, serverConfig.SdfFile()); diff --git a/src/World.cc b/src/World.cc index 333a0f430c..13a135500c 100644 --- a/src/World.cc +++ b/src/World.cc @@ -110,7 +110,7 @@ Entity World::LightByName(const EntityComponentManager &_ecm, const std::string &_name) const { // Can't use components::Light in EntityByComponents, see - // https://github.com/ignitionrobotics/ign-gazebo/issues/376 + // https://github.com/gazebosim/gz-sim/issues/376 auto entities = _ecm.EntitiesByComponents( components::ParentEntity(this->dataPtr->id), components::Name(_name)); @@ -128,7 +128,7 @@ Entity World::ActorByName(const EntityComponentManager &_ecm, const std::string &_name) const { // Can't use components::Actor in EntityByComponents, see - // https://github.com/ignitionrobotics/ign-gazebo/issues/376 + // https://github.com/gazebosim/gz-sim/issues/376 auto entities = _ecm.EntitiesByComponents( components::ParentEntity(this->dataPtr->id), components::Name(_name)); @@ -155,7 +155,7 @@ Entity World::ModelByName(const EntityComponentManager &_ecm, std::vector World::Lights(const EntityComponentManager &_ecm) const { // Can't use components::Light in EntityByComponents, see - // https://github.com/ignitionrobotics/ign-gazebo/issues/376 + // https://github.com/gazebosim/gz-sim/issues/376 auto entities = _ecm.EntitiesByComponents( components::ParentEntity(this->dataPtr->id)); @@ -172,7 +172,7 @@ std::vector World::Lights(const EntityComponentManager &_ecm) const std::vector World::Actors(const EntityComponentManager &_ecm) const { // Can't use components::Actor in EntityByComponents, see - // https://github.com/ignitionrobotics/ign-gazebo/issues/376 + // https://github.com/gazebosim/gz-sim/issues/376 auto entities = _ecm.EntitiesByComponents( components::ParentEntity(this->dataPtr->id)); diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index 0da50582a4..970955d051 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -456,7 +456,7 @@ has properly set the DYLD_LIBRARY_PATH environment variables." if plugin.end_with? ".dylib" puts "`ign gazebo` currently only works with the -s argument on macOS. -See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." +See https://github.com/gazebosim/gz-sim/issues/44 for more info." exit(-1) end @@ -511,7 +511,7 @@ See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." else options['gui'] if plugin.end_with? ".dylib" puts "`ign gazebo` currently only works with the -s argument on macOS. -See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." +See https://github.com/gazebosim/gz-sim/issues/44 for more info." exit(-1) end diff --git a/src/gui/AboutDialogHandler.cc b/src/gui/AboutDialogHandler.cc index 0b50dc5fb9..8e94a48e15 100644 --- a/src/gui/AboutDialogHandler.cc +++ b/src/gui/AboutDialogHandler.cc @@ -35,9 +35,9 @@ AboutDialogHandler::AboutDialogHandler() "" "" "" @@ -46,9 +46,9 @@ AboutDialogHandler::AboutDialogHandler() "Tutorials:" "" "" "" diff --git a/src/gui/Gui_TEST.cc b/src/gui/Gui_TEST.cc index 5c51810e75..933af792ba 100644 --- a/src/gui/Gui_TEST.cc +++ b/src/gui/Gui_TEST.cc @@ -38,7 +38,7 @@ using namespace ignition; using namespace ignition::gazebo::gui; ///////////////////////////////////////////////// -// https://github.com/ignitionrobotics/ign-gazebo/issues/8 +// https://github.com/gazebosim/gz-sim/issues/8 TEST(GuiTest, IGN_UTILS_TEST_DISABLED_ON_MAC(PathManager)) { common::Console::SetVerbosity(4); diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index c0a265af29..b1a1ba247b 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -2203,7 +2203,7 @@ void TextureNode::PrepareNode() newId, sz, QQuickWindow::TextureIsOpaque); #else // TODO(anyone) Use createTextureFromNativeObject - // https://github.com/ignitionrobotics/ign-gui/issues/113 + // https://github.com/gazebosim/gz-gui/issues/113 #ifndef _WIN32 # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wdeprecated-declarations" diff --git a/src/ign.hh b/src/ign.hh index 68355486a9..07b47a803a 100644 --- a/src/ign.hh +++ b/src/ign.hh @@ -78,7 +78,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runGui( /// \brief External hook to find or download a fuel world provided a URL. /// \param[in] _pathToResource Path to the fuel world resource, ie, -/// https://staging-fuel.ignitionrobotics.org/1.0/gmas/worlds/ShapesClone +/// https://staging-fuel.gazebosim.org/1.0/gmas/worlds/ShapesClone /// \return C-string containing the path to the local world sdf file extern "C" IGNITION_GAZEBO_VISIBLE const char *findFuelResource( char *_pathToResource); diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index a66def21b3..686055dd0b 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -37,6 +37,10 @@ static const std::string kIgnModelCommand( ///////////////////////////////////////////////// std::string customExecStr(std::string _cmd) { + // Augment the system plugin path. + gz::common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + gz::common::joinPaths(std::string(PROJECT_BINARY_PATH), "lib").c_str()); + _cmd += " 2>&1"; FILE *pipe = popen(_cmd.c_str(), "r"); diff --git a/src/network/NetworkManagerPrimary.cc b/src/network/NetworkManagerPrimary.cc index 84542b711c..0dcf6b8e77 100644 --- a/src/network/NetworkManagerPrimary.cc +++ b/src/network/NetworkManagerPrimary.cc @@ -212,7 +212,7 @@ bool NetworkManagerPrimary::SecondariesCanStep() const // TODO(anyone) Ideally we'd check the number of connections against the // number of expected secondaries, but there's no interface for that // on ign-transport yet: - // https://github.com/ignitionrobotics/ign-transport/issues/39 + // https://github.com/gazebosim/gz-transport/issues/39 return this->simStepPub.HasConnections(); } diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index 4313ec5ec4..5e1b23bb54 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -111,7 +111,7 @@ namespace systems /// right_steering_joint /// /// References: - /// https://github.com/ignitionrobotics/ign-gazebo/tree/ign-gazebo4/src/systems/diff_drive + /// https://github.com/gazebosim/gz-sim/tree/ign-gazebo4/src/systems/diff_drive /// https://www.auto.tuwien.ac.at/bib/pdf_TR/TR0183.pdf /// https://github.com/froohoo/ackermansteer/blob/master/ackermansteer/ diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.hh b/src/systems/battery_plugin/LinearBatteryPlugin.hh index 75149eef0f..a77440ed07 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.hh +++ b/src/systems/battery_plugin/LinearBatteryPlugin.hh @@ -57,7 +57,7 @@ namespace systems /// - `` Hours taken to fully charge the battery. /// (Required if `` is set to true) /// - `` True to change the battery behavior to fix some issues - /// described in https://github.com/ignitionrobotics/ign-gazebo/issues/225. + /// described in https://github.com/gazebosim/gz-sim/issues/225. class LinearBatteryPlugin : public System, public ISystemConfigure, From 7126035b4f12e5a0a652babaa29d860e3439b04a Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 13 Oct 2022 05:12:04 -0700 Subject: [PATCH 026/160] Convert ignitionrobotics to gazebosim in tutorials (#1759) Signed-off-by: Nate Koenig Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- src/ign_TEST.cc | 2 +- tutorials/battery.md | 2 +- tutorials/collada_world_exporter.md | 2 +- tutorials/detachable_joints.md | 2 +- tutorials/distributed_simulation.md | 2 +- tutorials/erb_template.md | 8 +++--- tutorials/gui_config.md | 8 +++--- tutorials/levels.md | 12 ++++----- tutorials/log.md | 4 +-- tutorials/logical_audio_sensor.md | 6 ++--- tutorials/mesh_to_fuel.md | 18 ++++++------- tutorials/migrating_ardupilot_plugin.md | 6 ++--- tutorials/migration_link_api.md | 12 ++++----- tutorials/migration_model_api.md | 12 ++++----- tutorials/migration_sdf.md | 20 +++++++-------- tutorials/migration_world_api.md | 10 ++++---- tutorials/physics.md | 6 ++--- tutorials/point_cloud_to_mesh.md | 22 ++++++++-------- tutorials/rendering_plugins.md | 12 ++++----- tutorials/resources.md | 34 ++++++++++++------------- tutorials/server_config.md | 8 +++--- tutorials/terminology.md | 2 +- tutorials/test_fixture.md | 2 +- tutorials/triggered_publisher.md | 4 +-- tutorials/video_recorder.md | 2 +- 25 files changed, 109 insertions(+), 109 deletions(-) diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 686055dd0b..6df0df2fab 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -100,7 +100,7 @@ TEST(CmdLine, CachedFuelWorld) std::string projectPath = std::string(PROJECT_SOURCE_PATH) + "/test/worlds"; ignition::common::setenv("IGN_FUEL_CACHE_PATH", projectPath.c_str()); std::string cmd = kIgnCommand + " -r -v 4 --iterations 5" + - " https://fuel.ignitionrobotics.org/1.0/OpenRobotics/worlds/Test%20world"; + " https://fuel.gazebosim.org/1.0/OpenRobotics/worlds/Test%20world"; std::cout << "Running command [" << cmd << "]" << std::endl; std::string output = customExecStr(cmd); diff --git a/tutorials/battery.md b/tutorials/battery.md index 4b235125c3..aacfaff581 100644 --- a/tutorials/battery.md +++ b/tutorials/battery.md @@ -53,7 +53,7 @@ Next, you can find a description of the SDF parameters used: * ``: Power load on battery (W). -* ``: As reported [here](https://github.com/ignitionrobotics/ign-gazebo/issues/225), +* ``: As reported [here](https://github.com/gazebosim/gz-sim/issues/225), there are some issues affecting batteries in Ignition Blueprint and Citadel. This parameter fixes the issues. Feel free to omit the parameter if you have legacy code and want to preserve the old behavior. diff --git a/tutorials/collada_world_exporter.md b/tutorials/collada_world_exporter.md index e1bdd4e60a..7e452d736a 100644 --- a/tutorials/collada_world_exporter.md +++ b/tutorials/collada_world_exporter.md @@ -22,4 +22,4 @@ ign gazebo -v 4 -s -r --iterations 1 WORLD_FILE_NAME 3. A subdirectory, named after the world, has been created in the current working directory. Within this subdirectory is the mesh and materials for the world. -Refer to the [collada_world_exporter.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo4/examples/worlds/collada_world_exporter.sdf) example. +Refer to the [collada_world_exporter.sdf](https://github.com/gazebosim/gz-sim/blob/ign-gazebo4/examples/worlds/collada_world_exporter.sdf) example. diff --git a/tutorials/detachable_joints.md b/tutorials/detachable_joints.md index 70a2456b8d..38f2d0d5a3 100644 --- a/tutorials/detachable_joints.md +++ b/tutorials/detachable_joints.md @@ -8,7 +8,7 @@ kinematic topology has to be a tree, i.e., kinematic loops are not currently supported. This affects the choice of the parent link, and therefore, the parent model, which is the model that contains the `DetachableJoint` system. -For example, [detachable_joint.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/detachable_joint.sdf) +For example, [detachable_joint.sdf](https://github.com/gazebosim/gz-sim/blob/ign-gazebo2/examples/worlds/detachable_joint.sdf) demonstrates a four wheel vehicle that holds three objects that are later detached from the vehicle. As seen in this example, the parent model is the vehicle. The kinematic topology is the following. diff --git a/tutorials/distributed_simulation.md b/tutorials/distributed_simulation.md index e81b91ac9a..21620a71d4 100644 --- a/tutorials/distributed_simulation.md +++ b/tutorials/distributed_simulation.md @@ -106,7 +106,7 @@ keeps all performers loaded, but performs no physics simulation. Stepping happens in 2 stages: the primary update and the secondaries update, according to the diagram below: - + 1. The primary publishes a `SimulationStep` message on the `/step` topic, containing: diff --git a/tutorials/erb_template.md b/tutorials/erb_template.md index b5cf20a1b0..6a1a21ea5e 100644 --- a/tutorials/erb_template.md +++ b/tutorials/erb_template.md @@ -17,7 +17,7 @@ Some of them are listed below and demonstrated in this [example ERB file](https: ## Set up Ruby Firstly, Ruby needs to be installed. -If you have gone through [Ignition's installation guide](https://ignitionrobotics.org/docs/latest/install), it's most likely you already have Ruby installed. +If you have gone through [Ignition's installation guide](https://gazebosim.org/docs/latest/install), it's most likely you already have Ruby installed. To check if Ruby is installed, use ```{.sh} ruby --version @@ -102,7 +102,7 @@ Each box model also has a different name and pose to ensure they show up as indi %> ``` -[Here](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo3/examples/worlds/shapes_population.sdf.erb) is a complete shapes simulation world example. +[Here](https://github.com/gazebosim/gz-sim/blob/ign-gazebo3/examples/worlds/shapes_population.sdf.erb) is a complete shapes simulation world example. Instead of simple shapes, you can also use a nested loop to generate 100 actors spaced out evenly in a simulation world. @@ -116,11 +116,11 @@ Instead of simple shapes, you can also use a nested loop to generate 100 actors - https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae 1.0 - https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae 0.055 true diff --git a/tutorials/gui_config.md b/tutorials/gui_config.md index fddc6b156f..58569d10a4 100644 --- a/tutorials/gui_config.md +++ b/tutorials/gui_config.md @@ -1,9 +1,9 @@ \page gui_config GUI Configuration Ignition Gazebo's graphical user interface is powered by -[Ignition GUI](https://ignitionrobotics.org/libs/gui). Therefore, Gazebo's +[Ignition GUI](https://gazebosim.org/libs/gui). Therefore, Gazebo's GUI layout can be defined in -[Ignition GUI configuration files](https://ignitionrobotics.org/api/gui/2.1/config.html). +[Ignition GUI configuration files](https://gazebosim.org/api/gui/2.1/config.html). These are XML files that describe what plugins to be loaded and with what settings. @@ -103,11 +103,11 @@ favorite editor and save this file as `fuel_preview.sdf`: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Gazebo + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Gazebo diff --git a/tutorials/levels.md b/tutorials/levels.md index c2f62b8667..f8f1f98df5 100644 --- a/tutorials/levels.md +++ b/tutorials/levels.md @@ -77,7 +77,7 @@ being added and removed. Take a look at the 2D example below. This example focuses on a single performer, but the same logic can be extended to multiple performers. - + * The **green area** represents the area of the world which this simulation is expected to take place in. @@ -115,23 +115,23 @@ Let's take a look at how levels are loaded / unloaded as the performer moves: * `M1` and `M3`, because they belong to the level. * `M6`, because it is global. - + 2. The performer moves south towards `L3` and enters its buffer zone, triggering a load of that level's models, `M4` and `M5`. Note that at this moment, both `L1` and `L3` are loaded. - + 3. The performer moves further south, exiting `L1` and entering `L3`. However, `L1` is still loaded, since `R1` is still within its buffer zone. - + 4. Eventually `R1` moves beyond `L1`'s buffer, triggering an unload of `L1`. The main effect is unloading `M1`. - + ## SDF elements @@ -248,7 +248,7 @@ ign service -s /world/levels/level/set_performer --reqtype ignition.msgs.StringM The following is a world file that could be an instance of the world shown in the figure - + ```xml diff --git a/tutorials/log.md b/tutorials/log.md index a401a32263..70e77222c0 100644 --- a/tutorials/log.md +++ b/tutorials/log.md @@ -8,7 +8,7 @@ Ignition records two types of information to files: * Always recorded * Simulation state * Entity poses, insertion and deletion - * Logged to an [Ignition Transport `state.tlog` file](https://ignitionrobotics.org/api/transport/7.0/logging.html) + * Logged to an [Ignition Transport `state.tlog` file](https://gazebosim.org/api/transport/7.0/logging.html) * Recording must be enabled from the command line or the C++ API * Can be played back using the command line or the C++ API @@ -42,7 +42,7 @@ Other options for recording: ### From C++ API All features available through the command line are also available through -[ignition::gazebo::ServerConfig](https://ignitionrobotics.org/api/gazebo/2.0/classignition_1_1gazebo_1_1ServerConfig.html). +[ignition::gazebo::ServerConfig](https://gazebosim.org/api/gazebo/2.0/classignition_1_1gazebo_1_1ServerConfig.html). When instantiating a server programmatically, logging options can be passed to the constructor, for example: diff --git a/tutorials/logical_audio_sensor.md b/tutorials/logical_audio_sensor.md index 0a1e776e47..526ece535a 100644 --- a/tutorials/logical_audio_sensor.md +++ b/tutorials/logical_audio_sensor.md @@ -8,7 +8,7 @@ The logical audio plugin does not play actual audio to a device like speakers, b ## Setup -Let's take a look at [logical_audio_sensor_plugin.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/460d2b1cfbf0addf05a1e61c05e1f7a675a83785/examples/worlds/logical_audio_sensor_plugin.sdf), which defines a simulation world with 4 models (in this case, boxes) that have an audio object attached to them. +Let's take a look at [logical_audio_sensor_plugin.sdf](https://github.com/gazebosim/gz-sim/blob/460d2b1cfbf0addf05a1e61c05e1f7a675a83785/examples/worlds/logical_audio_sensor_plugin.sdf), which defines a simulation world with 4 models (in this case, boxes) that have an audio object attached to them. This world attaches logical audio sources to the `red_box` and `blue_box` models, and attaches logical microphones to the `green_box` and `yellow_box` models. Let's take a look at the SDF relevant to the source for `red_box` to understand how to define a logical audio source in SDF: @@ -30,7 +30,7 @@ Let's take a look at the SDF relevant to the source for `red_box` to understand ``` As we can see, we use a `` tag to define an audio source. -An explanation of all of the tags can be found in the [plugin documentation](https://github.com/ignitionrobotics/ign-gazebo/blob/314477419d2aa946f384204dc99b17d9fcd963b3/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh#L35-L130), but there are a few important things to point out: +An explanation of all of the tags can be found in the [plugin documentation](https://github.com/gazebosim/gz-sim/blob/314477419d2aa946f384204dc99b17d9fcd963b3/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh#L35-L130), but there are a few important things to point out: * `` is used to identify this source when operating on it via services (services will be discussed later). Since a model can have multiple sources and microphones attached to it, each source attached to a particular model must have a unique ID. This means that no other sources attached to `red_box` can have an ID of 1, but sources attached to other models can have an ID of 1 (assuming that other models don't already have a source with an ID of 1 attached to it). @@ -55,7 +55,7 @@ Let's now take a look at the SDF relevant to the microphone for `green_box` to u ``` The same rules regarding `` and `` for a logical audio source also apply to a logical microphone. -You can also take a look at the [microphone documentation](https://github.com/ignitionrobotics/ign-gazebo/blob/314477419d2aa946f384204dc99b17d9fcd963b3/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh#L35-L130) for a detailed explanation of the tags embedded in the `` tag. +You can also take a look at the [microphone documentation](https://github.com/gazebosim/gz-sim/blob/314477419d2aa946f384204dc99b17d9fcd963b3/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh#L35-L130) for a detailed explanation of the tags embedded in the `` tag. ## Testing Source and Microphone Behavior diff --git a/tutorials/mesh_to_fuel.md b/tutorials/mesh_to_fuel.md index e534210eb3..710a5cb900 100644 --- a/tutorials/mesh_to_fuel.md +++ b/tutorials/mesh_to_fuel.md @@ -1,17 +1,17 @@ \page meshtofuel Importing a Mesh to Fuel -This tutorial will explain how to import a mesh to the [Ignition Fuel](https://app.ignitionrobotics.org) web application. +This tutorial will explain how to import a mesh to the [Ignition Fuel](https://app.gazebosim.org) web application. Adding models and/or worlds to Fuel will make your content readily available to the open source robotics simulation community, and easier to use with the Ignition GUI. ## Prerequisites To import meshes to Fuel, you need to have a user account. -Go to [app.ignitionrobotics.org](https://app.ignitionrobotics.org) and click Login in the top right corner of the screen, then click Sign Up. +Go to [app.gazebosim.org](https://app.gazebosim.org) and click Login in the top right corner of the screen, then click Sign Up. Once you verify your email address, your account will be ready. You'll need a mesh ready before trying to import to Fuel. There are several ways to acquire a mesh. -To save time, we'll use this [Electrical Box model](https://app.ignitionrobotics.org/openrobotics/fuel/models/Electrical%20Box) that you can download from Fuel. +To save time, we'll use this [Electrical Box model](https://app.gazebosim.org/openrobotics/fuel/models/Electrical%20Box) that you can download from Fuel. ## Model Directory Structure @@ -126,17 +126,17 @@ Click the `Add folders` button, or drag and drop the `Electrical Box` folder you All the files in your model description will be listed there. Press `Upload`, and the "Fuel Model Info" page for your model will open. -![Electrical Box Test](https://raw.githubusercontent.com/ignitionrobotics/ign-gazebo/983d50937cbcaa75c790515b2ec5797fe82f1188/tutorials/files/mesh_to_fuel/model_info2.png) +![Electrical Box Test](https://raw.githubusercontent.com/gazebosim/gz-sim/983d50937cbcaa75c790515b2ec5797fe82f1188/tutorials/files/mesh_to_fuel/model_info2.png) You can always delete a model by clicking the "Edit model" button and then selecting "Delete model" at the bottom of the page -![Delete model](https://raw.githubusercontent.com/ignitionrobotics/ign-gazebo/983d50937cbcaa75c790515b2ec5797fe82f1188/tutorials/files/mesh_to_fuel/delete2.png) +![Delete model](https://raw.githubusercontent.com/gazebosim/gz-sim/983d50937cbcaa75c790515b2ec5797fe82f1188/tutorials/files/mesh_to_fuel/delete2.png) ## Include the Model in a World With your mesh successfully uploaded to Fuel, you can now easily include it in a world SDF file. -Copy [this example world code](https://raw.githubusercontent.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo3/examples/worlds/import_mesh.sdf) into a text editor and save it as `import_mesh.sdf`. +Copy [this example world code](https://raw.githubusercontent.com/gazebosim/gz-sim/blob/ign-gazebo3/examples/worlds/import_mesh.sdf) into a text editor and save it as `import_mesh.sdf`. This is a simple world SDF file, which you can learn more about on the [SDF website](http://sdformat.org/). Scroll all the way to the bottom of the file until you see the `include` tag section following the `` comment line. @@ -156,7 +156,7 @@ Scroll all the way to the bottom of the file until you see the `include` tag sec true Electrical Box 0 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Electrical Box + https://fuel.gazebosim.org/1.0/openrobotics/models/Electrical Box @@ -171,7 +171,7 @@ Change `Electrical Box` to `Electrical Box Test`. The syntax for including any model from Fuel is: ```xml -https://fuel.ignitionrobotics.org/1.0//models/ +https://fuel.gazebosim.org/1.0//models/ ``` ### Launch World @@ -182,4 +182,4 @@ To launch the world and see your mesh, run Ignition from inside the directory wh ign gazebo import_mesh.sdf ``` -![Launch sample world with mesh](https://raw.githubusercontent.com/ignitionrobotics/ign-gazebo/983d50937cbcaa75c790515b2ec5797fe82f1188/tutorials/files/mesh_to_fuel/launch_world2.png) +![Launch sample world with mesh](https://raw.githubusercontent.com/gazebosim/gz-sim/983d50937cbcaa75c790515b2ec5797fe82f1188/tutorials/files/mesh_to_fuel/launch_world2.png) diff --git a/tutorials/migrating_ardupilot_plugin.md b/tutorials/migrating_ardupilot_plugin.md index afdb36ae77..bddfa6fb8b 100644 --- a/tutorials/migrating_ardupilot_plugin.md +++ b/tutorials/migrating_ardupilot_plugin.md @@ -22,7 +22,7 @@ documentation](https://ardupilot.org/dev/docs/using-gazebo-simulator-with-sitl.h As context to understand what we're migrating, here's a system diagram for how the ArduPilot Gazebo plugin works is used: - + *UAV icon credit: By Julian Herzog, CC BY 4.0, https://commons.wikimedia.org/w/index.php?curid=60965475* @@ -224,8 +224,8 @@ To better understand the ECS pattern as it is used in Ignition, it's helpful to learn about the EntityComponentManager (ECM), which is responsible for managing the ECS graph. A great resource to understand the logic under the hood of the ECM is the `SdfEntityCreator` class -([header](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo3/include/ignition/gazebo/SdfEntityCreator.hh), -[source](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo3/src/SdfEntityCreator.cc)). +([header](https://github.com/gazebosim/gz-sim/blob/ign-gazebo3/include/ignition/gazebo/SdfEntityCreator.hh), +[source](https://github.com/gazebosim/gz-sim/blob/ign-gazebo3/src/SdfEntityCreator.cc)). This class is responsible for mapping the content of an SDF file to the entities and components that form the graph handled by the ECM. For example, if you wonder which components can be accessed by default from the plugin, this diff --git a/tutorials/migration_link_api.md b/tutorials/migration_link_api.md index fc5f6c8349..aadb9c59ed 100644 --- a/tutorials/migration_link_api.md +++ b/tutorials/migration_link_api.md @@ -14,7 +14,7 @@ class. If you're trying to use some API which doesn't have an equivalent on Ignition yet, feel free to -[ticket an issue](https://github.com/ignitionrobotics/ign-gazebo/issues/). +[ticket an issue](https://github.com/gazebosim/gz-sim/issues/). ## Link API @@ -34,14 +34,14 @@ can be divided in these categories: You'll find the Ignition APIs below on the following headers: -* [ignition/gazebo/Link.hh](https://ignitionrobotics.org/api/gazebo/3.3/Link_8hh.html) -* [ignition/gazebo/Util.hh](https://ignitionrobotics.org/api/gazebo/3.3/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://ignitionrobotics.org/api/gazebo/3.3/SdfEntityCreator_8hh.html) -* [ignition/gazebo/EntityComponentManager.hh](https://ignitionrobotics.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [ignition/gazebo/Link.hh](https://gazebosim.org/api/gazebo/3.3/Link_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/3.3/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/3.3/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) It's worth remembering that most of this functionality can be performed using the -[EntityComponentManager](https://ignitionrobotics.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +[EntityComponentManager](https://gazebosim.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) directly. The functions presented here exist for convenience and readability. ### Properties diff --git a/tutorials/migration_model_api.md b/tutorials/migration_model_api.md index e3a4a5337e..bd4ecbbb63 100644 --- a/tutorials/migration_model_api.md +++ b/tutorials/migration_model_api.md @@ -14,7 +14,7 @@ class. If you're trying to use some API which doesn't have an equivalent on Ignition yet, feel free to -[ticket an issue](https://github.com/ignitionrobotics/ign-gazebo/issues/). +[ticket an issue](https://github.com/gazebosim/gz-sim/issues/). ## Model API @@ -34,14 +34,14 @@ can be divided in these categories: You'll find the Ignition APIs below on the following headers: -* [ignition/gazebo/Model.hh](https://ignitionrobotics.org/api/gazebo/3.3/Model_8hh.html) -* [ignition/gazebo/Util.hh](https://ignitionrobotics.org/api/gazebo/3.3/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://ignitionrobotics.org/api/gazebo/3.3/SdfEntityCreator_8hh.html) -* [ignition/gazebo/EntityComponentManager.hh](https://ignitionrobotics.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [ignition/gazebo/Model.hh](https://gazebosim.org/api/gazebo/3.3/Model_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/3.3/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/3.3/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) It's worth remembering that most of this functionality can be performed using the -[EntityComponentManager](https://ignitionrobotics.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +[EntityComponentManager](https://gazebosim.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) directly. The functions presented here exist for convenience and readability. ### Properties diff --git a/tutorials/migration_sdf.md b/tutorials/migration_sdf.md index 52e3716bb1..90d591b21c 100644 --- a/tutorials/migration_sdf.md +++ b/tutorials/migration_sdf.md @@ -33,7 +33,7 @@ Here are the recommended ways to use URIs from most recommended to least: ### Ignition Fuel URL It's possible to use URLs of resources on -[Ignition Fuel](https://app.ignitionrobotics.org) within any of the tags +[Ignition Fuel](https://app.gazebosim.org) within any of the tags above and both simulators will be able to load it. For example, this world can be loaded into both simulators: @@ -43,12 +43,12 @@ For example, this world can be loaded into both simulators: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Ground Plane + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane @@ -59,7 +59,7 @@ For example, this world can be loaded into both simulators: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae @@ -67,7 +67,7 @@ For example, this world can be loaded into both simulators: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae @@ -76,11 +76,11 @@ For example, this world can be loaded into both simulators: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/actor - relative paths/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/actor - relative paths/tip/files/meshes/talk_b.dae 1.0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/actor - relative paths/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/actor - relative paths/tip/files/meshes/talk_b.dae 1.0 + + + + + true + -5 0 0 0 0.5 0 + + + + + 0.1 0.1 0.1 + + + + + 0 0 0.0 0 0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 30 + camera + + + + + + From 224a6ce6b6e90324c94fc27a34c6d02f4db0918e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 25 Apr 2023 10:20:36 -0700 Subject: [PATCH 144/160] Optimize render updates and use of thread mutexes in Sensors system (#1938) This PR optimizes the threading and render updates in the sensors system by: * reducing the scope of the renderMutex lock in Sensors::Update function * changing a few bool variables to atomic, also to reduce the need to lock mutexes * performing scene tree updates only when necessary Signed-off-by: Ian Chen --- include/gz/sim/EventManager.hh | 25 ++ src/EventManager_TEST.cc | 9 + src/systems/sensors/Sensors.cc | 285 ++++++++++------ test/integration/CMakeLists.txt | 1 + test/integration/reset_sensors.cc | 18 +- .../integration/sensors_system_update_rate.cc | 311 ++++++++++++++++++ test/worlds/sensor.sdf | 9 +- 7 files changed, 554 insertions(+), 104 deletions(-) create mode 100644 test/integration/sensors_system_update_rate.cc diff --git a/include/gz/sim/EventManager.hh b/include/gz/sim/EventManager.hh index 02c97561d0..ace4848c04 100644 --- a/include/gz/sim/EventManager.hh +++ b/include/gz/sim/EventManager.hh @@ -118,6 +118,31 @@ namespace gz } } + /// \brief Get connection count for a particular event + /// Connection count for the event + public: template + unsigned int + ConnectionCount() + { + if (this->events.find(typeid(E)) == this->events.end()) + { + return 0u; + } + + E *eventPtr = dynamic_cast(this->events[typeid(E)].get()); + // All values in the map should be derived from Event, + // so this shouldn't be an issue, but it doesn't hurt to check. + if (eventPtr != nullptr) + { + return eventPtr->ConnectionCount(); + } + else + { + gzerr << "Failed to get connection count for event: " + << typeid(E).name() << std::endl; + return 0u; + } + } /// \brief Convenience type for storing typeinfo references. private: using TypeInfoRef = std::reference_wrapper; diff --git a/src/EventManager_TEST.cc b/src/EventManager_TEST.cc index ab7e86c9f2..9a7ae2abeb 100644 --- a/src/EventManager_TEST.cc +++ b/src/EventManager_TEST.cc @@ -29,12 +29,16 @@ TEST(EventManager, EmitConnectTest) { EventManager eventManager; + EXPECT_EQ(0u, eventManager.ConnectionCount()); + bool paused1 = false; auto connection1 = eventManager.Connect( [&paused1](bool _paused) { paused1 = _paused; }); + EXPECT_EQ(1u, eventManager.ConnectionCount()); + // Emitting events causes connection callbacks to be fired. eventManager.Emit(true); EXPECT_EQ(true, paused1); @@ -47,6 +51,8 @@ TEST(EventManager, EmitConnectTest) paused2 = _paused; }); + EXPECT_EQ(2u, eventManager.ConnectionCount()); + // Multiple connections should each be fired. eventManager.Emit(true); EXPECT_EQ(true, paused1); @@ -58,9 +64,12 @@ TEST(EventManager, EmitConnectTest) // Clearing the ConnectionPtr will cause it to no longer fire. connection1.reset(); + eventManager.Emit(true); EXPECT_EQ(false, paused1); EXPECT_EQ(true, paused2); + + EXPECT_EQ(1u, eventManager.ConnectionCount()); } /// Test that we are able to connect arbitrary events and signal them. diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 23e7abec16..0eff638de2 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -17,7 +17,6 @@ #include "Sensors.hh" -#include #include #include #include @@ -75,7 +74,7 @@ class gz::sim::systems::SensorsPrivate public: sensors::Manager sensorManager; /// \brief used to store whether rendering objects have been created. - public: bool initialized = false; + public: std::atomic initialized { false }; /// \brief Main rendering interface public: RenderUtil renderUtil; @@ -98,7 +97,7 @@ class gz::sim::systems::SensorsPrivate /// \brief Keep track of cameras, in case we need to handle stereo cameras. /// Key: Camera's parent scoped name /// Value: Pointer to camera - public: std::map cameras; + public: std::unordered_map cameras; /// \brief Maps gazebo entity to its matching sensor ID /// @@ -113,7 +112,7 @@ class gz::sim::systems::SensorsPrivate public: bool doInit { false }; /// \brief Flag to signal if rendering update is needed - public: bool updateAvailable { false }; + public: std::atomic updateAvailable { false }; /// \brief Flag to signal if a rendering update must be done public: std::atomic forceUpdate { false }; @@ -140,15 +139,17 @@ class gz::sim::systems::SensorsPrivate /// \brief Update time for the next rendering iteration public: std::chrono::steady_clock::duration updateTime; + /// \brief Next sensors update time + public: std::chrono::steady_clock::duration nextUpdateTime; + /// \brief Sensors to include in the next rendering iteration - public: std::vector activeSensors; + public: std::set activeSensors; - /// \brief Mutex to protect sensorMask - public: std::mutex sensorMaskMutex; + /// \brief Sensors to be updated next + public: std::set sensorsToUpdate; - /// \brief Mask sensor updates for sensors currently being rendered - public: std::map sensorMask; + /// \brief Mutex to protect sensorMask + public: std::mutex sensorsMutex; /// \brief Pointer to the event manager public: EventManager *eventManager{nullptr}; @@ -200,6 +201,14 @@ class gz::sim::systems::SensorsPrivate /// \param[in] _ecm Entity component manager public: void UpdateBatteryState(const EntityComponentManager &_ecm); + /// \brief Get the next closest sensor update time + public: std::chrono::steady_clock::duration NextUpdateTime( + std::set &_sensorsToUpdate, + const std::chrono::steady_clock::duration &_currentTime); + + /// \brief Check if any of the sensors have connections + public: bool SensorsHaveConnections(); + /// \brief Use to optionally set the background color. public: std::optional backgroundColor; @@ -216,7 +225,7 @@ class gz::sim::systems::SensorsPrivate public: std::unordered_map modelBatteryStateChanged; /// \brief A map of sensor ids to their active state - public: std::map sensorStateChanged; + public: std::unordered_map sensorStateChanged; /// \brief Disable sensors if parent model's battery is drained /// Affects sensors that are in nested models @@ -268,11 +277,13 @@ void SensorsPrivate::WaitForInit() ////////////////////////////////////////////////// void SensorsPrivate::RunOnce() { - std::unique_lock lock(this->renderMutex); - this->renderCv.wait(lock, [this]() { - return !this->running || this->updateAvailable; - }); + std::unique_lock cvLock(this->renderMutex); + this->renderCv.wait(cvLock, [this]() + { + return !this->running || this->updateAvailable; + }); + } if (!this->running) return; @@ -280,13 +291,22 @@ void SensorsPrivate::RunOnce() if (!this->scene) return; + std::chrono::steady_clock::duration updateTimeApplied; GZ_PROFILE("SensorsPrivate::RunOnce"); { GZ_PROFILE("Update"); + std::unique_lock timeLock(this->renderMutex); this->renderUtil.Update(); + updateTimeApplied = this->updateTime; + } + + bool activeSensorsEmpty = true; + { + std::unique_lock lk(this->sensorsMutex); + activeSensorsEmpty = this->activeSensors.empty(); } - if (!this->activeSensors.empty() || this->forceUpdate) + if (!activeSensorsEmpty || this->forceUpdate) { // disable sensors that are out of battery or re-enable sensors that are // being charged @@ -305,30 +325,10 @@ void SensorsPrivate::RunOnce() this->sensorStateChanged.clear(); } - // Check the active sensors against masked sensors. - // - // The internal state of a rendering sensor is not updated until the - // rendering operation is complete, which can leave us in a position - // where the sensor is falsely indicating that an update is needed. - // - // To prevent this, add sensors that are currently being rendered to - // a mask. Sensors are removed from the mask when 90% of the update - // delta has passed, which will allow rendering to proceed. - { - std::unique_lock lockMask(this->sensorMaskMutex); - for (const auto & sensor : this->activeSensors) - { - // 90% of update delta (1/UpdateRate()); - auto delta = std::chrono::duration_cast( - std::chrono::duration< double >(0.9 / sensor->UpdateRate())); - this->sensorMask[sensor->Id()] = this->updateTime + delta; - } - } - { GZ_PROFILE("PreRender"); this->eventManager->Emit(); - this->scene->SetTime(this->updateTime); + this->scene->SetTime(updateTimeApplied); // Update the scene graph manually to improve performance // We only need to do this once per frame It is important to call // sensors::RenderingSensor::SetManualSceneUpdate and set it to true @@ -339,7 +339,7 @@ void SensorsPrivate::RunOnce() // disable sensors that have no subscribers to prevent doing unnecessary // work std::unordered_set tmpDisabledSensors; - this->sensorMaskMutex.lock(); + this->sensorsMutex.lock(); for (auto id : this->sensorIds) { sensors::Sensor *s = this->sensorManager.Sensor(id); @@ -350,12 +350,16 @@ void SensorsPrivate::RunOnce() tmpDisabledSensors.insert(rs); } } - this->sensorMaskMutex.unlock(); + this->sensorsMutex.unlock(); + // safety check to see if reset occurred while we're rendering + // avoid publishing outdated data if reset occurred + std::unique_lock timeLock(this->renderMutex); + if (updateTimeApplied <= this->updateTime) { // publish data GZ_PROFILE("RunOnce"); - this->sensorManager.RunOnce(this->updateTime); + this->sensorManager.RunOnce(updateTimeApplied); this->eventManager->Emit(); } @@ -375,12 +379,12 @@ void SensorsPrivate::RunOnce() this->eventManager->Emit(); } + std::unique_lock lk(this->sensorsMutex); this->activeSensors.clear(); } this->updateAvailable = false; this->forceUpdate = false; - lock.unlock(); this->renderCv.notify_one(); } @@ -454,15 +458,9 @@ void Sensors::RemoveSensor(const Entity &_entity) // Locking mutex to make sure the vector is not being changed while // the rendering thread is iterating over it { - std::unique_lock lock(this->dataPtr->sensorMaskMutex); - sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(idIter->second); - auto rs = dynamic_cast(s); - auto activeSensorIt = std::find(this->dataPtr->activeSensors.begin(), - this->dataPtr->activeSensors.end(), rs); - if (activeSensorIt != this->dataPtr->activeSensors.end()) - { - this->dataPtr->activeSensors.erase(activeSensorIt); - } + std::unique_lock lock(this->dataPtr->sensorsMutex); + this->dataPtr->activeSensors.erase(idIter->second); + this->dataPtr->sensorsToUpdate.erase(idIter->second); } // update cameras list @@ -579,8 +577,9 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) gzdbg << "Resetting Sensors\n"; { - std::unique_lock lock(this->dataPtr->sensorMaskMutex); - this->dataPtr->sensorMask.clear(); + std::unique_lock lock(this->dataPtr->sensorsMutex); + this->dataPtr->activeSensors.clear(); + this->dataPtr->sensorsToUpdate.clear(); } for (auto id : this->dataPtr->sensorIds) @@ -595,6 +594,9 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) s->SetNextDataUpdateTime(_info.simTime); } + this->dataPtr->nextUpdateTime = _info.simTime; + std::unique_lock lock(this->dataPtr->renderMutex); + this->dataPtr->updateTime = _info.simTime; } } @@ -603,7 +605,6 @@ void Sensors::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { GZ_PROFILE("Sensors::Update"); - std::unique_lock lock(this->dataPtr->renderMutex); #ifdef __APPLE__ // On macOS the render engine must be initialised on the main thread. @@ -617,8 +618,10 @@ void Sensors::Update(const UpdateInfo &_info, _ecm.HasComponentType(components::SegmentationCamera::typeId) || _ecm.HasComponentType(components::WideAngleCamera::typeId))) { + std::unique_lock lock(this->dataPtr->renderMutex); igndbg << "Initialization needed" << std::endl; this->dataPtr->renderUtil.Init(); + this->dataPtr->nextUpdateTime = _info.simTime; } #endif @@ -633,9 +636,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { GZ_PROFILE("Sensors::PostUpdate"); - { - std::unique_lock lock(this->dataPtr->renderMutex); if (!this->dataPtr->initialized && (this->dataPtr->forceUpdate || _ecm.HasComponentType(components::BoundingBoxCamera::typeId) || @@ -647,6 +648,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, _ecm.HasComponentType(components::SegmentationCamera::typeId) || _ecm.HasComponentType(components::WideAngleCamera::typeId))) { + std::unique_lock lock(this->dataPtr->renderMutex); gzdbg << "Initialization needed" << std::endl; this->dataPtr->doInit = true; this->dataPtr->renderCv.notify_one(); @@ -655,67 +657,70 @@ void Sensors::PostUpdate(const UpdateInfo &_info, if (this->dataPtr->running && this->dataPtr->initialized) { - this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); - - std::vector activeSensors; - { - std::unique_lock lk(this->dataPtr->sensorMaskMutex); - for (auto id : this->dataPtr->sensorIds) - { - sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); - - if (nullptr == s) - { - continue; - } - - auto rs = dynamic_cast(s); - - if (nullptr == rs) - { - continue; - } - - auto it = this->dataPtr->sensorMask.find(id); - if (it != this->dataPtr->sensorMask.end()) - { - if (it->second <= _info.simTime) - { - this->dataPtr->sensorMask.erase(it); - } - else - { - continue; - } - } + std::unique_lock lock(this->dataPtr->renderMutex); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + this->dataPtr->updateTime = _info.simTime; + } - if (rs && rs->NextDataUpdateTime() <= _info.simTime) - { - activeSensors.push_back(rs); - } - } + // check connections to render events + // we will need to perform render updates if there are event subscribers + // \todo(anyone) This currently forces scene tree updates at the sim update + // rate which can be too frequent and causes a performance hit. + // We should look into throttling render updates + bool hasRenderConnections = + (this->dataPtr->eventManager->ConnectionCount() > 0u || + this->dataPtr->eventManager->ConnectionCount() > 0u || + this->dataPtr->eventManager->ConnectionCount() > 0u); + + // if nextUpdateTime is max, it probably means there are previously + // no active sensors or sensors with connections. + // In this case, check if sensors have connections now. If so, we need to + // set the nextUpdateTime + if (this->dataPtr->nextUpdateTime == + std::chrono::steady_clock::duration::max() && + this->dataPtr->SensorsHaveConnections()) + { + this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( + this->dataPtr->sensorsToUpdate, _info.simTime); } - if (!activeSensors.empty() || + // notify the render thread if updates are available + if (hasRenderConnections || + this->dataPtr->nextUpdateTime <= _info.simTime || this->dataPtr->renderUtil.PendingSensors() > 0 || this->dataPtr->forceUpdate) { if (this->dataPtr->disableOnDrainedBattery) this->dataPtr->UpdateBatteryState(_ecm); - std::unique_lock lock(this->dataPtr->renderMutex); - this->dataPtr->renderCv.wait(lock, [this] { - return !this->dataPtr->running || !this->dataPtr->updateAvailable; }); + { + std::unique_lock cvLock(this->dataPtr->renderMutex); + this->dataPtr->renderCv.wait(cvLock, [this] { + return !this->dataPtr->running || !this->dataPtr->updateAvailable; }); + } if (!this->dataPtr->running) { return; } - this->dataPtr->activeSensors = std::move(activeSensors); - this->dataPtr->updateTime = _info.simTime; - this->dataPtr->updateAvailable = true; + { + std::unique_lock lockSensors(this->dataPtr->sensorsMutex); + this->dataPtr->activeSensors = + std::move(this->dataPtr->sensorsToUpdate); + + this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( + this->dataPtr->sensorsToUpdate, _info.simTime); + + // Force scene tree update if there are sensors to be created or + // subscribes to the render events. This does not necessary force + // sensors to update. Only active sensors will be updated + this->dataPtr->forceUpdate = + (this->dataPtr->renderUtil.PendingSensors() > 0) || + hasRenderConnections; + this->dataPtr->updateAvailable = true; + } this->dataPtr->renderCv.notify_one(); } } @@ -921,6 +926,86 @@ std::string Sensors::CreateSensor(const Entity &_entity, return sensor->Name(); } +////////////////////////////////////////////////// +std::chrono::steady_clock::duration SensorsPrivate::NextUpdateTime( + std::set &_sensorsToUpdate, + const std::chrono::steady_clock::duration &_currentTime) +{ + _sensorsToUpdate.clear(); + std::chrono::steady_clock::duration minNextUpdateTime = + std::chrono::steady_clock::duration::max(); + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + + if (nullptr == s) + { + continue; + } + + auto rs = dynamic_cast(s); + + if (nullptr == rs) + { + continue; + } + + if (!rs->HasConnections()) + { + 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 + // Set their next update time to be current time + update period + if (rs->NextDataUpdateTime() <= _currentTime) + { + time = rs->NextDataUpdateTime() + + std::chrono::duration_cast( + std::chrono::duration(1.0 / rs->UpdateRate())); + } + else + { + time = rs->NextDataUpdateTime(); + } + + if (time <= minNextUpdateTime) + { + _sensorsToUpdate.clear(); + minNextUpdateTime = time; + } + _sensorsToUpdate.insert(id); + } + return minNextUpdateTime; +} + +////////////////////////////////////////////////// +bool SensorsPrivate::SensorsHaveConnections() +{ + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + if (nullptr == s) + { + continue; + } + + auto rs = dynamic_cast(s); + + if (nullptr == rs) + { + continue; + } + + if (rs->HasConnections()) + { + return true; + } + } + return false; +} + GZ_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, Sensors::ISystemReset, diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 95e560a439..73295e7137 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -103,6 +103,7 @@ set(tests_needing_display rgbd_camera.cc sensors_system.cc sensors_system_battery.cc + sensors_system_update_rate.cc shader_param_system.cc thermal_sensor_system.cc thermal_system.cc diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc index 6a0adbc6a2..592f918380 100644 --- a/test/integration/reset_sensors.cc +++ b/test/integration/reset_sensors.cc @@ -201,7 +201,7 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) // Run until a sensor measurement pressureReceiver.Start(topic); imageReceiver.Start("camera"); - while (!(pressureReceiver.msgReceived && imageReceiver.msgReceived)) + while (!pressureReceiver.msgReceived) { // Step once to get sensor to output measurement server.Run(true, 1, false); @@ -210,6 +210,12 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) EXPECT_GE(server.IterationCount().value(), current); EXPECT_FLOAT_EQ(kStartingPressure, pressureReceiver.Last().pressure()); + while (!imageReceiver.msgReceived) + { + // Step once to get sensor to output measurement + server.Run(true, 1, false); + } + // Mostly green box { auto image = toImage(imageReceiver.Last()); @@ -218,7 +224,6 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) EXPECT_FLOAT_EQ(0.0, centerPix.R()); EXPECT_FLOAT_EQ(0.0, centerPix.B()); } - // Run until 2000 steps pressureReceiver.msgReceived = false; imageReceiver.msgReceived = false; @@ -284,7 +289,7 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) current = 2001; target = 4001; - while (!(pressureReceiver.msgReceived && imageReceiver.msgReceived)) + while (!pressureReceiver.msgReceived) { // Step once to get sensor to output measurement server.Run(true, 1, false); @@ -292,6 +297,12 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) EXPECT_GE(server.IterationCount().value(), 1u); EXPECT_FLOAT_EQ(kStartingPressure, pressureReceiver.Last().pressure()); + while (!imageReceiver.msgReceived) + { + // Step once to get sensor to output measurement + server.Run(true, 1, false); + } + // Mostly green box { auto image = toImage(imageReceiver.Last()); @@ -304,6 +315,7 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) // Run until target steps pressureReceiver.msgReceived = false; imageReceiver.msgReceived = false; + server.Run(true, 2000 - server.IterationCount().value(), false); // Check iterator state diff --git a/test/integration/sensors_system_update_rate.cc b/test/integration/sensors_system_update_rate.cc new file mode 100644 index 0000000000..8cd5d802df --- /dev/null +++ b/test/integration/sensors_system_update_rate.cc @@ -0,0 +1,311 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include + +#include +#include + +#include "gz/sim/components/Camera.hh" +#include "gz/sim/components/DepthCamera.hh" +#include "gz/sim/components/GpuLidar.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Physics.hh" +#include "gz/sim/components/RgbdCamera.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/components/SegmentationCamera.hh" +#include "gz/sim/components/ThermalCamera.hh" +#include "gz/sim/components/World.hh" + +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/Types.hh" +#include "test_config.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace std::chrono_literals; +namespace components = gz::sim::components; + +////////////////////////////////////////////////// +class SensorsFixture : public InternalFixture> +{ + protected: void SetUp() override + { + InternalFixture::SetUp(); + + sdf::Plugin sdfPlugin; + sdfPlugin.SetFilename("libMockSystem.so"); + sdfPlugin.SetName("gz::sim::MockSystem"); + auto plugin = sm.LoadPlugin(sdfPlugin); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + public: gz::sim::SystemPluginPtr systemPtr; + public: sim::MockSystem *mockSystem; + + private: sim::SystemLoader sm; +}; + +///////////////////////////////////////////////// +TEST_F(SensorsFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(UpdateRate)) +{ + gz::sim::ServerConfig serverConfig; + + const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/sensor.sdf"; + + serverConfig.SetSdfFile(sdfFile); + + sim::Server server(serverConfig); + + // A pointer to the ecm. This will be valid once we run the mock system + sim::EntityComponentManager *ecm = nullptr; + this->mockSystem->configureCallback = + [&](const sim::Entity &, + const std::shared_ptr &, + sim::EntityComponentManager &_ecm, + sim::EventManager &) + { + ecm = &_ecm; + }; + + server.AddSystem(this->systemPtr); + transport::Node node; + std::string prefix{"/world/camera_sensor/model/default_topics/"}; + + std::vector imageTimestamps; + unsigned int imageCount = 0u; + auto cameraCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + imageTimestamps.push_back(t); + ++imageCount; + }); + node.Subscribe(prefix + "link/camera_link/sensor/camera/image", cameraCb); + + std::vector lidarTimestamps; + unsigned int lidarCount = 0u; + auto lidarCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + lidarTimestamps.push_back(t); + ++lidarCount; + }); + node.Subscribe(prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", lidarCb); + + std::vector depthTimestamps; + unsigned int depthCount = 0u; + auto depthCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + depthTimestamps.push_back(t); + ++depthCount; + }); + node.Subscribe( + prefix + "link/depth_camera_link/sensor/depth_camera/depth_image", + depthCb); + + std::vector rgbdTimestamps; + unsigned int rgbdCount = 0u; + auto rgbdCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + rgbdTimestamps.push_back(t); + ++rgbdCount; + }); + node.Subscribe( + prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image", + rgbdCb); + + std::vector thermalTimestamps; + unsigned int thermalCount = 0u; + auto thermalCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + thermalTimestamps.push_back(t); + ++thermalCount; + }); + node.Subscribe( + prefix + "link/thermal_camera_link/sensor/thermal_camera/image", + thermalCb); + + std::vector segmentationTimestamps; + unsigned int segmentationCount = 0u; + auto segmentationCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + segmentationTimestamps.push_back(t); + ++segmentationCount; + }); + node.Subscribe( + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/colored_map", + segmentationCb); + + unsigned int iterations = 2000u; + server.Run(true, iterations, false); + + EXPECT_NE(nullptr, ecm); + + // get the world step size + auto worldEntity = ecm->EntityByComponents(components::World()); + EXPECT_NE(sim::kNullEntity, worldEntity); + auto physicsSdf = ecm->Component(worldEntity)->Data(); + double stepSize = physicsSdf.MaxStepSize(); + EXPECT_LT(0, stepSize); + + // get the sensors update rates + auto camLinkEntity = ecm->EntityByComponents(components::Name("camera_link")); + EXPECT_NE(sim::kNullEntity, camLinkEntity); + auto camEntity = ecm->EntityByComponents(components::Name("camera"), + components::ParentEntity(camLinkEntity)); + EXPECT_NE(sim::kNullEntity, camEntity); + auto sensorSdf = ecm->Component(camEntity)->Data(); + unsigned int camRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, camRate); + + auto lidarEntity = ecm->EntityByComponents(components::Name("gpu_lidar")); + EXPECT_NE(sim::kNullEntity, lidarEntity); + sensorSdf = ecm->Component(lidarEntity)->Data(); + unsigned int lidarRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, lidarRate); + + auto depthEntity = ecm->EntityByComponents(components::Name("depth_camera")); + EXPECT_NE(sim::kNullEntity, depthEntity); + sensorSdf = ecm->Component(depthEntity)->Data(); + unsigned int depthRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, depthRate); + + auto rgbdEntity = ecm->EntityByComponents(components::Name("rgbd_camera")); + EXPECT_NE(sim::kNullEntity, rgbdEntity); + sensorSdf = ecm->Component(rgbdEntity)->Data(); + unsigned int rgbdRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, rgbdRate); + + auto thermalEntity = ecm->EntityByComponents( + components::Name("thermal_camera")); + EXPECT_NE(sim::kNullEntity, thermalEntity); + sensorSdf = ecm->Component(thermalEntity)->Data(); + unsigned int thermalRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, thermalRate); + + auto segmentationEntity = ecm->EntityByComponents( + components::Name("segmentation_camera")); + EXPECT_NE(sim::kNullEntity, segmentationEntity); + sensorSdf = ecm->Component( + segmentationEntity)->Data(); + unsigned int segmentationRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, segmentationRate); + + // compute and verify expected msg count based on update rate and sim time + double timeRan = iterations * stepSize; + + unsigned int expectedCamMsgCount = timeRan / (1.0 / camRate) + 1; + unsigned int expectedDepthMsgCount = timeRan / (1.0 / depthRate) + 1; + unsigned int expectedLidarMsgCount = timeRan / (1.0 / lidarRate) + 1; + unsigned int expectedRgbdMsgCount = timeRan / (1.0 / rgbdRate) + 1; + unsigned int expectedThermalMsgCount = timeRan / (1.0 / thermalRate) + 1; + unsigned int expectedSegmentationMsgCount = + timeRan / (1.0 / segmentationRate) + 1; + + unsigned int sleep = 0; + unsigned int maxSleep = 100; + while (sleep++ < maxSleep && + (imageCount < expectedCamMsgCount || + lidarCount < expectedLidarMsgCount || + depthCount < expectedDepthMsgCount || + rgbdCount < expectedRgbdMsgCount || + thermalCount < expectedThermalMsgCount || + segmentationCount < expectedSegmentationMsgCount)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + EXPECT_EQ(expectedCamMsgCount, imageCount); + EXPECT_EQ(expectedDepthMsgCount, depthCount); + EXPECT_EQ(expectedLidarMsgCount, lidarCount); + EXPECT_EQ(expectedRgbdMsgCount, rgbdCount); + EXPECT_EQ(expectedThermalMsgCount, thermalCount); + EXPECT_EQ(expectedSegmentationMsgCount, segmentationCount); + + // verify timestamps + // The first timestamp may not be 0 because the rendering + // may take some time to start and it does not block the main thread + // so start with index = 1 + // \todo(anyone) Make the sensors system thread block so we always + // generate data at t = 0? + EXPECT_FALSE(imageTimestamps.empty()); + EXPECT_FALSE(lidarTimestamps.empty()); + EXPECT_FALSE(depthTimestamps.empty()); + EXPECT_FALSE(rgbdTimestamps.empty()); + EXPECT_FALSE(thermalTimestamps.empty()); + EXPECT_FALSE(segmentationTimestamps.empty()); + for (unsigned int i = 1; i < imageTimestamps.size()-1; ++i) + { + double dt = imageTimestamps[i+1] - imageTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / camRate, dt); + } + for (unsigned int i = 1; i < lidarTimestamps.size()-1; ++i) + { + double dt = lidarTimestamps[i+1] - lidarTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / lidarRate, dt); + } + for (unsigned int i = 1; i < depthTimestamps.size()-1; ++i) + { + double dt = depthTimestamps[i+1] - depthTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / depthRate, dt); + } + for (unsigned int i = 1; i < rgbdTimestamps.size()-1; ++i) + { + double dt = rgbdTimestamps[i+1] - rgbdTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / rgbdRate, dt); + } + for (unsigned int i = 1; i < thermalTimestamps.size()-1; ++i) + { + double dt = thermalTimestamps[i+1] - thermalTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / thermalRate, dt); + } + for (unsigned int i = 1; i < segmentationTimestamps.size()-1; ++i) + { + double dt = segmentationTimestamps[i+1] - segmentationTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / segmentationRate, dt); + } +} diff --git a/test/worlds/sensor.sdf b/test/worlds/sensor.sdf index ed5e1eaab5..d025b942f9 100644 --- a/test/worlds/sensor.sdf +++ b/test/worlds/sensor.sdf @@ -2,7 +2,8 @@ - 0 + 0.001 + 1.0 true + 50 1.047 @@ -103,6 +105,7 @@ " + 40 @@ -129,6 +132,7 @@ + 25 1.05 @@ -152,6 +156,7 @@ + 10 1.05 @@ -168,6 +173,7 @@ + 40 1.15 @@ -184,6 +190,7 @@ + 25 1.0 From bfffa876f53077452780b919e1e31e06087d3d42 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 26 Apr 2023 13:01:24 -0700 Subject: [PATCH 145/160] Add back in the marker example (#1972) Signed-off-by: Nate Koenig --- examples/standalone/marker/CMakeLists.txt | 21 ++ examples/standalone/marker/README.md | 22 ++ examples/standalone/marker/marker.cc | 314 ++++++++++++++++++++++ 3 files changed, 357 insertions(+) create mode 100644 examples/standalone/marker/CMakeLists.txt create mode 100644 examples/standalone/marker/README.md create mode 100644 examples/standalone/marker/marker.cc diff --git a/examples/standalone/marker/CMakeLists.txt b/examples/standalone/marker/CMakeLists.txt new file mode 100644 index 0000000000..380d2b843a --- /dev/null +++ b/examples/standalone/marker/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +project(gz-sim-marker) + +if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") + find_package(gz-transport12 QUIET REQUIRED) + set(GZ_TRANSPORT_VER ${gz-transport12_VERSION_MAJOR}) + + find_package(gz-common5 REQUIRED) + set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) + + find_package(gz-msgs9 REQUIRED) + set(GZ_MSGS_VER ${gz-msgs9_VERSION_MAJOR}) + + add_executable(marker marker.cc) + target_link_libraries(marker + gz-transport${GZ_TRANSPORT_VER}::core + gz-msgs${GZ_MSGS_VER} + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + ) +endif() diff --git a/examples/standalone/marker/README.md b/examples/standalone/marker/README.md new file mode 100644 index 0000000000..83a6c367f7 --- /dev/null +++ b/examples/standalone/marker/README.md @@ -0,0 +1,22 @@ +# Gazebo Visualization Marker Example + +This example demonstrates how to create, modify, and delete visualization +markers in Gazebo. + +## Build Instructions + +From this directory: + + mkdir build + cd build + cmake .. + make + +## Execute Instructions + +Launch `gz sim` unpaused then from the build directory above: + + ./marker + +The terminal will output messages indicating visualization changes that +will occur in Gazebo's render window. diff --git a/examples/standalone/marker/marker.cc b/examples/standalone/marker/marker.cc new file mode 100644 index 0000000000..5cf7e725ba --- /dev/null +++ b/examples/standalone/marker/marker.cc @@ -0,0 +1,314 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include + +#include + +///////////////////////////////////////////////// +int main(int _argc, char **_argv) +{ + gz::transport::Node node; + + // Create the marker message + gz::msgs::Marker markerMsg; + gz::msgs::Material matMsg; + markerMsg.set_ns("default"); + markerMsg.set_id(0); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::SPHERE); + markerMsg.set_visibility(gz::msgs::Marker::GUI); + + // Set color to Blue + markerMsg.mutable_material()->mutable_ambient()->set_r(0); + markerMsg.mutable_material()->mutable_ambient()->set_g(0); + markerMsg.mutable_material()->mutable_ambient()->set_b(1); + markerMsg.mutable_material()->mutable_ambient()->set_a(1); + markerMsg.mutable_material()->mutable_diffuse()->set_r(0); + markerMsg.mutable_material()->mutable_diffuse()->set_g(0); + markerMsg.mutable_material()->mutable_diffuse()->set_b(1); + markerMsg.mutable_material()->mutable_diffuse()->set_a(1); + markerMsg.mutable_lifetime()->set_sec(2); + markerMsg.mutable_lifetime()->set_nsec(0); + gz::msgs::Set(markerMsg.mutable_scale(), + gz::math::Vector3d(1.0, 1.0, 1.0)); + + // The rest of this function adds different shapes and/or modifies shapes. + // Read the terminal statements to figure out what each node.Request + // call accomplishes. + std::cout << "Spawning a blue sphere with lifetime 2s\n"; + GZ_SLEEP_S(4); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(2, 2, 0, 0, 0, 0)); + node.Request("/marker", markerMsg); + std::cout << "Sleeping for 2 seconds\n"; + GZ_SLEEP_S(2); + + std::cout << "Spawning a black sphere at the origin with no lifetime\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(1); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d::Zero); + markerMsg.mutable_material()->mutable_ambient()->set_b(0); + markerMsg.mutable_material()->mutable_diffuse()->set_b(0); + markerMsg.mutable_lifetime()->set_sec(0); + node.Request("/marker", markerMsg); + + std::cout << "Moving the sphere to x=0, y=1, z=1\n"; + GZ_SLEEP_S(4); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(0, 1, 1, 0, 0, 0)); + node.Request("/marker", markerMsg); + + std::cout << "Shrinking the sphere\n"; + GZ_SLEEP_S(4); + gz::msgs::Set(markerMsg.mutable_scale(), + gz::math::Vector3d(0.2, 0.2, 0.2)); + node.Request("/marker", markerMsg); + + std::cout << "Changing the sphere to red\n"; + markerMsg.mutable_material()->mutable_ambient()->set_r(1); + markerMsg.mutable_material()->mutable_ambient()->set_g(0); + markerMsg.mutable_material()->mutable_ambient()->set_b(0); + markerMsg.mutable_material()->mutable_diffuse()->set_r(1); + markerMsg.mutable_material()->mutable_diffuse()->set_g(0); + markerMsg.mutable_material()->mutable_diffuse()->set_b(0); + GZ_SLEEP_S(4); + node.Request("/marker", markerMsg); + + std::cout << "Adding a green box\n"; + markerMsg.mutable_material()->mutable_ambient()->set_r(0); + markerMsg.mutable_material()->mutable_ambient()->set_g(1); + markerMsg.mutable_material()->mutable_ambient()->set_b(0); + markerMsg.mutable_material()->mutable_diffuse()->set_r(0); + markerMsg.mutable_material()->mutable_diffuse()->set_g(1); + markerMsg.mutable_material()->mutable_diffuse()->set_b(0); + GZ_SLEEP_S(4); + markerMsg.set_id(2); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::BOX); + gz::msgs::Set(markerMsg.mutable_scale(), + gz::math::Vector3d(1.0, 1.0, 1.0)); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(2, 0, .5, 0, 0, 0)); + node.Request("/marker", markerMsg); + + std::cout << "Changing the green box to a cylinder\n"; + GZ_SLEEP_S(4); + markerMsg.set_type(gz::msgs::Marker::CYLINDER); + node.Request("/marker", markerMsg); + + std::cout << "Connecting the sphere and cylinder with a line\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(3); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(0, 0, 0, 0, 0, 0)); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::LINE_LIST); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0.0, 1.0, 1.0)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(2, 0, 0.5)); + node.Request("/marker", markerMsg); + + std::cout << "Adding a square around the origin\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(4); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::LINE_STRIP); + gz::msgs::Set(markerMsg.mutable_point(0), + gz::math::Vector3d(0.5, 0.5, 0.05)); + gz::msgs::Set(markerMsg.mutable_point(1), + gz::math::Vector3d(0.5, -0.5, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(-0.5, -0.5, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(-0.5, 0.5, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0.5, 0.5, 0.05)); + node.Request("/marker", markerMsg); + + std::cout << "Adding 100 points inside the square\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(5); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::POINTS); + markerMsg.clear_point(); + for (int i = 0; i < 100; ++i) + { + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d( + gz::math::Rand::DblUniform(-0.49, 0.49), + gz::math::Rand::DblUniform(-0.49, 0.49), + 0.05)); + } + node.Request("/marker", markerMsg); + + std::cout << "Adding a semi-circular triangle fan\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(6); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::TRIANGLE_FAN); + markerMsg.clear_point(); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(0, 1.5, 0, 0, 0, 0)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0, 0, 0.05)); + double radius = 2; + for (double t = 0; t <= M_PI; t+= 0.01) + { + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(radius * cos(t), radius * sin(t), 0.05)); + } + node.Request("/marker", markerMsg); + + std::cout << "Adding two triangles using a triangle list\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(7); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::TRIANGLE_LIST); + markerMsg.clear_point(); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(0, -1.5, 0, 0, 0, 0)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0, 0, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 0, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 1, 0.05)); + + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 1, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(2, 1, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(2, 2, 0.05)); + + node.Request("/marker", markerMsg); + + std::cout << "Adding a rectangular triangle strip\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(8); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::TRIANGLE_STRIP); + markerMsg.clear_point(); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(-2, -2, 0, 0, 0, 0)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0, 0, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 0, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0, 1, 0.05)); + + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 1, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0, 2, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 2, 0.05)); + + node.Request("/marker", markerMsg); + std::cout << "Adding multiple markers via /marker_array\n"; + GZ_SLEEP_S(4); + + gz::msgs::Marker_V markerMsgs; + gz::msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + + // Create first blue sphere marker + auto markerMsg1 = markerMsgs.add_marker(); + markerMsg1->set_ns("default"); + markerMsg1->set_id(0); + markerMsg1->set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg1->set_type(gz::msgs::Marker::SPHERE); + markerMsg1->set_visibility(gz::msgs::Marker::GUI); + + // Set color to Blue + markerMsg1->mutable_material()->mutable_ambient()->set_r(0); + markerMsg1->mutable_material()->mutable_ambient()->set_g(0); + markerMsg1->mutable_material()->mutable_ambient()->set_b(1); + markerMsg1->mutable_material()->mutable_ambient()->set_a(1); + markerMsg1->mutable_material()->mutable_diffuse()->set_r(0); + markerMsg1->mutable_material()->mutable_diffuse()->set_g(0); + markerMsg1->mutable_material()->mutable_diffuse()->set_b(1); + markerMsg1->mutable_material()->mutable_diffuse()->set_a(1); + gz::msgs::Set(markerMsg1->mutable_scale(), + gz::math::Vector3d(1.0, 1.0, 1.0)); + gz::msgs::Set(markerMsg1->mutable_pose(), + gz::math::Pose3d(3, 3, 0, 0, 0, 0)); + + // Create second red box marker + auto markerMsg2 = markerMsgs.add_marker(); + markerMsg2->set_ns("default"); + markerMsg2->set_id(0); + markerMsg2->set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg2->set_type(gz::msgs::Marker::BOX); + markerMsg2->set_visibility(gz::msgs::Marker::GUI); + + // Set color to Red + markerMsg2->mutable_material()->mutable_ambient()->set_r(1); + markerMsg2->mutable_material()->mutable_ambient()->set_g(0); + markerMsg2->mutable_material()->mutable_ambient()->set_b(0); + markerMsg2->mutable_material()->mutable_ambient()->set_a(1); + markerMsg2->mutable_material()->mutable_diffuse()->set_r(1); + markerMsg2->mutable_material()->mutable_diffuse()->set_g(0); + markerMsg2->mutable_material()->mutable_diffuse()->set_b(0); + markerMsg2->mutable_material()->mutable_diffuse()->set_a(1); + markerMsg2->mutable_lifetime()->set_sec(2); + markerMsg2->mutable_lifetime()->set_nsec(0); + gz::msgs::Set(markerMsg2->mutable_scale(), + gz::math::Vector3d(1.0, 1.0, 1.0)); + gz::msgs::Set(markerMsg2->mutable_pose(), + gz::math::Pose3d(3, 3, 2, 0, 0, 0)); + + // Create third green cylinder marker + auto markerMsg3 = markerMsgs.add_marker(); + markerMsg3->set_ns("default"); + markerMsg3->set_id(0); + markerMsg3->set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg3->set_type(gz::msgs::Marker::CYLINDER); + markerMsg3->set_visibility(gz::msgs::Marker::GUI); + + // Set color to Green + markerMsg3->mutable_material()->mutable_ambient()->set_r(0); + markerMsg3->mutable_material()->mutable_ambient()->set_g(1); + markerMsg3->mutable_material()->mutable_ambient()->set_b(0); + markerMsg3->mutable_material()->mutable_ambient()->set_a(1); + markerMsg3->mutable_material()->mutable_diffuse()->set_r(0); + markerMsg3->mutable_material()->mutable_diffuse()->set_g(1); + markerMsg3->mutable_material()->mutable_diffuse()->set_b(0); + markerMsg3->mutable_material()->mutable_diffuse()->set_a(1); + markerMsg3->mutable_lifetime()->set_sec(2); + markerMsg3->mutable_lifetime()->set_nsec(0); + gz::msgs::Set(markerMsg3->mutable_scale(), + gz::math::Vector3d(1.0, 1.0, 1.0)); + gz::msgs::Set(markerMsg3->mutable_pose(), + gz::math::Pose3d(3, 3, 4, 0, 0, 0)); + + // Publish the three created markers above simultaneously + node.Request("/marker_array", markerMsgs, timeout, res, result); + + std::cout << "Deleting all the markers\n"; + GZ_SLEEP_S(4); + markerMsg.set_action(gz::msgs::Marker::DELETE_ALL); + node.Request("/marker", markerMsg); +} From e11c2c2d2c48c7d041629d42d794f4f3e70447d8 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 27 Apr 2023 10:21:27 -0700 Subject: [PATCH 146/160] Add tutorial on migrating the Actor class from gazebo classic (#1929) --------- Signed-off-by: Ian Chen --- tutorials.md.in | 1 + tutorials/migration_actor_api.md | 247 +++++++++++++++++++++++++++++++ 2 files changed, 248 insertions(+) create mode 100644 tutorials/migration_actor_api.md diff --git a/tutorials.md.in b/tutorials.md.in index 40d5528ef2..c454fc1ccc 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -51,6 +51,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage migrationmodelapi "Model API": Guide on what Model C++ functions to call in Gazebo when migrating from Gazebo classic * \subpage migrationlightapi "Light API": Guide on what Light C++ functions to call in Gazebo when migrating from Gazebo classic * \subpage migrationjointapi "Joint API": Guide on what Joint C++ functions to call in Gazebo when migrating from Gazebo classic +* \subpage migrationactorapi "Actor API": Guide on what Actor C++ functions to call in Gazebo when migrating from Gazebo classic * \subpage migrationlinkapi "Link API": Guide on what Link C++ functions to call in Gazebo when migrating from Gazebo classic * \subpage ardupilot "Case Study": Migrating the ArduPilot ModelPlugin from Gazebo classic to Gazebo. diff --git a/tutorials/migration_actor_api.md b/tutorials/migration_actor_api.md new file mode 100644 index 0000000000..25456e53ff --- /dev/null +++ b/tutorials/migration_actor_api.md @@ -0,0 +1,247 @@ +\page migrationactorapi + +# Migration from Gazebo-classic: Actor API + +When migrating plugins from Gazebo-classic to Gazebo, developers will +notice that the C++ APIs for both simulators are quite different. Be sure to +check the [plugin migration tutorial](migrationplugins.html) to get a high-level +view of the architecture differences before using this guide. + +This tutorial is meant to serve as a reference guide for developers migrating +functions from the +[gazebo::phyiscs::Actor](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Actor.html) +class. + +If you're trying to use some API which doesn't have an equivalent on Gazebo +yet, feel free to +[ticket an issue](https://github.com/gazebosim/gz-sim/issues/). + +## Actor API + +Gazebo-classic's `gazebo::physics::Actor` provides lots of functionality, which +can be divided in these categories: + +* **Properties**: Setting / getting properties + * Example: [Actor::GetName](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#a9a98946a64f3893b085f650932c9dfee) / [Actor::SetName](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Entity.html#a5d74ac4d7a230aed1ab4b11933b16e92) +* **Read family**: Getting children and parent + * Example: [Actor::GetParentModel](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Entity.html#a94d6f01102b5d949006fba3628d9f355) +* **Write family**: Adding children, changing parent + * Example: [Actor::RemoveChildren](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#aa85d2386e6fb02bdbec060a74b63238a) +* **Lifecycle**: Functions to control the actor's lifecycle + * Example: [Actor::Init](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Actor.html#ae048ef824aaf614707c1496a2aefd415) +* **Others**: Functions that don't fit any of the categories above + * Example: [Actor::PlaceOnEntity](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Entity.html#a9ecbfeb56940cacd75f55bed6aa9fcb4) + +You'll find the Gazebo APIs below on the following headers: + +* [ignition/gazebo/Actor.hh](https://gazebosim.org/api/gazebo/6/Actor_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/6/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/6/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/6/classignition_1_1gazebo_1_1EntityComponentManager.html) + +It's worth remembering that most of this functionality can be performed using +the +[EntityComponentManager](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1EntityComponentManager.html) +directly. + + +As an example the `Actor::Pose()` is a convienient function for querying the `Pose` component from the `EntityComponentManager`, i.e. + +``` + math::Pose3d pose = _ecm.Component(actorEntityId)->Data(); +``` + +The functions presented in the sections below exist for convenience and +readability. The items marked as `TODO` means that the equivalent API is not +implemented yet in Gazebo. + +### Properties + +Most of Gazebo-classic's Actor API is related to setting and getting +properties that are done via the Entity-Component-System by setting components +(properties) into entities such as actors. Note that a number of the public APIs +in Classic are inherited from the base Model / Entity class and they may not +have equivalents in Gazebo. + +--- + +Classic | Gazebo +-- | -- +AddType | `ecm.CreateComponent(entity, Type())` +AlignBvh | TODO +BoundingBox | TODO +CollisionBoundingBox | TODO +CustomTrajectory | see `SetCustomTrajectory` +DirtyPose | Not supported +FillMsg | TODO +GetAutoDisable | TODO +GetId | `ignition::gazebo::Model::Entity` +GetName | `ignition::gazebo::Actor::Name` +GetPluginCount | TODO +GetSaveable | Not supported +GetScopedName | `ignition::gazebo::scopedName` +GetSDF | TODO +GetSDFDom | TODO +GetSelfCollide | TODO +GetType | `ignition::gazebo::entityType` +GetWorldEnergy | TODO +GetWorldEnergyKinetic | TODO +GetWorldEnergyPotential | TODO +HasType | `gazebo::components::Link::typeId == entityTypeId(entity, ecm)` +InitialRelativePose | TODO +IsActive | TODO +IsCanonicalLink | Not applicable. See link API +IsSelected | Selection is client-specific, not porting +IsStatic | TODO +Mesh | TODO +Play | TODO +PluginInfo | TODO +Print | TODO +ProcessMsg | TODO +RelativeAngularAccel | TODO +RelativeAngularVel | TODO +RelativeLinearAccel | TODO +RelativeLinearVel | TODO +RelativePose | `ignition::gazebo::Actor::Pose` +Scale | TODO +ScriptTime | `ignition::gazebo::Actor::AnimationTime` +SDFPoseRelativeToParent | `ignition::gazebo::Actor::Pose` +SDFSemanticPose | `ignition::gazebo::Actor::Pose` +SensorScopedName | TODO +SetAngularVel | TODO +SetAnimation | use `ignition::gazebo::Actor::SetTrajectoryPose` +SetAutoDisable | TODO +SetCollideMode | TODO +SetCustomTrajectory | use `ignition::gazebo::Actor::SetTrajectoryPose`, `SetAnimationTime`, and `SetAnimationName` to achieve similar result. +SetEnabled | TODO +SetGravityMode | TODO +SetInitialRelativePose | TODO +SetJointAnimation | Not supported +SetJointPosition | Not supported +SetJointPositions | Not supported +SetLaserRetro | TODO +SetLinearVel | TODO +SetLinkWorldPose | TODO +SetName | TODO +SetRelativePose | TODO +SetSaveable | Not supported +SetScale | TODO +SetScriptTime | `ignition::gazebo::Actor::SetAnimationTime` +SetSelected | Selection is client-specific, not porting +SetSelfCollide | TODO +SetState | TODO +SetStatic | TODO +SetWindMode | TODO +SetWorldPose | TODO +SetWorldTwist | TODO +SkeletonAnimations | TODO +Stop | TODO +StopAnimation | use `ignition::gazebo::Actor::SetTrajectoryPose` +TypeStr | `ignition::gazebo::entityTypeStr` +UnscaledSDF | TODO +UpdateParamenters | TODO +URI | TODO +WindMode | TODO +WorldAngularAccel | TODO +WorldAngularVel | TODO +WorldLinearAccel | TODO +WorldLinearVel | TODO +WorldPose | `ignition::gazebo::Actor::WorldPose` + +--- + +## Read family + +These APIs deal with reading information related to child / parent +relationships. + +The main difference in these APIs across Gazebo generations is that +on classic, they deal with shared pointers to entities, while on Gazebo, +they deal with entity IDs. + +--- + +Classic | Gazebo +-- | -- +GetByName | TODO +GetChild | TODO +GetChildCollision | TODO +GetChildCount | TODO +GetChildLink | TODO +GetGripper | Not supported +GetGripperCount | Not supported +GetJoint | Not supported +GetJointCount | Not supported +GetJoints | Not supported +GetLink | TODO +GetLinks | TODO +GetParent | `ignition::gazebo::EntiyComponentManager::ParentEntity` +GetParentId | `ignition::gazebo::EntiyComponentManager::ParentEntity` +GetParentModel | `ignition::gazebo::EntiyComponentManager::ParentEntity` +GetSensorCount | TODO +GetWorld | const `ignition::gazebo::worldEntity` +NestedModel | TODO +NestedModels | TODO + +--- + +## Write family + +These functions deal with modifying the entity tree, attaching children to new +parents. Note that APIs for changing an Actor's entity tree structure are +currently not implemented yet. + +--- + +Classic | Gazebo +-- | -- +AddChild | TODO +AttachStaticModel | TODO +DetachStaticModel | TODO +RemoveChild | TODO +RemoveChildren | TODO +RemoveJoint | TODO +SetCanonicalLink | TODO +SetParent | TODO +SetWorld | TODO + +--- + +## Lifecycle + +These functions aren't related to the state of a actor, but perform some +processing related to the actor's lifecycle, like initializing, updating or +terminating it. + +--- + +Classic | Gazebo +-- | -- +Fini | N/A +Init | N/A +Load | `ignition::gazebo::SdfEntityCreator::CreateEntities` +LoadJoints | `ignition::gazebo::SdfEntityCreator::CreateEntities` +LoadPlugins | TODO +Reset | TODO +ResetCustomTrajectory | TODO +ResetPhysicsStates | TODO +Update | Entities are updated by systems + +--- + + +## Others + +Miscelaneous functions that don't fit the other categories. Most of them involve +logic that should be performed from within a system. + +--- + +Classic | Gazebo +-- | -- +GetJointController | Use this system: `ignition::gazebo::systems::JointController` +GetNearestEntityBelow | Requires a system +PlaceOnEntity | Requires a system +PlaceOnNearestEntityBelow | Requires a system + +--- From 14d583283a860efa71d80d9d6d620ba0b31afa97 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 28 Apr 2023 00:44:45 +0200 Subject: [PATCH 147/160] Use GzSpinBox. (#1969) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/gui/plugins/view_angle/ViewAngle.qml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/view_angle/ViewAngle.qml b/src/gui/plugins/view_angle/ViewAngle.qml index 3750f4e4c5..36eab6d999 100644 --- a/src/gui/plugins/view_angle/ViewAngle.qml +++ b/src/gui/plugins/view_angle/ViewAngle.qml @@ -347,7 +347,7 @@ ColumnLayout { Layout.column: 0 leftPadding: 5 } - IgnSpinBox { + GzSpinBox { id: horizontalFOV Layout.fillWidth: true Layout.row: 0 From 13aedfd731b95794981ac1e47cfbe0d378dee054 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 1 May 2023 06:32:17 -0700 Subject: [PATCH 148/160] Unload render engine when the sensors system exits (#1960) Signed-off-by: Ian Chen --- include/gz/sim/rendering/MarkerManager.hh | 5 + include/gz/sim/rendering/RenderUtil.hh | 3 + include/gz/sim/rendering/SceneManager.hh | 5 + src/rendering/MarkerManager.cc | 8 ++ src/rendering/RenderUtil.cc | 15 +++ src/rendering/SceneManager.cc | 15 +++ src/systems/sensors/Sensors.cc | 2 + test/integration/ModelPhotoShootTest.hh | 2 + test/performance/CMakeLists.txt | 21 ++++ test/performance/sensors_system.cc | 136 ++++++++++++++++++++++ test/worlds/lights_render.sdf | 2 +- test/worlds/sensors_system_mem_leak.sdf | 69 +++++++++++ 12 files changed, 282 insertions(+), 1 deletion(-) create mode 100644 test/performance/sensors_system.cc create mode 100644 test/worlds/sensors_system_mem_leak.sdf diff --git a/include/gz/sim/rendering/MarkerManager.hh b/include/gz/sim/rendering/MarkerManager.hh index 5be2a04cd9..3bed374c0d 100644 --- a/include/gz/sim/rendering/MarkerManager.hh +++ b/include/gz/sim/rendering/MarkerManager.hh @@ -66,6 +66,11 @@ class GZ_SIM_RENDERING_VISIBLE MarkerManager /// \param[in] _name Name of service public: void SetTopic(const std::string &_name); + /// \brief Clear the marker manager + /// Clears internal resources stored in the marker manager. + /// Note: this does not actually destroy the objects. + public: void Clear(); + /// \internal /// \brief Private data pointer private: std::unique_ptr dataPtr; diff --git a/include/gz/sim/rendering/RenderUtil.hh b/include/gz/sim/rendering/RenderUtil.hh index 6015494f7d..1c8e0e7557 100644 --- a/include/gz/sim/rendering/RenderUtil.hh +++ b/include/gz/sim/rendering/RenderUtil.hh @@ -53,6 +53,9 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { /// \brief Initialize the renderer. Must be called in the rendering thread. public: void Init(); + /// \brief Destroy the renderer. Must be called in the rendering thread. + public: void Destroy(); + /// \brief Count of pending sensors. Must be called in the rendering thread. /// \return Number of sensors to be added on the next `Update` call /// diff --git a/include/gz/sim/rendering/SceneManager.hh b/include/gz/sim/rendering/SceneManager.hh index 4d09ed7f86..a4bcdd297f 100644 --- a/include/gz/sim/rendering/SceneManager.hh +++ b/include/gz/sim/rendering/SceneManager.hh @@ -373,6 +373,11 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { /// IDs are available public: Entity UniqueId() const; + /// \brief Clear the scene manager + /// Clears internal resources stored in the scene manager. + /// Note: this does not actually destroy the objects. + public: void Clear(); + /// \internal /// \brief Pointer to private data class private: std::unique_ptr dataPtr; diff --git a/src/rendering/MarkerManager.cc b/src/rendering/MarkerManager.cc index 9f8e559875..6c216ce5cf 100644 --- a/src/rendering/MarkerManager.cc +++ b/src/rendering/MarkerManager.cc @@ -637,3 +637,11 @@ bool MarkerManagerPrivate::OnMarkerMsgArray( _res.set_data(true); return true; } + +///////////////////////////////////////////////// +void MarkerManager::Clear() +{ + this->dataPtr->visuals.clear(); + this->dataPtr->markerMsgs.clear(); + this->dataPtr->scene.reset(); +} diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index e79767dd5d..84b3b3fe23 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -2697,6 +2697,21 @@ void RenderUtil::Init() this->dataPtr->markerManager.Init(this->dataPtr->scene); } +///////////////////////////////////////////////// +void RenderUtil::Destroy() +{ + if (!this->dataPtr->engine || !this->dataPtr->scene) + return; + this->dataPtr->wireBoxes.clear(); + this->dataPtr->sceneManager.Clear(); + this->dataPtr->markerManager.Clear(); + this->dataPtr->engine->DestroyScene(this->dataPtr->scene); + this->dataPtr->scene.reset(); + rendering::unloadEngine(this->dataPtr->engine->Name()); + this->dataPtr->engine = nullptr; + this->dataPtr->initialized = false; +} + ///////////////////////////////////////////////// void RenderUtil::SetBackgroundColor(const math::Color &_color) { diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 07e24c07fa..f923bd9f94 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -2457,3 +2457,18 @@ SceneManagerPrivate::LoadTrajectories(const sdf::Actor &_actor, } return trajectories; } + +///////////////////////////////////////////////// +void SceneManager::Clear() +{ + this->dataPtr->visuals.clear(); + this->dataPtr->actors.clear(); + this->dataPtr->actorSkeletons.clear(); + this->dataPtr->actorTrajectories.clear(); + this->dataPtr->lights.clear(); + this->dataPtr->particleEmitters.clear(); + this->dataPtr->sensors.clear(); + this->dataPtr->scene.reset(); + this->dataPtr->originalTransparency.clear(); + this->dataPtr->originalDepthWrite.clear(); +} diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 0eff638de2..4790345300 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -258,6 +258,7 @@ void SensorsPrivate::WaitForInit() if (this->ambientLight) this->renderUtil.SetAmbientLight(*this->ambientLight); #ifndef __APPLE__ + this->renderUtil.Init(); #else // On macOS the render engine must be initialised on the main thread. @@ -409,6 +410,7 @@ void SensorsPrivate::RenderThread() for (const auto id : this->sensorIds) this->sensorManager.Remove(id); + this->renderUtil.Destroy(); gzdbg << "SensorsPrivate::RenderThread stopped" << std::endl; } diff --git a/test/integration/ModelPhotoShootTest.hh b/test/integration/ModelPhotoShootTest.hh index 2202ab9397..5636036267 100644 --- a/test/integration/ModelPhotoShootTest.hh +++ b/test/integration/ModelPhotoShootTest.hh @@ -301,6 +301,8 @@ class ModelPhotoShootTest : public InternalFixture<::testing::Test> testImages("3.png", "3_test.png"); testImages("4.png", "4_test.png"); testImages("5.png", "5_test.png"); + + postRenderConn.reset(); } private: bool takeTestPics{false}; diff --git a/test/performance/CMakeLists.txt b/test/performance/CMakeLists.txt index 770ba7b937..81cbd25f75 100644 --- a/test/performance/CMakeLists.txt +++ b/test/performance/CMakeLists.txt @@ -5,6 +5,21 @@ set(tests level_manager.cc ) +set(tests_needing_display + sensors_system.cc +) + +# Add systems that need a valid display here. +if(VALID_DISPLAY AND VALID_DRI_DISPLAY) + list(APPEND tests ${tests_needing_display}) +else() + message(STATUS + "Skipping these PERFORMANCE tests because a valid display was not found:") + foreach(test ${tests_needing_display}) + message(STATUS " ${test}") + endforeach(test) +endif() + set(exec sdf_runner ) @@ -39,3 +54,9 @@ target_link_libraries( gz-sim${PROJECT_VERSION_MAJOR} gz-sim${PROJECT_VERSION_MAJOR}-gui ) + +if(VALID_DISPLAY AND VALID_DRI_DISPLAY AND TARGET PERFORMANCE_sensors_system) + target_link_libraries(PERFORMANCE_sensors_system + gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER} + ) +endif() diff --git a/test/performance/sensors_system.cc b/test/performance/sensors_system.cc new file mode 100644 index 0000000000..86e8ee3356 --- /dev/null +++ b/test/performance/sensors_system.cc @@ -0,0 +1,136 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +// The following is needed to enable the GetMemInfo function for OSX +#ifdef __MACH__ +# include +#endif // __MACH__ + +#include + +#include +#include + +#include + +#include "gz/sim/Server.hh" +#include "test_config.hh" + +#include "../helpers/EnvTestFixture.hh" + +///////////////////////////////////////////////// +void getMemInfo(double &_resident, double &_share) +{ +#ifdef __linux__ + int totalSize, residentPages, sharePages; + totalSize = residentPages = sharePages = 0; + + std::ifstream buffer("/proc/self/statm"); + buffer >> totalSize >> residentPages >> sharePages; + buffer.close(); + + // in case x86-64 is configured to use 2MB pages + int64_t pageSizeKb = sysconf(_SC_PAGE_SIZE) / 1024; + + _resident = residentPages * pageSizeKb; + _share = sharePages * pageSizeKb; +#elif __MACH__ + // /proc is only available on Linux + // for OSX, use task_info to get resident and virtual memory + struct task_basic_info t_info; + mach_msg_type_number_t t_info_count = TASK_BASIC_INFO_COUNT; + if (KERN_SUCCESS != task_info(mach_task_self(), + TASK_BASIC_INFO, + (task_info_t)&t_info, + &t_info_count)) + { + gzerr << "failure calling task_info\n"; + return; + } + _resident = static_cast(t_info.resident_size/1024); + _share = static_cast(t_info.virtual_size/1024); +#else + gzerr << "Unsupported architecture\n"; + return; +#endif +} + +///////////////////////////////////////////////// +class SensorsSystemFixture : public InternalFixture<::testing::Test> +{ +}; + + +///////////////////////////////////////////////// +// Test repeatedly launching sim with sensors system and check for +// memory leaks +TEST_F(SensorsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(MemLeak)) +{ + gz::common::Console::SetVerbosity(4); + + // max memory change allowed + const double resMaxPercentChange = 1.0; + const double shareMaxPercentChange = 1.0; + + std::vector residentMemV; + std::vector shareMemV; + + for (unsigned int i = 0; i < 5; ++i) + { + // get resident and shared memory usage + double residentMem = 0; + double shareMem = 0; + getMemInfo(residentMem, shareMem); + + residentMemV.push_back(residentMem); + shareMemV.push_back(shareMem); + + gz::sim::ServerConfig serverConfig; + + const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/sensors_system_mem_leak.sdf"; + + serverConfig.SetSdfFile(sdfFile); + + gz::sim::Server server(serverConfig); + server.Run(true, 100, false); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + + // Calculate the percent change between runs + // Start from index 1 (instead of 0) since there is a big jump in memory usage + // when the system is first loaded. Make sure subsequent runs do not continue + // to increase memory usage + for (unsigned int i = 1; i < residentMemV.size() - 1; ++i) + { + double resPercentChange = + (residentMemV[i+1] - residentMemV[i]) / residentMemV[i]; + double sharePercentChange = (shareMemV[i+1] - shareMemV[i]) / shareMemV[i]; + + gzdbg << "ResPercentChange[" << resPercentChange << "] " + << "ResMaxPercentChange[" << resMaxPercentChange << "]" << std::endl; + gzdbg << "SharePercentChange[" << sharePercentChange << "] " + << "ShareMaxPercentChange[" << shareMaxPercentChange << "]" << std::endl; + + // check for memory leaks + // \todo(anyone) there is still gradual slow increase in memory usage + // between runs, likely from the sensors system / gz-rendering. + // Use tighter max percentage change once more mem leaks are fixed + EXPECT_LT(resPercentChange, resMaxPercentChange); + EXPECT_LT(sharePercentChange, shareMaxPercentChange); + } +} diff --git a/test/worlds/lights_render.sdf b/test/worlds/lights_render.sdf index c8ea520c75..bec495ceec 100644 --- a/test/worlds/lights_render.sdf +++ b/test/worlds/lights_render.sdf @@ -16,7 +16,7 @@ - ogre + ogre2 true diff --git a/test/worlds/sensors_system_mem_leak.sdf b/test/worlds/sensors_system_mem_leak.sdf new file mode 100644 index 0000000000..8e144eaa9b --- /dev/null +++ b/test/worlds/sensors_system_mem_leak.sdf @@ -0,0 +1,69 @@ + + + + + + ogre2 + + + + 0 0 0 0 0 0 + true + + + + 5 5 2.5 + + + + + 5 5 2.5 + + + 0 1 0 0.8 + 0 1 0 0.8 + 1 1 1 0.8 + + + + + + + true + -6 2 2 0 0.5 0 + + + + + 0.1 0.1 0.1 + + + + + 1 0 1.3 0 0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + gaussian_quantized + 0.01 + 0.0002 + + + 1 + 30 + camera + + + + + From 568410a5e7a085a9bd67c9a0c142a78864323926 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 1 May 2023 09:37:05 -0400 Subject: [PATCH 149/160] Update sdf plugins to use actuator_number. (#1976) Signed-off-by: Benjamin Perseghetti --- .../worlds/multicopter_velocity_control.sdf | 20 ++++++------ examples/worlds/quadcopter.sdf | 8 ++--- .../ackermann_steering/AckermannSteering.cc | 6 ++-- .../ackermann_steering/AckermannSteering.hh | 4 +-- .../MulticopterMotorModel.cc | 31 +++++++++++++------ .../ackermann_steering_only_actuators.sdf | 2 +- test/worlds/odometry_publisher_3d.sdf | 8 ++--- test/worlds/quadcopter.sdf | 8 ++--- test/worlds/quadcopter_velocity_control.sdf | 8 ++--- 9 files changed, 53 insertions(+), 42 deletions(-) diff --git a/examples/worlds/multicopter_velocity_control.sdf b/examples/worlds/multicopter_velocity_control.sdf index 8bbd2f8a5d..e34d2681ff 100644 --- a/examples/worlds/multicopter_velocity_control.sdf +++ b/examples/worlds/multicopter_velocity_control.sdf @@ -117,7 +117,7 @@ You can use the velocity controller and command linear velocity and yaw angular 8.54858e-06 0.016 gazebo/command/motor_speed - 0 + 0 8.06428e-05 1e-06 motor_speed/0 @@ -137,7 +137,7 @@ You can use the velocity controller and command linear velocity and yaw angular 8.54858e-06 0.016 gazebo/command/motor_speed - 1 + 1 8.06428e-05 1e-06 motor_speed/1 @@ -157,7 +157,7 @@ You can use the velocity controller and command linear velocity and yaw angular 8.54858e-06 0.016 gazebo/command/motor_speed - 2 + 2 8.06428e-05 1e-06 motor_speed/2 @@ -177,7 +177,7 @@ You can use the velocity controller and command linear velocity and yaw angular 8.54858e-06 0.016 gazebo/command/motor_speed - 3 + 3 8.06428e-05 1e-06 motor_speed/3 @@ -247,7 +247,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 0 + 0 2.0673e-04 0 0 @@ -267,7 +267,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 1 + 1 2.0673e-04 0 0 @@ -287,7 +287,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 2 + 2 2.0673e-04 0 0 @@ -307,7 +307,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 3 + 3 2.0673e-04 0 0 @@ -327,7 +327,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 4 + 4 2.0673e-04 0 0 @@ -347,7 +347,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 5 + 5 2.0673e-04 0 0 diff --git a/examples/worlds/quadcopter.sdf b/examples/worlds/quadcopter.sdf index 2cbda36f56..4b00c86c15 100644 --- a/examples/worlds/quadcopter.sdf +++ b/examples/worlds/quadcopter.sdf @@ -93,7 +93,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 0 + 0 8.06428e-05 1e-06 motor_speed/0 @@ -113,7 +113,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 1 + 1 8.06428e-05 1e-06 motor_speed/1 @@ -133,7 +133,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 2 + 2 8.06428e-05 1e-06 motor_speed/2 @@ -153,7 +153,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 3 + 3 8.06428e-05 1e-06 motor_speed/3 diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 920c8d2b5b..20b541dc41 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -259,10 +259,10 @@ void AckermannSteering::Configure(const Entity &_entity, if (_sdf->HasElement("use_actuator_msg") && _sdf->Get("use_actuator_msg")) { - if (_sdf->HasElement("actuatorNumber")) + if (_sdf->HasElement("actuator_number")) { this->dataPtr->actuatorNumber = - _sdf->Get("actuatorNumber"); + _sdf->Get("actuator_number"); this->dataPtr->useActuatorMsg = true; if (!this->dataPtr->steeringOnly) { @@ -271,7 +271,7 @@ void AckermannSteering::Configure(const Entity &_entity, } else { - gzerr << "Please specify an actuatorNumber" << + gzerr << "Please specify an actuator_number" << "to use Actuator position message control." << std::endl; } } diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index 031290b490..f82e0cec20 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -45,10 +45,10 @@ namespace systems /// `` is True, uses defaults topic name "/actuators". /// /// `` True to enable the use of actutor message - /// for steering angle command. Relies on `` for the + /// for steering angle command. Relies on `` for the /// index of the position actuator and defaults to topic "/actuators". /// - /// `` used with `` to set + /// `` used with `` to set /// the index of the steering angle position actuator. /// /// ``: Float used to control the steering angle P gain. diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index f581f308c8..1f4c27f723 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -157,8 +157,8 @@ class gz::sim::systems::MulticopterMotorModelPrivate /// \brief Sampling time (from motor_model.hpp). public: double samplingTime = 0.01; - /// \brief Index of motor on multirotor_base. - public: int motorNumber = 0; + /// \brief Index of motor in Actuators msg on multirotor_base. + public: int actuatorNumber = 0; /// \brief Turning direction of the motor. public: int turningDirection = turning_direction::kCw; @@ -283,11 +283,22 @@ void MulticopterMotorModel::Configure(const Entity &_entity, return; } - if (sdfClone->HasElement("motorNumber")) - this->dataPtr->motorNumber = + if (sdfClone->HasElement("actuator_number")) + { + this->dataPtr->actuatorNumber = + sdfClone->GetElement("actuator_number")->Get(); + } + else if (sdfClone->HasElement("motorNumber")) + { + this->dataPtr->actuatorNumber = sdfClone->GetElement("motorNumber")->Get(); + gzwarn << " is depricated, " + << "please use .\n"; + } else - gzerr << "Please specify a motorNumber.\n"; + { + gzerr << "Please specify a actuator_number.\n"; + } if (sdfClone->HasElement("turningDirection")) { @@ -512,9 +523,9 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( if (msg.has_value()) { - if (this->motorNumber > msg->velocity_size() - 1) + if (this->actuatorNumber > msg->velocity_size() - 1) { - gzerr << "You tried to access index " << this->motorNumber + gzerr << "You tried to access index " << this->actuatorNumber << " of the Actuator velocity array which is of size " << msg->velocity_size() << std::endl; return; @@ -523,13 +534,13 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( if (this->motorType == MotorType::kVelocity) { this->refMotorInput = std::min( - static_cast(msg->velocity(this->motorNumber)), + static_cast(msg->velocity(this->actuatorNumber)), static_cast(this->maxRotVelocity)); } // else if (this->motorType == MotorType::kPosition) else // if (this->motorType == MotorType::kForce) { { - this->refMotorInput = msg->velocity(this->motorNumber); + this->refMotorInput = msg->velocity(this->actuatorNumber); } } @@ -554,7 +565,7 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( double motorRotVel = jointVelocity->Data()[0]; if (motorRotVel / (2 * GZ_PI) > 1 / (2 * this->samplingTime)) { - gzerr << "Aliasing on motor [" << this->motorNumber + gzerr << "Aliasing on motor [" << this->actuatorNumber << "] might occur. Consider making smaller simulation time " "steps or raising the rotorVelocitySlowdownSim param.\n"; } diff --git a/test/worlds/ackermann_steering_only_actuators.sdf b/test/worlds/ackermann_steering_only_actuators.sdf index aa5d36424a..6b5f9232b5 100644 --- a/test/worlds/ackermann_steering_only_actuators.sdf +++ b/test/worlds/ackermann_steering_only_actuators.sdf @@ -365,7 +365,7 @@ front_right_wheel_steering_joint true true - 0 + 0 10.0 0.5 1.0 diff --git a/test/worlds/odometry_publisher_3d.sdf b/test/worlds/odometry_publisher_3d.sdf index 49df3a2f66..7b5695e6c0 100644 --- a/test/worlds/odometry_publisher_3d.sdf +++ b/test/worlds/odometry_publisher_3d.sdf @@ -273,7 +273,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 0 + 0 8.06428e-05 1e-06 motor_speed/0 @@ -293,7 +293,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 1 + 1 8.06428e-05 1e-06 motor_speed/1 @@ -313,7 +313,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 2 + 2 8.06428e-05 1e-06 motor_speed/2 @@ -333,7 +333,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 3 + 3 8.06428e-05 1e-06 motor_speed/3 diff --git a/test/worlds/quadcopter.sdf b/test/worlds/quadcopter.sdf index 1cd5b22364..bc93489ee5 100644 --- a/test/worlds/quadcopter.sdf +++ b/test/worlds/quadcopter.sdf @@ -262,7 +262,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 0 + 0 8.06428e-05 1e-06 motor_speed/0 @@ -282,7 +282,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 1 + 1 8.06428e-05 1e-06 motor_speed/1 @@ -302,7 +302,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 2 + 2 8.06428e-05 1e-06 motor_speed/2 @@ -322,7 +322,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 3 + 3 8.06428e-05 1e-06 motor_speed/3 diff --git a/test/worlds/quadcopter_velocity_control.sdf b/test/worlds/quadcopter_velocity_control.sdf index 697c5e9072..09390c0274 100644 --- a/test/worlds/quadcopter_velocity_control.sdf +++ b/test/worlds/quadcopter_velocity_control.sdf @@ -262,7 +262,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 0 + 0 8.06428e-05 1e-06 motor_speed/0 @@ -282,7 +282,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 1 + 1 8.06428e-05 1e-06 motor_speed/1 @@ -302,7 +302,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 2 + 2 8.06428e-05 1e-06 motor_speed/2 @@ -322,7 +322,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 3 + 3 8.06428e-05 1e-06 motor_speed/3 From 72211093a85ff91cddf9350628d870c3dbbdf635 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 1 May 2023 09:37:51 -0400 Subject: [PATCH 150/160] Actuators message for JointPositionController. (#1954) Signed-off-by: Benjamin Perseghetti Co-authored-by: Michael Carroll --- .../JointPositionController.cc | 77 ++++++++++- .../JointPositionController.hh | 7 + .../joint_position_controller_system.cc | 79 +++++++++++ .../joint_position_controller_actuators.sdf | 123 ++++++++++++++++++ 4 files changed, 283 insertions(+), 3 deletions(-) create mode 100644 test/worlds/joint_position_controller_actuators.sdf diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 270ab83027..0d0d42d161 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -18,6 +18,7 @@ #include "JointPositionController.hh" +#include #include #include @@ -29,6 +30,7 @@ #include #include +#include "gz/sim/components/Actuators.hh" #include "gz/sim/components/Joint.hh" #include "gz/sim/components/JointForceCmd.hh" #include "gz/sim/components/JointVelocityCmd.hh" @@ -46,6 +48,10 @@ class gz::sim::systems::JointPositionControllerPrivate /// \param[in] _msg Position message public: void OnCmdPos(const msgs::Double &_msg); + /// \brief Callback for actuator position subscription + /// \param[in] _msg Position message + public: void OnActuatorPos(const msgs::Actuators &_msg); + /// \brief Gazebo communication node. public: transport::Node node; @@ -58,12 +64,18 @@ class gz::sim::systems::JointPositionControllerPrivate /// \brief Commanded joint position public: double jointPosCmd{0.0}; + /// \brief Index of position actuator. + public: int actuatorNumber = 0; + /// \brief mutex to protect joint commands public: std::mutex jointCmdMutex; /// \brief Model interface public: Model model{kNullEntity}; + /// \brief True if using Actuator msg to control joint position. + public: bool useActuatorMsg{false}; + /// \brief Position PID controller. public: math::PID posPid; @@ -190,9 +202,26 @@ void JointPositionController::Configure(const Entity &_entity, this->dataPtr->jointPosCmd = _sdf->Get("initial_position"); } + if (_sdf->HasElement("use_actuator_msg") && + _sdf->Get("use_actuator_msg")) + { + if (_sdf->HasElement("actuator_number")) + { + this->dataPtr->actuatorNumber = + _sdf->Get("actuator_number"); + this->dataPtr->useActuatorMsg = true; + } + else + { + gzerr << "Please specify an actuator_number" << + "to use Actuator position message control." << std::endl; + } + } + // Subscribe to commands std::string topic; - if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic"))) + if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")) + && (!this->dataPtr->useActuatorMsg)) { topic = transport::TopicUtils::AsValidTopic("/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" + @@ -206,6 +235,18 @@ void JointPositionController::Configure(const Entity &_entity, return; } } + if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")) + && (this->dataPtr->useActuatorMsg)) + { + topic = transport::TopicUtils::AsValidTopic("/actuators"); + if (topic.empty()) + { + gzerr << "Failed to create Actuator topic for joint [" + << this->dataPtr->jointNames[0] + << "]" << std::endl; + return; + } + } if (_sdf->HasElement("sub_topic")) { topic = transport::TopicUtils::AsValidTopic("/model/" + @@ -235,8 +276,24 @@ void JointPositionController::Configure(const Entity &_entity, return; } } - this->dataPtr->node.Subscribe( - topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get()); + if (this->dataPtr->useActuatorMsg) + { + this->dataPtr->node.Subscribe(topic, + &JointPositionControllerPrivate::OnActuatorPos, + this->dataPtr.get()); + + gzmsg << "JointPositionController subscribing to Actuator messages on [" + << topic << "]" << std::endl; + } + else + { + this->dataPtr->node.Subscribe(topic, + &JointPositionControllerPrivate::OnCmdPos, + this->dataPtr.get()); + + gzmsg << "JointPositionController subscribing to Double messages on [" + << topic << "]" << std::endl; + } gzdbg << "[JointPositionController] system parameters:" << std::endl; gzdbg << "p_gain: [" << p << "]" << std::endl; @@ -426,6 +483,20 @@ void JointPositionControllerPrivate::OnCmdPos(const msgs::Double &_msg) this->jointPosCmd = _msg.data(); } +void JointPositionControllerPrivate::OnActuatorPos(const msgs::Actuators &_msg) +{ + std::lock_guard lock(this->jointCmdMutex); + if (this->actuatorNumber > _msg.position_size() - 1) + { + gzerr << "You tried to access index " << this->actuatorNumber + << " of the Actuator position array which is of size " + << _msg.position_size() << std::endl; + return; + } + + this->jointPosCmd = static_cast(_msg.position(this->actuatorNumber)); +} + GZ_ADD_PLUGIN(JointPositionController, System, JointPositionController::ISystemConfigure, diff --git a/src/systems/joint_position_controller/JointPositionController.hh b/src/systems/joint_position_controller/JointPositionController.hh index dc21602f6b..af139641d7 100644 --- a/src/systems/joint_position_controller/JointPositionController.hh +++ b/src/systems/joint_position_controller/JointPositionController.hh @@ -52,6 +52,13 @@ namespace systems /// `` Axis of the joint to control. Optional parameter. /// The default value is 0. /// + /// `` True to enable the use of actutor message + /// for position command. Relies on `` for the + /// index of the position actuator and defaults to topic "/actuators". + /// + /// `` used with `` to set + /// the index of the position actuator. + /// /// `` The proportional gain of the PID. Optional parameter. /// The default value is 1. /// diff --git a/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index 757e9588aa..0df56e165f 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/test/integration/joint_position_controller_system.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -128,6 +129,84 @@ TEST_F(JointPositionControllerTestFixture, EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); } +///////////////////////////////////////////////// +// Tests that the JointPositionController accepts joint position commands +// See https://github.com/gazebosim/gz-sim/issues/1175 +TEST_F(JointPositionControllerTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(JointPositionActuatorsCommand)) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_position_controller_actuators.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + const std::string jointName = "j1"; + + test::Relay testSystem; + std::vector currentPosition; + testSystem.OnPreUpdate( + [&](const UpdateInfo &, EntityComponentManager &_ecm) + { + auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name(jointName)); + // Create a JointPosition component if it doesn't exist. This signals + // physics system to populate the component + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + } + }); + + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &, + const components::Joint *, + const components::Name *_name, + const components::JointPosition *_position) -> bool + { + EXPECT_EQ(_name->Data(), jointName); + currentPosition = _position->Data(); + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + const std::size_t initIters = 10; + server.Run(true, initIters, false); + EXPECT_NEAR(0, currentPosition.at(0), TOL); + + // Publish command and check that the joint position is set + transport::Node node; + auto pub = node.Advertise( + "/actuators"); + + const double targetPosition{1.0}; + msgs::Actuators msg; + msg.add_position(targetPosition); + + pub.Publish(msg); + // Wait for the message to be published + std::this_thread::sleep_for(100ms); + + const std::size_t testIters = 3000; + server.Run(true, testIters , false); + + EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); +} + ///////////////////////////////////////////////// // Tests that the JointPositionController accepts joint position commands TEST_F(JointPositionControllerTestFixture, diff --git a/test/worlds/joint_position_controller_actuators.sdf b/test/worlds/joint_position_controller_actuators.sdf new file mode 100644 index 0000000000..5818a79ec7 --- /dev/null +++ b/test/worlds/joint_position_controller_actuators.sdf @@ -0,0 +1,123 @@ + + + + + 0 + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.005 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.0 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + + 0 0 -0.5 0 0 0 + base_link + rotor + + 0 0 1 + + + + j1 + true + 0 + + + + From 5d359c6383471d3d19a114d94a64633d9fdc67ff Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 2 May 2023 05:07:44 -0700 Subject: [PATCH 151/160] Add missing cmake exports from core library (#1978) Not all of the needed include paths are exported with the gz-sim target, so including the gz/sim.hh header doesn't work easily. This test fails to build and illustrates the problem. Signed-off-by: Steve Peters --- src/CMakeLists.txt | 2 ++ test/integration/CMakeLists.txt | 1 + test/integration/include_sim_hh.cc | 27 +++++++++++++++++++++++++++ 3 files changed, 30 insertions(+) create mode 100644 test/integration/include_sim_hh.cc diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 01f49907d2..7f35532ce1 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -176,6 +176,8 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} gz-common${GZ_COMMON_VER}::profiler gz-fuel_tools${GZ_FUEL_TOOLS_VER}::gz-fuel_tools${GZ_FUEL_TOOLS_VER} gz-gui${GZ_GUI_VER}::gz-gui${GZ_GUI_VER} + gz-physics${GZ_PHYSICS_VER}::core + gz-rendering${GZ_RENDERING_VER}::core gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER} gz-transport${GZ_TRANSPORT_VER}::parameters sdformat${SDF_VER}::sdformat${SDF_VER} diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 73295e7137..b45adfc311 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -33,6 +33,7 @@ set(tests hydrodynamics.cc hydrodynamics_flags.cc imu_system.cc + include_sim_hh.cc joint.cc joint_controller_system.cc joint_position_controller_system.cc diff --git a/test/integration/include_sim_hh.cc b/test/integration/include_sim_hh.cc new file mode 100644 index 0000000000..2c32b2be8c --- /dev/null +++ b/test/integration/include_sim_hh.cc @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +///////////////////////////////////////////////// +// Simple test to make sure it compiles +TEST(Compilation, sim_hh) +{ + gz::sim::System system; +} From e42d9132b76be4d5c7a89d972223c0aef15e6a26 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 2 May 2023 09:05:56 -0700 Subject: [PATCH 152/160] Addressed comments Signed-off-by: Nate Koenig --- test/worlds/actor_trajectory.sdf | 4 ++-- tutorials/migration_actor_api.md | 10 +++++----- tutorials/migration_joint_api.md | 10 +++++----- tutorials/migration_light_api.md | 10 +++++----- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/test/worlds/actor_trajectory.sdf b/test/worlds/actor_trajectory.sdf index ecb2e9f718..c5631c7f23 100644 --- a/test/worlds/actor_trajectory.sdf +++ b/test/worlds/actor_trajectory.sdf @@ -4,8 +4,8 @@ + filename="gz-sim-sensors-system" + name="gz::sim::systems::Sensors"> ogre2 diff --git a/tutorials/migration_actor_api.md b/tutorials/migration_actor_api.md index 25456e53ff..d1aacda04f 100644 --- a/tutorials/migration_actor_api.md +++ b/tutorials/migration_actor_api.md @@ -34,14 +34,14 @@ can be divided in these categories: You'll find the Gazebo APIs below on the following headers: -* [ignition/gazebo/Actor.hh](https://gazebosim.org/api/gazebo/6/Actor_8hh.html) -* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/6/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/6/SdfEntityCreator_8hh.html) -* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/6/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [ignition/gazebo/Actor.hh](https://gazebosim.org/api/gazebo/7/Actor_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) It's worth remembering that most of this functionality can be performed using the -[EntityComponentManager](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1EntityComponentManager.html) +[EntityComponentManager](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) directly. diff --git a/tutorials/migration_joint_api.md b/tutorials/migration_joint_api.md index 0442de59a8..54179c31c3 100644 --- a/tutorials/migration_joint_api.md +++ b/tutorials/migration_joint_api.md @@ -32,14 +32,14 @@ can be divided in these categories: You'll find the Gazebo APIs below on the following headers: -* [ignition/gazebo/Joint.hh](https://gazebosim.org/api/gazebo/6/Joint_8hh.html) -* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/6/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/6/SdfEntityCreator_8hh.html) -* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/6/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [ignition/gazebo/Joint.hh](https://gazebosim.org/api/gazebo/7/Joint_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) It's worth remembering that most of this functionality can be performed using the -[EntityComponentManager](https://gazebosim.org/api/gazebo/6/classignition_1_1gazebo_1_1EntityComponentManager.html) +[EntityComponentManager](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) directly. As an example the `Join::Pose()` is a convienient function for querying the `Pose` component from the `EntityComponentManager`, i.e. diff --git a/tutorials/migration_light_api.md b/tutorials/migration_light_api.md index c2924925f1..79ea920ddd 100644 --- a/tutorials/migration_light_api.md +++ b/tutorials/migration_light_api.md @@ -29,14 +29,14 @@ In Gazebo, the light APIs has been consolidated into a single Light class with some of generic functions available through other utility / core classes. You'll find the APIs below on the following headers: -* [ignition/gazebo/Light.hh](https://gazebosim.org/api/gazebo/6/Light_8hh.html) -* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/6/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/6/SdfEntityCreator_8hh.html) -* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/6/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [ignition/gazebo/Light.hh](https://gazebosim.org/api/gazebo/7/Light_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) It's worth remembering that most of this functionality can be performed using the -[EntityComponentManager](https://gazebosim.org/api/gazebo/6/classignition_1_1gazebo_1_1EntityComponentManager.html) +[EntityComponentManager](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) directly. As an example the `Light::Pose()` is a convienient function for querying the `Pose` component from the `EntityComponentManager`, i.e. From a18e217bd291abc63c0ae6b5fd8c5f779c741567 Mon Sep 17 00:00:00 2001 From: Matthew LeMay Date: Tue, 2 May 2023 17:32:57 -0400 Subject: [PATCH 153/160] fixed a code block in the python interfaces tutorial (#1982) Signed-off-by: Matthew LeMay --- tutorials/python_interfaces.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/python_interfaces.md b/tutorials/python_interfaces.md index 3406ff4948..297c07c0e5 100644 --- a/tutorials/python_interfaces.md +++ b/tutorials/python_interfaces.md @@ -9,7 +9,7 @@ step simulation and check entities and components. - **Step 1**: Load a world with a fixture -```{.python} +```python file_path = os.path.dirname(os.path.realpath(__file__)) fixture = TestFixture(os.path.join(file_path, 'gravity.sdf')) ``` From 8f06311db786fa1a4b539ead9484bf8b94fb47e9 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Fri, 12 May 2023 13:44:23 -0400 Subject: [PATCH 154/160] Actuators message input for JointController. (#1953) Signed-off-by: Benjamin Perseghetti Co-authored-by: Addisu Z. Taddese --- .../joint_controller/JointController.cc | 77 +++++++++++++++++- .../joint_controller/JointController.hh | 9 +++ test/integration/joint_controller_system.cc | 81 +++++++++++++++++++ test/worlds/joint_controller.sdf | 79 ++++++++++++++++++ 4 files changed, 242 insertions(+), 4 deletions(-) diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 19c9ce1f9f..6ef2322b31 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -18,6 +18,7 @@ #include "JointController.hh" +#include #include #include @@ -28,6 +29,7 @@ #include #include +#include "gz/sim/components/Actuators.hh" #include "gz/sim/components/JointForceCmd.hh" #include "gz/sim/components/JointVelocity.hh" #include "gz/sim/components/JointVelocityCmd.hh" @@ -43,6 +45,10 @@ class gz::sim::systems::JointControllerPrivate /// \param[in] _msg Velocity message public: void OnCmdVel(const msgs::Double &_msg); + /// \brief Callback for actuator velocity subscription + /// \param[in] _msg Velocity message + public: void OnActuatorVel(const msgs::Actuators &_msg); + /// \brief Gazebo communication node. public: transport::Node node; @@ -55,12 +61,18 @@ class gz::sim::systems::JointControllerPrivate /// \brief Commanded joint velocity public: double jointVelCmd{0.0}; + /// \brief Index of velocity actuator. + public: int actuatorNumber = 0; + /// \brief mutex to protect jointVelCmd public: std::mutex jointVelCmdMutex; /// \brief Model interface public: Model model{kNullEntity}; + /// \brief True if using Actuator msg to control joint velocity. + public: bool useActuatorMsg{false}; + /// \brief True if force commands are internally used to keep the target /// velocity. public: bool useForceCommands{false}; @@ -149,9 +161,26 @@ void JointController::Configure(const Entity &_entity, gzdbg << "[JointController] Velocity mode" << std::endl; } + if (_sdf->HasElement("use_actuator_msg") && + _sdf->Get("use_actuator_msg")) + { + if (_sdf->HasElement("actuator_number")) + { + this->dataPtr->actuatorNumber = + _sdf->Get("actuator_number"); + this->dataPtr->useActuatorMsg = true; + } + else + { + gzerr << "Please specify an actuator_number" << + "to use Actuator velocity message control." << std::endl; + } + } + // Subscribe to commands std::string topic; - if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic"))) + if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")) + && (!this->dataPtr->useActuatorMsg)) { topic = transport::TopicUtils::AsValidTopic("/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" + @@ -164,6 +193,18 @@ void JointController::Configure(const Entity &_entity, return; } } + if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")) + && (this->dataPtr->useActuatorMsg)) + { + topic = transport::TopicUtils::AsValidTopic("/actuators"); + if (topic.empty()) + { + gzerr << "Failed to create Actuator topic for joint [" + << this->dataPtr->jointNames[0] + << "]" << std::endl; + return; + } + } if (_sdf->HasElement("sub_topic")) { topic = transport::TopicUtils::AsValidTopic("/model/" + @@ -193,11 +234,25 @@ void JointController::Configure(const Entity &_entity, return; } } - this->dataPtr->node.Subscribe(topic, &JointControllerPrivate::OnCmdVel, - this->dataPtr.get()); - gzmsg << "JointController subscribing to Double messages on [" << topic + if (this->dataPtr->useActuatorMsg) + { + this->dataPtr->node.Subscribe(topic, + &JointControllerPrivate::OnActuatorVel, + this->dataPtr.get()); + + gzmsg << "JointController subscribing to Actuator messages on [" << topic + << "]" << std::endl; + } + else + { + this->dataPtr->node.Subscribe(topic, + &JointControllerPrivate::OnCmdVel, + this->dataPtr.get()); + + gzmsg << "JointController subscribing to Double messages on [" << topic << "]" << std::endl; + } } ////////////////////////////////////////////////// @@ -311,6 +366,20 @@ void JointControllerPrivate::OnCmdVel(const msgs::Double &_msg) this->jointVelCmd = _msg.data(); } +void JointControllerPrivate::OnActuatorVel(const msgs::Actuators &_msg) +{ + std::lock_guard lock(this->jointVelCmdMutex); + if (this->actuatorNumber > _msg.velocity_size() - 1) + { + gzerr << "You tried to access index " << this->actuatorNumber + << " of the Actuator velocity array which is of size " + << _msg.velocity_size() << std::endl; + return; + } + + this->jointVelCmd = static_cast(_msg.velocity(this->actuatorNumber)); +} + GZ_ADD_PLUGIN(JointController, System, JointController::ISystemConfigure, diff --git a/src/systems/joint_controller/JointController.hh b/src/systems/joint_controller/JointController.hh index 1e48484004..ae65473423 100644 --- a/src/systems/joint_controller/JointController.hh +++ b/src/systems/joint_controller/JointController.hh @@ -44,6 +44,15 @@ namespace systems /// using force commands. If this parameter is not set or is false, the /// controller will use velocity commands internally. /// + /// `` True to enable the use of actuator message + /// for velocity command. The actuator msg is an array of floats for + /// position, velocity and normalized commands. Relies on + /// `` for the index of the velocity actuator and + /// defaults to topic "/actuators" when `topic` or `subtopic not set. + /// + /// `` used with `` to set + /// the index of the velocity actuator. + /// /// `` Topic to receive commands in. Defaults to /// `/model//joint//cmd_vel`. /// diff --git a/test/integration/joint_controller_system.cc b/test/integration/joint_controller_system.cc index 0585bc0e24..894a9e5196 100644 --- a/test/integration/joint_controller_system.cc +++ b/test/integration/joint_controller_system.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -178,6 +179,86 @@ TEST_F(JointControllerTestFixture, pub.Publish(msg); } +///////////////////////////////////////////////// +// Tests that the JointController accepts actuator commands +TEST_F(JointControllerTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32( + JointControllerActuatorsCommand)) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_controller.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + const std::string linkName = "rotor4"; + + test::Relay testSystem; + math::Vector3d angularVelocity; + testSystem.OnPreUpdate( + [&](const UpdateInfo &, EntityComponentManager &_ecm) + { + auto link = _ecm.EntityByComponents(components::Link(), + components::Name(linkName)); + // Create an AngularVelocity component if it doesn't exist. This signals + // physics system to populate the component + if (nullptr == _ecm.Component(link)) + { + _ecm.CreateComponent(link, components::AngularVelocity()); + } + }); + + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &, + const components::Link *, + const components::Name *_name, + const components::AngularVelocity *_angularVel) -> bool + { + EXPECT_EQ(_name->Data(), linkName); + angularVelocity = _angularVel->Data(); + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + const std::size_t initIters = 10; + server.Run(true, initIters, false); + EXPECT_NEAR(0, angularVelocity.Length(), TOL); + + // Publish command and check that the joint velocity is set + transport::Node node; + auto pub = node.Advertise( + "/actuators"); + + const double testAngVel{10.0}; + msgs::Actuators msg; + msg.add_velocity(testAngVel); + + pub.Publish(msg); + // Wait for the message to be published + std::this_thread::sleep_for(100ms); + + const std::size_t testIters = 3000; + server.Run(true, testIters , false); + + EXPECT_NEAR(0, angularVelocity.X(), 1e-2); + EXPECT_NEAR(0, angularVelocity.Y(), 1e-2); + EXPECT_NEAR(testAngVel, angularVelocity.Z(), 1e-2); +} + ///////////////////////////////////////////////// // Tests the JointController using joint force commands TEST_F(JointControllerTestFixture, diff --git a/test/worlds/joint_controller.sdf b/test/worlds/joint_controller.sdf index 195d1198ca..21675dc327 100644 --- a/test/worlds/joint_controller.sdf +++ b/test/worlds/joint_controller.sdf @@ -313,5 +313,84 @@ 0.01 + + + -3 -3 0.005 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.5 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + + 0 0 -0.5 0 0 0 + base_link + rotor4 + + 0 0 1 + + + + j4 + true + 0 + + From bb85d05a373eb8cb21d0c8881918374234b2227f Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Sun, 14 May 2023 07:52:26 -0500 Subject: [PATCH 155/160] =?UTF-8?q?=F0=9F=8E=88=207.5.0=20(#1991)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michael Carroll --- CMakeLists.txt | 2 +- Changelog.md | 206 +++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 207 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 59332d3c45..77208a11eb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-sim7 VERSION 7.4.0) +project(gz-sim7 VERSION 7.5.0) set (GZ_DISTRIBUTION "Garden") #============================================================================ diff --git a/Changelog.md b/Changelog.md index 6722a168d8..3097aec077 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,211 @@ ## Gazebo Sim 7.x +### Gazebo Sim 7.5.0 (2023-05-14) + +1. Actuators message input for JointController. + * [Pull request #1953](https://github.com/gazebosim/gz-sim/pull/1953) + +1. fixed a code block in the python interfaces tutorial + * [Pull request #1982](https://github.com/gazebosim/gz-sim/pull/1982) + +1. Add missing cmake exports from core library + * [Pull request #1978](https://github.com/gazebosim/gz-sim/pull/1978) + +1. Actuators message for JointPositionController. + * [Pull request #1954](https://github.com/gazebosim/gz-sim/pull/1954) + +1. Update sdf plugins to use actuator_number. + * [Pull request #1976](https://github.com/gazebosim/gz-sim/pull/1976) + +1. Unload render engine when the sensors system exits + * [Pull request #1960](https://github.com/gazebosim/gz-sim/pull/1960) + +1. Use GzSpinBox. + * [Pull request #1969](https://github.com/gazebosim/gz-sim/pull/1969) + +1. Add tutorial on migrating the Actor class from gazebo classic + * [Pull request #1929](https://github.com/gazebosim/gz-sim/pull/1929) + +1. Add back in the marker example + * [Pull request #1972](https://github.com/gazebosim/gz-sim/pull/1972) + +1. Optimize render updates and use of thread mutexes in Sensors system + * [Pull request #1938](https://github.com/gazebosim/gz-sim/pull/1938) + +1. Fix use of actors that only has trajectory animation + * [Pull request #1947](https://github.com/gazebosim/gz-sim/pull/1947) + +1. Actuators message input for Ackermann Steering. + * [Pull request #1952](https://github.com/gazebosim/gz-sim/pull/1952) + +1. Add tutorial on migrating the Joint class from gazebo classic + * [Pull request #1925](https://github.com/gazebosim/gz-sim/pull/1925) + +1. Add tutorial on migrating the Light class from gazebo classic + * [Pull request #1931](https://github.com/gazebosim/gz-sim/pull/1931) + +1. Remove filtering from realtime factor (RTF) calculation + * [Pull request #1942](https://github.com/gazebosim/gz-sim/pull/1942) + +1. Fix docker/README.md + * [Pull request #1964](https://github.com/gazebosim/gz-sim/pull/1964) + +1. gz_TEST: improve initial sim time test reliability + * [Pull request #1916](https://github.com/gazebosim/gz-sim/pull/1916) + +1. Use a queue to track component registration from mulitiple sources + * [Pull request #1836](https://github.com/gazebosim/gz-sim/pull/1836) + +1. Initialize services in ViewAngle constructor + * [Pull request #1943](https://github.com/gazebosim/gz-sim/pull/1943) + +1. CI workflow: use checkout v3 + * [Pull request #1940](https://github.com/gazebosim/gz-sim/pull/1940) + +1. Rename COPYING to LICENSE + * [Pull request #1937](https://github.com/gazebosim/gz-sim/pull/1937) + +1. add comment on center of buoyancy force + * [Pull request #1935](https://github.com/gazebosim/gz-sim/pull/1935) + +1. Get Windows to green on gz-sim7 + * [Pull request #1917](https://github.com/gazebosim/gz-sim/pull/1917) + +1. Add Light class + * [Pull request #1918](https://github.com/gazebosim/gz-sim/pull/1918) + +1. Resolve inconsistent visibility on ign-gazebo6 + * [Pull request #1914](https://github.com/gazebosim/gz-sim/pull/1914) + +1. relax msg count check in RF comms integration test + * [Pull request #1920](https://github.com/gazebosim/gz-sim/pull/1920) + +1. Fix off-by-one error in physics test + * [Pull request #1921](https://github.com/gazebosim/gz-sim/pull/1921) + +1. Fix formatting of error messages with large mesh file names + * [Pull request #1654](https://github.com/gazebosim/gz-sim/pull/1654) + +1. Add Actor class + * [Pull request #1913](https://github.com/gazebosim/gz-sim/pull/1913) + +1. Update all velocity and acceleration components of non-link entities + * [Pull request #1868](https://github.com/gazebosim/gz-sim/pull/1868) + +1. Add Sensor class + * [Pull request #1912](https://github.com/gazebosim/gz-sim/pull/1912) + +1. Minor vocab fix + * [Pull request #1915](https://github.com/gazebosim/gz-sim/pull/1915) + +1. Allow to change camera user hfov in camera_view plugin + * [Pull request #1807](https://github.com/gazebosim/gz-sim/pull/1807) + +1. Address a few Windows CI Issues + * [Pull request #1911](https://github.com/gazebosim/gz-sim/pull/1911) + +1. Added magnetometer value based on location + * [Pull request #1907](https://github.com/gazebosim/gz-sim/pull/1907) + +1. Allow specifying initial simulation time with a CLI argument + * [Pull request #1801](https://github.com/gazebosim/gz-sim/pull/1801) + +1. Add Joint class + * [Pull request #1910](https://github.com/gazebosim/gz-sim/pull/1910) + +1. Added reset simulation tutorial + * [Pull request #1824](https://github.com/gazebosim/gz-sim/pull/1824) + +1. Add SensorTopic component to rendering sensors + * [Pull request #1908](https://github.com/gazebosim/gz-sim/pull/1908) + +1. Use a queue to track component registration from mulitiple sources + * [Pull request #1836](https://github.com/gazebosim/gz-sim/pull/1836) + +1. Document behaviour changes introduced #1784 + * [Pull request #1888](https://github.com/gazebosim/gz-sim/pull/1888) + +1. Fix GUI_clean_exit test by increasing thread delay + * [Pull request #1902](https://github.com/gazebosim/gz-sim/pull/1902) + +1. Partial backport of 1728 + * [Pull request #1901](https://github.com/gazebosim/gz-sim/pull/1901) + +1. Fix gz plugin paths in windows + * [Pull request #1899](https://github.com/gazebosim/gz-sim/pull/1899) + +1. Increase timeout for UNIT_Gui_clean_exit_TEST + * [Pull request #1897](https://github.com/gazebosim/gz-sim/pull/1897) + +1. fix triggered camera test by waiting for rendering / scene to be ready + * [Pull request #1895](https://github.com/gazebosim/gz-sim/pull/1895) + +1. cmdsim.rb: fix ruby syntax + * [Pull request #1884](https://github.com/gazebosim/gz-sim/pull/1884) + +1. Fix some windows warnings (C4244 and C4305) + * [Pull request #1874](https://github.com/gazebosim/gz-sim/pull/1874) + +1. Minor optimization to transform control tool + * [Pull request #1854](https://github.com/gazebosim/gz-sim/pull/1854) + +1. inherit material cast shadows property + * [Pull request #1856](https://github.com/gazebosim/gz-sim/pull/1856) + +1. fix record topic + * [Pull request #1855](https://github.com/gazebosim/gz-sim/pull/1855) + +1. Remove duplicate Fuel server used by ResourceSpawner + * [Pull request #1830](https://github.com/gazebosim/gz-sim/pull/1830) + +1. re-add namespace + * [Pull request #1826](https://github.com/gazebosim/gz-sim/pull/1826) + +1. Fix QML warnings regarding binding loops + * [Pull request #1829](https://github.com/gazebosim/gz-sim/pull/1829) + +1. Update documentation on `UpdateInfo::realTime` + * [Pull request #1817](https://github.com/gazebosim/gz-sim/pull/1817) + +1. Add jennuine as GUI codeowner + * [Pull request #1800](https://github.com/gazebosim/gz-sim/pull/1800) + +1. remove PlotIcon + * [Pull request #1658](https://github.com/gazebosim/gz-sim/pull/1658) + +1. Final update of ignitionrobotics to gazebosim for citadel + * [Pull request #1760](https://github.com/gazebosim/gz-sim/pull/1760) + +1. Convert ignitionrobotics to gazebosim in tutorials + * [Pull request #1759](https://github.com/gazebosim/gz-sim/pull/1759) + +1. Convert ignitionrobotics to gazebosim in sources and includes + * [Pull request #1758](https://github.com/gazebosim/gz-sim/pull/1758) + +1. Convert ignitionrobotics to gazebosim in tests directory + * [Pull request #1757](https://github.com/gazebosim/gz-sim/pull/1757) + +1. Added collection name to About Dialog + * [Pull request #1756](https://github.com/gazebosim/gz-sim/pull/1756) + +1. Citadel: Removed warnings + * [Pull request #1753](https://github.com/gazebosim/gz-sim/pull/1753) + +1. Remove actors from screen when they are supposed to + * [Pull request #1699](https://github.com/gazebosim/gz-sim/pull/1699) + +1. readd namespaces for Q_ARGS + * [Pull request #1670](https://github.com/gazebosim/gz-sim/pull/1670) + +1. 🎈 3.14.0 + * [Pull request #1657](https://github.com/gazebosim/gz-sim/pull/1657) + +1. Remove redundant namespace references + * [Pull request #1635](https://github.com/gazebosim/gz-sim/pull/1635) + +1. 🎈 3.14.0~pre1 + * [Pull request #1650](https://github.com/gazebosim/gz-sim/pull/1650) + ### Gazebo Sim 7.4.0 (2023-02-14) 1. Added airspeed sensor From 42c285ef1a7b35d7e64c8fcdc4f270fb2b42b9e3 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 22 May 2023 08:15:07 -0700 Subject: [PATCH 156/160] Support loading Projectors (#1979) Updated SdfEntityCreator to create projector entities Updated rendering component to load and create projector objects in the scene Updated scene broadcaster to publish scene info projectors Added conversion function for serializing / deserializing sdf::Projector to / from msgs::Projector Signed-off-by: Ian Chen --- examples/worlds/projector.sdf | 274 ++++++++++++++++++ include/gz/sim/Conversions.hh | 38 +++ include/gz/sim/SdfEntityCreator.hh | 7 + include/gz/sim/components/Projector.hh | 53 ++++ include/gz/sim/rendering/SceneManager.hh | 8 + src/Conversions.cc | 58 ++++ src/Conversions_TEST.cc | 35 +++ src/SdfEntityCreator.cc | 35 ++- src/Util.cc | 9 + src/Util_TEST.cc | 9 + src/rendering/RenderUtil.cc | 45 +++ src/rendering/SceneManager.cc | 84 +++++- .../scene_broadcaster/SceneBroadcaster.cc | 54 ++++ test/integration/CMakeLists.txt | 1 + test/integration/components.cc | 30 ++ test/integration/projector.cc | 105 +++++++ test/integration/scene_broadcaster_system.cc | 60 ++++ test/worlds/projector.sdf | 79 +++++ 18 files changed, 981 insertions(+), 3 deletions(-) create mode 100644 examples/worlds/projector.sdf create mode 100644 include/gz/sim/components/Projector.hh create mode 100644 test/integration/projector.cc create mode 100644 test/worlds/projector.sdf diff --git a/examples/worlds/projector.sdf b/examples/worlds/projector.sdf new file mode 100644 index 0000000000..d2a601c348 --- /dev/null +++ b/examples/worlds/projector.sdf @@ -0,0 +1,274 @@ + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + true + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + IR Camera Left + floating + 350 + 315 + + IR_camera_left + false + + + + IR Camera Right + floating + 350 + 315 + 500 + + IR_camera_right + false + + + + RGB Camera + floating + 350 + 315 + 320 + + RGB_camera + false + + + + + + docked + + + + + + + docked + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 5 0.0 1.57 0 + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Camera with Projector + + + + + 0 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + diff --git a/include/gz/sim/Conversions.hh b/include/gz/sim/Conversions.hh index 4979f5650b..527c383409 100644 --- a/include/gz/sim/Conversions.hh +++ b/include/gz/sim/Conversions.hh @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -714,6 +715,43 @@ namespace gz template<> sdf::ParticleEmitter convert(const msgs::ParticleEmitter &_in); + /// \brief Generic conversion from a projector SDF object to another + /// type. + /// \param[in] _in Projector SDF object. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::Projector &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a projector SDF object to + /// a projector message object. + /// \param[in] _in Projector SDF object. + /// \return Projector message. + template<> + msgs::Projector convert(const sdf::Projector &_in); + + /// \brief Generic conversion from a projector SDF object to another + /// type. + /// \param[in] _in Projector SDF object. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const msgs::Projector &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a projector SDF object to + /// a projector message object. + /// \param[in] _in Projecotr SDF object. + /// \return Projector message. + template<> + sdf::Projector convert(const msgs::Projector &_in); + + /// \brief Generic conversion from an SDF element to another type. /// \param[in] _in SDF element. /// \return Conversion result. diff --git a/include/gz/sim/SdfEntityCreator.hh b/include/gz/sim/SdfEntityCreator.hh index d65a20aae4..8e5292ef10 100644 --- a/include/gz/sim/SdfEntityCreator.hh +++ b/include/gz/sim/SdfEntityCreator.hh @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -155,6 +156,12 @@ namespace gz /// \sa CreateEntities(const sdf::Link *) public: Entity CreateEntities(const sdf::ParticleEmitter *_emitter); + /// \brief Create all entities that exist in the + /// sdf::Projector object. + /// \param[in] _projector SDF Projector object. + /// \return Projector entity. + public: Entity CreateEntities(const sdf::Projector *_projector); + /// \brief Request an entity deletion. This will insert the request /// into a queue. The queue is processed toward the end of a simulation /// update step. diff --git a/include/gz/sim/components/Projector.hh b/include/gz/sim/components/Projector.hh new file mode 100644 index 0000000000..9824e88594 --- /dev/null +++ b/include/gz/sim/components/Projector.hh @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_SIM_COMPONENTS_PROJECTOR_HH_ +#define GZ_SIM_COMPONENTS_PROJECTOR_HH_ + +#include + +#include + +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace serializers +{ + using ProjectorSerializer = + serializers::ComponentToMsgSerializer; +} + +namespace components +{ + /// \brief A component type that contains a projector, + /// sdf::Projector, information. + using Projector = Component; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.Projector", Projector) +} +} +} +} + +#endif diff --git a/include/gz/sim/rendering/SceneManager.hh b/include/gz/sim/rendering/SceneManager.hh index a4bcdd297f..2e7287075c 100644 --- a/include/gz/sim/rendering/SceneManager.hh +++ b/include/gz/sim/rendering/SceneManager.hh @@ -269,6 +269,14 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { public: rendering::ParticleEmitterPtr UpdateParticleEmitter(Entity _id, const msgs::ParticleEmitter &_emitter); + /// \brief Create a projector + /// \param[in] _id Unique projector id + /// \param[in] _projector Projector sdf dom + /// \param[in] _parentId Parent id + /// \return Projector object created from the sdf dom + public: rendering::ProjectorPtr CreateProjector( + Entity _id, const sdf::Projector &_projector, Entity _parentId); + /// \brief Gazebo Sensors is the one responsible for adding sensors /// to the scene. Here we just keep track of it and make sure it has /// the correct parent. diff --git a/src/Conversions.cc b/src/Conversions.cc index 94c1de01c5..d7d67f3b34 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -1752,6 +1753,63 @@ sdf::ParticleEmitter gz::sim::convert(const msgs::ParticleEmitter &_in) return out; } +////////////////////////////////////////////////// +template<> +GZ_SIM_VISIBLE +msgs::Projector gz::sim::convert(const sdf::Projector &_in) +{ + msgs::Projector out; + out.set_name(_in.Name()); + msgs::Set(out.mutable_pose(), _in.RawPose()); + out.set_near_clip(_in.NearClip()); + out.set_far_clip(_in.FarClip()); + 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())); + + return out; +} + +////////////////////////////////////////////////// +template<> +GZ_SIM_VISIBLE +sdf::Projector gz::sim::convert(const msgs::Projector &_in) +{ + sdf::Projector out; + out.SetName(_in.name()); + out.SetNearClip(_in.near_clip()); + out.SetFarClip(_in.far_clip()); + 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); + } + } + } + + return out; +} + ////////////////////////////////////////////////// template<> GZ_SIM_VISIBLE diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 93c045e1a9..3d6e3e5861 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -1077,6 +1078,40 @@ TEST(Conversions, ParticleEmitter) EXPECT_FLOAT_EQ(emitter2.ScatterRatio(), emitter.ScatterRatio()); } +///////////////////////////////////////////////// +TEST(Conversions, Projector) +{ + sdf::Projector projector; + projector.SetName("my_projector"); + projector.SetNearClip(0.03); + projector.SetFarClip(30); + projector.SetHorizontalFov(math::Angle(0.4)); + projector.SetTexture("projector.png"); + projector.SetVisibilityFlags(0xFF); + + // Convert SDF to a message. + msgs::Projector projectorMsg = convert(projector); + + EXPECT_EQ("my_projector", projectorMsg.name()); + EXPECT_NEAR(0.03, projectorMsg.near_clip(), 1e-3); + 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))); + + // Convert the message back to SDF. + sdf::Projector projector2 = convert(projectorMsg); + EXPECT_EQ(projector2.Name(), projector.Name()); + EXPECT_NEAR(projector2.NearClip(), projector.NearClip(), 1e-3); + EXPECT_NEAR(projector2.FarClip(), projector.FarClip(), 1e-3); + EXPECT_EQ(projector2.HorizontalFov(), projector.HorizontalFov()); + EXPECT_EQ(projector2.Texture(), projector.Texture()); + EXPECT_EQ(projector2.VisibilityFlags(), projector.VisibilityFlags()); +} + ///////////////////////////////////////////////// TEST(Conversions, PluginElement) { diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index f000f45cf0..fba9663f3e 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -70,11 +70,12 @@ #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/components/NavSat.hh" -#include "gz/sim/components/ParentLinkName.hh" #include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/ParentLinkName.hh" #include #include "gz/sim/components/Physics.hh" #include "gz/sim/components/Pose.hh" +#include #include "gz/sim/components/RgbdCamera.hh" #include "gz/sim/components/Scene.hh" #include "gz/sim/components/SegmentationCamera.hh" @@ -652,7 +653,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) } // Particle emitters - for (uint64_t emitterIndex = 0; emitterIndex < _link->ParticleEmitterCount(); + for (uint64_t emitterIndex = 0; emitterIndex < _link->ParticleEmitterCount(); ++emitterIndex) { auto emitter = _link->ParticleEmitterByIndex(emitterIndex); @@ -661,6 +662,17 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) this->SetParent(emitterEntity, linkEntity); } + // Projectors + for (uint64_t projectorIndex = 0; projectorIndex < _link->ProjectorCount(); + ++projectorIndex) + { + auto projector = _link->ProjectorByIndex(projectorIndex); + auto projectorEntity = this->CreateEntities(projector); + + this->SetParent(projectorEntity, linkEntity); + } + + return linkEntity; } @@ -871,6 +883,25 @@ Entity SdfEntityCreator::CreateEntities(const sdf::ParticleEmitter *_emitter) return emitterEntity; } +////////////////////////////////////////////////// +Entity SdfEntityCreator::CreateEntities(const sdf::Projector *_projector) +{ + GZ_PROFILE("SdfEntityCreator::CreateEntities(sdf::Projector)"); + + // Entity + Entity projectorEntity = this->dataPtr->ecm->CreateEntity(); + + // Components + this->dataPtr->ecm->CreateComponent(projectorEntity, + components::Projector(*_projector)); + this->dataPtr->ecm->CreateComponent(projectorEntity, + components::Pose(ResolveSdfPose(_projector->SemanticPose()))); + this->dataPtr->ecm->CreateComponent(projectorEntity, + components::Name(_projector->Name())); + + return projectorEntity; +} + ////////////////////////////////////////////////// Entity SdfEntityCreator::CreateEntities(const sdf::Collision *_collision) { diff --git a/src/Util.cc b/src/Util.cc index bd1087a858..552c68b2e1 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -41,6 +41,7 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/ParticleEmitter.hh" +#include "gz/sim/components/Projector.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/SphericalCoordinates.hh" @@ -277,6 +278,10 @@ ComponentTypeId entityTypeId(const Entity &_entity, { type = components::ParticleEmitter::typeId; } + else if (_ecm.Component(_entity)) + { + type = components::Projector::typeId; + } return type; } @@ -327,6 +332,10 @@ std::string entityTypeStr(const Entity &_entity, { type = "particle_emitter"; } + else if (_ecm.Component(_entity)) + { + type = "projector"; + } return type; } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index c29e0281df..d4bd50edf8 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -32,6 +32,7 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/ParticleEmitter.hh" +#include "gz/sim/components/Projector.hh" #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/Visual.hh" #include "gz/sim/components/World.hh" @@ -360,6 +361,10 @@ TEST_F(UtilTest, EntityTypeId) entity = ecm.CreateEntity(); ecm.CreateComponent(entity, components::ParticleEmitter()); EXPECT_EQ(components::ParticleEmitter::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Projector()); + EXPECT_EQ(components::Projector::typeId, entityTypeId(entity, ecm)); } ///////////////////////////////////////////////// @@ -409,6 +414,10 @@ TEST_F(UtilTest, EntityTypeStr) entity = ecm.CreateEntity(); ecm.CreateComponent(entity, components::ParticleEmitter()); EXPECT_EQ("particle_emitter", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Projector()); + EXPECT_EQ("projector", entityTypeStr(entity, ecm)); } ///////////////////////////////////////////////// diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 7920c65c3f..3c5e3da2d3 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -74,6 +74,7 @@ #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/ParentLinkName.hh" #include "gz/sim/components/ParticleEmitter.hh" +#include "gz/sim/components/Projector.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/components/RgbdCamera.hh" #include "gz/sim/components/Scene.hh" @@ -291,6 +292,11 @@ class gz::sim::RenderUtilPrivate public: std::unordered_map newParticleEmittersCmds; + /// \brief New projector to be created. The elements in the tuple are: + /// [0] entity id, [1] projector, [2] parent entity id + public: std::vector> + newProjectors; + /// \brief New sensor topics that should be added to ECM as new components public: std::unordered_map newSensorTopics; @@ -1095,6 +1101,7 @@ void RenderUtil::Update() auto newParticleEmitters = std::move(this->dataPtr->newParticleEmitters); auto newParticleEmittersCmds = std::move(this->dataPtr->newParticleEmittersCmds); + auto newProjectors = std::move(this->dataPtr->newProjectors); auto removeEntities = std::move(this->dataPtr->removeEntities); auto entityPoses = std::move(this->dataPtr->entityPoses); auto entityLights = std::move(this->dataPtr->entityLights); @@ -1124,6 +1131,7 @@ void RenderUtil::Update() this->dataPtr->newLights.clear(); this->dataPtr->newParticleEmitters.clear(); this->dataPtr->newParticleEmittersCmds.clear(); + this->dataPtr->newProjectors.clear(); this->dataPtr->removeEntities.clear(); this->dataPtr->entityPoses.clear(); this->dataPtr->entityLights.clear(); @@ -1274,6 +1282,13 @@ void RenderUtil::Update() emitterCmd.first, emitterCmd.second); } + for (const auto &projector : newProjectors) + { + this->dataPtr->sceneManager.CreateProjector( + std::get<0>(projector), std::get<1>(projector), + std::get<2>(projector)); + } + if (this->dataPtr->enableSensors && this->dataPtr->createSensorCb) { for (const auto &sensor : newSensors) @@ -1879,6 +1894,17 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( return true; }); + // projectors + _ecm.Each( + [&](const Entity &_entity, + const components::Projector *_projector, + const components::ParentEntity *_parent) -> bool + { + this->newProjectors.push_back( + std::make_tuple(_entity, _projector->Data(), _parent->Data())); + return true; + }); + if (this->enableSensors) { // Create cameras @@ -2157,6 +2183,17 @@ void RenderUtilPrivate::CreateEntitiesRuntime( return true; }); + // projectors + _ecm.EachNew( + [&](const Entity &_entity, + const components::Projector *_projector, + const components::ParentEntity *_parent) -> bool + { + this->newProjectors.push_back( + std::make_tuple(_entity, _projector->Data(), _parent->Data())); + return true; + }); + if (this->enableSensors) { // Create cameras @@ -2520,6 +2557,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( return true; }); + // projectors + _ecm.EachRemoved( + [&](const Entity &_entity, const components::Projector *)->bool + { + this->removeEntities[_entity] = _info.iterations; + return true; + }); + // cameras _ecm.EachRemoved( [&](const Entity &_entity, const components::Camera *)->bool diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 7629a1ad06..55d10e9534 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -110,7 +111,12 @@ class gz::sim::SceneManagerPrivate /// \brief Map of particle emitter entity in Gazebo to particle emitter /// rendering pointers. - public: std::map particleEmitters; + public: std::unordered_map + particleEmitters; + + /// \brief Map of projector entity in Gazebo to projector + /// rendering pointers. + public: std::unordered_map projectors; /// \brief Map of sensor entity in Gazebo to sensor pointers. public: std::unordered_map sensors; @@ -1666,6 +1672,65 @@ rendering::ParticleEmitterPtr SceneManager::UpdateParticleEmitter(Entity _id, return emitter; } +///////////////////////////////////////////////// +rendering::ProjectorPtr SceneManager::CreateProjector( + Entity _id, const sdf::Projector &_projector, Entity _parentId) +{ + if (!this->dataPtr->scene) + return rendering::ProjectorPtr(); + + if (this->dataPtr->projectors.find(_id) != + this->dataPtr->projectors.end()) + { + gzerr << "Projector with Id: [" << _id << "] already exists in the " + << "scene" << std::endl; + return rendering::ProjectorPtr(); + } + + rendering::VisualPtr parent; + if (_parentId != this->dataPtr->worldId) + { + auto it = this->dataPtr->visuals.find(_parentId); + if (it == this->dataPtr->visuals.end()) + { + // It is possible to get here if the model entity is created then + // removed in between render updates. + return rendering::ProjectorPtr(); + } + parent = it->second; + } + + // Name. + std::string name = _projector.Name().empty() ? std::to_string(_id) : + _projector.Name(); + if (parent) + name = parent->Name() + "::" + name; + + rendering::ProjectorPtr projector; + projector = std::dynamic_pointer_cast( + this->dataPtr->scene->Extension()->CreateExt("projector", name)); + // \todo(iche033) replace above call with CreateProjector in gz-rendering8 + // projector = this->dataPtr->scene->CreateProjector(name); + + this->dataPtr->projectors[_id] = projector; + + if (parent) + parent->AddChild(projector); + + projector->SetLocalPose(_projector.RawPose()); + projector->SetNearClipPlane(_projector.NearClip()); + projector->SetFarClipPlane(_projector.FarClip()); + std::string texture = _projector.Texture(); + std::string fullPath = common::findFile( + asFullPath(texture, _projector.FilePath())); + + projector->SetTexture(fullPath); + projector->SetHFOV(_projector.HorizontalFov()); + projector->SetVisibilityFlags(_projector.VisibilityFlags()); + + return projector; +} + ///////////////////////////////////////////////// bool SceneManager::AddSensor(Entity _gazeboId, const std::string &_sensorName, Entity _parentGazeboId) @@ -1720,6 +1785,8 @@ bool SceneManager::HasEntity(Entity _id) const this->dataPtr->lights.find(_id) != this->dataPtr->lights.end() || this->dataPtr->particleEmitters.find(_id) != this->dataPtr->particleEmitters.end() || + this->dataPtr->projectors.find(_id) != + this->dataPtr->projectors.end() || this->dataPtr->sensors.find(_id) != this->dataPtr->sensors.end(); } @@ -1746,6 +1813,11 @@ rendering::NodePtr SceneManager::NodeById(Entity _id) const { return pIt->second; } + auto prIt = this->dataPtr->projectors.find(_id); + if (prIt != this->dataPtr->projectors.end()) + { + return prIt->second; + } return rendering::NodePtr(); } @@ -2006,6 +2078,16 @@ void SceneManager::RemoveEntity(Entity _id) } } + { + auto it = this->dataPtr->projectors.find(_id); + if (it != this->dataPtr->projectors.end()) + { + this->dataPtr->scene->DestroyVisual(it->second); + this->dataPtr->projectors.erase(it); + return; + } + } + { auto it = this->dataPtr->sensors.find(_id); if (it != this->dataPtr->sensors.end()) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index b2f5562bee..a2f5fd5ead 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -53,6 +53,7 @@ #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/ParticleEmitter.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Projector.hh" #include "gz/sim/components/RgbdCamera.hh" #include "gz/sim/components/Scene.hh" #include "gz/sim/components/Sensor.hh" @@ -168,6 +169,15 @@ class gz::sim::systems::SceneBroadcasterPrivate public: static void AddParticleEmitters(msgs::Link *_msg, const Entity _entity, const SceneGraphType &_graph); + /// \brief Adds projectors to a msgs::Link object based on the + /// contents of the scene graph + /// \param[inout] _msg Pointer to msg object to which the projectors + /// will be added. + /// \param[in] _entity Parent entity in the graph + /// \param[in] _graph Scene graph + public: static void AddProjectors(msgs::Link *_msg, + const Entity _entity, const SceneGraphType &_graph); + /// \brief Recursively remove entities from the graph /// \param[in] _entity Entity /// \param[in/out] _graph Scene graph @@ -1079,6 +1089,29 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( return true; }); + // Projectors + _manager.EachNew( + [&](const Entity &_entity, + const components::Projector *_projectorComp, + const components::ParentEntity *_parentComp, + const components::Pose *_poseComp) -> bool + { + auto projectorMsg = std::make_shared(); + projectorMsg->CopyFrom( + convert(_projectorComp->Data())); + // \todo(anyone) add id field to projector msg + // projectorMsg->set_id(_entity); + projectorMsg->mutable_pose()->CopyFrom( + msgs::Convert(_poseComp->Data())); + + // Add to graph + newGraph.AddVertex(projectorMsg->name(), projectorMsg, _entity); + newGraph.AddEdge({_parentComp->Data(), _entity}, true); + newEntity = true; + return true; + }); + // Update the whole scene graph from the new graph { std::lock_guard lock(this->graphMutex); @@ -1260,6 +1293,24 @@ void SceneBroadcasterPrivate::AddParticleEmitters(msgs::Link *_msg, } } +////////////////////////////////////////////////// +void SceneBroadcasterPrivate::AddProjectors(msgs::Link *_msg, + const Entity _entity, const SceneGraphType &_graph) +{ + if (!_msg) + return; + + for (const auto &vertex : _graph.AdjacentsFrom(_entity)) + { + auto projectorMsg = std::dynamic_pointer_cast( + vertex.second.get().Data()); + if (!projectorMsg) + continue; + + _msg->add_projector()->CopyFrom(*projectorMsg); + } +} + ////////////////////////////////////////////////// void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity, const SceneGraphType &_graph) @@ -1288,6 +1339,9 @@ void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity, // Particle emitters AddParticleEmitters(msgOut, vertex.second.get().Id(), _graph); + + // Projectors + AddProjectors(msgOut, vertex.second.get().Id(), _graph); } } diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index a59910e399..df21fb46ec 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -63,6 +63,7 @@ set(tests physics_system.cc play_pause.cc pose_publisher_system.cc + projector.cc rf_comms.cc recreate_entities.cc reset.cc diff --git a/test/integration/components.cc b/test/integration/components.cc index 475993b092..37e75f4bbf 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -77,6 +77,7 @@ #include "gz/sim/components/PerformerLevels.hh" #include "gz/sim/components/PhysicsEnginePlugin.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Projector.hh" #include "gz/sim/components/Scene.hh" #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/SourceFilePath.hh" @@ -1783,6 +1784,35 @@ TEST_F(ComponentsTest, ParticleEmitterCmd) EXPECT_EQ(comp1.Data().name(), comp3.Data().name()); } +////////////////////////////////////////////////// +TEST_F(ComponentsTest, Projector) +{ + // Create components + sdf::Projector projector1; + projector1.SetName("projector1"); + projector1.SetRawPose(math::Pose3d(0, 3, 4, GZ_PI, 0, 0)); + projector1.SetNearClip(1.5); + projector1.SetFarClip(10.3); + projector1.SetHorizontalFov(math::Angle(3.0)); + projector1.SetVisibilityFlags(0xFE); + projector1.SetTexture("path_to_texture"); + auto comp1 = components::Projector(projector1); + + // stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + std::istringstream istr(ostr.str()); + components::Projector comp3; + comp3.Deserialize(istr); + EXPECT_EQ("projector1", comp3.Data().Name()); + EXPECT_EQ(math::Pose3d(0, 3, 4, GZ_PI, 0, 0), comp3.Data().RawPose()); + EXPECT_DOUBLE_EQ(1.5, comp3.Data().NearClip()); + EXPECT_DOUBLE_EQ(10.3, comp3.Data().FarClip()); + EXPECT_EQ(math::Angle(3.0), comp3.Data().HorizontalFov()); + EXPECT_EQ(0xFE, comp3.Data().VisibilityFlags()); + EXPECT_EQ("path_to_texture", comp3.Data().Texture()); +} + ////////////////////////////////////////////////// TEST_F(ComponentsTest, JointTransmittedWrench) { diff --git a/test/integration/projector.cc b/test/integration/projector.cc new file mode 100644 index 0000000000..55fdf1c1b7 --- /dev/null +++ b/test/integration/projector.cc @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include +#include +#include +#include + +#include "gz/sim/Entity.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Projector.hh" +#include "test_config.hh" + +#include "helpers/EnvTestFixture.hh" +#include "helpers/Relay.hh" + +using namespace gz; +using namespace sim; + +class ProjectorTest : public InternalFixture<::testing::Test> +{ + public: void LoadWorld(const std::string &_path, bool _useLevels = false) + { + this->serverConfig.SetSdfFile( + common::joinPaths(PROJECT_SOURCE_PATH, _path)); + this->serverConfig.SetUseLevels(_useLevels); + + this->server = std::make_unique(this->serverConfig); + EXPECT_FALSE(this->server->Running()); + EXPECT_FALSE(*this->server->Running(0)); + using namespace std::chrono_literals; + this->server->SetUpdatePeriod(1ns); + } + + public: ServerConfig serverConfig; + public: std::unique_ptr server; +}; + +///////////////////////////////////////////////// +// Load an SDF with a projector and verify its properties. +// See https://github.com/gazebosim/gz-sim/issues/1175 +TEST_F(ProjectorTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDFLoad)) +{ + bool projectorChecked{false}; + + this->LoadWorld(common::joinPaths("test", "worlds", "projector.sdf")); + + // Create a system that checks a projector. + test::Relay testSystem; + testSystem.OnPostUpdate([&](const sim::UpdateInfo &, + const sim::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const gz::sim::Entity &, + const components::Projector *_projector, + const components::Name *_name, + const components::Pose *_pose) -> bool + { + if (_name->Data() == "projector") + { + projectorChecked = true; + + EXPECT_EQ("projector", _name->Data()); + EXPECT_EQ(_name->Data(), _projector->Data().Name()); + EXPECT_EQ(math::Pose3d(0, 1, 0, 0, 0, 0), _pose->Data()); + EXPECT_EQ(_pose->Data(), _projector->Data().RawPose()); + EXPECT_DOUBLE_EQ(2.0, _projector->Data().NearClip()); + EXPECT_DOUBLE_EQ(7.0, _projector->Data().FarClip()); + EXPECT_EQ(math::Angle(0.5), _projector->Data().HorizontalFov()); + EXPECT_EQ(0x01, _projector->Data().VisibilityFlags()); + EXPECT_EQ(common::joinPaths("path", "to", "dummy_image.png"), + _projector->Data().Texture()); + } + return true; + }); + }); + + this->server->AddSystem(testSystem.systemPtr); + this->server->Run(true, 1, false); + + EXPECT_TRUE(projectorChecked); +} diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 278967d0d3..0ea532d4e1 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -1030,6 +1030,66 @@ TEST_P(SceneBroadcasterTest, EXPECT_EQ(1, count); } +TEST_P(SceneBroadcasterTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneInfoHasProjector)) +{ + // Start server + gz::sim::ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "projector.sdf")); + + sim::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + server.Run(true, 1, false); + + // Create requester + transport::Node node; + + bool result{false}; + unsigned int timeout{5000}; + gz::msgs::Scene res; + + EXPECT_TRUE(node.Request("/world/projectors/scene/info", + timeout, res, result)); + ASSERT_TRUE(result); + + ASSERT_EQ(2, res.model_size()); + int count = 0; + for (int i = 0; i < res.model_size(); ++i) + { + if (res.model(i).name() == "projector_model") + { + count++; + // There should be one link + ASSERT_EQ(1, res.model(i).link_size()); + // The link should have one projector + ASSERT_EQ(1, res.model(i).link(0).projector_size()); + + // Check a few parameter values to make sure we have the correct + // projector + const msgs::Projector &projector = + res.model(i).link(0).projector(0); + EXPECT_EQ("projector", projector.name()); + EXPECT_EQ(math::Pose3d(0, 1, 0, 0, 0, 0), + msgs::Convert(projector.pose())); + EXPECT_DOUBLE_EQ(2.0, projector.near_clip()); + EXPECT_DOUBLE_EQ(7.0, projector.far_clip()); + 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))); + } + } + + // Should have found 1 projector + EXPECT_EQ(1, count); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, SceneBroadcasterTest, ::testing::Range(1, 2)); diff --git a/test/worlds/projector.sdf b/test/worlds/projector.sdf new file mode 100644 index 0000000000..bf5facfd83 --- /dev/null +++ b/test/worlds/projector.sdf @@ -0,0 +1,79 @@ + + + + + + 0.001 + 0 + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + 0 0 0 0 0 0 + true + + + + + 1 1 1 + + + + + 0 1 0 0 0 0 + 2 + 7 + 0.5 + path/to/dummy_image.png + 0x01 + + + + + + From 3bde4b5318f9bed37e2fc8dbe5877bbdc9b73ae1 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Wed, 24 May 2023 08:08:38 -0400 Subject: [PATCH 157/160] Fix image links in Blender procedural generation tutorial (#1996) Signed-off-by: Mabel Zhang --- tutorials/blender_procedural_datasets.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tutorials/blender_procedural_datasets.md b/tutorials/blender_procedural_datasets.md index 4f8104c186..102af8e690 100644 --- a/tutorials/blender_procedural_datasets.md +++ b/tutorials/blender_procedural_datasets.md @@ -93,7 +93,7 @@ blender [blender options] file.blend 4. Run the script using the *Run script* button in the panel of the *Text Editor* tab at the top of the screen. -@image html https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/blender_instructions.png "Instructions in Blender" width=100% +![Instructions in Blender](https://raw.githubusercontent.com/gazebosim/gz-sim/gz-sim7/tutorials/files/blender_procedural_datasets/blender_instructions.png) Once you follow these steps and configure the script for your `.blend` project, you can save it and use Option B in the future. @@ -155,9 +155,9 @@ blender rock.blend --python-text procedural_dataset_generator.py -- -o sdf_model blender woodland.blend --python-text procedural_dataset_generator.py -- -o sdf_models/woodland ``` -@image html https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/demo_blender_rock.gif "Example of generating a dataset of rock SDF models" width=100% +![Example of generating a dataset of rock SDF models](https://raw.githubusercontent.com/gazebosim/gz-sim/gz-sim7/tutorials/files/blender_procedural_datasets/demo_blender_rock.gif) -@image html https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/demo_blender_woodland.gif "Example of generating a dataset of woodland SDF models" width=100% +![Example of generating a dataset of woodland SDF models](https://raw.githubusercontent.com/gazebosim/gz-sim/gz-sim7/tutorials/files/blender_procedural_datasets/demo_blender_woodland.gif) You can configure the script in several ways (see `blender rock.blend --python-text procedural_dataset_generator.py -- -h`). For @@ -220,9 +220,9 @@ Hereafter, you can spawn the generated models inside Gazebo with your preferred approach, e.g. via the Resource Spawner GUI plugin. Below are some examples of Gazebo environments using the rock and woodland SDF models. -@image html https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/demo_gazebo_rock.png "Example of the generated rock SDF models in Gazebo" width=100% +![Example of the generated rock SDF models in Gazebo](https://raw.githubusercontent.com/gazebosim/gz-sim/gz-sim7/tutorials/files/blender_procedural_datasets/demo_gazebo_rock.png) -@image html https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/demo_gazebo_woodland.png "Example of the generated woodland SDF models in Gazebo" width=100% +![Example of the generated woodland SDF models in Gazebo](https://raw.githubusercontent.com/gazebosim/gz-sim/gz-sim7/tutorials/files/blender_procedural_datasets/demo_gazebo_woodland.png) Every object that uses Geometry Nodes in these projects has several input attributes that can be configured. You can open the `.blend` projects again, From a65c4c91aff3fddb5e7241b777af9894cdfce28b Mon Sep 17 00:00:00 2001 From: mosfet80 Date: Mon, 12 Jun 2023 21:24:40 +0200 Subject: [PATCH 158/160] Update .pre-commit-config.yaml (#2012) update pre-commit-hooks Signed-off-by: mosfet80 --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d5472e5c9f..fbeddc84bc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.3.0 + rev: v4.4.0 hooks: - id: check-added-large-files args: ['--maxkb=500'] From 064f69ed7c441b90abbf0f84eeb0bfaeec628da9 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 13 Jun 2023 15:56:56 -0500 Subject: [PATCH 159/160] Support world joints (joints inside tags) (#1949) As of gazebosim/sdformat#1117, SDFormat supports joints under world (//world/joint in xpath). This PR adds support for it in gz-sim. Note that some systems that operate on joints, such as the JointPositionController, require that the system is instantiated inside a model, and therefore, will not work with world joints. It's not clear whether this is a real problem since world joints are most likely to be fixed joints used to attach two models together. Signed-off-by: Addisu Z. Taddese --- src/LevelManager.cc | 42 +++++++++++++++ src/systems/physics/Physics.cc | 19 ++++++- test/integration/physics_system.cc | 84 +++++++++++++++++++++++++++++- test/worlds/joints_in_world.sdf | 44 ++++++++++++++++ 4 files changed, 187 insertions(+), 2 deletions(-) create mode 100644 test/worlds/joints_in_world.sdf diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 09e8be6bc2..c5ae5e4926 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -35,6 +36,7 @@ #include "gz/sim/components/Atmosphere.hh" #include "gz/sim/components/Geometry.hh" #include "gz/sim/components/Gravity.hh" +#include "gz/sim/components/Joint.hh" #include "gz/sim/components/Level.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/Light.hh" @@ -508,6 +510,21 @@ void LevelManager::ConfigureDefaultLevel() entityNamesInDefault.insert(light->Name()); } } + + // Joints + // We assume no performers are joints + for (uint64_t jointIndex = 0; + jointIndex < this->runner->sdfWorld->JointCount(); ++jointIndex) + { + auto joint = this->runner->sdfWorld->JointByIndex(jointIndex); + + if (this->entityNamesInLevels.find(joint->Name()) == + this->entityNamesInLevels.end()) + { + entityNamesInDefault.insert(joint->Name()); + } + } + // Components this->runner->entityCompMgr.CreateComponent( defaultLevel, components::Level()); @@ -847,6 +864,20 @@ void LevelManager::LoadActiveEntities(const std::set &_namesToLoad) } } + // Joints + for (uint64_t jointIndex = 0; + jointIndex < this->runner->sdfWorld->JointCount(); ++jointIndex) + { + auto joint = this->runner->sdfWorld->JointByIndex(jointIndex); + if (_namesToLoad.find(joint->Name()) != _namesToLoad.end()) + { + Entity jointEntity = this->entityCreator->CreateEntities(joint); + + this->entityCreator->SetParent(jointEntity, this->worldEntity); + } + } + + this->activeEntityNames.insert(_namesToLoad.begin(), _namesToLoad.end()); } @@ -887,6 +918,17 @@ void LevelManager::UnloadInactiveEntities( return true; }); + this->runner->entityCompMgr.Each( + [&](const Entity &_entity, const components::Joint *, + const components::Name *_name) -> bool + { + if (_namesToUnload.find(_name->Data()) != _namesToUnload.end()) + { + this->entityCreator->RequestRemoveEntity(_entity, true); + } + return true; + }); + for (const auto &name : _namesToUnload) { this->activeEntityNames.erase(name); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index dac1022554..8fa201a12a 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -623,6 +623,12 @@ class gz::sim::systems::PhysicsPrivate MinimumFeatureList, physics::sdf::ConstructSdfNestedModel>{}; + ////////////////////////////////////////////////// + // World models (used for world joints) + public: struct WorldModelFeatureList : physics::FeatureList< + MinimumFeatureList, + physics::WorldModelFeature>{}; + ////////////////////////////////////////////////// /// \brief World EntityFeatureMap public: using WorldEntityMap = EntityFeatureMap3d< @@ -633,7 +639,9 @@ class gz::sim::systems::PhysicsPrivate SetContactPropertiesCallbackFeatureList, NestedModelFeatureList, CollisionDetectorFeatureList, - SolverFeatureList>; + SolverFeatureList, + WorldModelFeatureList + >; /// \brief A map between world entity ids in the ECM to World Entities in /// gz-physics. @@ -1035,6 +1043,15 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, } } + // World Model proxy (used for joints directly under in SDF) + auto worldModelFeature = + this->entityWorldMap.EntityCast(_entity); + if (worldModelFeature) + { + auto modelPtrPhys = worldModelFeature->GetWorldModel(); + this->entityModelMap.AddEntity(_entity, modelPtrPhys); + } + return true; }); } diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 7f1ea8bbe9..027005a406 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -121,7 +121,6 @@ TEST_F(PhysicsSystemFixture, CreatePhysicsWorld) server.Run(true, 1, false); EXPECT_FALSE(server.Running()); } - // TODO(addisu) add useful EXPECT calls } //////////////////////////////////////////////// @@ -2539,3 +2538,86 @@ TEST_F(PhysicsSystemFixture, << "World Angular Velocity should be increasing."; } } + +////////////////////////////////////////////////// +/// Tests that joints inside are supported by computing the position of +/// a pendulum bob from the joint angle. This also tests that commands such as +/// JointPositionReset work as expected. +TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(JointsInWorld)) +{ + ServerConfig serverConfig; + + const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", + "joints_in_world.sdf"); + + serverConfig.SetSdfFile(sdfFile); + Server server(serverConfig); + server.SetUpdatePeriod(1ns); + + const int kIters = 1000; + test::Relay testSystem; + testSystem.OnPreUpdate( + [&](const UpdateInfo &_info, EntityComponentManager &_ecm) + { + _ecm.EachNew( + [&](const Entity &_entity, const components::Joint *, + const components::ParentEntity *) -> bool + { + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + return true; + }); + + if (_info.iterations == kIters / 2) + { + // After half the iterations, reset the joint position and velocity so + // that the bob at its equilibrium point and at rest. + auto jointEntity = _ecm.EntityByComponents(components::Name("j1"), + components::Joint()); + ASSERT_NE(jointEntity, kNullEntity); + _ecm.SetComponentData(jointEntity, + {0}); + _ecm.SetComponentData(jointEntity, + {GZ_PI_2}); + } + }); + testSystem.OnPostUpdate( + [&](const UpdateInfo &_info, const EntityComponentManager &_ecm) + { + auto jointEntity = _ecm.EntityByComponents(components::Name("j1"), + components::Joint()); + ASSERT_NE(jointEntity, kNullEntity); + + auto m2Entity = _ecm.EntityByComponents(components::Name("m2"), + components::Model()); + ASSERT_NE(m2Entity, kNullEntity); + + math::Pose3d m2Pose = worldPose(m2Entity, _ecm); + + auto jointPos = + _ecm.ComponentData(jointEntity); + ASSERT_TRUE(jointPos.has_value()); + ASSERT_EQ(1u, jointPos->size()); + + if (_info.iterations < kIters / 2) + { + // If the joint is properly working, the position of the model can be + // determined from the joint angle with the equations: + // x = L*cos(θ) + // z = -L*sin(θ) + // where L is the length of the model from the pivot (2m for this + // test). + const double theta = (*jointPos)[0]; + const double kLength = 2.0; + EXPECT_NEAR(m2Pose.X(), kLength * cos(theta), 1e-2); + EXPECT_NEAR(m2Pose.Z(), -kLength * sin(theta), 1e-2); + } + else + { + EXPECT_NEAR((*jointPos)[0], GZ_PI_2, 1e-2); + } + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1000, false); +} diff --git a/test/worlds/joints_in_world.sdf b/test/worlds/joints_in_world.sdf new file mode 100644 index 0000000000..d838bbecb4 --- /dev/null +++ b/test/worlds/joints_in_world.sdf @@ -0,0 +1,44 @@ + + + + + world + m1 + + + + + + + + 0.5 + + + + + + + + + m1 + m2 + + 0 1 0 + + + + + 2 0 0 0 0 0 + + + + + 0.5 + + + + + + + + From 001dd9b829eb8e8b4465d87c6c70ccf4ee2e6b6a Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 14 Jun 2023 07:20:11 +0200 Subject: [PATCH 160/160] Protobuf: Do not require version 3 do support Protobuf 4.23.2 (23.2) (#2006) Signed-off-by: Silvio Traversaro * TriggeredPublisher: don't catch FatalException It has been removed from recent versions of protobuf. Signed-off-by: Steve Peters Co-authored-by: Steve Peters --- CMakeLists.txt | 8 +++----- src/systems/triggered_publisher/TriggeredPublisher.cc | 9 +-------- 2 files changed, 4 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 77208a11eb..5644db73f7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -186,12 +186,10 @@ set(GZ_UTILS_VER ${gz-utils2_VERSION_MAJOR}) #-------------------------------------- # Find protobuf -set(REQ_PROTOBUF_VER 3) gz_find_package(GzProtobuf - VERSION ${REQ_PROTOBUF_VER} - REQUIRED - COMPONENTS all - PRETTY Protobuf) + REQUIRED + COMPONENTS all + PRETTY Protobuf) set(Protobuf_IMPORT_DIRS ${gz-msgs9_INCLUDE_DIRS}) #-------------------------------------- diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 54b3756d4b..87d0add654 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -832,14 +832,7 @@ bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) return std::all_of(this->matchers.begin(), this->matchers.end(), [&](const auto &_matcher) { - try - { - return _matcher->Match(_inputMsg); - } catch (const google::protobuf::FatalException &err) - { - gzerr << err.what() << std::endl; - return false; - } + return _matcher->Match(_inputMsg); }); }
Documentation:" "" - "" - "https://ignitionrobotics.org/libs/gazebo" + "https://gazebosim.org/libs/sim" "" "
" - "" - "https://ignitionrobotics.org/docs/" + "https://gazebosim.org/docs/" "" "