Problems publishing to "~/visual" to change the scale of a box model
Hello,
I have a gazebo model plugin where i want to change the scale factor. I tried to use the "SetScale(const ignition::math::Vector3d &_scale, const bool _publish=false)" function but this doesn't work, so i tried publishing a message to the "~/visual" topic using gazebo::msg::Visual.
I tried printing the BoundingBox of the box model and i saw that publishing doesn't take effect until the Visual pointer of the box model is loaded. To wait for the Visual pointer to be loaded, I check if the Visual pointer is not equal to NULL, then i publish the message to change the Scale, but even though the Scale is changed and BoundingBox is scaled according to the Scale factor, the visual part of the box stays the same size.
If i try waiting for some iterations, say 5000 then this works okay.
Can someone explain what is happening? Is there a way to know when these changes will take effect?
Here is the .cpp source file
void Boxscale::Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr sdf)
{
model_ = _parent;
world_ = model_->GetWorld();
node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
node_->Init(world_->GetName());
pub_visual_ = node_->Advertise<gazebo::msgs::Visual>("~/visual");
this->updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(
std::bind(&Boxscale::OnUpdate, this));
}
void Boxscale::OnUpdate()
{
if(!skip)
{
scene_ptr= gazebo::rendering::get_scene();
box_ptr = scene_ptr->GetVisual(model_->GetName());
if(!box_ptr)
{
std::cout << "Box not loaded" << std::endl;
}
else
{
iterrations_++;
if (iterrations_ > 5000)
{
ignition::math::Vector3d initial_scale(3.0, 1.0, 1.0);
pub_visual_->Publish(box.setscale(initial_scale));
skip = true;
}
}
}
}
Here is the .h source file
class Boxscale : public gazebo::ModelPlugin
{
public: void Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf);
public: void OnUpdate();
class Scalefactor
{
public:
Scalefactor(void) = default;
Scalefactor(const std::string linkName, gazebo::physics::ModelPtr model)
{
model_ = model;
link_ = model->GetLink(linkName);
sdf::ElementPtr visualSDF = linkSDF->GetElement("visual");
visual_name_ = visualSDF->Get<std::string>("name");
}
Scalefactor(const Scalefactor&) = default;
Scalefactor(Scalefactor&&) = default;
~Scalefactor(void) = default;
Scalefactor& operator=(const Scalefactor&) = default;
Scalefactor& operator=(Scalefactor&&) = default;
gazebo::msgs::Visual setscale(const ignition::math::Vector3d scale_) const
{
gazebo::msgs::Visual visualMsg = link_->GetVisualMessage(visual_name_);
gazebo::msgs::Vector3d* scale_factor = new gazebo::msgs::Vector3d{gazebo::msgs::Convert(scale_)};
visualMsg.set_name(link_->GetScopedName());
visualMsg.set_parent_name(model_->GetScopedName());
visualMsg.set_allocated_scale(scale_factor);
return visualMsg;
}
}
private:
gazebo::rendering::ScenePtr scene_ptr ;
gazebo::rendering::VisualPtr box_ptr ;
gazebo::physics::ModelPtr model_;
gazebo::physics::WorldPtr world_;
gazebo::event::ConnectionPtr updateConnection;
skip = false;
private: size_t iterrations_ = 0;
gazebo::transport::NodePtr node_;
gazebo::transport::PublisherPtr pub_visual_;
Scalefactor box;
}
Here is the sdf file
<model name="box">
<pose frame=''>30 13.9 3.05 0 0 0.012944</pose>
<link name="box">
<gravity>false</gravity>
<visual name="visual">
<transparency>0.5</transparency>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
<plugin name="box_scaler" filename="libbox_scaler.so"/>
</model>
