How to add a dynamic visual marker in gazebo?

edit

You could add a visual plugin for that: briefly described here:

Add something like this in your model.sdf:

<link name="some_link">
    <visual name="some_visual">
        <plugin name="some_visual_plugin" filename="some_visual_plugin.so"/>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
            <!-- your geometry -->
        </geometry>
    </visual>
    <....>
</link>

And then to add you would write a plugin as:

in some gazebo_visual_plugin.h:

#ifndef SOME_VISUAL_PLUGIN_H
#define SOME_VISUAL_PLUGIN_H

#include "gazebo/physics/physics.hh"
#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/msgs/MessageTypes.hh"

#include "gazebo/common/Time.hh"
#include "gazebo/common/Plugin.hh"
#include "gazebo/common/Events.hh"

#include "gazebo/rendering/DynamicLines.hh"
#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/rendering/Visual.hh"
#include "gazebo/rendering/Scene.hh"

#include <ros/callback_queue.h>
#include <ros/advertise_options.h>
#include <ros/ros.h>

#include <geometry_msgs/Point.h>
// if you want some positions of the model use this....
#include <gazebo_msgs/ModelStates.h>

#include <boost/thread.hpp>
#include <boost/bind.hpp>


namespace gazebo
{
  namespace rendering
  {
    class SomeVisualPlugin : public VisualPlugin
    {
      public:
        /// \brief Constructor
        SomeVisualPlugin();

        /// \brief Destructor
        virtual ~SomeVisualPlugin();

        /// \brief Load the visual force plugin tags
        /// \param node XML config node
        void Load( VisualPtr _parent, sdf::ElementPtr _sdf );


      protected: 
        /// \brief Update the visual plugin
        virtual void UpdateChild();


      private:
        /// \brief pointer to ros node
        ros::NodeHandle* rosnode_;

        /// \brief store model name
        std::string model_name_;

        /// \brief topic name
        std::string topic_name_;

        // /// \brief The visual pointer used to visualize the force.
        VisualPtr visual_;

        // /// \brief The scene pointer.
        ScenePtr scene_;

        /// \brief For example a line to visualize the force
        DynamicLines *line;

        /// \brief for setting ROS name space
        std::string visual_namespace_;

        /// \Subscribe to some force
        ros::Subscriber force_sub_;

        /// \brief Visualize the force
        void VisualizeForceOnLink(const geometry_msgs::PointConstPtr &force_ms);

        // Pointer to the update event connection
        event::ConnectionPtr update_connection_;
    };
  }
}

#endif

and the gazebo_visual_plugin.cpp:

#include <test_pkg/gazebo_visual_plugin.h>

namespace gazebo
{
  namespace rendering
  {

    ////////////////////////////////////////////////////////////////////////////////
    // Constructor
    SomeVisualPlugin::SomeVisualPlugin(): 
      line(NULL),
    {

    }

    ////////////////////////////////////////////////////////////////////////////////
    // Destructor
    SomeVisualPlugin::~SomeVisualPlugin()
    {
      // Finalize the visualizer
      this->rosnode_->shutdown();
      delete this->rosnode_;
    }

    ////////////////////////////////////////////////////////////////////////////////
    // Load the plugin
    void SomeVisualPlugin::Load( VisualPtr _parent, sdf::ElementPtr _sdf )
    {
      this->visual_ = _parent;

      this->visual_namespace_ = "visual/";

      // start ros node
      if (!ros::isInitialized())
      {
        int argc = 0;
        char** argv = NULL;
        ros::init(argc,argv,"gazebo_visual",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
      }

      this->rosnode_ = new ros::NodeHandle(this->visited_visual_namespace_);
      this->force_sub_ = this->rosnode_->subscribe("/some_force", 1000, &SomeVisualPlugin::VisualizeForceOnLink, this);

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->update_connection_ = event::Events::ConnectRender(
          boost::bind(&SomeVisualPlugin::UpdateChild, this));
    }

    //////////////////////////////////////////////////////////////////////////////////
    // Update the visualizer
    void SomeVisualPlugin::UpdateChild()
    {
      ros::spinOnce();
    }

    //////////////////////////////////////////////////////////////////////////////////
    // VisualizeForceOnLink
    void SomeVisualPlugin::VisualizeForceOnLink(const geometry_msgs::PointConstPtr &force_msg)
    {
      this->line = this->visual_->CreateDynamicLine(RENDERING_LINE_STRIP);

      //TODO: Get the current link position
      link_pose = CurrentLinkPose();
      //TODO: Get the current end position
      endpoint = CalculateEndpointOfForceVector(link_pose, force_msg);

      // Add two points to a connecting line strip from link_pose to endpoint
      this->line->AddPoint(
        math::Vector3(
          link_pose.position.x,
          link_pose.position.y,
          link_pose.position.z
          )
        );
      this->line->AddPoint(math::Vector3(endpoint.x, endpoint.y, endpoint.z));
      // set the Material of the line, in this case to purple
      this->line->setMaterial("Gazebo/Purple")
      this->line->setVisibilityFlags(GZ_VISIBILITY_GUI);
      this->visual_->SetVisible(true);
    }

    // Register this plugin within the simulator
    GZ_REGISTER_VISUAL_PLUGIN(SomeVisualPlugin)
  }
}

Didn't ... (more)