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