diff --git a/Changelog.md b/Changelog.md index f69e217cfe..8009a6e700 100644 --- a/Changelog.md +++ b/Changelog.md @@ -556,7 +556,98 @@ ## Ignition Gazebo 5.x -### Ignition Gazebo 5.X.X (202X-XX-XX) +### Ignition Gazebo 5.4.0 (2022-03-31) + +1. Add the Model Photo Shoot system, port of Modelpropshop plugin from Gazebo classic + * [Pull request #1331](https://github.com/ignitionrobotics/ign-gazebo/pull/1331) + +1. Add wheel slip user command + * [Pull request #1241](https://github.com/ignitionrobotics/ign-gazebo/pull/1241) + +1. Added user command to set multiple entity poses + * [Pull request #1394](https://github.com/ignitionrobotics/ign-gazebo/pull/1394) + +1. Component inspector: refactor Pose3d C++ code into a separate class + * [Pull request #1400](https://github.com/ignitionrobotics/ign-gazebo/pull/1400) + +1. Toggle Light visuals + * [Pull request #1387](https://github.com/ignitionrobotics/ign-gazebo/pull/1387) + +1. Allow to turn on/off lights + * [Pull request #1343](https://github.com/ignitionrobotics/ign-gazebo/pull/1343) + +1. Added more sensor properties to scene/info topic + * [Pull request #1344](https://github.com/ignitionrobotics/ign-gazebo/pull/1344) + +1. JointStatePublisher publish parent, child and axis data + * [Pull request #1345](https://github.com/ignitionrobotics/ign-gazebo/pull/1345) + +1. Fixed light GUI component inspector + * [Pull request #1337](https://github.com/ignitionrobotics/ign-gazebo/pull/1337) + +1. Fix `UNIT_SdfGenerator_TEST` + * [Pull request #1319](https://github.com/ignitionrobotics/ign-gazebo/pull/1319) + +1. Add elevator system + * [Pull request #535](https://github.com/ignitionrobotics/ign-gazebo/pull/535) + +1. Removed unused variables in shapes plugin + * [Pull request #1321](https://github.com/ignitionrobotics/ign-gazebo/pull/1321) + +1. Log an error if JointPositionController cannot find the joint. (citadel retarget) + * [Pull request #1314](https://github.com/ignitionrobotics/ign-gazebo/pull/1314) + +1. Buoyancy: fix center of volume's reference frame + * [Pull request #1302](https://github.com/ignitionrobotics/ign-gazebo/pull/1302) + +1. Remove EachNew calls from sensor PreUpdates + * [Pull request #1281](https://github.com/ignitionrobotics/ign-gazebo/pull/1281) + +1. Prevent GzScene3D 💥 if another scene is already loaded + * [Pull request #1294](https://github.com/ignitionrobotics/ign-gazebo/pull/1294) + +1. Cleanup update call for non-rendering sensors + * [Pull request #1282](https://github.com/ignitionrobotics/ign-gazebo/pull/1282) + +1. Documentation Error + * [Pull request #1285](https://github.com/ignitionrobotics/ign-gazebo/pull/1285) + +1. Min and max parameters for velocity, acceleration, and jerk apply to linear and angular separately. + * [Pull request #1229](https://github.com/ignitionrobotics/ign-gazebo/pull/1229) + +1. Add project() call to examples + * [Pull request #1274](https://github.com/ignitionrobotics/ign-gazebo/pull/1274) + +1. Implement `/server_control::stop` + * [Pull request #1240](https://github.com/ignitionrobotics/ign-gazebo/pull/1240) + +1. 👩‍🌾 Make depth camera tests more robust (#897) + * [Pull request #897) (#1257](https://github.com/ignitionrobotics/ign-gazebo/pull/897) (#1257) + +1. Support battery draining start via topics + * [Pull request #1255](https://github.com/ignitionrobotics/ign-gazebo/pull/1255) + +1. Make tests run as fast as possible + * [Pull request #1194](https://github.com/ignitionrobotics/ign-gazebo/pull/1194) + * [Pull request #1250](https://github.com/ignitionrobotics/ign-gazebo/pull/1250) + +1. Fix visualize lidar + * [Pull request #1224](https://github.com/ignitionrobotics/ign-gazebo/pull/1224) + +1. Skip failing Windows tests + * [Pull request #1205](https://github.com/ignitionrobotics/ign-gazebo/pull/1205) + * [Pull request #1204](https://github.com/ignitionrobotics/ign-gazebo/pull/1204) + * [Pull request #1259](https://github.com/ignitionrobotics/ign-gazebo/pull/1259) + * [Pull request #1408](https://github.com/ignitionrobotics/ign-gazebo/pull/1408) + +1. Configurable joint state publisher's topic + * [Pull request #1076](https://github.com/ignitionrobotics/ign-gazebo/pull/1076) + +1. Thruster plugin: add tests and velocity control + * [Pull request #1190](https://github.com/ignitionrobotics/ign-gazebo/pull/1190) + +1. Limit thruster system's input thrust cmd + * [Pull request #1318](https://github.com/ignitionrobotics/ign-gazebo/pull/1318) ### Ignition Gazebo 5.3.0 (2021-11-12) diff --git a/examples/worlds/model_photo_shoot.sdf b/examples/worlds/model_photo_shoot.sdf new file mode 100644 index 0000000000..84ec5b913d --- /dev/null +++ b/examples/worlds/model_photo_shoot.sdf @@ -0,0 +1,55 @@ + + + + + 0 0 0 + + + + ogre2 + 1, 1, 1 + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Robonaut + + poses.txt + false + + + + + 0 0 0 0 0 0 + + + 1.047 + + 960 + 540 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + true + + + diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh new file mode 100644 index 0000000000..4daf7ee249 --- /dev/null +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -0,0 +1,46 @@ +/* + * 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. + * + */ +#ifndef IGNITION_GAZEBO_COMPONENTS_WHEELSLIPCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_WHEELSLIPCMD_HH_ + +#include +#include +#include +#include +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains commanded wheel slip parameters of + /// an entity in the world frame represented by msgs::WheelSlipParameters. + using WheelSlipCmd = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WheelSlipCmd", + WheelSlipCmd) +} +} +} +} +#endif diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index e26ad30b5a..3983736a68 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -126,6 +126,7 @@ add_subdirectory(log_video_recorder) add_subdirectory(logical_audio_sensor_plugin) add_subdirectory(logical_camera) add_subdirectory(magnetometer) +add_subdirectory(model_photo_shoot) add_subdirectory(mecanum_drive) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) diff --git a/src/systems/model_photo_shoot/CMakeLists.txt b/src/systems/model_photo_shoot/CMakeLists.txt new file mode 100644 index 0000000000..d97b0da5e7 --- /dev/null +++ b/src/systems/model_photo_shoot/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(model-photo-shoot + SOURCES + ModelPhotoShoot.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc new file mode 100644 index 0000000000..31f6373e1c --- /dev/null +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -0,0 +1,314 @@ +/* + * Copyright (C) 2019 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 "ModelPhotoShoot.hh" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointType.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointPositionReset.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/rendering/Events.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private ModelPhotoShoot data class. +class ignition::gazebo::systems::ModelPhotoShootPrivate +{ + /// \brief Callback for pos rendering operations. + public: void PerformPostRenderingOperations(); + + /// \brief Save a pitcture with the camera from the given pose. + public: void SavePicture (const ignition::rendering::CameraPtr _camera, + const ignition::math::Pose3d &_pose, + const std::string &_fileName) const; + + /// \brief Name of the loaded model. + public: std::string modelName; + + /// \brief model + public: std::shared_ptr model; + + /// \brief model world pose + public: ignition::math::Pose3d modelPose3D; + + /// \brief Connection to pre-render event callback. + public: ignition::common::ConnectionPtr connection{nullptr}; + + /// \brief Boolean to control we only take the pictures once. + public: bool takePicture{true}; + + /// \brief Boolean to control if joints should adopt random poses. + public: bool randomPoses{false}; + + /// \brief File to save translation and scaling info. + public: std::ofstream savingFile; +}; + +////////////////////////////////////////////////// +ModelPhotoShoot::ModelPhotoShoot() + : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void ModelPhotoShoot::Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) +{ + std::string saveDataLocation = + _sdf->Get("translation_data_file"); + if (saveDataLocation.empty()) + { + igndbg << "No data location specified, skipping translaiton data" + "saving.\n"; + } + else + { + igndbg << "Saving translation data to: " + << saveDataLocation << std::endl; + this->dataPtr->savingFile.open(saveDataLocation); + } + + if (_sdf->HasElement("random_joints_pose")) + { + this->dataPtr->randomPoses = _sdf->Get("random_joints_pose"); + } + + this->dataPtr->connection = + _eventMgr.Connect(std::bind( + &ModelPhotoShootPrivate::PerformPostRenderingOperations, + this->dataPtr.get())); + + this->dataPtr->model = std::make_shared(_entity); + this->dataPtr->modelName = this->dataPtr->model->Name(_ecm); + // Get the pose of the model + this->dataPtr->modelPose3D = + ignition::gazebo::worldPose(this->dataPtr->model->Entity(), _ecm); +} + +////////////////////////////////////////////////// +void ModelPhotoShoot::PreUpdate( + const ignition::gazebo::UpdateInfo &, + ignition::gazebo::EntityComponentManager &_ecm) +{ + if (this->dataPtr->randomPoses) + { + std::vector joints = this->dataPtr->model->Joints(_ecm); + unsigned seed = + std::chrono::system_clock::now().time_since_epoch().count(); + std::default_random_engine generator(seed); + for (const auto &joint : joints) + { + auto jointNameComp = _ecm.Component(joint); + if (jointNameComp) + { + auto jointType = _ecm.Component(joint)->Data(); + if (jointType != sdf::JointType::FIXED) + { + if (jointType == sdf::JointType::REVOLUTE || + jointType == sdf::JointType::PRISMATIC) + { + // Using the JointAxis component to extract the joint pose limits + auto jointAxisComp = _ecm.Component(joint); + if (jointAxisComp) + { + std::uniform_real_distribution distribution( + jointAxisComp->Data().Lower(), + jointAxisComp->Data().Upper()); + double jointPose = distribution(generator); + _ecm.SetComponentData( + joint, {jointPose}); + + // Create a JointPosition component if it doesn't exist. + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + _ecm.SetComponentData( + joint, {jointPose}); + } + + if (this->dataPtr->savingFile.is_open()) + { + this->dataPtr->savingFile << jointNameComp->Data() << ": " + << std::setprecision(17) + << jointPose << std::endl; + } + } + else + { + ignerr << "No jointAxisComp found, ignoring joint: " << + jointNameComp->Data() << std::endl; + } + } + else + { + ignerr << "Model Photo Shoot only supports single axis joints. " + "Skipping joint: "<< jointNameComp->Data() << std::endl; + } + } + else + { + igndbg << "Ignoring fixed joint: " << jointNameComp->Data() << + std::endl; + } + } + else + { + ignerr << "No jointNameComp found on entity: " << joint << + std:: endl; + } + } + // Only set random joint poses once + this->dataPtr->randomPoses = false; + } +} + +////////////////////////////////////////////////// +void ModelPhotoShootPrivate::PerformPostRenderingOperations() +{ + ignition::rendering::ScenePtr scene = + ignition::rendering::sceneFromFirstRenderEngine(); + ignition::rendering::VisualPtr modelVisual = + scene->VisualByName(this->modelName); + + ignition::rendering::VisualPtr root = scene->RootVisual(); + + if (modelVisual && this->takePicture) + { + scene->SetAmbientLight(0.3, 0.3, 0.3); + + // create directional light + ignition::rendering::DirectionalLightPtr light0 = + scene->CreateDirectionalLight(); + light0->SetDirection(-0.5, 0.5, -1); + light0->SetDiffuseColor(0.8, 0.8, 0.8); + light0->SetSpecularColor(0.5, 0.5, 0.5); + root->AddChild(light0); + + // create point light + ignition::rendering::PointLightPtr light2 = scene->CreatePointLight(); + light2->SetDiffuseColor(0.5, 0.5, 0.5); + light2->SetSpecularColor(0.5, 0.5, 0.5); + light2->SetLocalPosition(3, 5, 5); + root->AddChild(light2); + + for (unsigned int i = 0; i < scene->NodeCount(); ++i) + { + auto camera = std::dynamic_pointer_cast( + scene->NodeByIndex(i)); + if (nullptr != camera && camera->Name() == "photo_shoot::link::camera") + { + // Compute the translation we have to apply to the cameras to + // center the model in the image. + ignition::math::AxisAlignedBox bbox = modelVisual->LocalBoundingBox(); + double scaling = 1.0 / bbox.Size().Max(); + ignition::math::Vector3d bboxCenter = bbox.Center(); + ignition::math::Vector3d translation = + bboxCenter + this->modelPose3D.Pos(); + if (this->savingFile.is_open()) { + this->savingFile << "Translation: " << translation << std::endl; + this->savingFile << "Scaling: " << scaling << std::endl; + } + + ignition::math::Pose3d pose; + // Perspective view + pose.Pos().Set(1.6 / scaling + translation.X(), + -1.6 / scaling + translation.Y(), + 1.2 / scaling + translation.Z()); + pose.Rot().Euler(0, IGN_DTOR(30), IGN_DTOR(-225)); + SavePicture(camera, pose, "1.png"); + + // Top view + pose.Pos().Set(0 + translation.X(), + 0 + translation.Y(), + 2.2 / scaling + translation.Z()); + pose.Rot().Euler(0, IGN_DTOR(90), 0); + SavePicture(camera, pose, "2.png"); + + // Front view + pose.Pos().Set(2.2 / scaling + translation.X(), + 0 + translation.Y(), + 0 + translation.Z()); + pose.Rot().Euler(0, 0, IGN_DTOR(-180)); + SavePicture(camera, pose, "3.png"); + + // Side view + pose.Pos().Set(0 + translation.X(), + 2.2 / scaling + translation.Y(), + 0 + translation.Z()); + pose.Rot().Euler(0, 0, IGN_DTOR(-90)); + SavePicture(camera, pose, "4.png"); + + // Back view + pose.Pos().Set(-2.2 / scaling + translation.X(), + 0 + translation.Y(), + 0 + translation.Z()); + pose.Rot().Euler(0, 0, 0); + SavePicture(camera, pose, "5.png"); + + this->takePicture = false; + } + } + } +} + +////////////////////////////////////////////////// +void ModelPhotoShootPrivate::SavePicture( + const ignition::rendering::CameraPtr _camera, + const ignition::math::Pose3d &_pose, + const std::string &_fileName) const +{ + unsigned int width = _camera->ImageWidth(); + unsigned int height = _camera->ImageHeight(); + ignition::common::Image image; + + _camera->SetWorldPose(_pose); + auto cameraImage = _camera->CreateImage(); + _camera->Capture(cameraImage); + auto formatStr = + ignition::rendering::PixelUtil::Name(_camera->ImageFormat()); + auto format = ignition::common::Image::ConvertPixelFormat(formatStr); + image.SetFromData(cameraImage.Data(), width, height, format); + image.SavePNG(_fileName); + + igndbg << "Saved image to [" << _fileName << "]" << std::endl; +} + +IGNITION_ADD_PLUGIN(ModelPhotoShoot, ignition::gazebo::System, + ModelPhotoShoot::ISystemConfigure, + ModelPhotoShoot::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ModelPhotoShoot, + "ignition::gazebo::systems::ModelPhotoShoot") diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.hh b/src/systems/model_photo_shoot/ModelPhotoShoot.hh new file mode 100644 index 0000000000..b059f2486e --- /dev/null +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.hh @@ -0,0 +1,96 @@ +/* + * 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. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_MODELPHOTOSHOOT_HH_ +#define IGNITION_GAZEBO_SYSTEMS_MODELPHOTOSHOOT_HH_ + +#include + +#include + +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class ModelPhotoShootPrivate; + + /// \brief This plugin is a port of the old ModelPropShop plugin from gazebo + /// classic. It generates 5 pictures of a model: perspective, top, front, + /// side and back. It can do it using the default position or moving the joint + /// to random positions. It allows saving the camera and joint poses so it + /// can be replicated in other systems. + /// + /// ## System Parameters + /// - - Location to save the camera and joint poses. + /// [Optional] + /// - - Set to true to take pictures with the joints in + /// random poses instead of the default ones. This option only supports + /// single axis joints. [Optional] + /// - A camera sensor must be set in the SDF file as it will be used by the + /// plugin to take the pictures. This allows the plugin user to set the + /// camera parameters as needed. [Required] + /// + /// ## Example + /// An example configuration is installed with Gazebo. The example uses + /// the Ogre2 rendering plugin to set the background color of the pictures. + /// It also includes the camera sensor that will be used along with the + /// corresponding parameters so they can be easily tunned. + /// + /// To run the example: + /// ``` + /// ign gazebo model_photo_shoot.sdf -s -r --iterations 50 + /// ``` + /// This will start gazebo, load the model take the pictures and shutdown + /// after 50 iterations. You will find the pictures in the same location you + /// run the command. + + /// \brief System that takes snapshots of an sdf model + class ModelPhotoShoot : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + + /// \brief Constructor + public: ModelPhotoShoot(); + + /// \brief Destructor + public: ~ModelPhotoShoot() override = default; + + // Documentation inherited + public: void Configure(const ignition::gazebo::Entity &_id, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} +} +} +} +#endif diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index bdbacc24e2..9ac7892eaa 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -25,9 +25,11 @@ #include #include #include +#include #include #include +#include #include #include @@ -43,6 +45,8 @@ #include "ignition/common/Profiler.hh" +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LightCmd.hh" #include "ignition/gazebo/components/Link.hh" @@ -53,15 +57,19 @@ #include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/PhysicsCmd.hh" #include "ignition/gazebo/components/SphericalCoordinates.hh" +#include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/World.hh" #include "ignition/gazebo/components/ContactSensorData.hh" #include "ignition/gazebo/components/ContactSensor.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/VisualCmd.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" using namespace ignition; using namespace gazebo; @@ -449,6 +457,48 @@ class VisualCommand : public UserCommandBase aMaterial.emissive().a(), bMaterial.emissive().a(), 1e-6f); }}; }; + +/// \brief Command to modify a wheel entity from simulation. +class WheelSlipCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message containing the wheel slip parameters. + /// \param[in] _iface Pointer to user commands interface. + public: WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; + + /// \brief WheelSlip equality comparision function + public: std::function + wheelSlipEql { + []( + const msgs::WheelSlipParametersCmd &_a, + const msgs::WheelSlipParametersCmd &_b) + { + return + ( + ( + _a.entity().id() != kNullEntity && + _a.entity().id() == _b.entity().id() + ) || + ( + _a.entity().name() == _b.entity().name() && + _a.entity().type() == _b.entity().type() + ) + ) && + math::equal( + _a.slip_compliance_lateral(), + _b.slip_compliance_lateral(), + 1e-6) && + math::equal( + _a.slip_compliance_longitudinal(), + _b.slip_compliance_longitudinal(), + 1e-6); + }}; +}; } } } @@ -544,6 +594,16 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \return True if successful. public: bool VisualService(const msgs::Visual &_req, msgs::Boolean &_res); + /// \brief Callback for wheel slip service + /// \param[in] _req Request containing wheel slip parameter updates of an + /// entity. + /// \param[out] _res True if message sucessfully received and queued. + /// It does not mean that the wheel slip parameters will be successfully + /// updated. + /// \return True if successful. + public: bool WheelSlipService( + const msgs::WheelSlipParametersCmd &_req, msgs::Boolean &_res); + /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -731,6 +791,14 @@ void UserCommands::Configure(const Entity &_entity, &UserCommandsPrivate::VisualService, this->dataPtr.get()); ignmsg << "Material service on [" << visualService << "]" << std::endl; + + // Wheel slip service + std::string wheelSlipService + {"/world/" + validWorldName + "/wheel_slip"}; + this->dataPtr->node.Advertise(wheelSlipService, + &UserCommandsPrivate::WheelSlipService, this->dataPtr.get()); + + ignmsg << "Material service on [" << wheelSlipService << "]" << std::endl; } ////////////////////////////////////////////////// @@ -969,6 +1037,25 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::WheelSlipService( + const msgs::WheelSlipParametersCmd &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// bool UserCommandsPrivate::SphericalCoordinatesService( const msgs::SphericalCoordinates &_req, msgs::Boolean &_res) @@ -1750,6 +1837,130 @@ bool VisualCommand::Execute() return true; } +////////////////////////////////////////////////// +WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +// TODO(ivanpauno): Move this somewhere else +Entity scopedEntityFromMsg( + const msgs::Entity & _msg, const EntityComponentManager & _ecm) +{ + if (_msg.id() != kNullEntity) { + return _msg.id(); + } + std::unordered_set entities = entitiesFromScopedName( + _msg.name(), _ecm); + if (entities.empty()) { + ignerr << "Failed to find entity with scoped name [" << _msg.name() + << "]." << std::endl; + return kNullEntity; + } + if (_msg.type() == msgs::Entity::NONE) { + return *entities.begin(); + } + const components::BaseComponent * component; + std::string componentType; + for (const auto entity : entities) { + switch (_msg.type()) { + case msgs::Entity::LIGHT: + component = _ecm.Component(entity); + componentType = "LIGHT"; + break; + case msgs::Entity::MODEL: + component = _ecm.Component(entity); + componentType = "MODEL"; + break; + case msgs::Entity::LINK: + component = _ecm.Component(entity); + componentType = "LINK"; + break; + case msgs::Entity::VISUAL: + component = _ecm.Component(entity); + componentType = "VISUAL"; + break; + case msgs::Entity::COLLISION: + component = _ecm.Component(entity); + componentType = "COLLISION"; + break; + case msgs::Entity::SENSOR: + component = _ecm.Component(entity); + componentType = "SENSOR"; + break; + case msgs::Entity::JOINT: + component = _ecm.Component(entity); + componentType = "JOINT"; + break; + default: + componentType = "unknown"; + break; + } + if (component != nullptr) { + return entity; + } + } + ignerr << "Found entity with scoped name [" << _msg.name() + << "], but it doesn't have a component of the required type [" + << componentType << "]." << std::endl; + return kNullEntity; +} + +////////////////////////////////////////////////// +bool WheelSlipCommand::Execute() +{ + auto wheelSlipMsg = dynamic_cast( + this->msg); + if (nullptr == wheelSlipMsg) + { + ignerr << "Internal error, null wheel slip message" << std::endl; + return false; + } + const auto & ecm = *this->iface->ecm; + Entity entity = scopedEntityFromMsg(wheelSlipMsg->entity(), ecm); + if (kNullEntity == entity) + { + return false; + } + + auto doForEachLink = [this, wheelSlipMsg](Entity linkEntity) { + auto wheelSlipCmdComp = + this->iface->ecm->Component(linkEntity); + if (!wheelSlipCmdComp) + { + this->iface->ecm->CreateComponent( + linkEntity, components::WheelSlipCmd(*wheelSlipMsg)); + } + else + { + auto state = wheelSlipCmdComp->SetData( + *wheelSlipMsg, this->wheelSlipEql) ? ComponentState::OneTimeChange + : ComponentState::NoChange; + this->iface->ecm->SetChanged( + linkEntity, components::WheelSlipCmd::typeId, state); + } + }; + const components::BaseComponent * component = + ecm.Component(entity); + + if (nullptr != component) { + doForEachLink(entity); + return true; + } + component = ecm.Component(entity); + if (nullptr != component) { + Model model{entity}; + for (const auto & linkEntity : model.Links(*this->iface->ecm)) { + doForEachLink(linkEntity); + } + return true; + } + ignerr << "Found entity with scoped name [" << wheelSlipMsg->entity().name() + << "], is neither a model or a link." << std::endl; + return false; +} + IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, UserCommands::ISystemPreUpdate diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index df1dd2c5dd..78385295de 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -33,6 +33,7 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointVelocity.hh" #include "ignition/gazebo/components/SlipComplianceCmd.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" using namespace ignition; using namespace gazebo; @@ -107,7 +108,9 @@ class ignition::gazebo::systems::WheelSlipPrivate { if (_a.size() != _b.size() || _a.size() < 2 ||_b.size() < 2) + { return false; + } for (size_t i = 0; i < _a.size(); i++) { @@ -242,9 +245,32 @@ bool WheelSlipPrivate::Load(const EntityComponentManager &_ecm, ///////////////////////////////////////////////// void WheelSlipPrivate::Update(EntityComponentManager &_ecm) { - for (const auto &linkSurface : this->mapLinkSurfaceParams) + for (auto &linkSurface : this->mapLinkSurfaceParams) { - const auto ¶ms = linkSurface.second; + auto ¶ms = linkSurface.second; + const auto * wheelSlipCmdComp = + _ecm.Component(linkSurface.first); + if (wheelSlipCmdComp) + { + const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); + bool changed = (!math::equal( + params.slipComplianceLateral, + wheelSlipCmdParams.slip_compliance_lateral(), + 1e-6)) || + (!math::equal( + params.slipComplianceLongitudinal, + wheelSlipCmdParams.slip_compliance_longitudinal(), + 1e-6)); + + if (changed) + { + params.slipComplianceLateral = + wheelSlipCmdParams.slip_compliance_lateral(); + params.slipComplianceLongitudinal = + wheelSlipCmdParams.slip_compliance_longitudinal(); + } + _ecm.RemoveComponent(linkSurface.first); + } // get user-defined normal force constant double force = params.wheelNormalForce; diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 7bed8aaf98..f9c1612fdd 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -36,6 +36,8 @@ set(tests logical_audio_sensor_plugin.cc magnetometer_system.cc model.cc + model_photo_shoot_default_joints.cc + model_photo_shoot_random_joints.cc multicopter.cc multiple_servers.cc navsat_system.cc @@ -135,6 +137,14 @@ target_link_libraries(INTEGRATION_tracked_vehicle_system ignition-physics${IGN_PHYSICS_VER}::core ignition-plugin${IGN_PLUGIN_VER}::loader ) + +target_link_libraries(INTEGRATION_model_photo_shoot_default_joints + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} +) +target_link_libraries(INTEGRATION_model_photo_shoot_random_joints + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} +) + # The default timeout (240s) doesn't seem to be enough for this test. set_tests_properties(INTEGRATION_tracked_vehicle_system PROPERTIES TIMEOUT 300) diff --git a/test/integration/ModelPhotoShootTest.hh b/test/integration/ModelPhotoShootTest.hh new file mode 100644 index 0000000000..4fe1f08aff --- /dev/null +++ b/test/integration/ModelPhotoShootTest.hh @@ -0,0 +1,306 @@ +/* + * 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. + * +*/ + +#ifndef IGNITION_GAZEBO_TEST_INTEGRATION_MODELPHOTOSHOOTTEST_HH_ +#define IGNITION_GAZEBO_TEST_INTEGRATION_MODELPHOTOSHOOTTEST_HH_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointType.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" +#include "ignition/gazebo/TestFixture.hh" +#include "ignition/gazebo/rendering/Events.hh" +#include "ignition/gazebo/Model.hh" + +#include +#include +#include +#include +#include +#include + + +#include "helpers/UniqueTestDirectoryEnv.hh" +#include "helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Saves an image from a camera in a given position. +/// \param[in] _camera Camera to use for the picture. +/// \param[in] _pose Pose for the camera. +/// \param[in] _fileName Filename to save the image. +void SavePicture(const ignition::rendering::CameraPtr _camera, + const ignition::math::Pose3d &_pose, + const std::string &_fileName) +{ + unsigned int width = _camera->ImageWidth(); + unsigned int height = _camera->ImageHeight(); + ignition::common::Image image; + + _camera->SetWorldPose(_pose); + auto cameraImage = _camera->CreateImage(); + _camera->Capture(cameraImage); + auto formatStr = + ignition::rendering::PixelUtil::Name(_camera->ImageFormat()); + auto format = ignition::common::Image::ConvertPixelFormat(formatStr); + image.SetFromData(cameraImage.Data(), width, height, format); + image.SavePNG(_fileName); +} + +/// \brief Tests that two png files have the same values. +/// \param[in] _filename First png file. +/// \param[in] _testFilename Second png file. +void testImages(const std::string &_imageFile, + const std::string &_testImageFile) +{ + std::string imageFilePath = common::joinPaths(common::cwd(), _imageFile); + ignition::common::Image image(imageFilePath); + std::string testImageFilePath = + common::joinPaths(common::cwd(), _testImageFile); + ignition::common::Image testImage(testImageFilePath); + + EXPECT_TRUE(image.Valid()); + EXPECT_TRUE(testImage.Valid()); + EXPECT_EQ(image.Width(), testImage.Width()); + EXPECT_EQ(image.Height(), testImage.Height()); + EXPECT_EQ(image.PixelFormat(), testImage.PixelFormat()); + unsigned int dataLength; + unsigned char *imageData = nullptr; + image.Data(&imageData, dataLength); + unsigned int testDataLenght; + unsigned char *testImageData = nullptr; + image.Data(&testImageData, testDataLenght); + ASSERT_EQ(dataLength, testDataLenght); + ASSERT_EQ(memcmp(imageData, testImageData, dataLength), 0); + + // Deleting files so they do not affect future tests + EXPECT_EQ(remove(imageFilePath.c_str()), 0); + EXPECT_EQ(remove(testImageFilePath.c_str()), 0); +} + +/// \brief Test ModelPhotoShootTest system. +class ModelPhotoShootTest : public InternalFixture<::testing::Test> +{ + protected: void SetUp() override + { + EXPECT_TRUE(common::chdir(test::UniqueTestDirectoryEnv::Path())); + InternalFixture<::testing::Test>::SetUp(); + } + /// \brief PostRender callback. + public: void OnPostRender() + { + if (takeTestPics) + { + ignition::rendering::ScenePtr scene = + ignition::rendering::sceneFromFirstRenderEngine(); + for (unsigned int i = 0; i < scene->NodeCount(); ++i) + { + auto camera = std::dynamic_pointer_cast( + scene->NodeByIndex(i)); + if (nullptr != camera && camera->Name() == "photo_shoot::link::camera") + { + ignition::math::Pose3d pose; + // Perspective view + pose.Pos().Set(1.6 / scaling + translation.X(), + -1.6 / scaling + translation.Y(), + 1.2 / scaling + translation.Z()); + pose.Rot().Euler(0, IGN_DTOR(30), IGN_DTOR(-225)); + SavePicture(camera, pose, "1_test.png"); + // Top view + pose.Pos().Set(0 + translation.X(), + 0 + translation.Y(), + 2.2 / scaling + translation.Z()); + pose.Rot().Euler(0, IGN_DTOR(90), 0); + SavePicture(camera, pose, "2_test.png"); + + // Front view + pose.Pos().Set(2.2 / scaling + translation.X(), + 0 + translation.Y(), + 0 + translation.Z()); + pose.Rot().Euler(0, 0, IGN_DTOR(-180)); + SavePicture(camera, pose, "3_test.png"); + + // Side view + pose.Pos().Set(0 + translation.X(), + 2.2 / scaling + translation.Y(), + 0 + translation.Z()); + pose.Rot().Euler(0, 0, IGN_DTOR(-90)); + SavePicture(camera, pose, "4_test.png"); + + // Back view + pose.Pos().Set(-2.2 / scaling + translation.X(), + 0 + translation.Y(), + 0 + translation.Z()); + pose.Rot().Euler(0, 0, 0); + SavePicture(camera, pose, "5_test.png"); + } + } + takeTestPics = false; + } + } + + /// \brief Loads the pose values generated by the Model Photo Shoot plugin. + /// \param[in] _poseFile File containing the generated poses. + protected: void LoadPoseValues(std::string _poseFile = "poses.txt") + { + std::string poseFilePath = common::joinPaths(common::cwd(), _poseFile); + std::ifstream poseFile (poseFilePath); + std::string line; + ASSERT_TRUE(poseFile.is_open()); + while (getline(poseFile, line) ) + { + std::istringstream iss(line); + std::string word; + while (getline( iss, word, ' ' )) + { + if (word == "Translation:") + { + float tr_x, tr_y, tr_z; + getline( iss, word, ' ' ); + tr_x = std::stof(word); + getline( iss, word, ' ' ); + tr_y = std::stof(word); + getline( iss, word, ' ' ); + tr_z = std::stof(word); + this->translation = {tr_x, tr_y, tr_z}; + break; + } + else + { + if (word == "Scaling:") + { + getline( iss, word, ' ' ); + this->scaling = std::stod(word); + break; + } + else + { + std::string jointName = line.substr(0, line.find(": ")); + std::string jointPose = line.substr(line.find(": ")+2); + jointPositions[jointName] = std::stod(jointPose); + } + } + } + } + poseFile.close(); + EXPECT_EQ(remove(poseFilePath.c_str()), 0); + } + + /// \brief Tests the Model Photo Shoot plugin with a given sdf world. + /// \param[in] _sdfWorld SDF World to use for the test. + protected: void ModelPhotoShootTestCmd(const std::string _sdfWorld) + { + // First run of the server generating images through the plugin. + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + _sdfWorld)); + fixture.Server()->SetUpdatePeriod(1ns); + + common::ConnectionPtr postRenderConn; + fixture.OnConfigure([&]( + const Entity &, + const std::shared_ptr &, + EntityComponentManager &, + EventManager &_eventMgr) + { + postRenderConn = _eventMgr.Connect( + std::bind(&ModelPhotoShootTest::OnPostRender, this)); + }).Finalize(); + + fixture.Server()->Run(true, 50, false); + this->LoadPoseValues(); + + fixture.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if(!jointPositions.empty() && this->checkRandomJoints) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Model *) -> bool + { + auto modelName = _ecm.Component(_entity); + if (modelName->Data() == "r2") + { + this->model = std::make_shared(_entity); + } + return true; + }); + std::vector joints = this->model->Joints(_ecm); + for (const auto &joint : joints) + { + auto jointNameComp = _ecm.Component(joint); + std::map::iterator it = + jointPositions.find(jointNameComp->Data()); + if(it != jointPositions.end()) + { + auto jointType = _ecm.Component + (joint)->Data(); + ASSERT_TRUE(jointType == sdf::JointType::REVOLUTE || + jointType == sdf::JointType::PRISMATIC); + auto jointAxis = _ecm.Component(joint); + ASSERT_GE(it->second, jointAxis->Data().Lower()); + ASSERT_LE(it->second, jointAxis->Data().Upper()); + auto jointPosition = + _ecm.Component(joint); + ASSERT_NE(jointPosition, nullptr); + ASSERT_DOUBLE_EQ(jointPosition->Data()[0], it->second); + } + } + this->checkRandomJoints = false; + } + }).Finalize(); + + this->takeTestPics = true; + + const auto end_time = std::chrono::steady_clock::now() + + std::chrono::milliseconds(3000); + while (takeTestPics && end_time > std::chrono::steady_clock::now()) + { + fixture.Server()->Run(true, 1, false); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + testImages("1.png", "1_test.png"); + testImages("2.png", "2_test.png"); + testImages("3.png", "3_test.png"); + testImages("4.png", "4_test.png"); + testImages("5.png", "5_test.png"); + } + + private: bool takeTestPics{false}; + private: bool checkRandomJoints{true}; + private: double scaling; + private: ignition::math::Vector3d translation; + private: std::map jointPositions; + private: std::shared_ptr model; +}; + +#endif diff --git a/test/integration/model_photo_shoot_default_joints.cc b/test/integration/model_photo_shoot_default_joints.cc new file mode 100644 index 0000000000..620bf42e8a --- /dev/null +++ b/test/integration/model_photo_shoot_default_joints.cc @@ -0,0 +1,36 @@ +/* + * 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 "ModelPhotoShootTest.hh" + +#include + +// Test the Model Photo Shoot plugin on the example world. +TEST_F(ModelPhotoShootTest, + IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ModelPhotoShootDefaultJoints)) +{ + this->ModelPhotoShootTestCmd( + "examples/worlds/model_photo_shoot.sdf"); +} + +int main(int _argc, char **_argv) +{ + ::testing::InitGoogleTest(&_argc, _argv); + ::testing::AddGlobalTestEnvironment( + new test::UniqueTestDirectoryEnv("model_photo_shoot_test")); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/model_photo_shoot_random_joints.cc b/test/integration/model_photo_shoot_random_joints.cc new file mode 100644 index 0000000000..21cabd9681 --- /dev/null +++ b/test/integration/model_photo_shoot_random_joints.cc @@ -0,0 +1,36 @@ +/* + * 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 "ModelPhotoShootTest.hh" + +#include + +// Test the Model Photo Shoot plugin on the example world. +TEST_F(ModelPhotoShootTest, + IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ModelPhotoShootRandomJoints)) +{ + this->ModelPhotoShootTestCmd( + "test/worlds/model_photo_shoot_random_joints.sdf"); +} + +int main(int _argc, char **_argv) +{ + ::testing::InitGoogleTest(&_argc, _argv); + ::testing::AddGlobalTestEnvironment( + new test::UniqueTestDirectoryEnv("model_photo_shoot_test")); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 48c9a18bc8..14a7febd5d 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -33,7 +33,9 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" #include "ignition/gazebo/test_config.hh" @@ -1104,3 +1106,103 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Physics)) EXPECT_DOUBLE_EQ(0.123, physicsComp->Data().MaxStepSize()); EXPECT_DOUBLE_EQ(4.567, physicsComp->Data().RealTimeFactor()); } + +///////////////////////////////////////////////// +TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(WheelSlip)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/trisphere_cycle_wheel_slip.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + ASSERT_NE(nullptr, ecm); + + // Check that the physics properties are the ones specified in the sdf + auto worldEntity = ecm->EntityByComponents(components::World()); + ASSERT_NE(kNullEntity, worldEntity); + Entity tc0 = ecm->EntityByComponents( + components::Name("trisphere_cycle0")); + ASSERT_NE(kNullEntity, tc0); + Entity tc1 = ecm->EntityByComponents( + components::Name("trisphere_cycle1")); + ASSERT_NE(kNullEntity, tc1); + + Model tcModel0{tc0}; + Model tcModel1{tc1}; + Entity wf0 = tcModel0.LinkByName(*ecm, "wheel_front"); + ASSERT_NE(kNullEntity, wf0); + Entity wrl0 = tcModel0.LinkByName(*ecm, "wheel_rear_left"); + ASSERT_NE(kNullEntity, wrl0); + Entity wrf0 = tcModel0.LinkByName(*ecm, "wheel_rear_right"); + ASSERT_NE(kNullEntity, wrf0); + Entity wf1 = tcModel1.LinkByName(*ecm, "wheel_front"); + ASSERT_NE(kNullEntity, wf1); + Entity wrl1 = tcModel1.LinkByName(*ecm, "wheel_rear_left"); + ASSERT_NE(kNullEntity, wrl1); + Entity wrf1 = tcModel1.LinkByName(*ecm, "wheel_rear_right"); + ASSERT_NE(kNullEntity, wrf1); + + Entity links[] = {wf0, wrl0, wrf0, wf1, wrl1, wrf1}; + for (auto link : links) { + EXPECT_EQ(nullptr, ecm->Component(link)); + } + + // modify wheel slip parameters of one link of model 0 + msgs::WheelSlipParametersCmd req; + auto * entityMsg = req.mutable_entity(); + entityMsg->set_name("trisphere_cycle0::wheel_front"); + entityMsg->set_type(msgs::Entity::LINK); + req.set_slip_compliance_lateral(1); + req.set_slip_compliance_longitudinal(1); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/wheel_slip/wheel_slip"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run two iterations, in the first one the WheelSlipCmd component is created + // and processed. + // The second one is just to check everything went fine. + server.Run(true, 2, false); + + // modify wheel slip parameters of one link of model 1 + entityMsg->set_name("trisphere_cycle1"); + entityMsg->set_type(msgs::Entity::MODEL); + req.set_slip_compliance_lateral(2); + req.set_slip_compliance_longitudinal(1); + + result = false; + res = msgs::Boolean{}; + + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run two iterations, in the first one the WheelSlipCmd component is created + // and processed. + // The second one is just to check everything went fine. + server.Run(true, 3, false);} diff --git a/test/worlds/model_photo_shoot_random_joints.sdf b/test/worlds/model_photo_shoot_random_joints.sdf new file mode 100644 index 0000000000..dd22d60677 --- /dev/null +++ b/test/worlds/model_photo_shoot_random_joints.sdf @@ -0,0 +1,55 @@ + + + + + 0 0 0 + + + + ogre2 + 1, 1, 1 + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Robonaut + + poses.txt + true + + + + + 0 0 0 0 0 0 + + + 1.047 + + 960 + 540 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + true + + + diff --git a/tutorials.md.in b/tutorials.md.in index 779c6705c7..74988a1c5c 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -36,6 +36,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage model_and_optimize_meshes "Model and optimize meshes": Some recomendations when creating meshes in blender for simulations. * \subpage model_command "Model Command": Use the CLI to get information about the models in a simulation. * \subpage test_fixture "Test Fixture": Writing automated CI tests +* \subpage model_photo_shoot "Model Photo Shoot" Taking perspective, top, front, and side pictures of a model. * \subpage spherical_coordinates "Spherical coordinates": Working with latitude and longitude * \subpage python_interfaces Python interfaces * \subpage headless_rendering "Headless rendering": Access the GPU on a remote machine to produce sensor data without an X server. diff --git a/tutorials/model_photo_shoot.md b/tutorials/model_photo_shoot.md new file mode 100644 index 0000000000..7a1874e9a5 --- /dev/null +++ b/tutorials/model_photo_shoot.md @@ -0,0 +1,106 @@ +\page model_photo_shoot Model Photo Shoot + +## Using the model photo shot plugin + +Ignition Gazebo offers a model photo taking tool that will take perspective, +top, front, and both sides pictures of a model. You can test the demo world +in Ignition Gazebo, located at `examples/worlds/model_photo_shoot.sdf`, by +running the following command: + +``` +ign gazebo -s -r -v 4 --iterations 50 model_photo_shoot.sdf +``` + +This will start Ignition Gazebo server, load the model and the plugin, take the +pictures and shutdown after 50 iterations. The pictures can be found at the +same location where the command was issued. + +## Model Photo Shoot configurations + +SDF is used to load and configure the `Model Photo Shoot` plugin. The demo SDF +contains a good example of the different options and other related plugins: + +1. The physics plugin: + +``` + + +``` + +A physics plugin is needed only if the `` option is to +be used. This will allow the `Model Photo Shoot` plugin to set the joints +to random positions. + +2. The render engine plugin: + +``` + + ogre2 + 1, 1, 1 + +``` + +A render plugin is needed to render the image. If `ogre2` is used, as shown in +the example, the `` tag can be used to set the background +of the pictures taken by the plugin. Please note that lights added by the +plugin will also affect the final resulting background color on the images. + +3. The model and the photo shoot plugin: + +``` + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Robonaut + + poses.txt + true + + +``` + +The model is loaded through the `` tag. Then the `model photo shoot` +plugin and its options are specified: + +* ``: (optional) Location to store the camera +translation, scaling data and joints position (if using the +`` option) that can be used to replicate the +pictures using other systems. +* ``: (optional) When set to `true` the joints in the model +will be set to random positions prior to taking the pictures. + +4. Camera sensor: + +``` + + + 0 0 0 0 0 0 + + + 1.047 + + 960 + 540 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + true + +``` + +A `camera sensor` must be added as it will be used by the plugin to take the +pictures. This allows plugin users to set the different parameters of the +camera to their desired values.