From 0e6691b018db5f2811b8482498d5b47f5f720558 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Thu, 3 Mar 2022 11:53:23 +0000 Subject: [PATCH] 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 d9064bb89f2..4c6ec1360d0 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 f2a2068a971..8fdc0330c72 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 f524444a826..8b1a3d0a149 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,