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