Skip to content

Gazebo custom plugin with subscriber and publisher

Subscriber

Subscriber 
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
#include <gz/common/Console.hh>
#include <gz/msgs/stringmsg.pb.h>
#include <gz/plugin/Register.hh>
#include <gz/sim/System.hh>
#include <gz/transport/Node.hh>

#include <mutex>
#include <string>

using namespace gz;
using namespace sim;

/////////////////////////////////////////////////
class SimpleSub
    : public System,
      public ISystemConfigure,
      public ISystemPostUpdate
{
public:
  void Configure(const Entity &,
                 const std::shared_ptr<const sdf::Element> &_sdf,
                 EntityComponentManager &,
                 EventManager &) override
  {
    this->topic = "/simple_sub";
    if (_sdf && _sdf->HasElement("topic"))
      this->topic = _sdf->Get<std::string>("topic");

    if (!this->node.Subscribe(this->topic, &SimpleSub::OnMsg, this))
    {
      gzerr << "[SimpleSub] Failed to subscribe to [" << this->topic << "]"
            << std::endl;
    }
    else
    {
      gzmsg << "[SimpleSub] Subscribed to [" << this->topic << "]" << std::endl;
    }
  }

  void PostUpdate(const UpdateInfo &,
                  const EntityComponentManager &) override
  {
    std::string msg;
    {
      std::lock_guard<std::mutex> lock(this->mutex);
      if (this->pending.empty())
        return;
      msg = std::move(this->pending);
      this->pending.clear();
    }

    gzmsg << "[SimpleSub] " << msg << std::endl;
  }

private:
  void OnMsg(const gz::msgs::StringMsg &_msg)
  {
    std::lock_guard<std::mutex> lock(this->mutex);
    this->pending = _msg.data();
  }

  transport::Node node;
  std::mutex mutex;
  std::string pending;
  std::string topic;
};

GZ_ADD_PLUGIN(SimpleSub,
              System,
              SimpleSub::ISystemConfigure,
              SimpleSub::ISystemPostUpdate)

GZ_ADD_PLUGIN_ALIAS(SimpleSub,
                    "gz::sim::systems::SimpleSub")
CMakeLists.txt 
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
cmake_minimum_required(VERSION 3.22)
project(SimpleParamReader)

find_package(gz-sim8 REQUIRED)
find_package(gz-plugin2 REQUIRED)

add_library(SimpleSub SHARED src/simple_sub/simple_sub.cc)

target_link_libraries(SimpleSub
  gz-sim8::gz-sim8
  gz-plugin2::gz-plugin2
)

install(TARGETS SimpleSub
  LIBRARY DESTINATION ${CMAKE_SOURCE_DIR}/bin)

Publisher

Publisher 
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
#include <gz/common/Console.hh>
#include <gz/msgs/stringmsg.pb.h>
#include <gz/plugin/Register.hh>
#include <gz/sim/System.hh>
#include <gz/transport/Node.hh>

#include <chrono>
#include <string>

using namespace gz;
using namespace sim;

/////////////////////////////////////////////////
class SimplePub
    : public System,
      public ISystemConfigure,
      public ISystemPostUpdate
{
public:
  void Configure(const Entity &,
                 const std::shared_ptr<const sdf::Element> &_sdf,
                 EntityComponentManager &,
                 EventManager &) override
  {
    this->topic = "/simple_sub";
    this->payload = "hello";
    this->rateHz = 1.0;

    if (_sdf)
    {
      if (_sdf->HasElement("topic"))
        this->topic = _sdf->Get<std::string>("topic");
      if (_sdf->HasElement("message"))
        this->payload = _sdf->Get<std::string>("message");
      if (_sdf->HasElement("rate_hz"))
        this->rateHz = _sdf->Get<double>("rate_hz");
    }

    if (this->rateHz > 0.0)
      this->period = std::chrono::duration_cast<std::chrono::nanoseconds>(
          std::chrono::duration<double>(1.0 / this->rateHz));
    else
      this->period = std::chrono::nanoseconds::zero();

    // Advertise the topic
    this->publisher = this->node.Advertise<gz::msgs::StringMsg>(this->topic);
    if (!this->publisher)
    {
      gzerr << "[SimplePub] Failed to advertise [" << this->topic << "]"
            << std::endl;
    }
    else
    {
      gzmsg << "[SimplePub] Publishing on [" << this->topic << "]"
            << std::endl;
    }

    this->msg.set_data(this->payload);
    this->lastPubTime = std::chrono::nanoseconds::min();
  }

  void PostUpdate(const UpdateInfo &_info,
                  const EntityComponentManager &) override
  {
    if (_info.paused || !this->publisher)
      return;

    if (this->period == std::chrono::nanoseconds::zero())
      return;

    if (this->lastPubTime == std::chrono::nanoseconds::min() ||
        _info.simTime - this->lastPubTime >= this->period)
    {
      this->lastPubTime = _info.simTime;
      // publish the message
      this->publisher.Publish(this->msg);
    }
  }

private:
  transport::Node node;
  transport::Node::Publisher publisher;
  std::string topic;
  std::string payload;
  double rateHz{1.0};
  std::chrono::nanoseconds period{0};
  std::chrono::nanoseconds lastPubTime{0};
  gz::msgs::StringMsg msg;
};

GZ_ADD_PLUGIN(SimplePub,
              System,
              SimplePub::ISystemConfigure,
              SimplePub::ISystemPostUpdate)

GZ_ADD_PLUGIN_ALIAS(SimplePub,
                    "gz::sim::systems::SimplePub")