Skip to content

Commit d480501

Browse files
victorpaleologuevictorpaleologue
committed
Using new ALMemory subscription interface
1 parent 0590d87 commit d480501

File tree

3 files changed

+1478
-1035
lines changed

3 files changed

+1478
-1035
lines changed

‎src/event/touch.cpp

+138-102
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
* See the License for the specific language governing permissions and
1414
* limitations under the License.
1515
*
16-
*/
16+
*/
1717

1818
#include <iostream>
1919
#include <vector>
@@ -24,219 +24,255 @@
2424

2525
#include <qi/anyobject.hpp>
2626

27-
#include <naoqi_driver/recorder/globalrecorder.hpp>
2827
#include <naoqi_driver/message_actions.h>
28+
#include <naoqi_driver/recorder/globalrecorder.hpp>
2929

3030
#include "touch.hpp"
3131

3232
namespace naoqi
3333
{
3434

35-
template<class T>
35+
template <class T>
3636
TouchEventRegister<T>::TouchEventRegister()
3737
{
3838
}
3939

40-
template<class T>
41-
TouchEventRegister<T>::TouchEventRegister( const std::string& name, const std::vector<std::string> keys, const float& frequency, const qi::SessionPtr& session )
42-
: p_memory_( session->service("ALMemory").value()),
43-
isStarted_(false),
44-
isPublishing_(false),
45-
isRecording_(false),
46-
isDumping_(false)
40+
template <class T>
41+
TouchEventRegister<T>::TouchEventRegister(const std::string& name,
42+
const std::vector<std::string> keys,
43+
const float& frequency,
44+
const qi::SessionPtr& session,
45+
robot::NaoqiVersion naoqi_version)
46+
: session_(std::move(session)), naoqi_version_(std::move(naoqi_version)),
47+
p_memory_(session_->service("ALMemory").value()), isStarted_(false),
48+
isPublishing_(false), isRecording_(false), isDumping_(false)
4749
{
48-
publisher_ = boost::make_shared<publisher::BasicPublisher<T> >( name );
49-
//recorder_ = boost::make_shared<recorder::BasicEventRecorder<T> >( name );
50-
converter_ = boost::make_shared<converter::TouchEventConverter<T> >( name, frequency, session );
51-
52-
converter_->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher<T>::publish, publisher_, boost::placeholders::_1) );
53-
//converter_->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicEventRecorder<T>::write, recorder_, _1) );
54-
//converter_->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicEventRecorder<T>::bufferize, recorder_, _1) );
50+
publisher_ = boost::make_shared<publisher::BasicPublisher<T>>(name);
51+
// recorder_ = boost::make_shared<recorder::BasicEventRecorder<T> >( name );
52+
converter_ = boost::make_shared<converter::TouchEventConverter<T>>(
53+
name, frequency, session);
54+
55+
converter_->registerCallback(
56+
message_actions::PUBLISH,
57+
boost::bind(&publisher::BasicPublisher<T>::publish, publisher_,
58+
boost::placeholders::_1));
59+
// converter_->registerCallback( message_actions::RECORD,
60+
// boost::bind(&recorder::BasicEventRecorder<T>::write, recorder_, _1) );
61+
// converter_->registerCallback( message_actions::LOG,
62+
// boost::bind(&recorder::BasicEventRecorder<T>::bufferize, recorder_, _1)
63+
// );
5564

5665
keys_.resize(keys.size());
5766
size_t i = 0;
58-
for(std::vector<std::string>::const_iterator it = keys.begin(); it != keys.end(); ++it, ++i)
67+
for (std::vector<std::string>::const_iterator it = keys.begin();
68+
it != keys.end(); ++it, ++i)
5969
keys_[i] = *it;
6070

6171
name_ = name;
6272
}
6373

64-
template<class T>
74+
template <class T>
6575
TouchEventRegister<T>::~TouchEventRegister()
6676
{
6777
stopProcess();
6878
}
6979

70-
template<class T>
80+
template <class T>
7181
void TouchEventRegister<T>::resetPublisher(rclcpp::Node* node)
7282
{
7383
publisher_->reset(node);
7484
}
7585

76-
template<class T>
77-
void TouchEventRegister<T>::resetRecorder( boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr )
86+
template <class T>
87+
void TouchEventRegister<T>::resetRecorder(
88+
boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr)
7889
{
79-
//recorder_->reset(gr, converter_->frequency());
90+
// recorder_->reset(gr, converter_->frequency());
8091
}
8192

82-
template<class T>
93+
template <class T>
8394
void TouchEventRegister<T>::startProcess()
8495
{
8596
boost::mutex::scoped_lock start_lock(mutex_);
8697
if (!isStarted_)
87-
{
88-
if (subscriptions_.empty())
8998
{
90-
for (const auto& key : keys_) {
91-
auto subscriber = p_memory_.call<qi::AnyObject>("subscriber", key);
92-
qi::SignalLink subscription = subscriber.connect("signal", [=](const qi::AnyValue& v){ touchCallback(key, v); }).value();
93-
subscriptions_.emplace_back(std::move(subscriber), std::move(subscription));
94-
}
99+
if (subscriptions_.empty())
100+
{
101+
for (const auto& key : keys_)
102+
{
103+
auto subscriber = subscribe(
104+
naoqi_version_, session_, key,
105+
[=](const qi::AnyValue& v) { touchCallback(key, v); });
106+
subscriptions_.push_back(std::move(subscriber));
107+
}
108+
}
109+
isStarted_ = true;
95110
}
96-
isStarted_ = true;
97-
}
98111
}
99112

100-
template<class T>
113+
template <class T>
101114
void TouchEventRegister<T>::stopProcess()
102115
{
103116
boost::mutex::scoped_lock stop_lock(mutex_);
104117
if (isStarted_)
105-
{
106-
if (!subscriptions_.empty()){
107-
for (const auto& subscription : subscriptions_) {
108-
try {
109-
subscription.subscriber.disconnect(subscription.link).value();
110-
} catch (const std::exception& e) {
111-
std::cerr << "Error attempting to clean-up ALMemory subscription: " << e.what() << std::endl;
118+
{
119+
if (!subscriptions_.empty())
120+
{
121+
for (const auto& subscription : subscriptions_)
122+
{
123+
try
124+
{
125+
subscription->unsubscribe().value();
126+
}
127+
catch (const std::exception& e)
128+
{
129+
std::cerr << "Error attempting to clean-up "
130+
"ALMemory subscription: "
131+
<< e.what() << std::endl;
132+
}
133+
}
134+
subscriptions_.clear();
112135
}
113-
}
114-
subscriptions_.clear();
136+
isStarted_ = false;
115137
}
116-
isStarted_ = false;
117-
}
118138
}
119139

120-
template<class T>
140+
template <class T>
121141
void TouchEventRegister<T>::writeDump(const rclcpp::Time& time)
122142
{
123143
if (isStarted_)
124-
{
125-
//recorder_->writeDump(time);
126-
}
144+
{
145+
// recorder_->writeDump(time);
146+
}
127147
}
128148

129-
template<class T>
149+
template <class T>
130150
void TouchEventRegister<T>::setBufferDuration(float duration)
131151
{
132-
//recorder_->setBufferDuration(duration);
152+
// recorder_->setBufferDuration(duration);
133153
}
134154

135-
template<class T>
155+
template <class T>
136156
void TouchEventRegister<T>::isRecording(bool state)
137157
{
138158
boost::mutex::scoped_lock rec_lock(mutex_);
139159
isRecording_ = state;
140160
}
141161

142-
template<class T>
162+
template <class T>
143163
void TouchEventRegister<T>::isPublishing(bool state)
144164
{
145165
boost::mutex::scoped_lock pub_lock(mutex_);
146166
isPublishing_ = state;
147167
}
148168

149-
template<class T>
169+
template <class T>
150170
void TouchEventRegister<T>::isDumping(bool state)
151171
{
152172
boost::mutex::scoped_lock dump_lock(mutex_);
153173
isDumping_ = state;
154174
}
155175

156-
template<class T>
176+
template <class T>
157177
void TouchEventRegister<T>::registerCallback()
158178
{
159179
}
160180

161-
template<class T>
181+
template <class T>
162182
void TouchEventRegister<T>::unregisterCallback()
163183
{
164184
}
165185

166-
template<class T>
167-
void TouchEventRegister<T>::touchCallback(const std::string &key, const qi::AnyValue& value)
186+
template <class T>
187+
void TouchEventRegister<T>::touchCallback(const std::string& key,
188+
const qi::AnyValue& value)
168189
{
169190
T msg = T();
170-
bool state = value.toFloat() > 0.5f;
191+
bool state = value.toFloat() > 0.5f;
171192

172193
touchCallbackMessage(key, state, msg);
173194

174195
std::vector<message_actions::MessageAction> actions;
175196
boost::mutex::scoped_lock callback_lock(mutex_);
176-
if (isStarted_) {
177-
// CHECK FOR PUBLISH
178-
if ( isPublishing_ && publisher_->isSubscribed() )
179-
{
180-
actions.push_back(message_actions::PUBLISH);
181-
}
182-
// CHECK FOR RECORD
183-
if ( isRecording_ )
184-
{
185-
//actions.push_back(message_actions::RECORD);
186-
}
187-
if ( !isDumping_ )
188-
{
189-
//actions.push_back(message_actions::LOG);
190-
}
191-
if (actions.size() >0)
197+
if (isStarted_)
192198
{
193-
converter_->callAll( actions, msg );
199+
// CHECK FOR PUBLISH
200+
if (isPublishing_ && publisher_->isSubscribed())
201+
{
202+
actions.push_back(message_actions::PUBLISH);
203+
}
204+
// CHECK FOR RECORD
205+
if (isRecording_)
206+
{
207+
// actions.push_back(message_actions::RECORD);
208+
}
209+
if (!isDumping_)
210+
{
211+
// actions.push_back(message_actions::LOG);
212+
}
213+
if (actions.size() > 0)
214+
{
215+
converter_->callAll(actions, msg);
216+
}
194217
}
195-
}
196218
}
197219

198-
template<class T>
199-
void TouchEventRegister<T>::touchCallbackMessage(const std::string &key, bool &state, naoqi_bridge_msgs::msg::Bumper &msg)
220+
template <class T>
221+
void TouchEventRegister<T>::touchCallbackMessage(
222+
const std::string& key, bool& state, naoqi_bridge_msgs::msg::Bumper& msg)
200223
{
201224
int i = 0;
202-
for(std::vector<std::string>::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i)
203-
{
204-
if ( key == it->c_str() ) {
205-
msg.bumper = i;
206-
msg.state = state?(naoqi_bridge_msgs::msg::Bumper::STATE_PRESSED):(naoqi_bridge_msgs::msg::Bumper::STATE_RELEASED);
225+
for (std::vector<std::string>::const_iterator it = keys_.begin();
226+
it != keys_.end(); ++it, ++i)
227+
{
228+
if (key == it->c_str())
229+
{
230+
msg.bumper = i;
231+
msg.state = state ? (naoqi_bridge_msgs::msg::Bumper::STATE_PRESSED)
232+
: (naoqi_bridge_msgs::msg::Bumper::STATE_RELEASED);
233+
}
207234
}
208-
}
209235
}
210236

211-
template<class T>
212-
void TouchEventRegister<T>::touchCallbackMessage(const std::string &key, bool &state, naoqi_bridge_msgs::msg::HandTouch &msg)
237+
template <class T>
238+
void TouchEventRegister<T>::touchCallbackMessage(
239+
const std::string& key, bool& state, naoqi_bridge_msgs::msg::HandTouch& msg)
213240
{
214241
int i = 0;
215-
for(std::vector<std::string>::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i)
216-
{
217-
if ( key == it->c_str() ) {
218-
msg.hand = i;
219-
msg.state = state?(naoqi_bridge_msgs::msg::HandTouch::STATE_PRESSED):(naoqi_bridge_msgs::msg::HandTouch::STATE_RELEASED);
242+
for (std::vector<std::string>::const_iterator it = keys_.begin();
243+
it != keys_.end(); ++it, ++i)
244+
{
245+
if (key == it->c_str())
246+
{
247+
msg.hand = i;
248+
msg.state = state
249+
? (naoqi_bridge_msgs::msg::HandTouch::STATE_PRESSED)
250+
: (naoqi_bridge_msgs::msg::HandTouch::STATE_RELEASED);
251+
}
220252
}
221-
}
222253
}
223254

224-
template<class T>
225-
void TouchEventRegister<T>::touchCallbackMessage(const std::string &key, bool &state, naoqi_bridge_msgs::msg::HeadTouch &msg)
255+
template <class T>
256+
void TouchEventRegister<T>::touchCallbackMessage(
257+
const std::string& key, bool& state, naoqi_bridge_msgs::msg::HeadTouch& msg)
226258
{
227259
int i = 0;
228-
for(std::vector<std::string>::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i)
229-
{
230-
if ( key == it->c_str() ) {
231-
msg.button = i;
232-
msg.state = state?(naoqi_bridge_msgs::msg::HeadTouch::STATE_PRESSED):(naoqi_bridge_msgs::msg::HeadTouch::STATE_RELEASED);
260+
for (std::vector<std::string>::const_iterator it = keys_.begin();
261+
it != keys_.end(); ++it, ++i)
262+
{
263+
if (key == it->c_str())
264+
{
265+
msg.button = i;
266+
msg.state = state
267+
? (naoqi_bridge_msgs::msg::HeadTouch::STATE_PRESSED)
268+
: (naoqi_bridge_msgs::msg::HeadTouch::STATE_RELEASED);
269+
}
233270
}
234-
}
235271
}
236272

237273
// http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor
238274
template class TouchEventRegister<naoqi_bridge_msgs::msg::Bumper>;
239275
template class TouchEventRegister<naoqi_bridge_msgs::msg::HandTouch>;
240276
template class TouchEventRegister<naoqi_bridge_msgs::msg::HeadTouch>;
241277

242-
}//namespace
278+
} // namespace naoqi

0 commit comments

Comments
 (0)