diff --git a/CMakeLists.txt b/CMakeLists.txt
index b24e630e..939b3ec0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -5,6 +5,8 @@ project(naoqi_driver)
set(
CONVERTERS_SRC
src/converters/audio.cpp
+ src/converters/touch.cpp
+ src/converters/people.cpp
src/converters/camera.cpp
src/converters/diagnostics.cpp
src/converters/imu.cpp
@@ -63,6 +65,8 @@ set(
src/event/basic.hxx
src/event/basic.hpp
src/event/audio.cpp
+ src/event/touch.cpp
+ src/event/people.cpp
)
# use catkin if qibuild is not found
diff --git a/CMakeLists_catkin.txt b/CMakeLists_catkin.txt
index 33ac6bb7..d0ac5163 100644
--- a/CMakeLists_catkin.txt
+++ b/CMakeLists_catkin.txt
@@ -11,6 +11,7 @@ find_package(catkin COMPONENTS
image_transport
kdl_parser
naoqi_bridge_msgs
+ nao_interaction_msgs
naoqi_libqi
naoqi_libqicore
robot_state_publisher
diff --git a/package.xml b/package.xml
index 4fe68f68..9d0bf923 100644
--- a/package.xml
+++ b/package.xml
@@ -17,6 +17,7 @@
image_transport
kdl_parser
naoqi_bridge_msgs
+ nao_interaction_msgs
naoqi_libqi
naoqi_libqicore
orocos_kdl
@@ -35,6 +36,7 @@
image_transport
kdl_parser
naoqi_bridge_msgs
+ nao_interaction_msgs
naoqi_libqi
naoqi_libqicore
orocos_kdl
diff --git a/share/boot_config.json b/share/boot_config.json
index f375f56b..19f65d55 100644
--- a/share/boot_config.json
+++ b/share/boot_config.json
@@ -71,6 +71,10 @@
"frequency" : 10
},
"audio":
+ {
+ "enabled" : true
+ },
+ "people":
{
"enabled" : true
}
diff --git a/src/converters/people.cpp b/src/converters/people.cpp
new file mode 100644
index 00000000..71059701
--- /dev/null
+++ b/src/converters/people.cpp
@@ -0,0 +1,68 @@
+/*
+ * Copyright 2015 Aldebaran
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+/*
+* LOCAL includes
+*/
+#include "people.hpp"
+
+/*
+* BOOST includes
+*/
+#include
+#define for_each BOOST_FOREACH
+
+namespace naoqi{
+
+namespace converter{
+
+template
+PeopleEventConverter::PeopleEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session)
+ : BaseConverter >(name, frequency, session)
+{
+}
+
+template
+PeopleEventConverter::~PeopleEventConverter() {
+}
+
+template
+void PeopleEventConverter::reset()
+{
+}
+
+template
+void PeopleEventConverter::registerCallback( const message_actions::MessageAction action, Callback_t cb )
+{
+ callbacks_[action] = cb;
+}
+
+template
+void PeopleEventConverter::callAll(const std::vector& actions, T& msg)
+{
+ msg_ = msg;
+ for_each( message_actions::MessageAction action, actions )
+ {
+ callbacks_[action](msg_);
+ }
+}
+
+// http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor
+template class PeopleEventConverter;
+}
+
+}
diff --git a/src/converters/people.hpp b/src/converters/people.hpp
new file mode 100644
index 00000000..0b95dcb2
--- /dev/null
+++ b/src/converters/people.hpp
@@ -0,0 +1,68 @@
+/*
+ * Copyright 2015 Aldebaran
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef PEOPLE_EVENT_CONVERTER_HPP
+#define PEOPLE_EVENT_CONVERTER_HPP
+
+/*
+* LOCAL includes
+*/
+#include "converter_base.hpp"
+#include
+
+/*
+* ROS includes
+*/
+#include
+
+/*
+* ALDEBARAN includes
+*/
+#include
+
+namespace naoqi{
+
+namespace converter{
+
+template
+class PeopleEventConverter : public BaseConverter >
+{
+
+ typedef boost::function Callback_t;
+
+public:
+ PeopleEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session);
+
+ ~PeopleEventConverter();
+
+ virtual void reset();
+
+ void registerCallback( const message_actions::MessageAction action, Callback_t cb );
+
+ void callAll(const std::vector& actions, T& msg);
+
+private:
+ /** Registered Callbacks **/
+ std::map callbacks_;
+ T msg_;
+};
+
+}
+
+}
+
+#endif // PEOPLE_CONVERTER_HPP
diff --git a/src/converters/touch.cpp b/src/converters/touch.cpp
new file mode 100644
index 00000000..1966e595
--- /dev/null
+++ b/src/converters/touch.cpp
@@ -0,0 +1,69 @@
+/*
+ * Copyright 2015 Aldebaran
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+/*
+* LOCAL includes
+*/
+#include "touch.hpp"
+
+/*
+* BOOST includes
+*/
+#include
+#define for_each BOOST_FOREACH
+
+namespace naoqi{
+
+namespace converter{
+
+template
+TouchEventConverter::TouchEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session)
+ : BaseConverter >(name, frequency, session)
+{
+}
+
+template
+TouchEventConverter::~TouchEventConverter() {
+}
+
+template
+void TouchEventConverter::reset()
+{
+}
+
+template
+void TouchEventConverter::registerCallback( const message_actions::MessageAction action, Callback_t cb )
+{
+ callbacks_[action] = cb;
+}
+
+template
+void TouchEventConverter::callAll(const std::vector& actions, T& msg)
+{
+ msg_ = msg;
+ for_each( message_actions::MessageAction action, actions )
+ {
+ callbacks_[action](msg_);
+ }
+}
+
+// http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor
+template class TouchEventConverter;
+template class TouchEventConverter;
+}
+
+}
diff --git a/src/converters/touch.hpp b/src/converters/touch.hpp
new file mode 100644
index 00000000..a2b2c536
--- /dev/null
+++ b/src/converters/touch.hpp
@@ -0,0 +1,69 @@
+/*
+ * Copyright 2015 Aldebaran
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef TOUCH_EVENT_CONVERTER_HPP
+#define TOUCH_EVENT_CONVERTER_HPP
+
+/*
+* LOCAL includes
+*/
+#include "converter_base.hpp"
+#include
+
+/*
+* ROS includes
+*/
+#include
+#include
+
+/*
+* ALDEBARAN includes
+*/
+#include
+
+namespace naoqi{
+
+namespace converter{
+
+template
+class TouchEventConverter : public BaseConverter >
+{
+
+ typedef boost::function Callback_t;
+
+public:
+ TouchEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session);
+
+ ~TouchEventConverter();
+
+ virtual void reset();
+
+ void registerCallback( const message_actions::MessageAction action, Callback_t cb );
+
+ void callAll(const std::vector& actions, T& msg);
+
+private:
+ /** Registered Callbacks **/
+ std::map callbacks_;
+ T msg_;
+};
+
+}
+
+}
+
+#endif // TOUCH_CONVERTER_HPP
diff --git a/src/event/people.cpp b/src/event/people.cpp
new file mode 100644
index 00000000..607487db
--- /dev/null
+++ b/src/event/people.cpp
@@ -0,0 +1,263 @@
+/*
+ * Copyright 2015 Aldebaran
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include
+#include
+
+#include
+
+#include
+
+#include
+
+#include
+#include
+#include "../tools/from_any_value.hpp"
+
+#include "people.hpp"
+
+namespace naoqi
+{
+
+template
+PeopleEventRegister::PeopleEventRegister()
+{
+}
+
+template
+PeopleEventRegister::PeopleEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session )
+ : serviceId(0),
+ p_memory_( session->service("ALMemory")),
+ session_(session),
+ isStarted_(false),
+ isPublishing_(false),
+ isRecording_(false),
+ isDumping_(false)
+{
+ publisher_ = boost::make_shared >( name );
+ //recorder_ = boost::make_shared >( name );
+ converter_ = boost::make_shared >( name, frequency, session );
+
+ converter_->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, publisher_, _1) );
+ //converter_->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicEventRecorder::write, recorder_, _1) );
+ //converter_->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicEventRecorder::bufferize, recorder_, _1) );
+
+ keys_.resize(keys.size());
+ size_t i = 0;
+ for(std::vector::const_iterator it = keys.begin(); it != keys.end(); ++it, ++i)
+ keys_[i] = *it;
+
+ name_ = name;
+}
+
+template
+PeopleEventRegister::~PeopleEventRegister()
+{
+ stopProcess();
+}
+
+template
+void PeopleEventRegister::resetPublisher(ros::NodeHandle& nh)
+{
+ publisher_->reset(nh);
+}
+
+template
+void PeopleEventRegister::resetRecorder( boost::shared_ptr gr )
+{
+ //recorder_->reset(gr, converter_->frequency());
+}
+
+template
+void PeopleEventRegister::startProcess()
+{
+ boost::mutex::scoped_lock start_lock(mutex_);
+ if (!isStarted_)
+ {
+ if(!serviceId)
+ {
+ //std::string serviceName = std::string("ROS-Driver-") + typeid(T).name();
+ std::string serviceName = std::string("ROS-Driver-") + keys_[0];
+ serviceId = session_->registerService(serviceName, this->shared_from_this());
+ for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it) {
+ std::cerr << *it << std::endl;
+ p_memory_.call("subscribeToEvent",it->c_str(), serviceName, "peopleCallback");
+ }
+ std::cout << serviceName << " : Start" << std::endl;
+ }
+ isStarted_ = true;
+ }
+}
+
+template
+void PeopleEventRegister::stopProcess()
+{
+ boost::mutex::scoped_lock stop_lock(mutex_);
+ if (isStarted_)
+ {
+ //std::string serviceName = std::string("ROS-Driver-") + typeid(T).name();
+ std::string serviceName = std::string("ROS-Driver-") + keys_[0];
+ if(serviceId){
+ p_memory_.call("unsubscribeToEvent", serviceName, "peopleCallback");
+ session_->unregisterService(serviceId);
+ serviceId = 0;
+ }
+ std::cout << serviceName << " : Stop" << std::endl;
+ isStarted_ = false;
+ }
+}
+
+template
+void PeopleEventRegister::writeDump(const ros::Time& time)
+{
+ if (isStarted_)
+ {
+ //recorder_->writeDump(time);
+ }
+}
+
+template
+void PeopleEventRegister::setBufferDuration(float duration)
+{
+ //recorder_->setBufferDuration(duration);
+}
+
+template
+void PeopleEventRegister::isRecording(bool state)
+{
+ boost::mutex::scoped_lock rec_lock(mutex_);
+ isRecording_ = state;
+}
+
+template
+void PeopleEventRegister::isPublishing(bool state)
+{
+ boost::mutex::scoped_lock pub_lock(mutex_);
+ isPublishing_ = state;
+}
+
+template
+void PeopleEventRegister::isDumping(bool state)
+{
+ boost::mutex::scoped_lock dump_lock(mutex_);
+ isDumping_ = state;
+}
+
+template
+void PeopleEventRegister::registerCallback()
+{
+}
+
+template
+void PeopleEventRegister::unregisterCallback()
+{
+}
+
+template
+void PeopleEventRegister::peopleCallback(std::string &key, qi::AnyValue &value, qi::AnyValue &message)
+{
+ T msg = T();
+
+ tools::NaoqiFaceDetected faces;
+ try {
+ faces = tools::fromAnyValueToNaoqiFaceDetected(value);
+ }
+ catch(std::runtime_error& e)
+ {
+ std::cout << "Cannot retrieve facedetect" << std::endl;
+ return;
+ }
+
+ if ( faces.face_info.size() > 0 ) { // sometimes value does not have face information..
+ peopleCallbackMessage(key, faces, msg);
+ }
+
+ std::vector actions;
+ boost::mutex::scoped_lock callback_lock(mutex_);
+
+ if (isStarted_) {
+ // CHECK FOR PUBLISH
+ if ( isPublishing_ && publisher_->isSubscribed() )
+ {
+ actions.push_back(message_actions::PUBLISH);
+ }
+ // CHECK FOR RECORD
+ if ( isRecording_ )
+ {
+ //actions.push_back(message_actions::RECORD);
+ }
+ if ( !isDumping_ )
+ {
+ //actions.push_back(message_actions::LOG);
+ }
+ if (actions.size() >0)
+ {
+ converter_->callAll( actions, msg );
+ }
+ }
+}
+
+template
+void PeopleEventRegister::peopleCallbackMessage(std::string &key, tools::NaoqiFaceDetected &faces, nao_interaction_msgs::FaceDetected &msg)
+{
+ msg.header.frame_id = "";
+ msg.header.stamp = ros::Time(faces.timestamp.timestamp_s, faces.timestamp.timestamp_us);
+
+ if ( faces.face_info.size() > 0 ) {
+ msg.face_id.data = faces.face_info[0].extra_info[0].face_id;
+ msg.score_reco.data = faces.face_info[0].extra_info[0].score_reco;
+ msg.face_label.data = faces.face_info[0].extra_info[0].face_label;
+
+ msg.shape_alpha.data = faces.face_info[0].shape_info.alpha;
+ msg.shape_beta.data = faces.face_info[0].shape_info.beta;
+ msg.shape_sizeX.data = faces.face_info[0].shape_info.sizeX;
+ msg.shape_sizeY.data = faces.face_info[0].shape_info.sizeY;
+
+ msg.right_eye_eyeCenter_x.data = faces.face_info[0].extra_info[0].right_eye_points.eye_center_x;
+ msg.right_eye_eyeCenter_y.data = faces.face_info[0].extra_info[0].right_eye_points.eye_center_y;
+ msg.right_eye_noseSideLimit_x.data = faces.face_info[0].extra_info[0].right_eye_points.nose_side_limit_x;
+ msg.right_eye_noseSideLimit_y.data = faces.face_info[0].extra_info[0].right_eye_points.nose_side_limit_y;
+ msg.right_eye_earSideLimit_x.data = faces.face_info[0].extra_info[0].right_eye_points.ear_side_limit_x;
+ msg.right_eye_earSideLimit_y.data = faces.face_info[0].extra_info[0].right_eye_points.ear_side_limit_y;
+
+ msg.left_eye_eyeCenter_x.data = faces.face_info[0].extra_info[0].left_eye_points.eye_center_x;
+ msg.left_eye_eyeCenter_y.data = faces.face_info[0].extra_info[0].left_eye_points.eye_center_y;
+ msg.left_eye_noseSideLimit_x.data = faces.face_info[0].extra_info[0].left_eye_points.nose_side_limit_x;
+ msg.left_eye_noseSideLimit_y.data = faces.face_info[0].extra_info[0].left_eye_points.nose_side_limit_y;
+ msg.left_eye_earSideLimit_x.data = faces.face_info[0].extra_info[0].left_eye_points.ear_side_limit_x;
+ msg.left_eye_earSideLimit_y.data = faces.face_info[0].extra_info[0].left_eye_points.ear_side_limit_y;
+
+ msg.nose_bottomCenterLimit_x.data = faces.face_info[0].extra_info[0].nose_points.bottom_center_limit_x;
+ msg.nose_bottomCenterLimit_y.data = faces.face_info[0].extra_info[0].nose_points.bottom_center_limit_y;
+ msg.nose_bottomLeftLimit_x.data = faces.face_info[0].extra_info[0].nose_points.bottom_left_limit_x;
+ msg.nose_bottomLeftLimit_y.data = faces.face_info[0].extra_info[0].nose_points.bottom_left_limit_y;
+ msg.nose_bottomRightLimit_x.data = faces.face_info[0].extra_info[0].nose_points.bottom_right_limit_x;
+ msg.nose_bottomRightLimit_y.data = faces.face_info[0].extra_info[0].nose_points.bottom_right_limit_y;
+
+ msg.mouth_leftLimit_x.data = faces.face_info[0].extra_info[0].mouth_points.left_limit_x;
+ msg.mouth_leftLimit_y.data = faces.face_info[0].extra_info[0].mouth_points.left_limit_y;
+ msg.mouth_rightLimit_x.data = faces.face_info[0].extra_info[0].mouth_points.right_limit_x;
+ msg.mouth_rightLimit_y.data = faces.face_info[0].extra_info[0].mouth_points.right_limit_y;
+ msg.mouth_topLimit_x.data = faces.face_info[0].extra_info[0].mouth_points.top_limit_x;
+ msg.mouth_topLimit_y.data = faces.face_info[0].extra_info[0].mouth_points.top_limit_y;
+ }
+}
+
+// http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor
+template class PeopleEventRegister;
+
+}//namespace
diff --git a/src/event/people.hpp b/src/event/people.hpp
new file mode 100644
index 00000000..126e6616
--- /dev/null
+++ b/src/event/people.hpp
@@ -0,0 +1,130 @@
+/*
+ * Copyright 2015 Aldebaran
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef PEOPLE_EVENT_REGISTER_HPP
+#define PEOPLE_EVENT_REGISTER_HPP
+
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+
+#include
+#include
+
+#include "../tools/naoqi_facedetected.hpp"
+
+// Converter
+#include "../src/converters/people.hpp"
+// Publisher
+#include "../src/publishers/basic.hpp"
+// Recorder
+#include "../recorder/basic_event.hpp"
+
+namespace naoqi
+{
+
+/**
+* @brief GlobalRecorder concept interface
+* @note this defines an private concept struct,
+* which each instance has to implement
+* @note a type erasure pattern in implemented here to avoid strict inheritance,
+* thus each possible publisher instance has to implement the virtual functions mentioned in the concept
+*/
+template
+class PeopleEventRegister: public boost::enable_shared_from_this >
+{
+
+public:
+
+ /**
+ * @brief Constructor for recorder interface
+ */
+ PeopleEventRegister();
+ PeopleEventRegister(const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session );
+ ~PeopleEventRegister();
+
+ void resetPublisher( ros::NodeHandle& nh );
+ void resetRecorder( boost::shared_ptr gr );
+
+ void startProcess();
+ void stopProcess();
+
+ void writeDump(const ros::Time& time);
+ void setBufferDuration(float duration);
+
+ void isRecording(bool state);
+ void isPublishing(bool state);
+ void isDumping(bool state);
+
+ void peopleCallback(std::string &key, qi::AnyValue &value, qi::AnyValue &message);
+ void peopleCallbackMessage(std::string &key, tools::NaoqiFaceDetected &faces, nao_interaction_msgs::FaceDetected &msg);
+
+
+private:
+ void registerCallback();
+ void unregisterCallback();
+ void onEvent();
+
+private:
+ boost::shared_ptr > converter_;
+ boost::shared_ptr > publisher_;
+ //boost::shared_ptr > recorder_;
+
+ qi::SessionPtr session_;
+ qi::AnyObject p_memory_;
+ unsigned int serviceId;
+ std::string name_;
+
+ boost::mutex mutex_;
+
+ bool isStarted_;
+ bool isPublishing_;
+ bool isRecording_;
+ bool isDumping_;
+
+protected:
+ std::vector keys_;
+}; // class
+
+
+class FaceDetectedEventRegister: public PeopleEventRegister
+{
+public:
+ FaceDetectedEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session ) : PeopleEventRegister(name, keys, frequency, session) {}
+};
+
+//QI_REGISTER_OBJECT(FaceDetectEventRegister, peopleCallback)
+
+static bool _qiregisterPeopleEventRegisterFaceDetected() {
+ ::qi::ObjectTypeBuilder > b;
+ QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback)
+ b.registerType();
+ return true;
+ }
+static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterPeopleEventRegisterFaceDetected();
+
+} //naoqi
+
+#endif
diff --git a/src/event/touch.cpp b/src/event/touch.cpp
new file mode 100644
index 00000000..7af66005
--- /dev/null
+++ b/src/event/touch.cpp
@@ -0,0 +1,234 @@
+/*
+ * Copyright 2015 Aldebaran
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include
+#include
+
+#include
+
+#include
+
+#include
+
+#include
+#include
+
+#include "touch.hpp"
+
+namespace naoqi
+{
+
+template
+TouchEventRegister::TouchEventRegister()
+{
+}
+
+template
+TouchEventRegister::TouchEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session )
+ : serviceId(0),
+ p_memory_( session->service("ALMemory")),
+ session_(session),
+ isStarted_(false),
+ isPublishing_(false),
+ isRecording_(false),
+ isDumping_(false)
+{
+ publisher_ = boost::make_shared >( name );
+ //recorder_ = boost::make_shared >( name );
+ converter_ = boost::make_shared >( name, frequency, session );
+
+ converter_->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, publisher_, _1) );
+ //converter_->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicEventRecorder::write, recorder_, _1) );
+ //converter_->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicEventRecorder::bufferize, recorder_, _1) );
+
+ keys_.resize(keys.size());
+ size_t i = 0;
+ for(std::vector::const_iterator it = keys.begin(); it != keys.end(); ++it, ++i)
+ keys_[i] = *it;
+
+ name_ = name;
+}
+
+template
+TouchEventRegister::~TouchEventRegister()
+{
+ stopProcess();
+}
+
+template
+void TouchEventRegister::resetPublisher(ros::NodeHandle& nh)
+{
+ publisher_->reset(nh);
+}
+
+template
+void TouchEventRegister::resetRecorder( boost::shared_ptr gr )
+{
+ //recorder_->reset(gr, converter_->frequency());
+}
+
+template
+void TouchEventRegister::startProcess()
+{
+ boost::mutex::scoped_lock start_lock(mutex_);
+ if (!isStarted_)
+ {
+ if(!serviceId)
+ {
+ //std::string serviceName = std::string("ROS-Driver-") + typeid(T).name();
+ std::string serviceName = std::string("ROS-Driver-") + keys_[0];
+ serviceId = session_->registerService(serviceName, this->shared_from_this());
+ for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it) {
+ std::cerr << *it << std::endl;
+ p_memory_.call("subscribeToEvent",it->c_str(), serviceName, "touchCallback");
+ }
+ std::cout << serviceName << " : Start" << std::endl;
+ }
+ isStarted_ = true;
+ }
+}
+
+template
+void TouchEventRegister::stopProcess()
+{
+ boost::mutex::scoped_lock stop_lock(mutex_);
+ if (isStarted_)
+ {
+ //std::string serviceName = std::string("ROS-Driver-") + typeid(T).name();
+ std::string serviceName = std::string("ROS-Driver-") + keys_[0];
+ if(serviceId){
+ p_memory_.call("unsubscribeToEvent", serviceName, "touchCallback");
+ session_->unregisterService(serviceId);
+ serviceId = 0;
+ }
+ std::cout << serviceName << " : Stop" << std::endl;
+ isStarted_ = false;
+ }
+}
+
+template
+void TouchEventRegister::writeDump(const ros::Time& time)
+{
+ if (isStarted_)
+ {
+ //recorder_->writeDump(time);
+ }
+}
+
+template
+void TouchEventRegister::setBufferDuration(float duration)
+{
+ //recorder_->setBufferDuration(duration);
+}
+
+template
+void TouchEventRegister::isRecording(bool state)
+{
+ boost::mutex::scoped_lock rec_lock(mutex_);
+ isRecording_ = state;
+}
+
+template
+void TouchEventRegister::isPublishing(bool state)
+{
+ boost::mutex::scoped_lock pub_lock(mutex_);
+ isPublishing_ = state;
+}
+
+template
+void TouchEventRegister::isDumping(bool state)
+{
+ boost::mutex::scoped_lock dump_lock(mutex_);
+ isDumping_ = state;
+}
+
+template
+void TouchEventRegister::registerCallback()
+{
+}
+
+template
+void TouchEventRegister::unregisterCallback()
+{
+}
+
+template
+void TouchEventRegister::touchCallback(std::string &key, qi::AnyValue &value, qi::AnyValue &message)
+{
+ T msg = T();
+
+ bool state = value.toFloat() > 0.5f;
+
+ //std::cerr << key << " " << state << std::endl;
+
+ touchCallbackMessage(key, state, msg);
+
+ std::vector actions;
+ boost::mutex::scoped_lock callback_lock(mutex_);
+ if (isStarted_) {
+ // CHECK FOR PUBLISH
+ if ( isPublishing_ && publisher_->isSubscribed() )
+ {
+ actions.push_back(message_actions::PUBLISH);
+ }
+ // CHECK FOR RECORD
+ if ( isRecording_ )
+ {
+ //actions.push_back(message_actions::RECORD);
+ }
+ if ( !isDumping_ )
+ {
+ //actions.push_back(message_actions::LOG);
+ }
+ if (actions.size() >0)
+ {
+ converter_->callAll( actions, msg );
+ }
+ }
+}
+
+template
+void TouchEventRegister::touchCallbackMessage(std::string &key, bool &state, naoqi_bridge_msgs::Bumper &msg)
+{
+ int i = 0;
+ for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i)
+ {
+ if ( key == it->c_str() ) {
+ msg.bumper = i;
+ msg.state = state?(naoqi_bridge_msgs::Bumper::statePressed):(naoqi_bridge_msgs::Bumper::stateReleased);
+ }
+ }
+}
+
+template
+void TouchEventRegister::touchCallbackMessage(std::string &key, bool &state, naoqi_bridge_msgs::TactileTouch &msg)
+{
+ int i = 0;
+ for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i)
+ {
+ if ( key == it->c_str() ) {
+ msg.button = i;
+ msg.state = state?(naoqi_bridge_msgs::TactileTouch::statePressed):(naoqi_bridge_msgs::TactileTouch::stateReleased);
+ }
+ }
+}
+
+// http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor
+template class TouchEventRegister;
+template class TouchEventRegister;
+
+}//namespace
diff --git a/src/event/touch.hpp b/src/event/touch.hpp
new file mode 100644
index 00000000..8c2907c1
--- /dev/null
+++ b/src/event/touch.hpp
@@ -0,0 +1,145 @@
+/*
+ * Copyright 2015 Aldebaran
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef TOUCH_EVENT_REGISTER_HPP
+#define TOUCH_EVENT_REGISTER_HPP
+
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+
+// Converter
+#include "../src/converters/touch.hpp"
+// Publisher
+#include "../src/publishers/basic.hpp"
+// Recorder
+#include "../recorder/basic_event.hpp"
+
+namespace naoqi
+{
+
+/**
+* @brief GlobalRecorder concept interface
+* @note this defines an private concept struct,
+* which each instance has to implement
+* @note a type erasure pattern in implemented here to avoid strict inheritance,
+* thus each possible publisher instance has to implement the virtual functions mentioned in the concept
+*/
+template
+class TouchEventRegister: public boost::enable_shared_from_this >
+{
+
+public:
+
+ /**
+ * @brief Constructor for recorder interface
+ */
+ TouchEventRegister();
+ TouchEventRegister(const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session );
+ ~TouchEventRegister();
+
+ void resetPublisher( ros::NodeHandle& nh );
+ void resetRecorder( boost::shared_ptr gr );
+
+ void startProcess();
+ void stopProcess();
+
+ void writeDump(const ros::Time& time);
+ void setBufferDuration(float duration);
+
+ void isRecording(bool state);
+ void isPublishing(bool state);
+ void isDumping(bool state);
+
+ void touchCallback(std::string &key, qi::AnyValue &value, qi::AnyValue &message);
+ void touchCallbackMessage(std::string &key, bool &state, naoqi_bridge_msgs::Bumper &msg);
+ void touchCallbackMessage(std::string &key, bool &state, naoqi_bridge_msgs::TactileTouch &msg);
+
+
+private:
+ void registerCallback();
+ void unregisterCallback();
+ void onEvent();
+
+private:
+ boost::shared_ptr > converter_;
+ boost::shared_ptr > publisher_;
+ //boost::shared_ptr > recorder_;
+
+ qi::SessionPtr session_;
+ qi::AnyObject p_memory_;
+ unsigned int serviceId;
+ std::string name_;
+
+ boost::mutex mutex_;
+
+ bool isStarted_;
+ bool isPublishing_;
+ bool isRecording_;
+ bool isDumping_;
+
+protected:
+ std::vector keys_;
+}; // class
+
+
+class BumperEventRegister: public TouchEventRegister
+{
+public:
+ BumperEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session ) : TouchEventRegister(name, keys, frequency, session) {}
+};
+
+class TactileTouchEventRegister: public TouchEventRegister
+{
+public:
+ TactileTouchEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session ) : TouchEventRegister(name, keys, frequency, session) {}
+};
+
+//QI_REGISTER_OBJECT(BumperEventRegister, touchCallback)
+//QI_REGISTER_OBJECT(TactileTouchEventRegister, touchCallback)
+
+static bool _qiregisterTouchEventRegisterBumper() {
+ ::qi::ObjectTypeBuilder > b;
+ QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, TouchEventRegister, touchCallback)
+ b.registerType();
+ return true;
+ }
+static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterTouchEventRegisterBumper();
+
+static bool _qiregisterTouchEventRegisterTactileTouch() {
+ ::qi::ObjectTypeBuilder > b;
+ QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, TouchEventRegister, touchCallback)
+ b.registerType();
+ return true;
+ }
+static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterTouchEventRegisterTactileTouch();
+
+} //naoqi
+
+#endif
diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp
index a587dd44..47e8902f 100644
--- a/src/naoqi_driver.cpp
+++ b/src/naoqi_driver.cpp
@@ -25,6 +25,8 @@
* CONVERTERS
*/
#include "converters/audio.hpp"
+#include "converters/touch.hpp"
+#include "converters/people.hpp"
#include "converters/camera.hpp"
#include "converters/diagnostics.hpp"
#include "converters/imu.hpp"
@@ -83,6 +85,8 @@
*/
#include "event/basic.hpp"
#include "event/audio.hpp"
+#include "event/touch.hpp"
+#include "event/people.hpp"
/*
* STATIC FUNCTIONS INCLUDE
@@ -575,6 +579,9 @@ void Driver::registerDefaultConverter()
bool sonar_enabled = boot_config_.get( "converters.sonar.enabled", true);
size_t sonar_frequency = boot_config_.get( "converters.sonar.frequency", 10);
+ bool bumper_enabled = boot_config_.get( "converters.bumper.enabled", true);
+ bool tactile_enabled = boot_config_.get( "converters.tactile.enabled", true);
+ bool people_enabled = boot_config_.get( "converters.people.enabled", true);
/*
* The info converter will be called once after it was added to the priority queue. Once it is its turn to be called, its
* callAll method will be triggered (because InfoPublisher is considered to always have subscribers, isSubscribed always
@@ -758,6 +765,60 @@ void Driver::registerDefaultConverter()
event_map_.find("audio")->second.isPublishing(true);
}
}
+
+ /** TOUCH **/
+ if ( bumper_enabled )
+ {
+ std::vector bumper_events;
+ bumper_events.push_back("RightBumperPressed");
+ bumper_events.push_back("LeftBumperPressed");
+ if (robot_ == robot::PEPPER)
+ {
+ bumper_events.push_back("BackBumperPressed");
+ }
+ boost::shared_ptr event_register =
+ boost::make_shared( "bumper", bumper_events, 0, sessionPtr_ );
+ insertEventConverter("bumper", event_register);
+ if (keep_looping) {
+ event_map_.find("bumper")->second.startProcess();
+ }
+ if (publish_enabled_) {
+ event_map_.find("bumper")->second.isPublishing(true);
+ }
+ }
+
+ if ( tactile_enabled )
+ {
+ std::vector tactile_touch_events;
+ tactile_touch_events.push_back("FrontTactilTouched");
+ tactile_touch_events.push_back("MiddleTactilTouched");
+ tactile_touch_events.push_back("RearTactilTouched");
+ boost::shared_ptr event_register =
+ boost::make_shared( "tactile_touch", tactile_touch_events, 0, sessionPtr_ );
+ insertEventConverter("tactile_touch", event_register);
+ if (keep_looping) {
+ event_map_.find("tactile_touch")->second.startProcess();
+ }
+ if (publish_enabled_) {
+ event_map_.find("tactile_touch")->second.isPublishing(true);
+ }
+ }
+
+ /** PEOPLE **/
+ if ( people_enabled )
+ {
+ std::vector people_events;
+ people_events.push_back("FaceDetected");
+ boost::shared_ptr event_register =
+ boost::make_shared( "face_detected", people_events, 0, sessionPtr_ );
+ insertEventConverter("face_detected", event_register);
+ if (keep_looping) {
+ event_map_.find("face_detected")->second.startProcess();
+ }
+ if (publish_enabled_) {
+ event_map_.find("face_detected")->second.isPublishing(true);
+ }
+ }
}
// public interface here
diff --git a/src/tools/from_any_value.cpp b/src/tools/from_any_value.cpp
index 960f6e7f..4eb84881 100644
--- a/src/tools/from_any_value.cpp
+++ b/src/tools/from_any_value.cpp
@@ -181,6 +181,473 @@ NaoqiImage fromAnyValueToNaoqiImage(qi::AnyValue& value){
return result;
}
+NaoqiTimeStamp fromAnyValueToTimeStamp(qi::AnyReference& anyrefs, NaoqiTimeStamp& result){
+ qi::AnyReference ref;
+ std::ostringstream ss;
+
+ /** timestamp_s **/
+ ref = anyrefs[0].content();
+ if(ref.kind() == qi::TypeKind_Int)
+ {
+ result.timestamp_s = ref.asInt32();
+ }
+ else
+ {
+ ss << "Could not retrieve timestamp_s";
+ throw std::runtime_error(ss.str());
+ }
+
+ /** timestamp_us **/
+ ref = anyrefs[1].content();
+ if(ref.kind() == qi::TypeKind_Int)
+ {
+ result.timestamp_us = ref.asInt32();
+ }
+ else
+ {
+ ss << "Could not retrieve timestamp_us";
+ throw std::runtime_error(ss.str());
+ }
+
+ return result;
+}
+
+NaoqiEyePoints fromAnyValueToEyePoints(qi::AnyReference& anyrefs, NaoqiEyePoints& result){
+ qi::AnyReference ref;
+ std::ostringstream ss;
+
+ // eye_center
+ ref = anyrefs[0].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.eye_center_x = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve eye_center_x";
+ throw std::runtime_error(ss.str());
+ }
+
+ ref = anyrefs[1].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.eye_center_y = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve eye_center_y";
+ throw std::runtime_error(ss.str());
+ }
+
+ // nose_side_limit
+ ref = anyrefs[2].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.nose_side_limit_x = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve nose_side_limit_x";
+ throw std::runtime_error(ss.str());
+ }
+
+ ref = anyrefs[3].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.nose_side_limit_y = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve nose_side_limit_y";
+ throw std::runtime_error(ss.str());
+ }
+
+ // ear_side_limit
+ ref = anyrefs[4].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.ear_side_limit_x = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve ear_side_limit_x";
+ throw std::runtime_error(ss.str());
+ }
+
+ ref = anyrefs[5].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.ear_side_limit_y = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve ear_side_limit_y";
+ throw std::runtime_error(ss.str());
+ }
+
+ return result;
+}
+
+NaoqiNosePoints fromAnyValueToNosePoints(qi::AnyReference& anyrefs, NaoqiNosePoints& result){
+ qi::AnyReference ref;
+ std::ostringstream ss;
+
+ // bottom_center_limit
+ ref = anyrefs[0].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.bottom_center_limit_x = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve bottom_center_limit_x";
+ throw std::runtime_error(ss.str());
+ }
+
+ ref = anyrefs[1].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.bottom_center_limit_y = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve bottom_center_limit_y";
+ throw std::runtime_error(ss.str());
+ }
+
+ // bottom_left_limit
+ ref = anyrefs[2].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.bottom_left_limit_x = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve bottom_left_limit_x";
+ throw std::runtime_error(ss.str());
+ }
+
+ ref = anyrefs[3].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.bottom_left_limit_y = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve bottom_left_limit_y";
+ throw std::runtime_error(ss.str());
+ }
+
+ // bottom_right_limit
+ ref = anyrefs[4].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.bottom_right_limit_x = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve bottom_right_limit_x";
+ throw std::runtime_error(ss.str());
+ }
+
+ ref = anyrefs[5].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.bottom_right_limit_y = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve bottom_right_limit_y";
+ throw std::runtime_error(ss.str());
+ }
+
+ return result;
+}
+
+NaoqiMouthPoints fromAnyValueToMouthPoints(qi::AnyReference& anyrefs, NaoqiMouthPoints& result){
+ qi::AnyReference ref;
+ std::ostringstream ss;
+
+ // left_limit
+ ref = anyrefs[0].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.left_limit_x = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve left_limit_x";
+ throw std::runtime_error(ss.str());
+ }
+
+ ref = anyrefs[1].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.left_limit_y = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve left_limit_y";
+ throw std::runtime_error(ss.str());
+ }
+
+ // right_limit
+ ref = anyrefs[0].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.right_limit_x = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve right_limit_x";
+ throw std::runtime_error(ss.str());
+ }
+
+ ref = anyrefs[1].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.right_limit_y = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve right_limit_y";
+ throw std::runtime_error(ss.str());
+ }
+
+ // top_limit
+ ref = anyrefs[0].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.top_limit_x = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve top_limit_x";
+ throw std::runtime_error(ss.str());
+ }
+
+ ref = anyrefs[1].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.top_limit_y = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve top_limit_y";
+ throw std::runtime_error(ss.str());
+ }
+
+ return result;
+}
+
+NaoqiShapeInfo fromAnyValueToShapeInfo(qi::AnyReference& anyrefs, NaoqiShapeInfo& result){
+ qi::AnyReference ref;
+ std::ostringstream ss;
+ //0
+ //alpha
+ ref = anyrefs[1].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.alpha = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve alpha";
+ throw std::runtime_error(ss.str());
+ }
+
+ //beta
+ ref = anyrefs[2].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.beta = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve beta";
+ throw std::runtime_error(ss.str());
+ }
+
+ //sizeX
+ ref = anyrefs[3].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.sizeX = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve sizeX";
+ throw std::runtime_error(ss.str());
+ }
+
+ //sizeY
+ ref = anyrefs[4].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.sizeY = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve sizeY";
+ throw std::runtime_error(ss.str());
+ }
+ return result;
+}
+
+NaoqiExtraInfo fromAnyValueToExtraInfo(qi::AnyReference& anyrefs, NaoqiExtraInfo& result){
+ qi::AnyReference ref;
+ std::ostringstream ss;
+
+ //faceID
+ ref = anyrefs[0].content();
+ if(ref.kind() == qi::TypeKind_Int)
+ {
+ result.face_id = ref.asInt32();
+ }
+ else
+ {
+ ss << "Could not retrieve faceID";
+ throw std::runtime_error(ss.str());
+ }
+
+ //scoreReco
+ ref = anyrefs[1].content();
+ if(ref.kind() == qi::TypeKind_Float)
+ {
+ result.score_reco = ref.asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve beta";
+ throw std::runtime_error(ss.str());
+ }
+
+ //faceLabel
+ ref = anyrefs[2].content();
+ if(ref.kind() == qi::TypeKind_String)
+ {
+ result.face_label = ref.asString();
+ }
+ else
+ {
+ ss << "Could not retrieve faceLabel";
+ throw std::runtime_error(ss.str());
+ }
+
+ /* leftEyePoints */
+ ref = anyrefs[3].content();
+ result.left_eye_points = fromAnyValueToEyePoints(ref, result.left_eye_points);
+
+ /* rightEyePoints */
+ ref = anyrefs[4].content();
+ result.right_eye_points = fromAnyValueToEyePoints(ref, result.right_eye_points);
+
+ /* nosePoints */
+ ref = anyrefs[7].content();
+ result.nose_points = fromAnyValueToNosePoints(ref, result.nose_points);
+
+ /* mouthPoints */
+ ref = anyrefs[8].content();
+ result.mouth_points = fromAnyValueToMouthPoints(ref, result.mouth_points);
+
+ return result;
+}
+
+NaoqiFaceInfo fromAnyValueToFaceInfo(qi::AnyReference& anyrefs, NaoqiFaceInfo& result){
+ qi::AnyReference ref;
+ std::ostringstream ss;
+
+ /* ShapeInfo */
+ ref = anyrefs[0].content();
+ result.shape_info = fromAnyValueToShapeInfo(ref, result.shape_info);
+
+ /* ExtraInfo */
+ ref = anyrefs[1].content();
+ NaoqiExtraInfo extra_info;
+ result.extra_info.push_back(fromAnyValueToExtraInfo(ref, extra_info));
+
+ return result;
+}
+
+NaoqiFaceDetected fromAnyValueToNaoqiFaceDetected(qi::AnyValue& value){
+ qi::AnyReferenceVector anyref;
+ NaoqiFaceDetected result;
+ std::ostringstream ss;
+ try{
+ anyref = value.asListValuePtr();
+ }
+ catch(std::runtime_error& e)
+ {
+ ss << "Could not transform AnyValue into list: " << e.what();
+ throw std::runtime_error(ss.str());
+ }
+ qi::AnyReference ref;
+
+ if ( anyref.size() != 5 ) {
+ return result;
+ }
+ /** TimeStamp_ **/
+ ref = anyref[0].content();
+ if(ref.kind() == qi::TypeKind_List)
+ {
+ result.timestamp = fromAnyValueToTimeStamp(ref, result.timestamp);
+ }
+ else
+ {
+ ss << "Could not retrieve timestamp";
+ throw std::runtime_error(ss.str());
+ }
+
+ qi::AnyReferenceVector vec;
+ vec = anyref[1].asListValuePtr(); // FaceInfo[N]
+ for(int i = 0; i < vec.size()-1; i++) // N
+ {
+ qi::AnyReference ref2 = vec[i].content();
+ struct NaoqiFaceInfo face_info;
+ face_info = fromAnyValueToFaceInfo(ref2, face_info);
+ result.face_info.push_back(face_info);
+ }
+
+ //CameraPose_InTorsoFrame,
+ ref = anyref[2].content();
+ for (int i = 0; i < 6; i++ ){
+ if(ref[i].content().kind() == qi::TypeKind_Float)
+ {
+ result.camera_pose_in_torso_frame[i] = ref[i].content().asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve Position6D";
+ throw std::runtime_error(ss.str());
+ }
+ }
+ //CameraPose_InRobotFrame,
+ ref = anyref[3].content();
+ for (int i = 0; i < 6; i++ ){
+ if(ref[i].content().kind() == qi::TypeKind_Float)
+ {
+ result.camera_pose_in_robot_frame[i] = ref[i].content().asFloat();
+ }
+ else
+ {
+ ss << "Could not retrieve Position6D";
+ throw std::runtime_error(ss.str());
+ }
+ }
+ //Camera_Id
+ ref = anyref[4].content();
+ if(ref.kind() == qi::TypeKind_Int)
+ {
+ result.camera_id = ref.asInt32();
+ }
+ else
+ {
+ ss << "Could not retrieve timestamp_us";
+ throw std::runtime_error(ss.str());
+ }
+
+ return result;
+}
std::vector fromAnyValueToFloatVector(qi::AnyValue& value, std::vector& result){
qi::AnyReferenceVector anyrefs = value.asListValuePtr();
diff --git a/src/tools/from_any_value.hpp b/src/tools/from_any_value.hpp
index e939f3f7..6a5d6676 100644
--- a/src/tools/from_any_value.hpp
+++ b/src/tools/from_any_value.hpp
@@ -22,7 +22,7 @@
* LOCAL includes
*/
#include "naoqi_image.hpp"
-
+#include "naoqi_facedetected.hpp"
/*
* ALDEBARAN includes
*/
@@ -34,6 +34,8 @@ namespace tools {
NaoqiImage fromAnyValueToNaoqiImage(qi::AnyValue& value);
+NaoqiFaceDetected fromAnyValueToNaoqiFaceDetected(qi::AnyValue& value);
+
std::vector fromAnyValueToStringVector(qi::AnyValue& value, std::vector& result);
std::vector fromAnyValueToFloatVector(qi::AnyValue& value, std::vector& result);
diff --git a/src/tools/naoqi_facedetected.hpp b/src/tools/naoqi_facedetected.hpp
new file mode 100644
index 00000000..443b4e66
--- /dev/null
+++ b/src/tools/naoqi_facedetected.hpp
@@ -0,0 +1,99 @@
+/*
+ * Copyright 2015 Aldebaran
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef NAOQI_FACEDETECTED_HPP
+#define NAOQI_FACEDETECTED_HPP
+
+#include
+#include
+
+namespace naoqi{
+
+namespace tools {
+
+/**
+ * @brief The struct describing an facedetected retrieved by ALFaceDetection
+ * This specification can be found here:
+ * http://doc.aldebaran.com/2-1/naoqi/peopleperception/alfacedetection.html#alfacedetection
+ */
+struct NaoqiEyePoints{
+ float eye_center_x;
+ float eye_center_y;
+ float nose_side_limit_x;
+ float nose_side_limit_y;
+ float ear_side_limit_x;
+ float ear_side_limit_y;
+};
+struct NaoqiNosePoints{
+ float bottom_center_limit_x;
+ float bottom_center_limit_y;
+ float bottom_left_limit_x;
+ float bottom_left_limit_y;
+ float bottom_right_limit_x;
+ float bottom_right_limit_y;
+};
+struct NaoqiMouthPoints{
+ float left_limit_x;
+ float left_limit_y;
+ float right_limit_x;
+ float right_limit_y;
+ float top_limit_x;
+ float top_limit_y;
+};
+struct NaoqiExtraInfo{
+ int face_id;
+ float score_reco;
+ std::string face_label;
+ struct NaoqiEyePoints left_eye_points;
+ struct NaoqiEyePoints right_eye_points;
+ struct NaoqiNosePoints nose_points;
+ struct NaoqiMouthPoints mouth_points;
+};
+struct NaoqiShapeInfo{
+ float alpha;
+ float beta;
+ float sizeX;
+ float sizeY;
+};
+struct NaoqiFaceInfo{
+ struct NaoqiShapeInfo shape_info;
+ std::vector extra_info;
+};
+struct NaoqiTimeStamp{
+ int timestamp_s;
+ int timestamp_us;
+};
+
+
+struct NaoqiFaceDetected{
+ //TimeStamp,
+ struct NaoqiTimeStamp timestamp;
+ //[ FaceInfo[N], Time_Filtered_Reco_Info ]
+ std::vector face_info;
+ //CameraPose_InTorsoFrame,
+ float camera_pose_in_torso_frame[6];
+ //CameraPose_InRobotFrame,
+ float camera_pose_in_robot_frame[6];
+ //Camera_Id
+ int camera_id;
+};
+
+}
+
+}
+
+#endif // NAOQI_FACEDETECTED_HPP