Gazebo | Ignition | Community
Ask Your Question
0

Problems publishing to "~/visual" to change the scale of a box model

asked 2018-05-28 07:37:36 -0600

fitimhalimi gravatar image

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>
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-06-17 07:04:01 -0600

JakobWelner gravatar image

Hi Fitimhalimi, I can not explain what is happening, but I used your code as a foundation for making my own function which changes both scale and color during runtime, and it works without the 5000 iteration delay.

I've spent about 2 weeks trying to sort this out myself and have found very little documentation on how to use the visual topic. So, I don't know if this can help you in any way, but for what it's worth I'll post my version to serve as a functional example of controlling scale and color through the "~/visual" topic. My world file is the same as Fitimhalimi

One issue I am still having is that it doesn't seem to update until I play the simulation, which makes my scene look a little strange initially. If anyone has any idea how to programmetrically set color and scale before playback (ideally through a visual message) then I'd be happy to hear :)

Cheers

visual_msg_test_penguin.cc

#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>

#include "visual_msg_test_penguin.h"

namespace gazebo
{
    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()
    {
        ignition::math::Vector3d initial_scale(3.0, 1.0+accum, 1.0);
        pub_visual_->Publish( SetScale(initial_scale) );
        if (accum < 0) {
            accum = 0;
            speed = speed * -1;
        }
        else if (accum > 1) {
            accum = 1;
            speed = speed * -1;
        }
        accum = speed + accum;
    }

    gazebo::msgs::Visual Boxscale::SetScale(ignition::math::Vector3d scale_)
    {
        std::cerr << "\nSetting scale: " << scale_;

        std::string visual_name_ = "link_visual";
        std::string linkName = "link";
        physics::LinkPtr link_ = this->model_->GetLink(linkName);
        //sdf::ElementPtr visualSDF = linkSDF->GetElement("visual");
        //visual_name_ = visualSDF->Get<std::string>("name");

        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(this->model_->GetScopedName());
        visualMsg.set_allocated_scale(scale_factor);

        // Initiate material
        if ((!visualMsg.has_material()) || visualMsg.mutable_material() == NULL) {
            gazebo::msgs::Material *materialMsg = new gazebo::msgs::Material;
            visualMsg.set_allocated_material(materialMsg);
        }


        // Set color
        gazebo::common::Color newColor(1, accum, 0, 0.5);
        gazebo::msgs::Color *colorMsg = new gazebo::msgs::Color(gazebo::msgs::Convert(newColor));
        gazebo::msgs::Color *diffuseMsg = new gazebo::msgs::Color(*colorMsg);

        gazebo::msgs::Material *materialMsg = visualMsg.mutable_material();
        if (materialMsg->has_ambient())
        {
          materialMsg->clear_ambient();
        }
        materialMsg->set_allocated_ambient(colorMsg);
        if (materialMsg->has_diffuse())
        {
          materialMsg->clear_diffuse();
        }
        materialMsg->set_allocated_diffuse(diffuseMsg);

        return visualMsg;
    }

    // Register this plugin with the simulator
    GZ_REGISTER_MODEL_PLUGIN(Boxscale)
}

visual_msg_test_penguin.h

#ifndef VISUAL_MSG_TEST_PENGUIN_H
#define VISUAL_MSG_TEST_PENGUIN_H

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>

namespace gazebo
{
    class Boxscale : public ModelPlugin
    {
        public: void Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf);
        public: void OnUpdate();
        public: gazebo::msgs::Visual SetScale(ignition::math::Vector3d scale_);
        private:
            gazebo::rendering::ScenePtr scene_ptr ;
            gazebo::rendering::VisualPtr box_ptr ;
            gazebo::physics::ModelPtr model_;
            gazebo::physics::WorldPtr ...
(more)
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2018-05-28 07:37:36 -0600

Seen: 1,240 times

Last updated: May 28 '18