Render plugin force for debugging

Question

I have a plugin that is applying a force on a link, and I would want that plugin to inject a visual for that force into the link. For instance, say that the link is a box, and my plugin applies a force on the side of the box that makes it translate in the world, I would want my plugin to add an arrow showing that force.

Because I don't know how to make an arrow, for now let's say I just draw a sphere where that force applies. So in my plugin's Load function, I get the model and the SDF:

void MyPlugin::Load(physics::ModelPtr model, sdf::ElementPtr sdf)

I can get the link, say my_link, with model->GetLink("my_link"). And I would like to inject the following visual into it:

  <visual name='test_sphere_visual'>
    <pose>0 0 1</pose>
    <geometry>
      <sphere>
          <radius>0.05</radius>
      </sphere>
    </geometry>
  </visual>

I have been following the "spawn spheres" tutorial, where I successfully manage to create an msgs::Model from a string and inject it into the world. But in this case, instead of injecting a whole new model, I want to inject a visual into an existing link.

Is there a way to do that from a plugin?

---

Originally posted by jonesv on Gazebo Answers with karma: 1 on 2021-02-24

Post score: 0

Answer 1

The only way I have found is to create a VisualPlugin and send the force as a message to it.

So in the Load() function of my ModelPlugin, I initialize a factory like this:

factory_pub_ = node_handle_->Advertise<msgs::Vector3d>("~/lift_force");

Which I then use to publish the force in the OnUpdate() function:

msgs::Vector3d force_msg;
force_msg.set_x(force.X());
force_msg.set_y(force.Y());
force_msg.set_z(force.Z());
factory_pub_->Publish(force_msg);

The next step is to fetch that value and draw it from the VisualPlugin

---

Originally posted by jonesv with karma: 1 on 2021-04-18

This answer was ACCEPTED on the original site

Post score: 0