MRPT  2.0.3
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]
p_tx
const mrpt::math::TPose3D p_tx(1.0, 2.0, 3.0, 0.2, 0.4, 0.6)
[example-nodelets]
mrpt::comms::TopicDirectory::create
static Ptr create()
Definition: nodelets.cpp:60
nodelets.h
thread_publisher
void thread_publisher()
Definition: NodeletsTest_impl.cpp:29
thread_subscriber
void thread_subscriber()
Definition: NodeletsTest_impl.cpp:75
mrpt::comms::Subscriber::Ptr
std::shared_ptr< Subscriber > Ptr
Definition: nodelets.h:37
mrpt::comms
Serial and networking devices and utilities.
Definition: CClientTCPSocket.h:21
onNewMsg
void onNewMsg(const mrpt::math::TPose3D &p)
Definition: NodeletsTest_impl.cpp:60
onNewMsg2
void onNewMsg2(int idx, const mrpt::math::TPose3D &p)
Definition: NodeletsTest_impl.cpp:67
TPose3D.h
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
dir
const auto dir
Definition: chessboard_stereo_camera_calib_unittest.cpp:28
nodelets_test_passed_ok
bool nodelets_test_passed_ok
Definition: NodeletsTest_impl.cpp:12
mrpt::math::TPose3D::asString
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]",...
Definition: TPose3D.cpp:36
NodeletsTest
void NodeletsTest()
Definition: NodeletsTest_impl.cpp:136



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020