From 387aaa7e0dee70676a178e0fe893dc99955e55b0 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Tue, 25 Jan 2022 23:38:28 +0000 Subject: [PATCH 01/20] first working version of modelpropshop plugin Signed-off-by: Marco A. Gutierrez --- examples/worlds/model_photo_shoot.sdf | 51 ++++ src/systems/CMakeLists.txt | 1 + src/systems/model_photoshoot/CMakeLists.txt | 8 + .../model_photoshoot/ModelPhotoShoot.cc | 223 ++++++++++++++++++ .../model_photoshoot/ModelPhotoShoot.hh | 84 +++++++ 5 files changed, 367 insertions(+) create mode 100644 examples/worlds/model_photo_shoot.sdf create mode 100644 src/systems/model_photoshoot/CMakeLists.txt create mode 100644 src/systems/model_photoshoot/ModelPhotoShoot.cc create mode 100644 src/systems/model_photoshoot/ModelPhotoShoot.hh diff --git a/examples/worlds/model_photo_shoot.sdf b/examples/worlds/model_photo_shoot.sdf new file mode 100644 index 0000000000..99f1e627cc --- /dev/null +++ b/examples/worlds/model_photo_shoot.sdf @@ -0,0 +1,51 @@ + + + + + + ogre2 + + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/barcs_qav500_sensor_config_1 + poses.txt + + + 2.2 0 0 0 0 -3.14 + + 0 0 0 0 0 0 + + + 1.047 + + 960 + 540 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + true + + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 2c84fc2648..677db80231 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -112,6 +112,7 @@ add_subdirectory(log_video_recorder) add_subdirectory(logical_audio_sensor_plugin) add_subdirectory(logical_camera) add_subdirectory(magnetometer) +add_subdirectory(model_photoshoot) add_subdirectory(mecanum_drive) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) diff --git a/src/systems/model_photoshoot/CMakeLists.txt b/src/systems/model_photoshoot/CMakeLists.txt new file mode 100644 index 0000000000..d97b0da5e7 --- /dev/null +++ b/src/systems/model_photoshoot/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_photoshoot/ModelPhotoShoot.cc b/src/systems/model_photoshoot/ModelPhotoShoot.cc new file mode 100644 index 0000000000..720c3ad46a --- /dev/null +++ b/src/systems/model_photoshoot/ModelPhotoShoot.cc @@ -0,0 +1,223 @@ +/* + * 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 + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +ModelPhotoShoot::ModelPhotoShoot() + : factoryPub(std::make_shared()), + takePicture(true) {} + +ModelPhotoShoot::~ModelPhotoShoot() {} + +void ModelPhotoShoot::Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) { + + // Read the configuration values from the sdf + this->modelLocation = _sdf->Get("model_uri"); + if (this->modelLocation.empty()) { + ignerr << "Please specify the model location through the " + "tag in the sdf file.\n"; + return; + } + std::string save_data_location = + _sdf->Get("translation_data_file"); + if (save_data_location.empty()) { + igndbg << "No data location specified, skipping translaiton data" + "saving.\n"; + } else { + igndbg << "Saving translation data to: " << save_data_location << std::endl; + this->savingFile.open(save_data_location); + } + + this->connection = _eventMgr.Connect( + std::bind(&ModelPhotoShoot::PerformPostRenderingOperations, this)); + + LoadModel(_entity, _ecm); +} + +void ModelPhotoShoot::LoadModel( + const ignition::gazebo::Entity &_entity, + ignition::gazebo::EntityComponentManager &_ecm) { + auto world = std::make_shared(_entity); + std::string worldName = world->Name(_ecm); + + sdf::SDFPtr modelSdf(new sdf::SDF()); + + if (!sdf::init(modelSdf)) { + ignerr << "ERROR: SDF parsing the xml failed\n"; + return; + } + + if (!sdf::readFile(this->modelLocation, modelSdf)) { + ignerr << "Error: SDF parsing the xml failed\n"; + return; + } + + sdf::ElementPtr modelElem = modelSdf->Root()->GetElement("model"); + this->modelName = modelElem->Get("name"); + + // Create entity + std::string service = "/world/" + worldName + "/create"; + ignition::msgs::EntityFactory request; + request.set_sdf(modelSdf->ToString()); + request.set_name(this->modelName); + + ignition::msgs::Boolean response; + bool result; + uint32_t timeout = 5000; + bool executed = + this->factoryPub->Request(service, request, timeout, response, result); + if (executed) { + if (result && response.data()) { + igndbg << "Requested creation of entity: " << this->modelName.c_str() + << std::endl; + } else { + ignerr << "Failed request to create entity.\n" + << request.DebugString().c_str(); + } + } else { + ignerr << "Request to create entity from service " + << request.DebugString().c_str() << "timer out ...\n"; + } +} + +void ModelPhotoShoot::PerformPostRenderingOperations() { + igndbg << "PerformPostRenderingOperations\n"; + + ignition::rendering::ScenePtr scene = + ignition::rendering::sceneFromFirstRenderEngine(); + ignition::rendering::VisualPtr modelVisual = + scene->VisualByName(this->modelName); + + ignition::rendering::VisualPtr root = scene->RootVisual(); + + if (modelVisual && takePicture) { + + scene->SetAmbientLight(0.3, 0.3, 0.3); + scene->SetBackgroundColor(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) + std::cout << camera->Name() << std::endl; + if (nullptr != camera && camera->Name() == "photo_shoot::link::camera") { + + // Set the model pose + ignition::math::AxisAlignedBox bbox = modelVisual->LocalBoundingBox(); + ignition::rendering::WireBoxPtr wireBox = scene->CreateWireBox(); + + double scaling = 1.0 / bbox.Size().Max(); + + // Compute the model translation. + ignition::math::Vector3d trans = bbox.Center(); + trans *= -scaling; + + if (savingFile.is_open()) + savingFile << "Translation: " << trans << std::endl; + + // Normalize the size of the visual + // modelVisual->SetLocalScale(ignition::math::Vector3d(scaling, + // scaling, scaling)); + modelVisual->SetWorldPose( + ignition::math::Pose3d(trans.X(), trans.Y(), trans.Z(), 0, 0, 0)); + + ignition::math::Pose3d pose; + + // Perspective view + pose.Pos().Set(1.6, -1.6, 1.2); + pose.Rot().Euler(0, IGN_DTOR(30), IGN_DTOR(-225)); + SavePicture(camera, pose, "1"); + + // Top view + pose.Pos().Set(0, 0, 2.2); + pose.Rot().Euler(0, IGN_DTOR(90), 0); + SavePicture(camera, pose, "2"); + + // Front view + pose.Pos().Set(2.2, 0, 0); + pose.Rot().Euler(0, 0, IGN_DTOR(-180)); + SavePicture(camera, pose, "3"); + + // Side view + pose.Pos().Set(0, 2.2, 0); + pose.Rot().Euler(0, 0, IGN_DTOR(-90)); + SavePicture(camera, pose, "4"); + + // Back view + pose.Pos().Set(-2.2, 0, 0); + pose.Rot().Euler(0, 0, 0); + SavePicture(camera, pose, "5"); + + takePicture = false; + } + } + } +} + +void ModelPhotoShoot::SavePicture(const ignition::rendering::CameraPtr camera, + const ignition::math::Pose3d pose, + const std::string name) { + 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); + std::string fullname = name + ".png"; + image.SavePNG(fullname); + + igndbg << "Saved image to [" << fullname << "]" << std::endl; +} + +IGNITION_ADD_PLUGIN(ModelPhotoShoot, ignition::gazebo::System, + ModelPhotoShoot::ISystemConfigure) + +IGNITION_ADD_PLUGIN_ALIAS(ModelPhotoShoot, + "ignition::gazebo::systems::ModelPhotoShoot") diff --git a/src/systems/model_photoshoot/ModelPhotoShoot.hh b/src/systems/model_photoshoot/ModelPhotoShoot.hh new file mode 100644 index 0000000000..fae5f30d9a --- /dev/null +++ b/src/systems/model_photoshoot/ModelPhotoShoot.hh @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2018 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 +#include +#include +#include + +namespace ignition { +namespace gazebo { +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems { + /// \brief System that takes snapshots of an sdf model + class ModelPhotoShoot : public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure { + public: + ModelPhotoShoot(); + + public: + ~ModelPhotoShoot(); + + public: + void Configure(const ignition::gazebo::Entity &_id, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) final; + + void PerformPostRenderingOperations(); + + void LoadModel(const ignition::gazebo::Entity &_entity, + ignition::gazebo::EntityComponentManager &_ecm); + + void SavePicture(const ignition::rendering::CameraPtr camera, + const ignition::math::Pose3d pose, const std::string name); + + /// \brief Name of the loaded model. + private: + std::string modelName; + + /// \brief Location of the modeal to load. + private: + std::string modelLocation; + + /// \brief Ignition publisher used to spawn the model. + private: + std::shared_ptr factoryPub; + + /// \brief Connection to pre-render event callback. + private: + ignition::common::ConnectionPtr connection{nullptr}; + + /// \brief Boolean to control we only take the pictures once. + private: + bool takePicture; + + /// \brief File to save translation data. + private: + std::ofstream savingFile; + }; + } // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition +#endif From b7a0ac7dc4ecb985000df8bf5508ef8f0c2af152 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Fri, 25 Feb 2022 10:46:13 +0000 Subject: [PATCH 02/20] background, random joints and some refactoring Signed-off-by: Marco A. Gutierrez --- examples/worlds/model_photo_shoot.sdf | 16 +- src/systems/CMakeLists.txt | 2 +- .../CMakeLists.txt | 0 .../model_photo_shoot/ModelPhotoShoot.cc | 261 ++++++++++++++++++ .../model_photo_shoot/ModelPhotoShoot.hh | 71 +++++ .../model_photoshoot/ModelPhotoShoot.cc | 223 --------------- .../model_photoshoot/ModelPhotoShoot.hh | 84 ------ 7 files changed, 343 insertions(+), 314 deletions(-) rename src/systems/{model_photoshoot => model_photo_shoot}/CMakeLists.txt (100%) create mode 100644 src/systems/model_photo_shoot/ModelPhotoShoot.cc create mode 100644 src/systems/model_photo_shoot/ModelPhotoShoot.hh delete mode 100644 src/systems/model_photoshoot/ModelPhotoShoot.cc delete mode 100644 src/systems/model_photoshoot/ModelPhotoShoot.hh diff --git a/examples/worlds/model_photo_shoot.sdf b/examples/worlds/model_photo_shoot.sdf index 99f1e627cc..d9064bb89f 100644 --- a/examples/worlds/model_photo_shoot.sdf +++ b/examples/worlds/model_photo_shoot.sdf @@ -8,6 +8,7 @@ filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors"> ogre2 + 1, 1, 1 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/barcs_qav500_sensor_config_1 - poses.txt - + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/barcs_qav500_sensor_config_1 + + poses.txt + false + + 2.2 0 0 0 0 -3.14 diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 677db80231..193e76c5e3 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -112,7 +112,7 @@ add_subdirectory(log_video_recorder) add_subdirectory(logical_audio_sensor_plugin) add_subdirectory(logical_camera) add_subdirectory(magnetometer) -add_subdirectory(model_photoshoot) +add_subdirectory(model_photo_shoot) add_subdirectory(mecanum_drive) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) diff --git a/src/systems/model_photoshoot/CMakeLists.txt b/src/systems/model_photo_shoot/CMakeLists.txt similarity index 100% rename from src/systems/model_photoshoot/CMakeLists.txt rename to src/systems/model_photo_shoot/CMakeLists.txt diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc new file mode 100644 index 0000000000..58c330c0f9 --- /dev/null +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -0,0 +1,261 @@ +/* + * 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 "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointPositionLimitsCmd.hh" +#include "ignition/gazebo/components/JointPositionReset.hh" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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 save_data_location = + _sdf->Get("translation_data_file"); + if (save_data_location.empty()) + { + igndbg << "No data location specified, skipping translaiton data" + "saving.\n"; + } + else + { + igndbg << "Saving translation data to: " << save_data_location << std::endl; + this->dataPtr->savingFile.open(save_data_location); + } + + 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); +} + +////////////////////////////////////////////////// +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 (auto joint = joints.begin(); joint != joints.end(); ++joint) + { + + auto jointNameComp = _ecm.Component(*joint); + if (jointNameComp->Data() != "World") + { + // Using the JointAxis component to extract the joint pose limits + auto jointAxisComp = _ecm.Component(*joint); + + float mean = + (jointAxisComp->Data().Lower() + jointAxisComp->Data().Upper()) / 2; + float stdv = (jointAxisComp->Data().Upper() - mean) / 3; + std::normal_distribution distribution(mean, stdv); + float jointPose = distribution(generator); + _ecm.SetComponentData(*joint, + {jointPose}); + if (this->dataPtr->savingFile.is_open()) + { + this->dataPtr->savingFile << jointNameComp->Data() << ": " + << jointPose << 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") + { + + // Set the model pose + ignition::math::AxisAlignedBox bbox = modelVisual->LocalBoundingBox(); + double scaling = 1.0 / bbox.Size().Max(); + // Compute the model translation. + ignition::math::Vector3d trans = -bbox.Center(); + if (this->savingFile.is_open()) { + this->savingFile << "Translation: " << trans << std::endl; + this->savingFile << "Scaling: " << scaling << std::endl; + } + // Normalize the size of the visual + modelVisual->SetWorldPose( + ignition::math::Pose3d(trans.X(), trans.Y(), trans.Z(), 0, 0, 0)); + + ignition::math::Pose3d pose; + // Perspective view + pose.Pos().Set(1.6 / scaling, -1.6 / scaling, 1.2 / scaling); + pose.Rot().Euler(0, IGN_DTOR(30), IGN_DTOR(-225)); + SavePicture(camera, pose, "1.png"); + + // Top view + pose.Pos().Set(0, 0, 2.2 / scaling); + pose.Rot().Euler(0, IGN_DTOR(90), 0); + SavePicture(camera, pose, "2.png"); + + // Front view + pose.Pos().Set(2.2 / scaling, 0, 0); + pose.Rot().Euler(0, 0, IGN_DTOR(-180)); + SavePicture(camera, pose, "3.png"); + + // Side view + pose.Pos().Set(0, 2.2 / scaling, 0); + pose.Rot().Euler(0, 0, IGN_DTOR(-90)); + SavePicture(camera, pose, "4.png"); + + // Back view + pose.Pos().Set(-2.2 / scaling, 0, 0); + 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); + std::string fullName = _fileName; + image.SavePNG(fullName); + + igndbg << "Saved image to [" << fullName << "]" << 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..f524444a82 --- /dev/null +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.hh @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2018 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 +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class ModelPhotoShootPrivate; + + /// \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/model_photoshoot/ModelPhotoShoot.cc b/src/systems/model_photoshoot/ModelPhotoShoot.cc deleted file mode 100644 index 720c3ad46a..0000000000 --- a/src/systems/model_photoshoot/ModelPhotoShoot.cc +++ /dev/null @@ -1,223 +0,0 @@ -/* - * 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 - -using namespace ignition; -using namespace gazebo; -using namespace systems; - -ModelPhotoShoot::ModelPhotoShoot() - : factoryPub(std::make_shared()), - takePicture(true) {} - -ModelPhotoShoot::~ModelPhotoShoot() {} - -void ModelPhotoShoot::Configure(const ignition::gazebo::Entity &_entity, - const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) { - - // Read the configuration values from the sdf - this->modelLocation = _sdf->Get("model_uri"); - if (this->modelLocation.empty()) { - ignerr << "Please specify the model location through the " - "tag in the sdf file.\n"; - return; - } - std::string save_data_location = - _sdf->Get("translation_data_file"); - if (save_data_location.empty()) { - igndbg << "No data location specified, skipping translaiton data" - "saving.\n"; - } else { - igndbg << "Saving translation data to: " << save_data_location << std::endl; - this->savingFile.open(save_data_location); - } - - this->connection = _eventMgr.Connect( - std::bind(&ModelPhotoShoot::PerformPostRenderingOperations, this)); - - LoadModel(_entity, _ecm); -} - -void ModelPhotoShoot::LoadModel( - const ignition::gazebo::Entity &_entity, - ignition::gazebo::EntityComponentManager &_ecm) { - auto world = std::make_shared(_entity); - std::string worldName = world->Name(_ecm); - - sdf::SDFPtr modelSdf(new sdf::SDF()); - - if (!sdf::init(modelSdf)) { - ignerr << "ERROR: SDF parsing the xml failed\n"; - return; - } - - if (!sdf::readFile(this->modelLocation, modelSdf)) { - ignerr << "Error: SDF parsing the xml failed\n"; - return; - } - - sdf::ElementPtr modelElem = modelSdf->Root()->GetElement("model"); - this->modelName = modelElem->Get("name"); - - // Create entity - std::string service = "/world/" + worldName + "/create"; - ignition::msgs::EntityFactory request; - request.set_sdf(modelSdf->ToString()); - request.set_name(this->modelName); - - ignition::msgs::Boolean response; - bool result; - uint32_t timeout = 5000; - bool executed = - this->factoryPub->Request(service, request, timeout, response, result); - if (executed) { - if (result && response.data()) { - igndbg << "Requested creation of entity: " << this->modelName.c_str() - << std::endl; - } else { - ignerr << "Failed request to create entity.\n" - << request.DebugString().c_str(); - } - } else { - ignerr << "Request to create entity from service " - << request.DebugString().c_str() << "timer out ...\n"; - } -} - -void ModelPhotoShoot::PerformPostRenderingOperations() { - igndbg << "PerformPostRenderingOperations\n"; - - ignition::rendering::ScenePtr scene = - ignition::rendering::sceneFromFirstRenderEngine(); - ignition::rendering::VisualPtr modelVisual = - scene->VisualByName(this->modelName); - - ignition::rendering::VisualPtr root = scene->RootVisual(); - - if (modelVisual && takePicture) { - - scene->SetAmbientLight(0.3, 0.3, 0.3); - scene->SetBackgroundColor(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) - std::cout << camera->Name() << std::endl; - if (nullptr != camera && camera->Name() == "photo_shoot::link::camera") { - - // Set the model pose - ignition::math::AxisAlignedBox bbox = modelVisual->LocalBoundingBox(); - ignition::rendering::WireBoxPtr wireBox = scene->CreateWireBox(); - - double scaling = 1.0 / bbox.Size().Max(); - - // Compute the model translation. - ignition::math::Vector3d trans = bbox.Center(); - trans *= -scaling; - - if (savingFile.is_open()) - savingFile << "Translation: " << trans << std::endl; - - // Normalize the size of the visual - // modelVisual->SetLocalScale(ignition::math::Vector3d(scaling, - // scaling, scaling)); - modelVisual->SetWorldPose( - ignition::math::Pose3d(trans.X(), trans.Y(), trans.Z(), 0, 0, 0)); - - ignition::math::Pose3d pose; - - // Perspective view - pose.Pos().Set(1.6, -1.6, 1.2); - pose.Rot().Euler(0, IGN_DTOR(30), IGN_DTOR(-225)); - SavePicture(camera, pose, "1"); - - // Top view - pose.Pos().Set(0, 0, 2.2); - pose.Rot().Euler(0, IGN_DTOR(90), 0); - SavePicture(camera, pose, "2"); - - // Front view - pose.Pos().Set(2.2, 0, 0); - pose.Rot().Euler(0, 0, IGN_DTOR(-180)); - SavePicture(camera, pose, "3"); - - // Side view - pose.Pos().Set(0, 2.2, 0); - pose.Rot().Euler(0, 0, IGN_DTOR(-90)); - SavePicture(camera, pose, "4"); - - // Back view - pose.Pos().Set(-2.2, 0, 0); - pose.Rot().Euler(0, 0, 0); - SavePicture(camera, pose, "5"); - - takePicture = false; - } - } - } -} - -void ModelPhotoShoot::SavePicture(const ignition::rendering::CameraPtr camera, - const ignition::math::Pose3d pose, - const std::string name) { - 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); - std::string fullname = name + ".png"; - image.SavePNG(fullname); - - igndbg << "Saved image to [" << fullname << "]" << std::endl; -} - -IGNITION_ADD_PLUGIN(ModelPhotoShoot, ignition::gazebo::System, - ModelPhotoShoot::ISystemConfigure) - -IGNITION_ADD_PLUGIN_ALIAS(ModelPhotoShoot, - "ignition::gazebo::systems::ModelPhotoShoot") diff --git a/src/systems/model_photoshoot/ModelPhotoShoot.hh b/src/systems/model_photoshoot/ModelPhotoShoot.hh deleted file mode 100644 index fae5f30d9a..0000000000 --- a/src/systems/model_photoshoot/ModelPhotoShoot.hh +++ /dev/null @@ -1,84 +0,0 @@ -/* - * Copyright (C) 2018 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 -#include -#include -#include - -namespace ignition { -namespace gazebo { -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems { - /// \brief System that takes snapshots of an sdf model - class ModelPhotoShoot : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure { - public: - ModelPhotoShoot(); - - public: - ~ModelPhotoShoot(); - - public: - void Configure(const ignition::gazebo::Entity &_id, - const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) final; - - void PerformPostRenderingOperations(); - - void LoadModel(const ignition::gazebo::Entity &_entity, - ignition::gazebo::EntityComponentManager &_ecm); - - void SavePicture(const ignition::rendering::CameraPtr camera, - const ignition::math::Pose3d pose, const std::string name); - - /// \brief Name of the loaded model. - private: - std::string modelName; - - /// \brief Location of the modeal to load. - private: - std::string modelLocation; - - /// \brief Ignition publisher used to spawn the model. - private: - std::shared_ptr factoryPub; - - /// \brief Connection to pre-render event callback. - private: - ignition::common::ConnectionPtr connection{nullptr}; - - /// \brief Boolean to control we only take the pictures once. - private: - bool takePicture; - - /// \brief File to save translation data. - private: - std::ofstream savingFile; - }; - } // namespace systems -} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace gazebo -} // namespace ignition -#endif From 8b12d5c122b23137e74f7f65a04ad8f4dde4ac06 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Tue, 1 Mar 2022 11:52:47 +0000 Subject: [PATCH 03/20] moving camera to center object in image Signed-off-by: Marco A. Gutierrez --- .../model_photo_shoot/ModelPhotoShoot.cc | 86 ++++++++++++------- 1 file changed, 54 insertions(+), 32 deletions(-) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index 58c330c0f9..29a1988227 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -57,6 +58,9 @@ class ignition::gazebo::systems::ModelPhotoShootPrivate /// \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}; @@ -91,7 +95,8 @@ void ModelPhotoShoot::Configure(const ignition::gazebo::Entity &_entity, } else { - igndbg << "Saving translation data to: " << save_data_location << std::endl; + igndbg << "Saving translation data to: " + << save_data_location << std::endl; this->dataPtr->savingFile.open(save_data_location); } @@ -107,6 +112,9 @@ void ModelPhotoShoot::Configure(const ignition::gazebo::Entity &_entity, 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); } ////////////////////////////////////////////////// @@ -117,28 +125,36 @@ void ModelPhotoShoot::PreUpdate( if (this->dataPtr->randomPoses) { std::vector joints = this->dataPtr->model->Joints(_ecm); - unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); + unsigned seed = + std::chrono::system_clock::now().time_since_epoch().count(); std::default_random_engine generator(seed); for (auto joint = joints.begin(); joint != joints.end(); ++joint) { - auto jointNameComp = _ecm.Component(*joint); - if (jointNameComp->Data() != "World") + if (jointNameComp && jointNameComp->Data() != "World") { // Using the JointAxis component to extract the joint pose limits auto jointAxisComp = _ecm.Component(*joint); - - float mean = - (jointAxisComp->Data().Lower() + jointAxisComp->Data().Upper()) / 2; - float stdv = (jointAxisComp->Data().Upper() - mean) / 3; - std::normal_distribution distribution(mean, stdv); - float jointPose = distribution(generator); - _ecm.SetComponentData(*joint, - {jointPose}); - if (this->dataPtr->savingFile.is_open()) + if (jointAxisComp) + { + float mean = + (jointAxisComp->Data().Lower() + + jointAxisComp->Data().Upper()) / 2; + float stdv = (jointAxisComp->Data().Upper() - mean) / 3; + std::normal_distribution distribution(mean, stdv); + float jointPose = distribution(generator); + _ecm.SetComponentData(*joint, + {jointPose}); + if (this->dataPtr->savingFile.is_open()) + { + this->dataPtr->savingFile << jointNameComp->Data() << ": " + << jointPose << std::endl; + } + } + else { - this->dataPtr->savingFile << jointNameComp->Data() << ": " - << jointPose << std::endl; + igndbg << "No jointAxisComp found, ignoring joint: " << + jointNameComp->Data() << std::endl; } } } @@ -150,7 +166,6 @@ void ModelPhotoShoot::PreUpdate( ////////////////////////////////////////////////// void ModelPhotoShootPrivate::PerformPostRenderingOperations() { - ignition::rendering::ScenePtr scene = ignition::rendering::sceneFromFirstRenderEngine(); ignition::rendering::VisualPtr modelVisual = @@ -160,7 +175,6 @@ void ModelPhotoShootPrivate::PerformPostRenderingOperations() if (modelVisual && this->takePicture) { - scene->SetAmbientLight(0.3, 0.3, 0.3); // create directional light @@ -184,43 +198,51 @@ void ModelPhotoShootPrivate::PerformPostRenderingOperations() scene->NodeByIndex(i)); if (nullptr != camera && camera->Name() == "photo_shoot::link::camera") { - - // Set the model pose + // 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(); - // Compute the model translation. - ignition::math::Vector3d trans = -bbox.Center(); + ignition::math::Vector3d bboxCenter = bbox.Center(); + ignition::math::Vector3d translation = + bboxCenter + this->modelPose3D.Pos(); if (this->savingFile.is_open()) { - this->savingFile << "Translation: " << trans << std::endl; + this->savingFile << "Translation: " << translation << std::endl; this->savingFile << "Scaling: " << scaling << std::endl; } - // Normalize the size of the visual - modelVisual->SetWorldPose( - ignition::math::Pose3d(trans.X(), trans.Y(), trans.Z(), 0, 0, 0)); ignition::math::Pose3d pose; // Perspective view - pose.Pos().Set(1.6 / scaling, -1.6 / scaling, 1.2 / scaling); + pose.Pos().Set(1.6 / scaling + translation.X(), + -1.6 / scaling + translation.Y(), + 1.2 / scaling + translation.Z()+0.078); pose.Rot().Euler(0, IGN_DTOR(30), IGN_DTOR(-225)); SavePicture(camera, pose, "1.png"); // Top view - pose.Pos().Set(0, 0, 2.2 / scaling); + pose.Pos().Set(0 + translation.X(), + 0 + translation.Y(), + 2.2 / scaling + translation.Z()+0.078); pose.Rot().Euler(0, IGN_DTOR(90), 0); SavePicture(camera, pose, "2.png"); // Front view - pose.Pos().Set(2.2 / scaling, 0, 0); + pose.Pos().Set(2.2 / scaling + translation.X(), + 0 + translation.Y(), + 0 + translation.Z()+0.078); pose.Rot().Euler(0, 0, IGN_DTOR(-180)); SavePicture(camera, pose, "3.png"); // Side view - pose.Pos().Set(0, 2.2 / scaling, 0); + pose.Pos().Set(0 + translation.X(), + 2.2 / scaling + translation.Y(), + 0 + translation.Z()+0.078); pose.Rot().Euler(0, 0, IGN_DTOR(-90)); SavePicture(camera, pose, "4.png"); // Back view - pose.Pos().Set(-2.2 / scaling, 0, 0); + pose.Pos().Set(-2.2 / scaling + translation.X(), + 0 + translation.Y(), + 0 + translation.Z()+0.078); pose.Rot().Euler(0, 0, 0); SavePicture(camera, pose, "5.png"); @@ -238,13 +260,13 @@ void ModelPhotoShootPrivate::SavePicture( { 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 formatStr = + ignition::rendering::PixelUtil::Name(_camera->ImageFormat()); auto format = ignition::common::Image::ConvertPixelFormat(formatStr); image.SetFromData(cameraImage.Data(), width, height, format); std::string fullName = _fileName; From 43dc07d4818ed82209380f5c18bcf0d6205292bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Thu, 3 Mar 2022 09:36:24 +0800 Subject: [PATCH 04/20] Update src/systems/model_photo_shoot/ModelPhotoShoot.cc Co-authored-by: Addisu Z. Taddese Signed-off-by: Marco A. Gutierrez --- src/systems/model_photo_shoot/ModelPhotoShoot.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index 29a1988227..f2a2068a97 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -128,7 +128,7 @@ void ModelPhotoShoot::PreUpdate( unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); std::default_random_engine generator(seed); - for (auto joint = joints.begin(); joint != joints.end(); ++joint) + for (const auto &joint = joints) { auto jointNameComp = _ecm.Component(*joint); if (jointNameComp && jointNameComp->Data() != "World") From d16f688d300da7f8b39f1f0a499db7c9eb9f3681 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Thu, 3 Mar 2022 11:53:23 +0000 Subject: [PATCH 05/20] several fixes to address PR suggestions Signed-off-by: Marco A. Gutierrez --- examples/worlds/model_photo_shoot.sdf | 1 - .../model_photo_shoot/ModelPhotoShoot.cc | 39 ++++++++----------- .../model_photo_shoot/ModelPhotoShoot.hh | 27 +++++++++++++ 3 files changed, 43 insertions(+), 24 deletions(-) diff --git a/examples/worlds/model_photo_shoot.sdf b/examples/worlds/model_photo_shoot.sdf index d9064bb89f..4c6ec1360d 100644 --- a/examples/worlds/model_photo_shoot.sdf +++ b/examples/worlds/model_photo_shoot.sdf @@ -28,7 +28,6 @@ - 2.2 0 0 0 0 -3.14 0 0 0 0 0 0 diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index f2a2068a97..8fdc0330c7 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -23,19 +23,15 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" #include "ignition/gazebo/components/JointPosition.hh" -#include "ignition/gazebo/components/JointPositionLimitsCmd.hh" #include "ignition/gazebo/components/JointPositionReset.hh" #include -#include #include #include #include -#include #include #include #include #include -#include using namespace ignition; using namespace gazebo; @@ -86,9 +82,9 @@ void ModelPhotoShoot::Configure(const ignition::gazebo::Entity &_entity, ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &_eventMgr) { - std::string save_data_location = + std::string saveDataLocation = _sdf->Get("translation_data_file"); - if (save_data_location.empty()) + if (saveDataLocation.empty()) { igndbg << "No data location specified, skipping translaiton data" "saving.\n"; @@ -96,8 +92,8 @@ void ModelPhotoShoot::Configure(const ignition::gazebo::Entity &_entity, else { igndbg << "Saving translation data to: " - << save_data_location << std::endl; - this->dataPtr->savingFile.open(save_data_location); + << saveDataLocation << std::endl; + this->dataPtr->savingFile.open(saveDataLocation); } if (_sdf->HasElement("random_joints_pose")) @@ -128,22 +124,19 @@ void ModelPhotoShoot::PreUpdate( unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); std::default_random_engine generator(seed); - for (const auto &joint = joints) + for (const auto &joint : joints) { - auto jointNameComp = _ecm.Component(*joint); - if (jointNameComp && jointNameComp->Data() != "World") + auto jointNameComp = _ecm.Component(joint); + if (jointNameComp) { // Using the JointAxis component to extract the joint pose limits - auto jointAxisComp = _ecm.Component(*joint); + auto jointAxisComp = _ecm.Component(joint); if (jointAxisComp) { - float mean = - (jointAxisComp->Data().Lower() + - jointAxisComp->Data().Upper()) / 2; - float stdv = (jointAxisComp->Data().Upper() - mean) / 3; - std::normal_distribution distribution(mean, stdv); + std::uniform_real_distribution distribution( + jointAxisComp->Data().Lower(), jointAxisComp->Data().Upper()); float jointPose = distribution(generator); - _ecm.SetComponentData(*joint, + _ecm.SetComponentData(joint, {jointPose}); if (this->dataPtr->savingFile.is_open()) { @@ -214,35 +207,35 @@ void ModelPhotoShootPrivate::PerformPostRenderingOperations() // Perspective view pose.Pos().Set(1.6 / scaling + translation.X(), -1.6 / scaling + translation.Y(), - 1.2 / scaling + translation.Z()+0.078); + 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()+0.078); + 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()+0.078); + 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()+0.078); + 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()+0.078); + 0 + translation.Z()); pose.Rot().Euler(0, 0, 0); SavePicture(camera, pose, "5.png"); diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.hh b/src/systems/model_photo_shoot/ModelPhotoShoot.hh index f524444a82..8b1a3d0a14 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.hh +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.hh @@ -39,6 +39,33 @@ 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, + /// sideand 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] + /// + /// ## 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, From 4c64557fefdb821e74572e087dfbf746ab8400c2 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Fri, 4 Mar 2022 08:31:10 +0000 Subject: [PATCH 06/20] adding tutorials and fixes to PR comments Signed-off-by: Marco A. Gutierrez --- examples/worlds/model_photo_shoot.sdf | 18 +-- .../model_photo_shoot/ModelPhotoShoot.cc | 15 +-- .../model_photo_shoot/ModelPhotoShoot.hh | 12 +- tutorials.md.in | 1 + tutorials/model_photo_shoot.md | 105 ++++++++++++++++++ 5 files changed, 128 insertions(+), 23 deletions(-) create mode 100644 tutorials/model_photo_shoot.md diff --git a/examples/worlds/model_photo_shoot.sdf b/examples/worlds/model_photo_shoot.sdf index 4c6ec1360d..9b35eb8903 100644 --- a/examples/worlds/model_photo_shoot.sdf +++ b/examples/worlds/model_photo_shoot.sdf @@ -1,3 +1,11 @@ + + ogre2 1, 1, 1 - - - - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/barcs_qav500_sensor_config_1 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Robonaut diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index 8fdc0330c7..e5ef3931c9 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -20,17 +20,19 @@ #include #include +#include #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" -#include "ignition/gazebo/components/JointPosition.hh" #include "ignition/gazebo/components/JointPositionReset.hh" -#include #include #include +#include #include +#include #include +#include #include -#include +#include #include using namespace ignition; @@ -146,7 +148,7 @@ void ModelPhotoShoot::PreUpdate( } else { - igndbg << "No jointAxisComp found, ignoring joint: " << + ignerr << "No jointAxisComp found, ignoring joint: " << jointNameComp->Data() << std::endl; } } @@ -262,10 +264,9 @@ void ModelPhotoShootPrivate::SavePicture( ignition::rendering::PixelUtil::Name(_camera->ImageFormat()); auto format = ignition::common::Image::ConvertPixelFormat(formatStr); image.SetFromData(cameraImage.Data(), width, height, format); - std::string fullName = _fileName; - image.SavePNG(fullName); + image.SavePNG(_fileName); - igndbg << "Saved image to [" << fullName << "]" << std::endl; + igndbg << "Saved image to [" << _fileName << "]" << std::endl; } IGNITION_ADD_PLUGIN(ModelPhotoShoot, ignition::gazebo::System, diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.hh b/src/systems/model_photo_shoot/ModelPhotoShoot.hh index 8b1a3d0a14..79b0f17f47 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.hh +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -21,12 +21,7 @@ #include -#include #include -#include -#include -#include -#include namespace ignition { @@ -41,7 +36,7 @@ namespace systems /// \brief This plugin is a port of the old ModelPropShop plugin from gazebo /// classic. It generates 5 pictures of a model: perspective, top, front, - /// sideand back. It can do it using the default position or moving the joint + /// 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. /// @@ -51,6 +46,9 @@ namespace systems /// - - 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 diff --git a/tutorials.md.in b/tutorials.md.in index a035bb9f84..95b84058fa 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -34,6 +34,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. **Migration from Gazebo classic** diff --git a/tutorials/model_photo_shoot.md b/tutorials/model_photo_shoot.md new file mode 100644 index 0000000000..2551babaf1 --- /dev/null +++ b/tutorials/model_photo_shoot.md @@ -0,0 +1,105 @@ +\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 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. From ebcf122df4d81aa06e2a7ced74633873a4008b3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Fri, 4 Mar 2022 16:32:35 +0800 Subject: [PATCH 07/20] Update src/systems/model_photo_shoot/ModelPhotoShoot.cc Co-authored-by: Addisu Z. Taddese Signed-off-by: Marco A. Gutierrez --- src/systems/model_photo_shoot/ModelPhotoShoot.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index e5ef3931c9..d2fd50a5e1 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -251,7 +251,7 @@ void ModelPhotoShootPrivate::PerformPostRenderingOperations() void ModelPhotoShootPrivate::SavePicture( const ignition::rendering::CameraPtr _camera, const ignition::math::Pose3d _pose, - const std::string _fileName) const + const std::string &_fileName) const { unsigned int width = _camera->ImageWidth(); unsigned int height = _camera->ImageHeight(); From 00dbb5c17094c3f51391bbc3366f30438dbe42f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Fri, 4 Mar 2022 16:32:52 +0800 Subject: [PATCH 08/20] Update src/systems/model_photo_shoot/ModelPhotoShoot.cc Co-authored-by: Addisu Z. Taddese Signed-off-by: Marco A. Gutierrez --- src/systems/model_photo_shoot/ModelPhotoShoot.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index d2fd50a5e1..e08204c2ab 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -250,7 +250,7 @@ void ModelPhotoShootPrivate::PerformPostRenderingOperations() ////////////////////////////////////////////////// void ModelPhotoShootPrivate::SavePicture( const ignition::rendering::CameraPtr _camera, - const ignition::math::Pose3d _pose, + const ignition::math::Pose3d &_pose, const std::string &_fileName) const { unsigned int width = _camera->ImageWidth(); From 3f1e5a40596021811f0642fa2dcd3e32486f5be3 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Fri, 4 Mar 2022 09:41:54 +0000 Subject: [PATCH 09/20] fixes to github commits Signed-off-by: Marco A. Gutierrez --- src/systems/model_photo_shoot/ModelPhotoShoot.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index e08204c2ab..3037bdbe72 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -47,8 +47,8 @@ class ignition::gazebo::systems::ModelPhotoShootPrivate /// \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; + const ignition::math::Pose3d &_pose, + const std::string &_fileName) const; /// \brief Name of the loaded model. public: std::string modelName; From cd6f7c6d01331a397b55e1ecca98ed334f40b1dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marco=20A=2E=20Guti=C3=A9rrez?= Date: Mon, 7 Mar 2022 11:37:20 +0800 Subject: [PATCH 10/20] Update examples/worlds/model_photo_shoot.sdf Co-authored-by: Addisu Z. Taddese Signed-off-by: Marco A. Gutierrez --- examples/worlds/model_photo_shoot.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/worlds/model_photo_shoot.sdf b/examples/worlds/model_photo_shoot.sdf index 9b35eb8903..b6666e867f 100644 --- a/examples/worlds/model_photo_shoot.sdf +++ b/examples/worlds/model_photo_shoot.sdf @@ -3,7 +3,7 @@ Ignition Gazebo Model Photo Shoot plugin demo This will take perspective, top, front, and both sides pictures of the model: - ign gazebo -s -r -v 4 --iterations 50 model_photo_shoot.sdf + ign gazebo -s -r -v 4 \-\-iterations 50 model_photo_shoot.sdf --> From a712a0e2b6cd6f6c77f25250018142de653a4d11 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Mon, 7 Mar 2022 07:22:20 +0000 Subject: [PATCH 11/20] adding jointType checks Signed-off-by: Marco A. Gutierrez --- .../model_photo_shoot/ModelPhotoShoot.cc | 50 ++++++++++++++----- 1 file changed, 37 insertions(+), 13 deletions(-) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index 3037bdbe72..45eeae177b 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -23,6 +23,7 @@ #include #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointType.hh" #include "ignition/gazebo/components/JointPositionReset.hh" #include #include @@ -131,27 +132,50 @@ void ModelPhotoShoot::PreUpdate( auto jointNameComp = _ecm.Component(joint); if (jointNameComp) { - // Using the JointAxis component to extract the joint pose limits - auto jointAxisComp = _ecm.Component(joint); - if (jointAxisComp) + auto jointType = _ecm.Component(joint)->Data(); + if (jointType != sdf::JointType::FIXED) { - std::uniform_real_distribution distribution( - jointAxisComp->Data().Lower(), jointAxisComp->Data().Upper()); - float jointPose = distribution(generator); - _ecm.SetComponentData(joint, - {jointPose}); - if (this->dataPtr->savingFile.is_open()) + if (jointType == sdf::JointType::REVOLUTE || + jointType == sdf::JointType::PRISMATIC) { - this->dataPtr->savingFile << jointNameComp->Data() << ": " - << jointPose << std::endl; + // 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}); + if (this->dataPtr->savingFile.is_open()) + { + this->dataPtr->savingFile << jointNameComp->Data() << ": " + << 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 { - ignerr << "No jointAxisComp found, ignoring joint: " << - jointNameComp->Data() << std::endl; + 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; From b0dc697506b3f3ba2d1ab1d12fc27acb5295df7c Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Mon, 7 Mar 2022 09:47:26 +0000 Subject: [PATCH 12/20] lint fixes Signed-off-by: Marco A. Gutierrez --- .../model_photo_shoot/ModelPhotoShoot.cc | 26 ++++++++++--------- .../model_photo_shoot/ModelPhotoShoot.hh | 2 +- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index 45eeae177b..ac1ac7cdd3 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -21,21 +21,22 @@ #include #include -#include "ignition/gazebo/components/Joint.hh" -#include "ignition/gazebo/components/JointAxis.hh" -#include "ignition/gazebo/components/JointType.hh" -#include "ignition/gazebo/components/JointPositionReset.hh" -#include -#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/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; @@ -143,10 +144,11 @@ void ModelPhotoShoot::PreUpdate( if (jointAxisComp) { std::uniform_real_distribution distribution( - jointAxisComp->Data().Lower(), jointAxisComp->Data().Upper()); + jointAxisComp->Data().Lower(), + jointAxisComp->Data().Upper()); double jointPose = distribution(generator); - _ecm.SetComponentData(joint, - {jointPose}); + _ecm.SetComponentData( + joint, {jointPose}); if (this->dataPtr->savingFile.is_open()) { this->dataPtr->savingFile << jointNameComp->Data() << ": " diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.hh b/src/systems/model_photo_shoot/ModelPhotoShoot.hh index 79b0f17f47..b059f2486e 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.hh +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.hh @@ -21,7 +21,7 @@ #include -#include +#include "ignition/gazebo/System.hh" namespace ignition { From c0c14b0609452e7e2e9f0d87ec9c651bb1db8f6d Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Mon, 7 Mar 2022 10:11:48 +0000 Subject: [PATCH 13/20] adding exmple world location Signed-off-by: Marco A. Gutierrez --- tutorials/model_photo_shoot.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tutorials/model_photo_shoot.md b/tutorials/model_photo_shoot.md index 2551babaf1..7a1874e9a5 100644 --- a/tutorials/model_photo_shoot.md +++ b/tutorials/model_photo_shoot.md @@ -4,7 +4,8 @@ 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 by running the following command: +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 From 2ed80f327986369fb855d37233330063385e4d13 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 23 Mar 2022 10:32:10 +0000 Subject: [PATCH 14/20] adding model photo shoot test Signed-off-by: Marco A. Gutierrez --- examples/worlds/model_photo_shoot.sdf | 1 + test/integration/CMakeLists.txt | 6 + test/integration/model_photo_shoot.cc | 265 ++++++++++++++++++++++++++ 3 files changed, 272 insertions(+) create mode 100644 test/integration/model_photo_shoot.cc diff --git a/examples/worlds/model_photo_shoot.sdf b/examples/worlds/model_photo_shoot.sdf index b6666e867f..84ec5b913d 100644 --- a/examples/worlds/model_photo_shoot.sdf +++ b/examples/worlds/model_photo_shoot.sdf @@ -8,6 +8,7 @@ --> + 0 0 0 diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index c44d3fcfc4..e09216b2ea 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -34,6 +34,7 @@ set(tests logical_audio_sensor_plugin.cc magnetometer_system.cc model.cc + model_photo_shoot.cc multicopter.cc multiple_servers.cc nested_model_physics.cc @@ -125,5 +126,10 @@ 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 + 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/model_photo_shoot.cc b/test/integration/model_photo_shoot.cc new file mode 100644 index 0000000000..05899a7627 --- /dev/null +++ b/test/integration/model_photo_shoot.cc @@ -0,0 +1,265 @@ +/* + * 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 "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" +#include "ignition/gazebo/rendering/Events.hh" + +#include +#include +#include +#include +#include +#include + +#include "helpers/UniqueTestDirectoryEnv.hh" +#include "plugins/MockSystem.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 &_filename, + const std::string &_testFilename) +{ + ignition::common::Image image(std::string(PROJECT_BINARY_PATH) + + "/test/integration/" + _filename); + ignition::common::Image testImage(std::string(PROJECT_BINARY_PATH) + + "/test/integration/" + _testFilename); + 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; + image.Data(&imageData, dataLength); + unsigned int testDataLenght; + unsigned char *testImageData; + 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(_filename.c_str()), 0); + EXPECT_EQ(remove(_testFilename.c_str()), 0); +} + +/// \brief Test ModelPhotoShootTest system. +class ModelPhotoShootTest : public InternalFixture<::testing::Test> +{ + /// \brief PostRender callback. + public: void OnPostRender() + { + if (!testPicturesTaken) + { + 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"); + } + } + testPicturesTaken = true; + } + } + + /// \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::ifstream poseFile (_poseFile); + 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; + } + if (word == "Scaling:") + { + getline( iss, word, ' ' ); + this->scaling = std::stod(word); + break; + } + } + } + poseFile.close(); + EXPECT_EQ(remove(_poseFile.c_str()), 0); + } + + // Documentation inherited + protected: void SetUp() override + { + InternalFixture::SetUp(); + + auto plugin = sm.LoadPlugin("libMockSystem.so", + "ignition::gazebo::MockSystem", + nullptr); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + /// \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. + this->serverConfig.SetResourceCache(test::UniqueTestDirectoryEnv::Path()); + this->serverConfig.SetSdfFile( + common::joinPaths(PROJECT_SOURCE_PATH, _sdfWorld)); + + this->server = std::make_unique(this->serverConfig); + EXPECT_FALSE(this->server->Running()); + EXPECT_FALSE(*this->server->Running(0)); + + this->server->SetUpdatePeriod(1ns); + this->server->Run(true, 50, false); + + common::ConnectionPtr postRenderConn; + + // A pointer to the ecm. This will be valid once we run the mock system. + gazebo::EntityComponentManager *ecm = nullptr; + this->mockSystem->preUpdateCallback = + [&ecm](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }; + this->mockSystem->configureCallback = + [&](const gazebo::Entity &, + const std::shared_ptr &, + gazebo::EntityComponentManager &, + gazebo::EventManager &_eventMgr) + { + postRenderConn = _eventMgr.Connect( + std::bind(&ModelPhotoShootTest::OnPostRender, this)); + }; + this->LoadPoseValues(); + this->server->AddSystem(this->systemPtr); + + while (!testPicturesTaken) + { + this->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"); + } + + public: ServerConfig serverConfig; + public: std::unique_ptr server; + public: ignition::gazebo::SystemPluginPtr systemPtr; + public: gazebo::MockSystem *mockSystem; + + private: gazebo::SystemLoader sm; + private: bool testPicturesTaken{false}; + private: double scaling; + private: ignition::math::Vector3d translation; +}; + +// Test the Model Photo Shoot plugin on the example world. +TEST_F(ModelPhotoShootTest, ModelPhotoShootExampleWorld) +{ + this->modelPhotoShootTestCmd("examples/worlds/model_photo_shoot.sdf"); +} From 8899e20f39b4231e17a29a3d622d28ea540754be Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 30 Mar 2022 09:20:28 +0000 Subject: [PATCH 15/20] adding random joints test Signed-off-by: Marco A. Gutierrez --- test/integration/CMakeLists.txt | 8 +- ..._photo_shoot.cc => ModelPhotoShootTest.hh} | 178 ++++++++++-------- .../model_photo_shoot_default_joints.cc | 8 + .../model_photo_shoot_random_joints.cc | 8 + .../model_photo_shoot_random_joints.sdf | 55 ++++++ 5 files changed, 179 insertions(+), 78 deletions(-) rename test/integration/{model_photo_shoot.cc => ModelPhotoShootTest.hh} (61%) create mode 100644 test/integration/model_photo_shoot_default_joints.cc create mode 100644 test/integration/model_photo_shoot_random_joints.cc create mode 100644 test/worlds/model_photo_shoot_random_joints.sdf diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index e09216b2ea..4b2714d636 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -34,7 +34,8 @@ set(tests logical_audio_sensor_plugin.cc magnetometer_system.cc model.cc - model_photo_shoot.cc + model_photo_shoot_default_joints.cc + model_photo_shoot_random_joints.cc multicopter.cc multiple_servers.cc nested_model_physics.cc @@ -127,7 +128,10 @@ target_link_libraries(INTEGRATION_tracked_vehicle_system ignition-plugin${IGN_PLUGIN_VER}::loader ) -target_link_libraries(INTEGRATION_model_photo_shoot +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} ) diff --git a/test/integration/model_photo_shoot.cc b/test/integration/ModelPhotoShootTest.hh similarity index 61% rename from test/integration/model_photo_shoot.cc rename to test/integration/ModelPhotoShootTest.hh index 05899a7627..a1fff88226 100644 --- a/test/integration/model_photo_shoot.cc +++ b/test/integration/ModelPhotoShootTest.hh @@ -19,11 +19,19 @@ #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 @@ -32,8 +40,8 @@ #include #include + #include "helpers/UniqueTestDirectoryEnv.hh" -#include "plugins/MockSystem.hh" #include "helpers/EnvTestFixture.hh" using namespace ignition; @@ -65,30 +73,33 @@ void SavePicture(const ignition::rendering::CameraPtr _camera, /// \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 &_filename, - const std::string &_testFilename) +void testImages(const std::string &_imageFile, + const std::string &_testImageFile) { - ignition::common::Image image(std::string(PROJECT_BINARY_PATH) + - "/test/integration/" + _filename); - ignition::common::Image testImage(std::string(PROJECT_BINARY_PATH) + - "/test/integration/" + _testFilename); + std::string imageFilePath = common::joinPaths( + std::string(PROJECT_BINARY_PATH), "test", "integration", _imageFile); + ignition::common::Image image(imageFilePath); + std::string testImageFilePath = common::joinPaths( + std::string(PROJECT_BINARY_PATH), "test", "integration", _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; + unsigned char *imageData = nullptr; image.Data(&imageData, dataLength); unsigned int testDataLenght; - unsigned char *testImageData; + 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(_filename.c_str()), 0); - EXPECT_EQ(remove(_testFilename.c_str()), 0); + EXPECT_EQ(remove(imageFilePath.c_str()), 0); + EXPECT_EQ(remove(testImageFilePath.c_str()), 0); } /// \brief Test ModelPhotoShootTest system. @@ -97,7 +108,7 @@ class ModelPhotoShootTest : public InternalFixture<::testing::Test> /// \brief PostRender callback. public: void OnPostRender() { - if (!testPicturesTaken) + if (takeTestPics) { ignition::rendering::ScenePtr scene = ignition::rendering::sceneFromFirstRenderEngine(); @@ -143,7 +154,7 @@ class ModelPhotoShootTest : public InternalFixture<::testing::Test> SavePicture(camera, pose, "5_test.png"); } } - testPicturesTaken = true; + takeTestPics = false; } } @@ -151,7 +162,9 @@ class ModelPhotoShootTest : public InternalFixture<::testing::Test> /// \param[in] _poseFile File containing the generated poses. protected: void LoadPoseValues(std::string _poseFile = "poses.txt") { - std::ifstream poseFile (_poseFile); + std::string poseFilePath = common::joinPaths( + std::string(PROJECT_BINARY_PATH), "test", "integration", _poseFile); + std::ifstream poseFile (poseFilePath); std::string line; ASSERT_TRUE(poseFile.is_open()); while (getline(poseFile, line) ) @@ -172,72 +185,94 @@ class ModelPhotoShootTest : public InternalFixture<::testing::Test> this->translation = {tr_x, tr_y, tr_z}; break; } - if (word == "Scaling:") + else { - getline( iss, word, ' ' ); - this->scaling = std::stod(word); - break; + 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(_poseFile.c_str()), 0); - } - - // Documentation inherited - protected: void SetUp() override - { - InternalFixture::SetUp(); - - auto plugin = sm.LoadPlugin("libMockSystem.so", - "ignition::gazebo::MockSystem", - nullptr); - EXPECT_TRUE(plugin.has_value()); - this->systemPtr = plugin.value(); - this->mockSystem = static_cast( - systemPtr->QueryInterface()); + 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) + protected: void ModelPhotoShootTestCmd(const std::string _sdfWorld) { // First run of the server generating images through the plugin. - this->serverConfig.SetResourceCache(test::UniqueTestDirectoryEnv::Path()); - this->serverConfig.SetSdfFile( - common::joinPaths(PROJECT_SOURCE_PATH, _sdfWorld)); - - this->server = std::make_unique(this->serverConfig); - EXPECT_FALSE(this->server->Running()); - EXPECT_FALSE(*this->server->Running(0)); - - this->server->SetUpdatePeriod(1ns); - this->server->Run(true, 50, false); + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + _sdfWorld)); + fixture.Server()->SetUpdatePeriod(1ns); common::ConnectionPtr postRenderConn; - - // A pointer to the ecm. This will be valid once we run the mock system. - gazebo::EntityComponentManager *ecm = nullptr; - this->mockSystem->preUpdateCallback = - [&ecm](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) - { - ecm = &_ecm; - }; - this->mockSystem->configureCallback = - [&](const gazebo::Entity &, - const std::shared_ptr &, - gazebo::EntityComponentManager &, - gazebo::EventManager &_eventMgr) - { - postRenderConn = _eventMgr.Connect( + 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(); - this->server->AddSystem(this->systemPtr); - while (!testPicturesTaken) + fixture.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) { - this->server->Run(true, 1, false); + 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()); + } + } + 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"); @@ -247,19 +282,10 @@ class ModelPhotoShootTest : public InternalFixture<::testing::Test> testImages("5.png", "5_test.png"); } - public: ServerConfig serverConfig; - public: std::unique_ptr server; - public: ignition::gazebo::SystemPluginPtr systemPtr; - public: gazebo::MockSystem *mockSystem; - - private: gazebo::SystemLoader sm; - private: bool testPicturesTaken{false}; + 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; }; - -// Test the Model Photo Shoot plugin on the example world. -TEST_F(ModelPhotoShootTest, ModelPhotoShootExampleWorld) -{ - this->modelPhotoShootTestCmd("examples/worlds/model_photo_shoot.sdf"); -} 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..3a47ff8236 --- /dev/null +++ b/test/integration/model_photo_shoot_default_joints.cc @@ -0,0 +1,8 @@ +#include "ModelPhotoShootTest.hh" + +// Test the Model Photo Shoot plugin on the example world. +TEST_F(ModelPhotoShootTest, ModelPhotoShootDefaultJoints) +{ + this->ModelPhotoShootTestCmd( + "examples/worlds/model_photo_shoot.sdf"); +} 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..88b483d028 --- /dev/null +++ b/test/integration/model_photo_shoot_random_joints.cc @@ -0,0 +1,8 @@ +#include "ModelPhotoShootTest.hh" + +// Test the Model Photo Shoot plugin on the example world. +TEST_F(ModelPhotoShootTest, ModelPhotoShootRandomJoints) +{ + this->ModelPhotoShootTestCmd( + "test/worlds/model_photo_shoot_random_joints.sdf"); +} 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 + + + From bb3d3914f2a8406378a282bf212fee7e0aaed75d Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 30 Mar 2022 09:33:38 +0000 Subject: [PATCH 16/20] adding copyright message Signed-off-by: Marco A. Gutierrez --- .../model_photo_shoot_default_joints.cc | 17 +++++++++++++++++ .../model_photo_shoot_random_joints.cc | 17 +++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/test/integration/model_photo_shoot_default_joints.cc b/test/integration/model_photo_shoot_default_joints.cc index 3a47ff8236..dc53cc9380 100644 --- a/test/integration/model_photo_shoot_default_joints.cc +++ b/test/integration/model_photo_shoot_default_joints.cc @@ -1,3 +1,20 @@ +/* + * 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" // Test the Model Photo Shoot plugin on the example world. diff --git a/test/integration/model_photo_shoot_random_joints.cc b/test/integration/model_photo_shoot_random_joints.cc index 88b483d028..0c4fb56624 100644 --- a/test/integration/model_photo_shoot_random_joints.cc +++ b/test/integration/model_photo_shoot_random_joints.cc @@ -1,3 +1,20 @@ +/* + * 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" // Test the Model Photo Shoot plugin on the example world. From e0965ca410a73c73f3fb5bf14243370ad60a1cad Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 30 Mar 2022 15:42:45 +0000 Subject: [PATCH 17/20] plugin adds JointPosition component with position Signed-off-by: Marco A. Gutierrez --- src/systems/model_photo_shoot/ModelPhotoShoot.cc | 12 ++++++++++++ test/integration/ModelPhotoShootTest.hh | 13 ++++++++++--- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index ac1ac7cdd3..ae3922d678 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -30,6 +30,7 @@ #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" @@ -149,9 +150,20 @@ void ModelPhotoShoot::PreUpdate( double jointPose = distribution(generator); _ecm.SetComponentData( joint, {jointPose}); + + // 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()); + _ecm.SetComponentData( + joint, {jointPose}); + } + if (this->dataPtr->savingFile.is_open()) { this->dataPtr->savingFile << jointNameComp->Data() << ": " + << std::setprecision(17) << jointPose << std::endl; } } diff --git a/test/integration/ModelPhotoShootTest.hh b/test/integration/ModelPhotoShootTest.hh index a1fff88226..6cbadf6553 100644 --- a/test/integration/ModelPhotoShootTest.hh +++ b/test/integration/ModelPhotoShootTest.hh @@ -18,8 +18,11 @@ #include #include +#include #include #include +#include +#include #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Model.hh" @@ -255,11 +258,15 @@ class ModelPhotoShootTest : public InternalFixture<::testing::Test> { auto jointType = _ecm.Component (joint)->Data(); - ASSERT_TRUE(jointType == sdf::JointType::REVOLUTE || + 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()); + 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; From 40796db25fe8b000c702b0e718726669cf5b1295 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Wed, 30 Mar 2022 15:59:30 +0000 Subject: [PATCH 18/20] fixed comment Signed-off-by: Marco A. Gutierrez --- src/systems/model_photo_shoot/ModelPhotoShoot.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc index ae3922d678..31f6373e1c 100644 --- a/src/systems/model_photo_shoot/ModelPhotoShoot.cc +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -151,8 +151,7 @@ void ModelPhotoShoot::PreUpdate( _ecm.SetComponentData( joint, {jointPose}); - // Create a JointPosition component if it doesn't exist. This signals - // physics system to populate the component + // Create a JointPosition component if it doesn't exist. if (nullptr == _ecm.Component(joint)) { _ecm.CreateComponent(joint, components::JointPosition()); From 59a88707ba093210896fe0dce9b8185ead312239 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 30 Mar 2022 14:11:45 -0500 Subject: [PATCH 19/20] Enable test only on Linux Signed-off-by: Addisu Z. Taddese --- test/integration/ModelPhotoShootTest.hh | 5 +++++ test/integration/model_photo_shoot_default_joints.cc | 5 ++++- test/integration/model_photo_shoot_random_joints.cc | 5 ++++- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/test/integration/ModelPhotoShootTest.hh b/test/integration/ModelPhotoShootTest.hh index 6cbadf6553..f5197b1ebd 100644 --- a/test/integration/ModelPhotoShootTest.hh +++ b/test/integration/ModelPhotoShootTest.hh @@ -15,6 +15,9 @@ * */ +#ifndef IGNITION_GAZEBO_TEST_INTEGRATION_MODELPHOTOSHOOTTEST_HH_ +#define IGNITION_GAZEBO_TEST_INTEGRATION_MODELPHOTOSHOOTTEST_HH_ + #include #include @@ -296,3 +299,5 @@ class ModelPhotoShootTest : public InternalFixture<::testing::Test> 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 index dc53cc9380..09f62e93d3 100644 --- a/test/integration/model_photo_shoot_default_joints.cc +++ b/test/integration/model_photo_shoot_default_joints.cc @@ -17,8 +17,11 @@ #include "ModelPhotoShootTest.hh" +#include + // Test the Model Photo Shoot plugin on the example world. -TEST_F(ModelPhotoShootTest, ModelPhotoShootDefaultJoints) +TEST_F(ModelPhotoShootTest, + IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ModelPhotoShootDefaultJoints)) { this->ModelPhotoShootTestCmd( "examples/worlds/model_photo_shoot.sdf"); diff --git a/test/integration/model_photo_shoot_random_joints.cc b/test/integration/model_photo_shoot_random_joints.cc index 0c4fb56624..2559b0c889 100644 --- a/test/integration/model_photo_shoot_random_joints.cc +++ b/test/integration/model_photo_shoot_random_joints.cc @@ -17,8 +17,11 @@ #include "ModelPhotoShootTest.hh" +#include + // Test the Model Photo Shoot plugin on the example world. -TEST_F(ModelPhotoShootTest, ModelPhotoShootRandomJoints) +TEST_F(ModelPhotoShootTest, + IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ModelPhotoShootRandomJoints)) { this->ModelPhotoShootTestCmd( "test/worlds/model_photo_shoot_random_joints.sdf"); From a9dee08655931cccea3b94fe747fa13a620cbd45 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 30 Mar 2022 15:01:08 -0500 Subject: [PATCH 20/20] Create unique directory for test Signed-off-by: Addisu Z. Taddese --- test/integration/ModelPhotoShootTest.hh | 15 +++++++++------ .../model_photo_shoot_default_joints.cc | 8 ++++++++ .../model_photo_shoot_random_joints.cc | 8 ++++++++ 3 files changed, 25 insertions(+), 6 deletions(-) diff --git a/test/integration/ModelPhotoShootTest.hh b/test/integration/ModelPhotoShootTest.hh index f5197b1ebd..4fe1f08aff 100644 --- a/test/integration/ModelPhotoShootTest.hh +++ b/test/integration/ModelPhotoShootTest.hh @@ -82,11 +82,10 @@ void SavePicture(const ignition::rendering::CameraPtr _camera, void testImages(const std::string &_imageFile, const std::string &_testImageFile) { - std::string imageFilePath = common::joinPaths( - std::string(PROJECT_BINARY_PATH), "test", "integration", _imageFile); + std::string imageFilePath = common::joinPaths(common::cwd(), _imageFile); ignition::common::Image image(imageFilePath); - std::string testImageFilePath = common::joinPaths( - std::string(PROJECT_BINARY_PATH), "test", "integration", _testImageFile); + std::string testImageFilePath = + common::joinPaths(common::cwd(), _testImageFile); ignition::common::Image testImage(testImageFilePath); EXPECT_TRUE(image.Valid()); @@ -111,6 +110,11 @@ void testImages(const std::string &_imageFile, /// \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() { @@ -168,8 +172,7 @@ class ModelPhotoShootTest : public InternalFixture<::testing::Test> /// \param[in] _poseFile File containing the generated poses. protected: void LoadPoseValues(std::string _poseFile = "poses.txt") { - std::string poseFilePath = common::joinPaths( - std::string(PROJECT_BINARY_PATH), "test", "integration", _poseFile); + std::string poseFilePath = common::joinPaths(common::cwd(), _poseFile); std::ifstream poseFile (poseFilePath); std::string line; ASSERT_TRUE(poseFile.is_open()); diff --git a/test/integration/model_photo_shoot_default_joints.cc b/test/integration/model_photo_shoot_default_joints.cc index 09f62e93d3..620bf42e8a 100644 --- a/test/integration/model_photo_shoot_default_joints.cc +++ b/test/integration/model_photo_shoot_default_joints.cc @@ -26,3 +26,11 @@ TEST_F(ModelPhotoShootTest, 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 index 2559b0c889..21cabd9681 100644 --- a/test/integration/model_photo_shoot_random_joints.cc +++ b/test/integration/model_photo_shoot_random_joints.cc @@ -26,3 +26,11 @@ TEST_F(ModelPhotoShootTest, 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(); +}