Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // MODEL PLUGIN
- #include "abcData.pb.h"
- #include "zyxData.pb.h"
- using namespace std;
- namespace gazebo
- {
- typedef const boost::shared_ptr<const abcData_msg_package::msgs::abcData> abcDataPtr;
- class ModelPl: public ModelPlugin
- {
- bool stopit;
- physics::ModelPtr modelPointer;
- event::ConnectionPtr updateConnection;
- transport::NodePtr receiver;
- transport::SubscriberPtr abcDataSubscriber;
- public: ModelPl(): ModelPlugin()
- {
- stopit= false;
- }
- public: ~ModelPl()
- {
- this->receiver->Fini();
- }
- public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
- {
- cout << "MODEL: Load" << endl;
- this->receiver= transport::NodePtr(new transport::Node());
- this->receiver->Init( this->modelPointer->GetWorld()->GetName() );
- this->abcDataSubscriber = this->receiver->Subscribe("~/abcData", &ModelPl::Cb, this);
- cout << "MODEL: subscribe" << endl;
- this->updateConnection = event::Events::ConnectWorldUpdateBegin(
- boost::bind(&ModelPl::OnUpdate, this));
- }
- public: void Cb(abcDataPtr &msg)
- {
- cout << "received" << endl;
- }
- public: void OnUpdate()
- {
- if(!stopit)
- {
- cout << "MODEL: UPDATE" << endl;
- SendMethod();
- }
- }
- private: void SendMethod()
- {
- cout << "MODEL: SendMethod" << endl;
- gazebo::transport::NodePtr advert(new gazebo::transport::Node());
- nodeVersand->Init( this->modelPointer->GetWorld()->GetName() );
- gazebo::transport::PublisherPtr imagePub =
- advert->Advertise<zyxData_msg_package::msgs::zyxData>("~/zyxData");
- zyxData_msg_package::msgs::zyxData zyxDataMessage;
- zyxDataMessage.add_something(1.0);
- imagePub->WaitForConnection();
- imagePub->Publish( zyxDataMessage);
- cout << "MODEL: Message sent" << endl;
- advert->Fini();
- ababab = true;
- }
- }
- }
- // World Plugin
- #include "abcData.pb.h"
- #include "zyxData.pb.h"
- using namespace std;
- namespace gazebo
- {
- typedef const boost::shared_ptr<const zyxData_msg_package::msgs::zyxData> zyxDataPtr;
- class WorldPl: public WorldPlugin
- {
- private: transport::NodePtr pubNode;
- private: transport::NodePtr subNode;
- private: transport::SubscriberPtr zyxDataSubscriber;
- private: physics::WorldPtr world;
- public: WorldPl() : WorldPlugin()
- {
- cout << "WORLD: CONSTRUCTOR" << endl;
- }
- public: ~WorldPl()
- {
- subNode->Fini();
- }
- public: void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/)
- {
- world = _parent;
- subNode = transport::NodePtr(new transport::Node());
- subNode->Init(world->GetName());
- cout << "WORLD: Subscribing" << endl;
- zyxDataSubscriber= subNode->Subscribe("~/zyxData", &WorldPl::CallbackFkt, this);
- }
- public: void CallbackFkt(zyxDataPtr &msg)
- {
- cout << "WORLD: zyxDataPtr empfangen!" << endl;
- gazebo::transport::NodePtr pubNode(new gazebo::transport::Node());
- pubNode->Init(world->GetName());
- cout << "WORLD: Weltname: "<< world->GetName() << endl;
- abcData_msg_package::msgs::abcData abcDataMessage;
- abcDataMessage.set_something(42);
- cout << "WORLD: abcDataMessage filled" << endl;
- gazebo::transport::PublisherPtr imagePub =
- pubNode->Advertise<abcData_msg_package::msgs::abcData>("~/abcData");
- imagePub->WaitForConnection();
- cout << "WORLD: wait for connection! - connected" << endl;
- imagePub->Publish(abcDataMessage);
- pubNode->Fini();
- cout << "WORLD: abcData sent"<< endl;
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement