Example: comms_nodelets_example

Example output:

[publisher] Started
[subscriber] Connecting
[subscriber] Connected. Waiting for a message...
sub1: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub2: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub2: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
onNewMsg2: idx=123 rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub1: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub2: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub2: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
onNewMsg2: idx=123 rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub1: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub2: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub2: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
onNewMsg2: idx=123 rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub1: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub2: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub2: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
onNewMsg2: idx=123 rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub1: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub2: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
sub2: rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
onNewMsg2: idx=123 rx TPose3D[1.000000 2.000000 3.000000 11.459156 22.918312 34.377468]
[publisher] Finish

Contents of comms_nodelets_example/NodeletsTest_impl.cpp :

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <atomic>

std::atomic_bool nodelets_test_passed_ok = false;

#include <mrpt/comms/nodelets.h>
#include <mrpt/math/TPose3D.h>

#include <chrono>
#include <cstdio>  // printf()
#include <iostream>
#include <thread>

// Test payload:
const mrpt::math::TPose3D p_tx(1.0, 2.0, 3.0, 0.2, 0.4, 0.6);

// Create the topic directory. Only once per process, and must be shared
// by all nodelets/threads.
auto dir = mrpt::comms::TopicDirectory::create();

void thread_publisher()
{
  using namespace mrpt::comms;
  using namespace std;

  try
  {
#ifdef NODELETS_TEST_VERBOSE
    printf("[publisher] Started\n");
#endif

    for (int i = 0; i < 5; i++)
    {
      std::this_thread::sleep_for(100ms);
      dir->getTopic("/robot/odom")->publish(p_tx);
    }

#ifdef NODELETS_TEST_VERBOSE
    printf("[publisher] Finish\n");
#endif
  }
  catch (const std::exception& e)
  {
    cerr << e.what() << endl;
  }
  catch (...)
  {
    printf("[thread_publisher] Runtime error!\n");
  }
}

void onNewMsg(const mrpt::math::TPose3D& p)
{
#ifdef NODELETS_TEST_VERBOSE
  std::cout << "sub2: rx TPose3D" << p.asString() << std::endl;
#endif
}

void onNewMsg2(int idx, const mrpt::math::TPose3D& p)
{
#ifdef NODELETS_TEST_VERBOSE
  std::cout << "onNewMsg2: idx=" << idx << " rx TPose3D" << p.asString() << std::endl;
#endif
}

void thread_subscriber()
{
  using namespace mrpt::comms;
  using namespace std;

  try
  {
#ifdef NODELETS_TEST_VERBOSE
    printf("[subscriber] Connecting\n");
#endif

#ifdef NODELETS_TEST_VERBOSE
    printf("[subscriber] Connected. Waiting for a message...\n");
#endif

    // Create a subscriber with a lambda:
    Subscriber::Ptr sub1 = dir->getTopic("/robot/odom")
                               ->createSubscriber<mrpt::math::TPose3D>(
                                   [](const mrpt::math::TPose3D& p_rx) -> void
                                   {
#ifdef NODELETS_TEST_VERBOSE
                                     std::cout << "sub1: rx TPose3D" << p_rx.asString()
                                               << std::endl;
#endif
                                     nodelets_test_passed_ok = (p_rx == p_tx);
                                   });

    // Create a subscriber with a regular function via std::function:
    auto sub2 = dir->getTopic("/robot/odom")
                    ->createSubscriber<mrpt::math::TPose3D>(
                        std::function<void(const mrpt::math::TPose3D&)>(&onNewMsg));

    // Create a subscriber with a regular function:
    auto sub3 = dir->getTopic("/robot/odom")->createSubscriber<mrpt::math::TPose3D>(&onNewMsg);

    // Create a subscriber with std::bind:
    using namespace std::placeholders;
    auto sub4 = dir->getTopic("/robot/odom")
                    ->createSubscriber<mrpt::math::TPose3D>([](auto&& arg1)
                                                            { return onNewMsg2(123, arg1); });

    // wait for messages to arrive.
    // The nodelet is up and live until "sub" gets out of scope.
    std::this_thread::sleep_for(2000ms);

#ifdef NODELETS_TEST_VERBOSE
    printf("[subscriber] Finish\n");
#endif
  }
  catch (const std::exception& e)
  {
    cerr << e.what() << endl;
  }
  catch (...)
  {
    cerr << "[thread_subscriber] Runtime error!" << endl;
  }
}

void NodeletsTest()
{
  using namespace std::chrono_literals;

  std::thread(thread_publisher).detach();
  std::thread(thread_subscriber).detach();
  std::this_thread::sleep_for(1000ms);
}

C++ example source code:

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#define NODELETS_TEST_VERBOSE
#include <mrpt/core/exceptions.h>

#include <iostream>

#include "NodeletsTest_impl.cpp"

int main()
{
  try
  {
    NodeletsTest();
    return 0;
  }
  catch (const std::exception& e)
  {
    std::cerr << "MRPT exception caught: " << e.what() << std::endl;
    return -1;
  }
}