Hello gazebo community,
I want to create a plugin responsible for light animations. I made a simple plugin based on the examples. However, I encountered a strange behavior where the value of an object’s property changes, but this does not translate into the visualizations. Any ideas?
Code for reproduction:
C++
#include "SampleSystem.hh"
#include <gz/plugin/Register.hh>
#include <random>
#include "gz/sim/components/LightCmd.hh"
SampleSystem::SampleSystem() : gz::sim::System()
{}
SampleSystem::~SampleSystem()= default;
void SampleSystem::Configure(const gz::sim::Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
gz::sim::EntityComponentManager &_ecm,
gz::sim::EventManager &_eventMgr)
{
this->entity = _entity;
ignmsg << "Command actor for entity [" << _entity << "]" << std::endl;
// Iterate through entities to find the light entity by name
_ecm.Each<gz::sim::components::Name, gz::sim::components::Light>(
[&](const gz::sim::Entity &_entity,
const gz::sim::components::Name *_name,
const gz::sim::components::Light *) -> bool
{
if (_name->Data() == "frontlight")
{
this->lightEntity = _entity;
ignmsg << "Light entity found: " << this->lightEntity << std::endl;
return true; // Stop searching
}
return true;
});
if (this->lightEntity == gz::sim::kNullEntity)
{
std::cerr << "Error: Light entity not found." << std::endl;
return;
}
this->lastUpdateTime = std::chrono::steady_clock::now();
}
void SampleSystem::PreUpdate(
const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm)
{
auto currentTime = std::chrono::steady_clock::now();
if (currentTime - this->lastUpdateTime >= std::chrono::seconds(1))
{
this->lastUpdateTime = std::chrono::steady_clock::now();
// Retrieve the light component
static auto lightComp = _ecm.Component<gz::sim::components::Light>(this->lightEntity);
if (!lightComp)
{
ignerr << "Error: Light component not found on light entity: " << this->lightEntity << std::endl;
return;
}
// Generate random RGB values
double r = this->distribution(this->generator);
double g = this->distribution(this->generator);
double b = this->distribution(this->generator);
// Create new light with random color
sdf::Light newLight = lightComp->Data();
newLight.SetDiffuse(gz::math::Color(r, g, b, 1));
newLight.SetSpecular(gz::math::Color(r, g, b, 1));
// Update the light component
*lightComp = gz::sim::components::Light(newLight);
_ecm.SetChanged(
this->lightEntity,
gz::sim::components::Light::typeId,
gz::sim::ComponentState::PeriodicChange
);
this->lastUpdateTime = currentTime;
}
}
IGNITION_ADD_PLUGIN(SampleSystem,
gz::sim::System,
SampleSystem::ISystemConfigure,
SampleSystem::ISystemPreUpdate)
H++
#ifndef SYSTEM_PLUGIN_SAMPLESYSTEM_HH_
#define SYSTEM_PLUGIN_SAMPLESYSTEM_HH_
#include <chrono>
#include <gz/sim/System.hh>
#include <gz/sim/components/Light.hh>
#include <gz/sim/components/Material.hh>
#include <gz/sim/components/Visual.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/EventManager.hh>
#include <gz/sim/System.hh>
#include <gz/sim/rendering/Events.hh>
#include <gz/common/Timer.hh>
#include <sdf/Pbr.hh>
// class UserCommandsPrivate;
class SampleSystem:
public gz::sim::System,
public gz::sim::ISystemConfigure,
public gz::sim::ISystemPreUpdate
{
public: SampleSystem();
public: ~SampleSystem() final;
public: void Configure(const gz::sim::Entity &_id,
const std::shared_ptr<const sdf::Element> &_sdf,
gz::sim::EntityComponentManager &_ecm,
gz::sim::EventManager &_eventMgr) final;
public: void PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) override;
private:
gz::sim::Entity entity;
gz::sim::Entity visualEntity{gz::sim::kNullEntity};
gz::sim::Entity lightEntity{gz::sim::kNullEntity};
std::chrono::steady_clock::time_point lastUpdateTime;
std::default_random_engine generator;
std::uniform_real_distribution<double> distribution{0.0, 1.0};
// std::unique_ptr<UserCommandsPrivate> dataPtr;
};
#endif
SDF
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="default">
<physics name="fast" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>0</real_time_factor>
</physics>
<plugin
filename="ignition-gazebo-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="ignition-gazebo-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin
filename="ignition-gazebo-contact-system"
name="gz::sim::systems::Contact">
</plugin>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<plugin filename="SampleSystem" name="SampleSystem"/>
<static>true</static>
<link name="light">
<pose>0 0 0.1 0 0 0</pose>
<light name="frontlight" type="spot">
<cast_shadows>true</cast_shadows>
<visualize>0</visualize>
<diffuse>1.0 1.0 0.0</diffuse>
<specular>1.0 1.0 0.0</specular>
<direction>1 0 0</direction>
<intensity>1.0</intensity>
<spot>
<inner_angle>1.0</inner_angle>
<outer_angle>2.0</outer_angle>
<falloff>0.4</falloff>
</spot>
</light>
</link>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
</world>
</sdf>