Home | Tutorials | Wiki | Issues
Ask Your Question
0

Problem with echo of a message

asked 2014-03-23 16:08:56 -0600

Luigi gravatar image

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

1 Answer

Sort by » oldest newest most voted
0

answered 2014-03-25 13:28:00 -0600

nkoenig gravatar image

The following line in your external program:

void StampData(AnswerPtr &_msg)

should be:

void StampData(ConstAnswerPtr &_msg)

That may fix your problem.

edit flag offensive delete link more

Comments

I have tried but it doesn't change anything, moreover I tried with an integrated message in Gazebo and it keep giving me back the same warning (and didn't send me back the original message). In your opinion, are there any other errors?

Luigi gravatar imageLuigi ( 2014-03-28 11:27:07 -0600 )edit

Take a look at the publisher and listener examples. They work, assuming you have gzserver running.

nkoenig gravatar imagenkoenig ( 2014-03-28 13:50:01 -0600 )edit
Login/Signup to Answer

Question Tools

Stats

Asked: 2014-03-23 16:08:56 -0600

Seen: 1,527 times

Last updated: Mar 25 '14