Advertisement
Chris_To

subscribing doesn't work

Jul 23rd, 2013
265
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 3.71 KB | None | 0 0
  1. // MODEL PLUGIN
  2. #include "abcData.pb.h"
  3. #include "zyxData.pb.h"
  4.  
  5. using namespace std;
  6. namespace gazebo
  7. {  
  8.   typedef const boost::shared_ptr<const abcData_msg_package::msgs::abcData> abcDataPtr;
  9.  
  10.   class ModelPl: public ModelPlugin
  11.   {
  12.     bool stopit;
  13.    
  14.     physics::ModelPtr modelPointer;
  15.    
  16.     event::ConnectionPtr updateConnection;
  17.  
  18.     transport::NodePtr receiver;
  19.     transport::SubscriberPtr abcDataSubscriber;
  20.    
  21.  
  22.     public: ModelPl(): ModelPlugin()
  23.     {
  24.       stopit= false;  
  25.     }  
  26.  
  27.     public: ~ModelPl()
  28.     {
  29.       this->receiver->Fini();
  30.     }
  31.  
  32.     public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
  33.     {
  34.       cout << "MODEL: Load" << endl;
  35.  
  36.       this->receiver= transport::NodePtr(new transport::Node());
  37.       this->receiver->Init( this->modelPointer->GetWorld()->GetName() );
  38.       this->abcDataSubscriber = this->receiver->Subscribe("~/abcData", &ModelPl::Cb, this);
  39.       cout << "MODEL: subscribe" << endl;
  40.  
  41.       this->updateConnection = event::Events::ConnectWorldUpdateBegin(
  42.         boost::bind(&ModelPl::OnUpdate, this));            
  43.     }
  44.    
  45.     public: void Cb(abcDataPtr &msg)
  46.     {
  47.       cout << "received" << endl;
  48.     }
  49.    
  50.     public: void OnUpdate()
  51.     {
  52.       if(!stopit)
  53.       {
  54.     cout << "MODEL: UPDATE" << endl;
  55.     SendMethod();
  56.       }
  57.      }
  58.  
  59.     private: void SendMethod()
  60.     {
  61.       cout << "MODEL: SendMethod" << endl;
  62.  
  63.       gazebo::transport::NodePtr advert(new gazebo::transport::Node());
  64.       nodeVersand->Init( this->modelPointer->GetWorld()->GetName() );
  65.      
  66.       gazebo::transport::PublisherPtr imagePub =
  67.     advert->Advertise<zyxData_msg_package::msgs::zyxData>("~/zyxData");
  68.       zyxData_msg_package::msgs::zyxData zyxDataMessage;
  69.    
  70.       zyxDataMessage.add_something(1.0);
  71.  
  72.       imagePub->WaitForConnection();
  73.       imagePub->Publish( zyxDataMessage);
  74.       cout << "MODEL: Message sent" << endl;
  75.       advert->Fini();
  76.       ababab = true;
  77.     }
  78.   }
  79. }
  80.  
  81.  
  82. // World Plugin
  83. #include "abcData.pb.h"
  84. #include "zyxData.pb.h"
  85.  
  86. using namespace std;
  87. namespace gazebo
  88. {
  89.  
  90.   typedef const boost::shared_ptr<const zyxData_msg_package::msgs::zyxData> zyxDataPtr;
  91.    
  92.   class WorldPl: public WorldPlugin
  93.   {
  94.  
  95.       private: transport::NodePtr pubNode;
  96.       private: transport::NodePtr subNode;
  97.       private: transport::SubscriberPtr zyxDataSubscriber;
  98.       private: physics::WorldPtr world;
  99.  
  100.  
  101.       public: WorldPl() : WorldPlugin()
  102.       {
  103.     cout << "WORLD: CONSTRUCTOR" << endl;
  104.       }
  105.  
  106.       public: ~WorldPl()
  107.       {
  108.         subNode->Fini();
  109.       }
  110.    
  111.       public: void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/)
  112.       {
  113.  
  114.       world = _parent;
  115.       subNode = transport::NodePtr(new transport::Node());
  116.      
  117.       subNode->Init(world->GetName());
  118.       cout << "WORLD: Subscribing" << endl;
  119.       zyxDataSubscriber= subNode->Subscribe("~/zyxData", &WorldPl::CallbackFkt, this);
  120.      
  121.       }
  122.  
  123.       public: void CallbackFkt(zyxDataPtr &msg)
  124.       {
  125.        
  126.     cout << "WORLD: zyxDataPtr empfangen!" << endl;
  127.  
  128.      gazebo::transport::NodePtr pubNode(new gazebo::transport::Node());
  129.       pubNode->Init(world->GetName());
  130.       cout << "WORLD: Weltname: "<< world->GetName() << endl;
  131.      
  132.       abcData_msg_package::msgs::abcData abcDataMessage;
  133.      
  134.       abcDataMessage.set_something(42);  
  135.      
  136.       cout << "WORLD: abcDataMessage filled" << endl;
  137.      
  138.       gazebo::transport::PublisherPtr imagePub =
  139.           pubNode->Advertise<abcData_msg_package::msgs::abcData>("~/abcData");
  140.       imagePub->WaitForConnection();
  141.       cout << "WORLD: wait for connection! - connected" << endl;
  142.       imagePub->Publish(abcDataMessage);
  143.       pubNode->Fini();
  144.       cout << "WORLD: abcData sent"<< endl;
  145.       }
  146.   }
  147. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement