MRPT  2.0.4
comms_nodelets_example/NodeletsTest_impl.cpp
/* +------------------------------------------------------------------------+
| Mobile Robot Programming Toolkit (MRPT) |
| https://www.mrpt.org/ |
| |
| Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
| See: https://www.mrpt.org/Authors - All rights reserved. |
| Released under BSD License. See: https://www.mrpt.org/License |
+------------------------------------------------------------------------+ */
/** \example comms_nodelets_example/NodeletsTest_impl.cpp */
//! [example-nodelets]
#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.
{
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");
}
}
{
#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
}
{
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:
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
});
// 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(1000ms);
#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;
}
}
{
using namespace std::chrono_literals;
std::thread(thread_publisher).detach();
std::thread(thread_subscriber).detach();
std::this_thread::sleep_for(1000ms);
}
//! [example-nodelets]



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020