Problem with echo of a message

edit

Hello,

I'm trying to do an echo of a message between gazebo and a external application with a custom message. I have no problem sending messages from the application to gazebo, but i can't return the message, and everytime that i try i receive this warning:

Warning [Publisher.cc:144] Queue limit reached for topic /gazebo/default/physics/contacts, deleting message. This warning is printed only once.

i don't understand what is the problem, if anyone has a solution i'll be very gratefull

here is the external application:

#include <iostream>
#include <sdf/sdf.hh>

#include "gazebo/gazebo.hh"
#include "gazebo/common/common.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/transport/transport.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/msgs/msgs.hh"

#include "call_answer.pb.h"

#define MAX_WAITING 1500 //tempo espresso in centesimi di secondo 

using namespace std;


typedef const boost::shared_ptr<const call_answer_msgs::msgs::Response> AnswerPtr;

void StampData(AnswerPtr &_msg);

int main(int argc, char * argv[])
{
    call_answer_msgs::msgs::Request request;

        request.set_answ(20);

    // Initialize transport 
    gazebo::transport::init();
    // Create our node for communication
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    node->Init();
    // Start transport
    gazebo::transport::run();
    //invio la richiesta di temperatura
    cout << "invio richiesta..." << endl;
    gazebo::transport::PublisherPtr Req;
    Req = node->Advertise<call_answer_msgs::msgs::Request>("~/MyRequest");
    Req->WaitForConnection();
    Req->Publish(request);

        // Listen to Gazebo world_stats topic
    gazebo::transport::SubscriberPtr sub = node->Subscribe("~/MyAnswer", StampData);
        // Busy wait loop...replace with your own code as needed.
    int i=0;
    while (true && i < MAX_WAITING){ 
            i++;
            gazebo::common::Time::MSleep(10);
        }

        // Make sure to shut everything down
    gazebo::transport::fini();
    return 0;
}

void StampData(AnswerPtr &_msg)
{
    cout << _msg->resp() << endl;
}

here the gazebo's plugin:

  #include <iostream>
#include <vector>
#include <string>
#include <math.h>
#include <sdf/sdf.hh>


#include <gazebo/gazebo.hh>
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/transport/transport.hh"


#include "call_answer.pb.h"


namespace gazebo
{
  typedef const boost::shared_ptr<const call_answer_msgs::msgs::Request> ConstRequestPtr;

  class MySensor : public ModelPlugin
  {
    transport::NodePtr node;    //Nodo per la connessione
    transport::SubscriberPtr commandSubscriber;

    public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {
         physics::WorldPtr world = _model->GetWorld();
        //inizializzo il nodo
        node = transport::NodePtr(new transport::Node());
        node->Init(world->GetName());
        std::cout << std::endl; 
        std::cout << "Subscribing to: " << "~/MyRequest" << std::endl;
        commandSubscriber = node->Subscribe("~/MyRequest", &MySensor::create,this);


    }//fine load


    public: void create(ConstRequestPtr &msg)
   {    
        std::cout << std::endl;
        std::cout << "è arrivato il messaggio: "<< msg->answ() << std::endl;        
        double val = msg->answ();

        //invio il messaggio a ROS/eseguibile esterno

      call_answer_msgs::msgs::Response answer;

        answer.set_resp(val);

        std::cout << std::endl;

        std::cout << "i'm here 1" << std::endl;     
        gazebo::transport::PublisherPtr tmprRes;
        std::cout << "i'm here 2" << std::endl; 
    tmprRes = node->Advertise<call_answer_msgs::msgs::Response>("~/MyAnswer", 100);
        std::cout << "i'm here 3" << std::endl; 
        tmprRes->WaitForConnection();
        tmprRes->Publish(answer);
        //      }
        //else {
        //  std::cout << "messaggio non inviato" << std::endl;
          //  }

  }//fine create

  };
  GZ_REGISTER_MODEL_PLUGIN(MySensor)
}