Skip to content

Commit

Permalink
several fixes to address PR suggestions
Browse files Browse the repository at this point in the history
Signed-off-by: Marco A. Gutierrez <[email protected]>
  • Loading branch information
Marco A. Gutierrez committed Mar 3, 2022
1 parent de56234 commit 0e6691b
Show file tree
Hide file tree
Showing 3 changed files with 43 additions and 24 deletions.
1 change: 0 additions & 1 deletion examples/worlds/model_photo_shoot.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@
</plugin>
</include>
<model name="photo_shoot">
<pose>2.2 0 0 0 0 -3.14</pose>
<link name="link">
<pose>0 0 0 0 0 0</pose>
<sensor name="camera" type="camera">
Expand Down
39 changes: 16 additions & 23 deletions src/systems/model_photo_shoot/ModelPhotoShoot.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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 <ignition/common/Image.hh>
#include <ignition/gazebo/World.hh>
#include <ignition/gazebo/components/Name.hh>
#include <ignition/gazebo/components/Pose.hh>
#include <ignition/gazebo/rendering/Events.hh>
#include <ignition/msgs.hh>
#include <ignition/plugin/Register.hh>
#include <ignition/rendering/Scene.hh>
#include <ignition/gazebo/Util.hh>
#include <ignition/rendering/Visual.hh>
#include <ignition/rendering/WireBox.hh>

using namespace ignition;
using namespace gazebo;
Expand Down Expand Up @@ -86,18 +82,18 @@ 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<std::string>("translation_data_file");
if (save_data_location.empty())
if (saveDataLocation.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);
<< saveDataLocation << std::endl;
this->dataPtr->savingFile.open(saveDataLocation);
}

if (_sdf->HasElement("random_joints_pose"))
Expand Down Expand Up @@ -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<components::Name>(*joint);
if (jointNameComp && jointNameComp->Data() != "World")
auto jointNameComp = _ecm.Component<components::Name>(joint);
if (jointNameComp)
{
// Using the JointAxis component to extract the joint pose limits
auto jointAxisComp = _ecm.Component<components::JointAxis>(*joint);
auto jointAxisComp = _ecm.Component<components::JointAxis>(joint);
if (jointAxisComp)
{
float mean =
(jointAxisComp->Data().Lower() +
jointAxisComp->Data().Upper()) / 2;
float stdv = (jointAxisComp->Data().Upper() - mean) / 3;
std::normal_distribution<float> distribution(mean, stdv);
std::uniform_real_distribution<double> distribution(
jointAxisComp->Data().Lower(), jointAxisComp->Data().Upper());
float jointPose = distribution(generator);
_ecm.SetComponentData<components::JointPositionReset>(*joint,
_ecm.SetComponentData<components::JointPositionReset>(joint,
{jointPose});
if (this->dataPtr->savingFile.is_open())
{
Expand Down Expand Up @@ -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");

Expand Down
27 changes: 27 additions & 0 deletions src/systems/model_photo_shoot/ModelPhotoShoot.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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
/// - <translation_data_file> - Location to save the camera and joint poses.
/// [Optional]
/// - <random_joints_pose> - 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,
Expand Down

0 comments on commit 0e6691b

Please sign in to comment.