MRPT  2.0.4
NodeletsTest_impl.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /** \example comms_nodelets_example/NodeletsTest_impl.cpp */
11 
13 
14 //! [example-nodelets]
15 #include <mrpt/comms/nodelets.h>
16 #include <mrpt/math/TPose3D.h>
17 #include <chrono>
18 #include <cstdio> // printf()
19 #include <iostream>
20 #include <thread>
21 
22 // Test payload:
23 const mrpt::math::TPose3D p_tx(1.0, 2.0, 3.0, 0.2, 0.4, 0.6);
24 
25 // Create the topic directory. Only once per process, and must be shared
26 // by all nodelets/threads.
28 
30 {
31  using namespace mrpt::comms;
32  using namespace std;
33 
34  try
35  {
36 #ifdef NODELETS_TEST_VERBOSE
37  printf("[publisher] Started\n");
38 #endif
39 
40  for (int i = 0; i < 5; i++)
41  {
42  std::this_thread::sleep_for(100ms);
43  dir->getTopic("/robot/odom")->publish(p_tx);
44  }
45 
46 #ifdef NODELETS_TEST_VERBOSE
47  printf("[publisher] Finish\n");
48 #endif
49  }
50  catch (const std::exception& e)
51  {
52  cerr << e.what() << endl;
53  }
54  catch (...)
55  {
56  printf("[thread_publisher] Runtime error!\n");
57  }
58 }
59 
61 {
62 #ifdef NODELETS_TEST_VERBOSE
63  std::cout << "sub2: rx TPose3D" << p.asString() << std::endl;
64 #endif
65 }
66 
67 void onNewMsg2(int idx, const mrpt::math::TPose3D& p)
68 {
69 #ifdef NODELETS_TEST_VERBOSE
70  std::cout << "onNewMsg2: idx=" << idx << " rx TPose3D" << p.asString()
71  << std::endl;
72 #endif
73 }
74 
76 {
77  using namespace mrpt::comms;
78  using namespace std;
79 
80  try
81  {
82 #ifdef NODELETS_TEST_VERBOSE
83  printf("[subscriber] Connecting\n");
84 #endif
85 
86 #ifdef NODELETS_TEST_VERBOSE
87  printf("[subscriber] Connected. Waiting for a message...\n");
88 #endif
89 
90  // Create a subscriber with a lambda:
91  Subscriber::Ptr sub1 =
92  dir->getTopic("/robot/odom")
93  ->createSubscriber<mrpt::math::TPose3D>(
94  [](const mrpt::math::TPose3D& p_rx) -> void {
95 #ifdef NODELETS_TEST_VERBOSE
96  std::cout << "sub1: rx TPose3D" << p_rx.asString()
97  << std::endl;
98 #endif
99  nodelets_test_passed_ok = (p_rx == p_tx);
100  });
101 
102  // Create a subscriber with a regular function via std::function:
103  auto sub2 =
104  dir->getTopic("/robot/odom")
105  ->createSubscriber<mrpt::math::TPose3D>(
106  std::function<void(const mrpt::math::TPose3D&)>(&onNewMsg));
107 
108  // Create a subscriber with a regular function:
109  auto sub3 = dir->getTopic("/robot/odom")
110  ->createSubscriber<mrpt::math::TPose3D>(&onNewMsg);
111 
112  // Create a subscriber with std::bind:
113  using namespace std::placeholders;
114  auto sub4 = dir->getTopic("/robot/odom")
115  ->createSubscriber<mrpt::math::TPose3D>(
116  [](auto&& arg1) { return onNewMsg2(123, arg1); });
117 
118  // wait for messages to arrive.
119  // The nodelet is up and live until "sub" gets out of scope.
120  std::this_thread::sleep_for(1000ms);
121 
122 #ifdef NODELETS_TEST_VERBOSE
123  printf("[subscriber] Finish\n");
124 #endif
125  }
126  catch (const std::exception& e)
127  {
128  cerr << e.what() << endl;
129  }
130  catch (...)
131  {
132  cerr << "[thread_subscriber] Runtime error!" << endl;
133  }
134 }
135 
137 {
138  using namespace std::chrono_literals;
139 
140  std::thread(thread_publisher).detach();
141  std::thread(thread_subscriber).detach();
142  std::this_thread::sleep_for(1000ms);
143 }
144 //! [example-nodelets]
void onNewMsg(const mrpt::math::TPose3D &p)
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
STL namespace.
void thread_subscriber()
void onNewMsg2(int idx, const mrpt::math::TPose3D &p)
bool nodelets_test_passed_ok
auto dir
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
void thread_publisher()
const mrpt::math::TPose3D p_tx(1.0, 2.0, 3.0, 0.2, 0.4, 0.6)
[example-nodelets]
Serial and networking devices and utilities.
void NodeletsTest()



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