From 45ef82b0291a8b8a406ee002d8e1d3a986576445 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 8 Jan 2016 14:34:11 +0900 Subject: [PATCH 01/44] add event call back for peopledetect --- CMakeLists.txt | 2 + CMakeLists_catkin.txt | 1 + package.xml | 2 + share/boot_config.json | 4 + src/converters/people.cpp | 68 +++++ src/converters/people.hpp | 68 +++++ src/event/people.cpp | 263 +++++++++++++++++ src/event/people.hpp | 130 +++++++++ src/naoqi_driver.cpp | 19 ++ src/tools/from_any_value.cpp | 467 +++++++++++++++++++++++++++++++ src/tools/from_any_value.hpp | 4 +- src/tools/naoqi_facedetected.hpp | 99 +++++++ 12 files changed, 1126 insertions(+), 1 deletion(-) create mode 100644 src/converters/people.cpp create mode 100644 src/converters/people.hpp create mode 100644 src/event/people.cpp create mode 100644 src/event/people.hpp create mode 100644 src/tools/naoqi_facedetected.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 671ca98e..a97bafd7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,7 @@ 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 @@ -66,6 +67,7 @@ set( 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 3f42cc14..7498e259 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 aecd7151..b9a2b03e 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,7 @@ image_transport kdl_parser naoqi_bridge_msgs + nao_interaction_msgs naoqi_libqi naoqi_libqicore orocos_kdl @@ -36,6 +37,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 513254e8..96b34c0a 100644 --- a/share/boot_config.json +++ b/share/boot_config.json @@ -86,6 +86,10 @@ { "enabled" : true }, + "people": + { + "enabled" : true + }, "odom": { "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/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/naoqi_driver.cpp b/src/naoqi_driver.cpp index 43b805c6..dc8bc273 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -26,6 +26,7 @@ */ #include "converters/audio.hpp" #include "converters/touch.hpp" +#include "converters/people.hpp" #include "converters/camera.hpp" #include "converters/diagnostics.hpp" #include "converters/imu.hpp" @@ -86,6 +87,7 @@ #include "event/basic.hpp" #include "event/audio.hpp" #include "event/touch.hpp" +#include "event/people.hpp" /* * STATIC FUNCTIONS INCLUDE @@ -584,6 +586,8 @@ void Driver::registerDefaultConverter() bool bumper_enabled = boot_config_.get( "converters.bumper.enabled", true); bool hand_enabled = boot_config_.get( "converters.touch_hand.enabled", true); bool head_enabled = boot_config_.get( "converters.touch_head.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 @@ -839,6 +843,21 @@ void Driver::registerDefaultConverter() registerConverter( lc, lp, lr ); } + /** 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); + } + } } 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 From c5aef7a339cf21ab74954c5476d4957b35b2e9b9 Mon Sep 17 00:00:00 2001 From: nlyubova Date: Wed, 24 Feb 2016 11:03:44 +0100 Subject: [PATCH 02/44] adding kRawColorSpace --- src/naoqi_driver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index dc8bc273..85bd6c08 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -565,6 +565,7 @@ void Driver::registerDefaultConverter() size_t camera_depth_resolution = boot_config_.get( "converters.depth_camera.resolution", 1); // QVGA size_t camera_depth_fps = boot_config_.get( "converters.depth_camera.fps", 10); size_t camera_depth_recorder_fps = boot_config_.get( "converters.depth_camera.recorder_fps", 5); + size_t camera_depth_color_space = boot_config_.get( "converters.depth_camera.color_space", 23); bool camera_ir_enabled = boot_config_.get( "converters.ir_camera.enabled", true); size_t camera_ir_resolution = boot_config_.get( "converters.ir_camera.resolution", 1); // QVGA From 5b43f23b31e0dfb06b063d1c231ab861f58d6785 Mon Sep 17 00:00:00 2001 From: nlyubova Date: Mon, 19 Sep 2016 16:16:04 +0200 Subject: [PATCH 03/44] adding distortion --- src/converters/camera_info_definitions.hpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/converters/camera_info_definitions.hpp b/src/converters/camera_info_definitions.hpp index 7d935f3a..2f313af0 100644 --- a/src/converters/camera_info_definitions.hpp +++ b/src/converters/camera_info_definitions.hpp @@ -60,14 +60,14 @@ inline sensor_msgs::CameraInfo createCameraInfoTOPQVGA() cam_info_msg.width = 320; cam_info_msg.height = 240; - cam_info_msg.K = boost::array{{ 274.139508945831, 0, 141.184472810944, 0, 275.741846757374, 106.693773654172, 0, 0, 1 }}; + cam_info_msg.K = boost::array{{ 299.6062514048825, 0.0, 158.1342557533627, 0.0, 299.0567251221516, 128.32075058816002, 0.0, 0.0, 1.0 }}; cam_info_msg.distortion_model = "plumb_bob"; - cam_info_msg.D = boost::assign::list_of(-0.0870160932911717)(0.128210165050533)(0.003379500659424)(-0.00106205540818586)(0).convert_to_container >(); + cam_info_msg.D = boost::assign::list_of(0.10703186895554358)(-0.29398440554631)(-0.000510763491296248)(-0.0007849727454197071)(0).convert_to_container >(); cam_info_msg.R = boost::array{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }}; - cam_info_msg.P = boost::array{{ 272.423675537109, 0, 141.131930791285, 0, 0, 273.515747070312, 107.391746054313, 0, 0, 0, 1, 0 }}; + cam_info_msg.P = boost::array{{ 300.6907958984375, 0.0, 157.3720124539832, 0.0, 0.0, 300.78662109375, 127.71553373332426, 0.0, 0.0, 0.0, 1.0, 0.0 }}; return cam_info_msg; } @@ -173,8 +173,8 @@ inline sensor_msgs::CameraInfo createCameraInfoDEPTHVGA() cam_info_msg.height = 480; cam_info_msg.K = boost::array{{ 525, 0, 319.5000000, 0, 525, 239.5000000000000, 0, 0, 1 }}; - //cam_info_msg.distortion_model = "plumb_bob"; - //cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0); + cam_info_msg.distortion_model = "plumb_bob"; + cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0); cam_info_msg.R = boost::array{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }}; @@ -192,14 +192,14 @@ inline sensor_msgs::CameraInfo createCameraInfoDEPTHQVGA() cam_info_msg.width = 320; cam_info_msg.height = 240; - cam_info_msg.K = boost::array{{ 525/2.0f, 0, 319.5000000/2.0f, 0, 525/2.0f, 239.5000000000000/2.0f, 0, 0, 1 }}; + cam_info_msg.K = boost::array{{ 285.6768151355473, 0.0, 161.1691935721059, 0.0, 285.5373311462721, 127.602991640182, 0.0, 0.0, 1.0 }}; - //cam_info_msg.distortion_model = "plumb_bob"; - //cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0); + cam_info_msg.distortion_model = "plumb_bob"; + cam_info_msg.D = boost::assign::list_of(0.016936081367960803)(-0.06749793149686571)(-0.0003138996585151219)(-0.0006992075603777127)(0); cam_info_msg.R = boost::array{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }}; - cam_info_msg.P = boost::array{{ 525/2.0f, 0, 319.500000/2.0f, 0, 0, 525/2.0f, 239.5000000000/2.0f, 0, 0, 0, 1, 0 }}; + cam_info_msg.P = boost::array{{ 284.4026794433594, 0.0, 160.49353466767207, 0.0, 0.0, 284.6112365722656, 127.04678797627457, 0.0, 0.0, 0.0, 1.0, 0.0 }}; return cam_info_msg; } @@ -214,8 +214,8 @@ inline sensor_msgs::CameraInfo createCameraInfoDEPTHQQVGA() cam_info_msg.height = 120; cam_info_msg.K = boost::array{{ 525/4.0f, 0, 319.5000000/4.0f, 0, 525/4.0f, 239.5000000000000/4.0f, 0, 0, 1 }}; - //cam_info_msg.distortion_model = "plumb_bob"; - //cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0); + cam_info_msg.distortion_model = "plumb_bob"; + cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0); cam_info_msg.R = boost::array{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }}; From b4f64a71954cfd71349d7928fbfddb4405c65aea Mon Sep 17 00:00:00 2001 From: nlyubova Date: Mon, 19 Sep 2016 16:24:44 +0200 Subject: [PATCH 04/44] adding a launch file for depth registration --- launch/register_depth.launch | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 launch/register_depth.launch diff --git a/launch/register_depth.launch b/launch/register_depth.launch new file mode 100644 index 00000000..5a79da73 --- /dev/null +++ b/launch/register_depth.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + From 96d05a128f1df968ef5245222d43f9d75e7f39ca Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Mon, 26 Sep 2016 17:10:23 +0100 Subject: [PATCH 05/44] Adding PersonDetected event. Published under ~person_detected as a PoseArray Also adding the possibility of publishing mutliple faces --- src/converters/people.cpp | 3 +- src/converters/people.hpp | 4 +- src/event/people.cpp | 149 +++++++++++++++++++------------ src/event/people.hpp | 34 +++++-- src/naoqi_driver.cpp | 14 +++ src/tools/from_any_value.cpp | 131 +++++++++++++++++++++++++++ src/tools/from_any_value.hpp | 2 + src/tools/naoqi_facedetected.hpp | 19 ++++ 8 files changed, 293 insertions(+), 63 deletions(-) diff --git a/src/converters/people.cpp b/src/converters/people.cpp index 71059701..78e94064 100644 --- a/src/converters/people.cpp +++ b/src/converters/people.cpp @@ -62,7 +62,8 @@ void PeopleEventConverter::callAll(const std::vector; +template class PeopleEventConverter; +template class PeopleEventConverter; } } diff --git a/src/converters/people.hpp b/src/converters/people.hpp index 0b95dcb2..3caa0192 100644 --- a/src/converters/people.hpp +++ b/src/converters/people.hpp @@ -27,7 +27,9 @@ /* * ROS includes */ -#include +#include +#include +#include /* * ALDEBARAN includes diff --git a/src/event/people.cpp b/src/event/people.cpp index 607487db..5b4c1c3a 100644 --- a/src/event/people.cpp +++ b/src/event/people.cpp @@ -27,6 +27,7 @@ #include #include #include "../tools/from_any_value.hpp" +#include #include "people.hpp" @@ -112,7 +113,9 @@ void PeopleEventRegister::stopProcess() //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"); + for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it) { + p_memory_.call("unsubscribeToEvent",it->c_str(), serviceName); + } session_->unregisterService(serviceId); serviceId = 0; } @@ -171,20 +174,8 @@ 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); - } + peopleCallbackMessage(key, value, msg); std::vector actions; boost::mutex::scoped_lock callback_lock(mutex_); @@ -212,52 +203,100 @@ void PeopleEventRegister::peopleCallback(std::string &key, qi::AnyValue &valu } template -void PeopleEventRegister::peopleCallbackMessage(std::string &key, tools::NaoqiFaceDetected &faces, nao_interaction_msgs::FaceDetected &msg) +void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::FacesDetected &msg) { + 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 ) return; + 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; + msg.header.stamp = ros::Time::now(); // ros::Time(faces.timestamp.timestamp_s, faces.timestamp.timestamp_us); // This gives time till start not the system time + + nao_interaction_msgs::FaceDetected face; + for(int i = 0; i < faces.face_info.size(); i++) { + face.face_id.data = faces.face_info[i].extra_info[0].face_id; + face.score_reco.data = faces.face_info[i].extra_info[0].score_reco; + face.face_label.data = faces.face_info[i].extra_info[0].face_label; + + face.shape_alpha.data = faces.face_info[i].shape_info.alpha; + face.shape_beta.data = faces.face_info[i].shape_info.beta; + face.shape_sizeX.data = faces.face_info[i].shape_info.sizeX; + face.shape_sizeY.data = faces.face_info[i].shape_info.sizeY; + + face.right_eye_eyeCenter_x.data = faces.face_info[i].extra_info[0].right_eye_points.eye_center_x; + face.right_eye_eyeCenter_y.data = faces.face_info[i].extra_info[0].right_eye_points.eye_center_y; + face.right_eye_noseSideLimit_x.data = faces.face_info[i].extra_info[0].right_eye_points.nose_side_limit_x; + face.right_eye_noseSideLimit_y.data = faces.face_info[i].extra_info[0].right_eye_points.nose_side_limit_y; + face.right_eye_earSideLimit_x.data = faces.face_info[i].extra_info[0].right_eye_points.ear_side_limit_x; + face.right_eye_earSideLimit_y.data = faces.face_info[i].extra_info[0].right_eye_points.ear_side_limit_y; + + face.left_eye_eyeCenter_x.data = faces.face_info[i].extra_info[0].left_eye_points.eye_center_x; + face.left_eye_eyeCenter_y.data = faces.face_info[i].extra_info[0].left_eye_points.eye_center_y; + face.left_eye_noseSideLimit_x.data = faces.face_info[i].extra_info[0].left_eye_points.nose_side_limit_x; + face.left_eye_noseSideLimit_y.data = faces.face_info[i].extra_info[0].left_eye_points.nose_side_limit_y; + face.left_eye_earSideLimit_x.data = faces.face_info[i].extra_info[0].left_eye_points.ear_side_limit_x; + face.left_eye_earSideLimit_y.data = faces.face_info[i].extra_info[0].left_eye_points.ear_side_limit_y; + + face.nose_bottomCenterLimit_x.data = faces.face_info[i].extra_info[0].nose_points.bottom_center_limit_x; + face.nose_bottomCenterLimit_y.data = faces.face_info[i].extra_info[0].nose_points.bottom_center_limit_y; + face.nose_bottomLeftLimit_x.data = faces.face_info[i].extra_info[0].nose_points.bottom_left_limit_x; + face.nose_bottomLeftLimit_y.data = faces.face_info[i].extra_info[0].nose_points.bottom_left_limit_y; + face.nose_bottomRightLimit_x.data = faces.face_info[i].extra_info[0].nose_points.bottom_right_limit_x; + face.nose_bottomRightLimit_y.data = faces.face_info[i].extra_info[0].nose_points.bottom_right_limit_y; + + face.mouth_leftLimit_x.data = faces.face_info[i].extra_info[0].mouth_points.left_limit_x; + face.mouth_leftLimit_y.data = faces.face_info[i].extra_info[0].mouth_points.left_limit_y; + face.mouth_rightLimit_x.data = faces.face_info[i].extra_info[0].mouth_points.right_limit_x; + face.mouth_rightLimit_y.data = faces.face_info[i].extra_info[0].mouth_points.right_limit_y; + face.mouth_topLimit_x.data = faces.face_info[i].extra_info[0].mouth_points.top_limit_x; + face.mouth_topLimit_y.data = faces.face_info[i].extra_info[0].mouth_points.top_limit_y; + + msg.faces.push_back(face); } } +template +geometry_msgs::Point PeopleEventRegister::toCartesian(float dist, float azi, float inc) { + geometry_msgs::Point p; + p.x = dist * std::sin(inc) * std::cos(azi) * (-1); // Inverted + p.y = dist * std::sin(inc) * std::sin(azi); + p.z = dist * std::cos(inc); + return p; +} + +template +void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, geometry_msgs::PoseArray &msg) +{ + tools::NaoqiPersonDetected people; + try { + people = tools::fromAnyValueToNaoqiPersonDetected(value); + } + catch(std::runtime_error& e) + { + std::cout << "Cannot retrieve persondetected: " << e.what() << std::endl; + return; + } + msg.header.frame_id = "CameraDepth_optical_frame"; + msg.header.stamp = ros::Time::now(); //ros::Time(people.timestamp.timestamp_s, people.timestamp.timestamp_us); // This gives time till start not the system time + + for(int i = 0; i < people.person_info.size(); i++) { + geometry_msgs::Pose p; + p.position = toCartesian(people.person_info[i].distance_to_camera, people.person_info[i].pitch_angle_in_image, people.person_info[i].yaw_angle_in_image); + p.orientation.w = 1.0; + + msg.poses.push_back(p); + } +} + // http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor -template class PeopleEventRegister; +template class PeopleEventRegister; +template class PeopleEventRegister; }//namespace diff --git a/src/event/people.hpp b/src/event/people.hpp index 126e6616..ed42d1c7 100644 --- a/src/event/people.hpp +++ b/src/event/people.hpp @@ -19,6 +19,7 @@ #define PEOPLE_EVENT_REGISTER_HPP #include +#include #include #include @@ -29,6 +30,9 @@ #include #include +#include +#include +#include #include #include @@ -79,13 +83,14 @@ class PeopleEventRegister: public boost::enable_shared_from_this > converter_; @@ -103,28 +108,45 @@ class PeopleEventRegister: public boost::enable_shared_from_this keys_; }; // class -class FaceDetectedEventRegister: public PeopleEventRegister +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) {} +}; + +class PersonDetectedEventRegister: public PeopleEventRegister { public: - FaceDetectedEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session ) : PeopleEventRegister(name, keys, frequency, session) {} + PersonDetectedEventRegister( 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) +//QI_REGISTER_OBJECT(PersonDetectedEventRegister, peopleCallback) static bool _qiregisterPeopleEventRegisterFaceDetected() { - ::qi::ObjectTypeBuilder > b; - QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback) + ::qi::ObjectTypeBuilder > b; + QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback) b.registerType(); return true; } static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterPeopleEventRegisterFaceDetected(); +static bool _qiregisterPeopleEventRegisterPersonDetected() { + ::qi::ObjectTypeBuilder > b; + QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback) + b.registerType(); + return true; + } +static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterPeopleEventRegisterPersonDetected(); + } //naoqi #endif diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 85bd6c08..3954a3a2 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -859,6 +859,20 @@ void Driver::registerDefaultConverter() event_map_.find("face_detected")->second.isPublishing(true); } } + if ( people_enabled ) + { + std::vector people_events; + people_events.push_back("PeoplePerception/PeopleDetected"); + boost::shared_ptr event_register = + boost::make_shared( "people_detected", people_events, 0, sessionPtr_ ); + insertEventConverter("people_detected", event_register); + if (keep_looping) { + event_map_.find("people_detected")->second.startProcess(); + } + if (publish_enabled_) { + event_map_.find("people_detected")->second.isPublishing(true); + } + } } diff --git a/src/tools/from_any_value.cpp b/src/tools/from_any_value.cpp index 4eb84881..1c8291c7 100644 --- a/src/tools/from_any_value.cpp +++ b/src/tools/from_any_value.cpp @@ -649,6 +649,137 @@ NaoqiFaceDetected fromAnyValueToNaoqiFaceDetected(qi::AnyValue& value){ return result; } +NaoqiPersonInfo fromAnyValueToPersonInfo(qi::AnyReference& anyrefs, NaoqiPersonInfo& result){ + qi::AnyReference ref; + std::ostringstream ss; + + ref = anyrefs[0].content(); + if(ref.kind() == qi::TypeKind_Int) + { + result.id = ref.asInt32(); + } + else + { + ss << "Could not retrieve id"; + throw std::runtime_error(ss.str()); + } + + ref = anyrefs[1].content(); + if(ref.kind() == qi::TypeKind_Float) + { + result.distance_to_camera = ref.asFloat(); + } + else + { + ss << "Could not retrieve distance"; + throw std::runtime_error(ss.str()); + } + + ref = anyrefs[2].content(); + if(ref.kind() == qi::TypeKind_Float) + { + result.pitch_angle_in_image = ref.asFloat(); + } + else + { + ss << "Could not retrieve pitch"; + throw std::runtime_error(ss.str()); + } + + ref = anyrefs[3].content(); + if(ref.kind() == qi::TypeKind_Float) + { + result.yaw_angle_in_image = ref.asFloat(); + } + else + { + ss << "Could not retrieve yaw"; + throw std::runtime_error(ss.str()); + } + + return result; +} + +NaoqiPersonDetected fromAnyValueToNaoqiPersonDetected(qi::AnyValue& value) { + qi::AnyReferenceVector anyref; + NaoqiPersonDetected 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(); i++) // N + { + qi::AnyReference ref2 = vec[i].content(); + struct NaoqiPersonInfo person_info; + person_info = fromAnyValueToPersonInfo(ref2, person_info); + result.person_info.push_back(person_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 6a5d6676..5b6e0d2c 100644 --- a/src/tools/from_any_value.hpp +++ b/src/tools/from_any_value.hpp @@ -36,6 +36,8 @@ NaoqiImage fromAnyValueToNaoqiImage(qi::AnyValue& value); NaoqiFaceDetected fromAnyValueToNaoqiFaceDetected(qi::AnyValue& value); +NaoqiPersonDetected fromAnyValueToNaoqiPersonDetected(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 index 443b4e66..587483d3 100644 --- a/src/tools/naoqi_facedetected.hpp +++ b/src/tools/naoqi_facedetected.hpp @@ -77,6 +77,12 @@ struct NaoqiTimeStamp{ int timestamp_s; int timestamp_us; }; +struct NaoqiPersonInfo{ + int id; + float distance_to_camera; + float pitch_angle_in_image; + float yaw_angle_in_image; +}; struct NaoqiFaceDetected{ @@ -92,6 +98,19 @@ struct NaoqiFaceDetected{ int camera_id; }; +struct NaoqiPersonDetected{ + //TimeStamp, + struct NaoqiTimeStamp timestamp; + // Person info + std::vector person_info; + //CameraPose_InTorsoFrame, + float camera_pose_in_torso_frame[6]; + //CameraPose_InRobotFrame, + float camera_pose_in_robot_frame[6]; + //Camera_Id + int camera_id; +}; + } } From 6bb05af4b2e0aab5ee753dbe8255f0da56830be5 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Thu, 29 Sep 2016 17:27:01 +0100 Subject: [PATCH 06/44] Adding person characteristics --- src/converters/people.cpp | 2 +- src/converters/people.hpp | 4 +- src/event/people.cpp | 229 ++++++++++++++++++++++++++++++++++++-- src/event/people.hpp | 31 ++++-- src/naoqi_driver.cpp | 7 +- 5 files changed, 245 insertions(+), 28 deletions(-) diff --git a/src/converters/people.cpp b/src/converters/people.cpp index 78e94064..07e45349 100644 --- a/src/converters/people.cpp +++ b/src/converters/people.cpp @@ -63,7 +63,7 @@ void PeopleEventConverter::callAll(const std::vector; -template class PeopleEventConverter; +template class PeopleEventConverter; } } diff --git a/src/converters/people.hpp b/src/converters/people.hpp index 3caa0192..38de0081 100644 --- a/src/converters/people.hpp +++ b/src/converters/people.hpp @@ -28,9 +28,7 @@ * ROS includes */ #include -#include -#include - +#include /* * ALDEBARAN includes */ diff --git a/src/event/people.cpp b/src/event/people.cpp index 5b4c1c3a..55f4aa60 100644 --- a/src/event/people.cpp +++ b/src/event/people.cpp @@ -43,12 +43,26 @@ 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")), + p_people_( session->service("ALPeoplePerception") ), + p_gaze_( session->service("ALGazeAnalysis") ), + p_face_( session->service("ALFaceCharacteristics") ), session_(session), isStarted_(false), isPublishing_(false), isRecording_(false), - isDumping_(false) + isDumping_(false), + prefix("PeoplePerception/Person/") { + memory_keys.push_back("/IsFaceDetected"); + memory_keys.push_back("/GazeDirection"); + memory_keys.push_back("/HeadAngles"); + memory_keys.push_back("/IsLookingAtRobot"); + memory_keys.push_back("/LookingAtRobotScore"); + memory_keys.push_back("/AgeProperties"); + memory_keys.push_back("/GenderProperties"); + memory_keys.push_back("/SmileProperties"); + memory_keys.push_back("/ExpressionProperties"); + publisher_ = boost::make_shared >( name ); //recorder_ = boost::make_shared >( name ); converter_ = boost::make_shared >( name, frequency, session ); @@ -98,6 +112,14 @@ void PeopleEventRegister::startProcess() std::cerr << *it << std::endl; p_memory_.call("subscribeToEvent",it->c_str(), serviceName, "peopleCallback"); } + if(keys_[0].compare("PeoplePerception/PeopleDetected")==0) { + std::cout< People : Start"<("subscribe", "ROS"); + std::cout< Gaze : Start"<("subscribe", "ROS"); + std::cout< Face : Start"<("subscribe", "ROS"); + } std::cout << serviceName << " : Start" << std::endl; } isStarted_ = true; @@ -113,6 +135,14 @@ void PeopleEventRegister::stopProcess() //std::string serviceName = std::string("ROS-Driver-") + typeid(T).name(); std::string serviceName = std::string("ROS-Driver-") + keys_[0]; if(serviceId){ + if(keys_[0].compare("PeoplePerception/PeopleDetected")==0) { + std::cout< People : Stop"<("unsubscribe", "ROS"); + std::cout< Gaze : Stop"<("unsubscribe", "ROS"); + std::cout< Face : Stop"<("unsubscribe", "ROS"); + } for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it) { p_memory_.call("unsubscribeToEvent",it->c_str(), serviceName); } @@ -205,6 +235,7 @@ void PeopleEventRegister::peopleCallback(std::string &key, qi::AnyValue &valu template void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::FacesDetected &msg) { + std::cout<<"THERE"<::peopleCallbackMessage(std::string &key, qi::AnyValu } if ( faces.face_info.size() == 0 ) return; - msg.header.frame_id = ""; + msg.header.frame_id = "CameraTop_optical_frame"; msg.header.stamp = ros::Time::now(); // ros::Time(faces.timestamp.timestamp_s, faces.timestamp.timestamp_us); // This gives time till start not the system time - nao_interaction_msgs::FaceDetected face; for(int i = 0; i < faces.face_info.size(); i++) { + nao_interaction_msgs::FaceDetected face; + face.header = msg.header; face.face_id.data = faces.face_info[i].extra_info[0].face_id; face.score_reco.data = faces.face_info[i].extra_info[0].score_reco; face.face_label.data = faces.face_info[i].extra_info[0].face_label; @@ -272,8 +304,9 @@ geometry_msgs::Point PeopleEventRegister::toCartesian(float dist, float azi, } template -void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, geometry_msgs::PoseArray &msg) +void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::PersonCharacteristicsArray &msg) { + std::cout<<"HERE"<::peopleCallbackMessage(std::string &key, qi::AnyValu std::cout << "Cannot retrieve persondetected: " << e.what() << std::endl; return; } + msg.header.frame_id = "CameraDepth_optical_frame"; msg.header.stamp = ros::Time::now(); //ros::Time(people.timestamp.timestamp_s, people.timestamp.timestamp_us); // This gives time till start not the system time for(int i = 0; i < people.person_info.size(); i++) { - geometry_msgs::Pose p; - p.position = toCartesian(people.person_info[i].distance_to_camera, people.person_info[i].pitch_angle_in_image, people.person_info[i].yaw_angle_in_image); - p.orientation.w = 1.0; + std::string sid = num_to_str(people.person_info[i].id); + std::vector keys; + for(int j = 0; j < memory_keys.size(); j++) { + keys.push_back(prefix+sid+memory_keys[j]); + } + + qi::AnyValue data = (qi::AnyValue)p_memory_.call("getListData", keys); + + nao_interaction_msgs::PersonCharacteristics pc; + pc.face.gender = -1; // Since 0 = female, initialising as -1 to disambigute no data from femal. + + /* People Perception */ + try { + pc.id = people.person_info[i].id; + pc.person.distance = people.person_info[i].distance_to_camera; + pc.person.yaw = people.person_info[i].yaw_angle_in_image; + pc.person.pitch = people.person_info[i].pitch_angle_in_image; + pc.person.position.position = toCartesian(pc.person.distance, pc.person.pitch, pc.person.yaw); + pc.person.position.orientation.w = 1.0; + + if(data.size() != 9) { + msg.person_array.push_back(pc); + ROS_DEBUG("Could not retrieve any face information"); + continue; + } + + try { + pc.person.face_detected = (bool)data[0].content().asInt32(); + } catch(...) { + ROS_DEBUG("Error retreiving face detected"); + } + + if(pc.person.face_detected) { + /* Gaze Analysis */ + try { + qi::AnyReference gaze = data[1].content(); + if(gaze.kind() == qi::TypeKind_List && gaze.size() == 2) + { + qi::AnyReference g, yaw, pitch; + yaw = gaze[0].content(); + pitch = gaze[1].content(); + if(yaw.kind() == qi::TypeKind_Float && pitch.kind() == qi::TypeKind_Float) { + tf::Quaternion q; + q.setRPY(0.0, pitch.asFloat(), yaw.asFloat()); + pc.gaze.gaze.x = q.x(); + pc.gaze.gaze.y = q.y(); + pc.gaze.gaze.z = q.z(); + pc.gaze.gaze.w = q.w(); + } else { + ROS_DEBUG("Could not retrieve yaw/pitch"); + } + } else { + ROS_DEBUG("Could not retrieve gaze"); + } + } catch(std::runtime_error& e) { + ROS_DEBUG_STREAM("Error retrieving gaze angle: " << e.what()); + } + try { + qi::AnyReference head_angle = data[2].content(); + if(head_angle.kind() == qi::TypeKind_List && head_angle.size() == 3) + { + qi::AnyReference yaw, pitch, roll; + yaw = head_angle[0].content(); + pitch = head_angle[1].content(); + roll = head_angle[2].content(); + if(yaw.kind() == qi::TypeKind_Float && pitch.kind() == qi::TypeKind_Float && roll.kind() == qi::TypeKind_Float) { + tf::Quaternion q; + q.setRPY(roll.asFloat(), pitch.asFloat(), yaw.asFloat()); + pc.gaze.head_angle.x = q.x(); + pc.gaze.head_angle.y = q.y(); + pc.gaze.head_angle.z = q.z(); + pc.gaze.head_angle.w = q.w(); + } else { + ROS_DEBUG("Could not retrieve yaw/pitch/roll"); + } + } else { + ROS_DEBUG("Could not retrieve head angle"); + } + } catch(std::runtime_error& e) { + ROS_DEBUG_STREAM("Error retrieving head angle: " << e.what()); + } + try { + pc.gaze.looking_at_robot = (bool)data[3].content().asInt32(); + pc.gaze.looking_at_robot_score = data[4].content().asFloat(); + } catch(std::runtime_error& e) { + ROS_DEBUG_STREAM("Error retrieving looking at robot: " << e.what()); + } + + /* Face Charcteristics */ + try { + qi::AnyReference age_props = data[5].content(); + if(age_props.kind() == qi::TypeKind_List && age_props.size() == 2) + { + qi::AnyReference age, conf; + age = age_props[0].content(); + conf = age_props[1].content(); + if(age.kind() == qi::TypeKind_Float && conf.kind() == qi::TypeKind_Float) { + pc.face.age = (int)age.asFloat(); + pc.face.age_confidence = conf.asFloat(); + } else { + ROS_DEBUG("Could not retrieve age and confidence"); + } + } else { + ROS_DEBUG("Could not retrieve age"); + } + } catch(std::runtime_error& e) { + ROS_DEBUG_STREAM("Error retrieving age: " << e.what()); + } + try { + qi::AnyReference gender_props = data[6].content(); + if(gender_props.kind() == qi::TypeKind_List && gender_props.size() == 2) + { + qi::AnyReference gender, conf; + gender = gender_props[0].content(); + conf = gender_props[1].content(); + if(gender.kind() == qi::TypeKind_Float && conf.kind() == qi::TypeKind_Float) { + pc.face.gender = (int)gender.asFloat(); + pc.face.gender_confidence = conf.asFloat(); + } else { + ROS_DEBUG("Could not retrieve gender and confidence"); + } + } else { + ROS_DEBUG("Could not retrieve gender"); + } + } catch(std::runtime_error& e) { + ROS_DEBUG_STREAM("Error retrieving gender: " << e.what()); + } + try { + qi::AnyReference smile_props = data[7].content(); + if(smile_props.kind() == qi::TypeKind_List && smile_props.size() == 2) + { + qi::AnyReference smile_degree, conf; + smile_degree = smile_props[0].content(); + conf = smile_props[1].content(); + if(smile_degree.kind() == qi::TypeKind_Float && conf.kind() == qi::TypeKind_Float) { + pc.face.smile_degree = smile_degree.asFloat(); + pc.face.smile_degree_confidence = conf.asFloat(); + } else { + ROS_DEBUG("Could not retrieve smile degree and confidence"); + } + } else { + ROS_DEBUG("Could not retrieve smile"); + } + } catch(std::runtime_error& e) { + ROS_DEBUG_STREAM("Error retrieving smile: " << e.what()); + } + try { + qi::AnyReference expression_props = data[8].content(); + if(expression_props.kind() == qi::TypeKind_List && expression_props.size() == 5) + { + qi::AnyReference neutral, happy, surprised, angry, sad; + neutral = expression_props[0].content(); + happy = expression_props[1].content(); + surprised = expression_props[2].content(); + angry = expression_props[3].content(); + sad = expression_props[4].content(); + if(neutral.kind() == qi::TypeKind_Float + && happy.kind() == qi::TypeKind_Float + && surprised.kind() == qi::TypeKind_Float + && angry.kind() == qi::TypeKind_Float + && sad.kind() == qi::TypeKind_Float) { + pc.face.expression_properties.neutral = neutral.asFloat(); + pc.face.expression_properties.happy = happy.asFloat(); + pc.face.expression_properties.surprised = surprised.asFloat(); + pc.face.expression_properties.angry = angry.asFloat(); + pc.face.expression_properties.sad = sad.asFloat(); + } else { + ROS_DEBUG("Could not retrieve neutral/happy/surprised/angry/sad"); + } + } else { + ROS_DEBUG("Could not retrieve expression"); + } + } catch(std::runtime_error& e) { + ROS_DEBUG_STREAM("Error retrieving expression: " << e.what()); + } + } + } catch(std::runtime_error& e) { + ROS_DEBUG_STREAM("Error retrieving person information: " << e.what()); + } + - msg.poses.push_back(p); + msg.person_array.push_back(pc); } } // http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor template class PeopleEventRegister; -template class PeopleEventRegister; +template class PeopleEventRegister; }//namespace diff --git a/src/event/people.hpp b/src/event/people.hpp index ed42d1c7..8a52e676 100644 --- a/src/event/people.hpp +++ b/src/event/people.hpp @@ -31,8 +31,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -84,13 +84,19 @@ class PeopleEventRegister: public boost::enable_shared_from_this + std::string num_to_str(N num) { + std::stringstream ss; + ss << num; + return ss.str(); + } private: boost::shared_ptr > converter_; @@ -98,7 +104,7 @@ class PeopleEventRegister: public boost::enable_shared_from_this > recorder_; qi::SessionPtr session_; - qi::AnyObject p_memory_; + qi::AnyObject p_memory_, p_people_, p_gaze_, p_face_; unsigned int serviceId; std::string name_; @@ -109,7 +115,8 @@ class PeopleEventRegister: public boost::enable_shared_from_this memory_keys; protected: std::vector keys_; @@ -122,14 +129,14 @@ class FaceDetectedEventRegister: public PeopleEventRegister keys, const float& frequency, const qi::SessionPtr& session ) : PeopleEventRegister(name, keys, frequency, session) {} }; -class PersonDetectedEventRegister: public PeopleEventRegister +class PersonCharacteristicsEventRegister: public PeopleEventRegister { public: - PersonDetectedEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session ) : PeopleEventRegister(name, keys, frequency, session) {} + PersonCharacteristicsEventRegister( 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) -//QI_REGISTER_OBJECT(PersonDetectedEventRegister, peopleCallback) +//QI_REGISTER_OBJECT(PersonCharacteristicsEventRegister, peopleCallback) static bool _qiregisterPeopleEventRegisterFaceDetected() { ::qi::ObjectTypeBuilder > b; @@ -139,13 +146,13 @@ static bool _qiregisterPeopleEventRegisterFaceDetected() { } static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterPeopleEventRegisterFaceDetected(); -static bool _qiregisterPeopleEventRegisterPersonDetected() { - ::qi::ObjectTypeBuilder > b; - QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback) +static bool _qiregisterPeopleEventRegisterPersonCharacteristics() { + ::qi::ObjectTypeBuilder > b; + QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback) b.registerType(); return true; } -static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterPeopleEventRegisterPersonDetected(); +static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterPeopleEventRegisterPersonCharacteristics(); } //naoqi diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 3954a3a2..dcc2facc 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -588,6 +588,7 @@ void Driver::registerDefaultConverter() bool hand_enabled = boot_config_.get( "converters.touch_hand.enabled", true); bool head_enabled = boot_config_.get( "converters.touch_head.enabled", true); + bool face_enabled = boot_config_.get( "converters.face.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 @@ -845,7 +846,7 @@ void Driver::registerDefaultConverter() } /** PEOPLE **/ - if ( people_enabled ) + if ( face_enabled ) { std::vector people_events; people_events.push_back("FaceDetected"); @@ -863,8 +864,8 @@ void Driver::registerDefaultConverter() { std::vector people_events; people_events.push_back("PeoplePerception/PeopleDetected"); - boost::shared_ptr event_register = - boost::make_shared( "people_detected", people_events, 0, sessionPtr_ ); + boost::shared_ptr event_register = + boost::make_shared( "people_detected", people_events, 0, sessionPtr_ ); insertEventConverter("people_detected", event_register); if (keep_looping) { event_map_.find("people_detected")->second.startProcess(); From f61fcc59731d9dc6474c803de0e547c588913603 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Fri, 30 Sep 2016 09:49:02 +0100 Subject: [PATCH 07/44] Renaming messages to conform with naming scheme --- src/converters/people.cpp | 4 +-- src/converters/people.hpp | 4 +-- src/event/people.cpp | 76 +++++++++++++++++++-------------------- src/event/people.hpp | 25 +++++++------ 4 files changed, 54 insertions(+), 55 deletions(-) diff --git a/src/converters/people.cpp b/src/converters/people.cpp index 07e45349..f5ef665a 100644 --- a/src/converters/people.cpp +++ b/src/converters/people.cpp @@ -62,8 +62,8 @@ void PeopleEventConverter::callAll(const std::vector; -template class PeopleEventConverter; +template class PeopleEventConverter; +template class PeopleEventConverter; } } diff --git a/src/converters/people.hpp b/src/converters/people.hpp index 38de0081..9f2e04d8 100644 --- a/src/converters/people.hpp +++ b/src/converters/people.hpp @@ -27,8 +27,8 @@ /* * ROS includes */ -#include -#include +#include +#include /* * ALDEBARAN includes */ diff --git a/src/event/people.cpp b/src/event/people.cpp index 55f4aa60..15e5a2d1 100644 --- a/src/event/people.cpp +++ b/src/event/people.cpp @@ -233,7 +233,7 @@ void PeopleEventRegister::peopleCallback(std::string &key, qi::AnyValue &valu } template -void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::FacesDetected &msg) +void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::FaceDetectedArray &msg) { std::cout<<"THERE"<::peopleCallbackMessage(std::string &key, qi::AnyValu face.mouth_topLimit_x.data = faces.face_info[i].extra_info[0].mouth_points.top_limit_x; face.mouth_topLimit_y.data = faces.face_info[i].extra_info[0].mouth_points.top_limit_y; - msg.faces.push_back(face); + msg.face_array.push_back(face); } } @@ -304,7 +304,7 @@ geometry_msgs::Point PeopleEventRegister::toCartesian(float dist, float azi, } template -void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::PersonCharacteristicsArray &msg) +void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::PersonDetectedArray &msg) { std::cout<<"HERE"<::peopleCallbackMessage(std::string &key, qi::AnyValu qi::AnyValue data = (qi::AnyValue)p_memory_.call("getListData", keys); - nao_interaction_msgs::PersonCharacteristics pc; - pc.face.gender = -1; // Since 0 = female, initialising as -1 to disambigute no data from femal. + nao_interaction_msgs::PersonDetected pd; + pd.face.gender = -1; // Since 0 = female, initialising as -1 to disambigute no data from femal. /* People Perception */ try { - pc.id = people.person_info[i].id; - pc.person.distance = people.person_info[i].distance_to_camera; - pc.person.yaw = people.person_info[i].yaw_angle_in_image; - pc.person.pitch = people.person_info[i].pitch_angle_in_image; - pc.person.position.position = toCartesian(pc.person.distance, pc.person.pitch, pc.person.yaw); - pc.person.position.orientation.w = 1.0; + pd.id = people.person_info[i].id; + pd.person.distance = people.person_info[i].distance_to_camera; + pd.person.yaw = people.person_info[i].yaw_angle_in_image; + pd.person.pitch = people.person_info[i].pitch_angle_in_image; + pd.person.position.position = toCartesian(pd.person.distance, pd.person.pitch, pd.person.yaw); + pd.person.position.orientation.w = 1.0; if(data.size() != 9) { - msg.person_array.push_back(pc); + msg.person_array.push_back(pd); ROS_DEBUG("Could not retrieve any face information"); continue; } try { - pc.person.face_detected = (bool)data[0].content().asInt32(); + pd.person.face_detected = (bool)data[0].content().asInt32(); } catch(...) { ROS_DEBUG("Error retreiving face detected"); } - if(pc.person.face_detected) { + if(pd.person.face_detected) { /* Gaze Analysis */ try { qi::AnyReference gaze = data[1].content(); @@ -365,10 +365,10 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu if(yaw.kind() == qi::TypeKind_Float && pitch.kind() == qi::TypeKind_Float) { tf::Quaternion q; q.setRPY(0.0, pitch.asFloat(), yaw.asFloat()); - pc.gaze.gaze.x = q.x(); - pc.gaze.gaze.y = q.y(); - pc.gaze.gaze.z = q.z(); - pc.gaze.gaze.w = q.w(); + pd.gaze.gaze_angle.x = q.x(); + pd.gaze.gaze_angle.y = q.y(); + pd.gaze.gaze_angle.z = q.z(); + pd.gaze.gaze_angle.w = q.w(); } else { ROS_DEBUG("Could not retrieve yaw/pitch"); } @@ -389,10 +389,10 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu if(yaw.kind() == qi::TypeKind_Float && pitch.kind() == qi::TypeKind_Float && roll.kind() == qi::TypeKind_Float) { tf::Quaternion q; q.setRPY(roll.asFloat(), pitch.asFloat(), yaw.asFloat()); - pc.gaze.head_angle.x = q.x(); - pc.gaze.head_angle.y = q.y(); - pc.gaze.head_angle.z = q.z(); - pc.gaze.head_angle.w = q.w(); + pd.gaze.head_angle.x = q.x(); + pd.gaze.head_angle.y = q.y(); + pd.gaze.head_angle.z = q.z(); + pd.gaze.head_angle.w = q.w(); } else { ROS_DEBUG("Could not retrieve yaw/pitch/roll"); } @@ -403,8 +403,8 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu ROS_DEBUG_STREAM("Error retrieving head angle: " << e.what()); } try { - pc.gaze.looking_at_robot = (bool)data[3].content().asInt32(); - pc.gaze.looking_at_robot_score = data[4].content().asFloat(); + pd.gaze.looking_at_robot = (bool)data[3].content().asInt32(); + pd.gaze.looking_at_robot_score = data[4].content().asFloat(); } catch(std::runtime_error& e) { ROS_DEBUG_STREAM("Error retrieving looking at robot: " << e.what()); } @@ -418,8 +418,8 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu age = age_props[0].content(); conf = age_props[1].content(); if(age.kind() == qi::TypeKind_Float && conf.kind() == qi::TypeKind_Float) { - pc.face.age = (int)age.asFloat(); - pc.face.age_confidence = conf.asFloat(); + pd.face.age = (int)age.asFloat(); + pd.face.age_confidence = conf.asFloat(); } else { ROS_DEBUG("Could not retrieve age and confidence"); } @@ -437,8 +437,8 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu gender = gender_props[0].content(); conf = gender_props[1].content(); if(gender.kind() == qi::TypeKind_Float && conf.kind() == qi::TypeKind_Float) { - pc.face.gender = (int)gender.asFloat(); - pc.face.gender_confidence = conf.asFloat(); + pd.face.gender = (int)gender.asFloat(); + pd.face.gender_confidence = conf.asFloat(); } else { ROS_DEBUG("Could not retrieve gender and confidence"); } @@ -456,8 +456,8 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu smile_degree = smile_props[0].content(); conf = smile_props[1].content(); if(smile_degree.kind() == qi::TypeKind_Float && conf.kind() == qi::TypeKind_Float) { - pc.face.smile_degree = smile_degree.asFloat(); - pc.face.smile_degree_confidence = conf.asFloat(); + pd.face.smile_degree = smile_degree.asFloat(); + pd.face.smile_degree_confidence = conf.asFloat(); } else { ROS_DEBUG("Could not retrieve smile degree and confidence"); } @@ -482,11 +482,11 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu && surprised.kind() == qi::TypeKind_Float && angry.kind() == qi::TypeKind_Float && sad.kind() == qi::TypeKind_Float) { - pc.face.expression_properties.neutral = neutral.asFloat(); - pc.face.expression_properties.happy = happy.asFloat(); - pc.face.expression_properties.surprised = surprised.asFloat(); - pc.face.expression_properties.angry = angry.asFloat(); - pc.face.expression_properties.sad = sad.asFloat(); + pd.face.expression_properties.neutral = neutral.asFloat(); + pd.face.expression_properties.happy = happy.asFloat(); + pd.face.expression_properties.surprised = surprised.asFloat(); + pd.face.expression_properties.angry = angry.asFloat(); + pd.face.expression_properties.sad = sad.asFloat(); } else { ROS_DEBUG("Could not retrieve neutral/happy/surprised/angry/sad"); } @@ -502,12 +502,12 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu } - msg.person_array.push_back(pc); + msg.person_array.push_back(pd); } } // http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor -template class PeopleEventRegister; -template class PeopleEventRegister; +template class PeopleEventRegister; +template class PeopleEventRegister; }//namespace diff --git a/src/event/people.hpp b/src/event/people.hpp index 8a52e676..0e08a565 100644 --- a/src/event/people.hpp +++ b/src/event/people.hpp @@ -29,9 +29,8 @@ #include #include -#include -#include -#include +#include +#include #include #include @@ -83,8 +82,8 @@ class PeopleEventRegister: public boost::enable_shared_from_this +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) {} + FaceDetectedEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session ) : PeopleEventRegister(name, keys, frequency, session) {} }; -class PersonCharacteristicsEventRegister: public PeopleEventRegister +class PersonCharacteristicsEventRegister: public PeopleEventRegister { public: - PersonCharacteristicsEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session ) : PeopleEventRegister(name, keys, frequency, session) {} + PersonCharacteristicsEventRegister( 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) //QI_REGISTER_OBJECT(PersonCharacteristicsEventRegister, peopleCallback) static bool _qiregisterPeopleEventRegisterFaceDetected() { - ::qi::ObjectTypeBuilder > b; - QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback) + ::qi::ObjectTypeBuilder > b; + QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback) b.registerType(); return true; } static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterPeopleEventRegisterFaceDetected(); static bool _qiregisterPeopleEventRegisterPersonCharacteristics() { - ::qi::ObjectTypeBuilder > b; - QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback) + ::qi::ObjectTypeBuilder > b; + QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback) b.registerType(); return true; } From fa535ca666b4d99ccea95f097ab58847621caa67 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Fri, 30 Sep 2016 10:22:33 +0100 Subject: [PATCH 08/44] Clean-up and adding face to boot_config --- share/boot_config.json | 4 ++++ src/event/people.cpp | 7 ++----- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/share/boot_config.json b/share/boot_config.json index 96b34c0a..11a7f548 100644 --- a/share/boot_config.json +++ b/share/boot_config.json @@ -86,6 +86,10 @@ { "enabled" : true }, + "face": + { + "enabled" : true + }, "people": { "enabled" : true diff --git a/src/event/people.cpp b/src/event/people.cpp index 15e5a2d1..6474de43 100644 --- a/src/event/people.cpp +++ b/src/event/people.cpp @@ -235,7 +235,6 @@ void PeopleEventRegister::peopleCallback(std::string &key, qi::AnyValue &valu template void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::FaceDetectedArray &msg) { - std::cout<<"THERE"<::peopleCallbackMessage(std::string &key, qi::AnyValu if ( faces.face_info.size() == 0 ) return; msg.header.frame_id = "CameraTop_optical_frame"; - msg.header.stamp = ros::Time::now(); // ros::Time(faces.timestamp.timestamp_s, faces.timestamp.timestamp_us); // This gives time till start not the system time + msg.header.stamp = ros::Time::now(); for(int i = 0; i < faces.face_info.size(); i++) { nao_interaction_msgs::FaceDetected face; @@ -306,7 +305,6 @@ geometry_msgs::Point PeopleEventRegister::toCartesian(float dist, float azi, template void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::PersonDetectedArray &msg) { - std::cout<<"HERE"<::peopleCallbackMessage(std::string &key, qi::AnyValu } msg.header.frame_id = "CameraDepth_optical_frame"; - msg.header.stamp = ros::Time::now(); //ros::Time(people.timestamp.timestamp_s, people.timestamp.timestamp_us); // This gives time till start not the system time + msg.header.stamp = ros::Time::now(); for(int i = 0; i < people.person_info.size(); i++) { std::string sid = num_to_str(people.person_info[i].id); @@ -501,7 +499,6 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu ROS_DEBUG_STREAM("Error retrieving person information: " << e.what()); } - msg.person_array.push_back(pd); } } From 25cdd78dc380bebd614128bd7ea8e87847bb798d Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Fri, 30 Sep 2016 12:06:09 +0100 Subject: [PATCH 09/44] Adding battery state publisher --- CMakeLists.txt | 1 + share/boot_config.json | 5 ++ src/converters/battery.cpp | 108 +++++++++++++++++++++++++++++++++++++ src/converters/battery.hpp | 74 +++++++++++++++++++++++++ src/naoqi_driver.cpp | 4 ++ 5 files changed, 192 insertions(+) create mode 100644 src/converters/battery.cpp create mode 100644 src/converters/battery.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a97bafd7..c64fbdd6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ set( src/converters/sonar.cpp src/converters/log.cpp src/converters/odom.cpp + src/converters/battery.cpp ) set( TOOLS_SRC diff --git a/share/boot_config.json b/share/boot_config.json index 11a7f548..5ee70cb0 100644 --- a/share/boot_config.json +++ b/share/boot_config.json @@ -70,6 +70,11 @@ "enabled" : true, "frequency" : 10 }, + "battery": + { + "enabled" : true, + "frequency" : 10 + }, "audio": { "enabled" : true diff --git a/src/converters/battery.cpp b/src/converters/battery.cpp new file mode 100644 index 00000000..34960a2d --- /dev/null +++ b/src/converters/battery.cpp @@ -0,0 +1,108 @@ +/* + * 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 "battery.hpp" +#include "../tools/from_any_value.hpp" + +/* +* BOOST includes +*/ +#include +#define for_each BOOST_FOREACH + +namespace naoqi +{ +namespace converter +{ + +BatteryConverter::BatteryConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ) + : BaseConverter( name, frequency, session ), + p_memory_( session->service("ALMemory") ), + p_battery_( session->service("ALBattery") ), + is_subscribed_(false) +{ + std::vector keys; + keys.push_back("Device/SubDeviceList/Battery/Charge/Sensor/Value"); + keys.push_back("Device/SubDeviceList/Battery/Charge/Sensor/TotalVoltage"); + keys.push_back("Device/SubDeviceList/Battery/Current/Sensor/Value"); + keys.push_back("Device/SubDeviceList/Platform/ILS/Sensor/Value"); + keys.push_back("Device/SubDeviceList/Battery/Charge/Sensor/Charging"); + keys.push_back("ALBattery/ConnectedToChargingStation"); + + keys_.resize(keys.size()); + size_t i = 0; + for(std::vector::const_iterator it = keys.begin(); it != keys.end(); ++it, ++i) + keys_[i] = *it; +} + +BatteryConverter::~BatteryConverter() +{ + if (is_subscribed_) + { + is_subscribed_ = false; + } +} + +void BatteryConverter::registerCallback( message_actions::MessageAction action, Callback_t cb ) +{ + callbacks_[action] = cb; +} + +void BatteryConverter::callAll( const std::vector& actions ) +{ + if (!is_subscribed_) + { + is_subscribed_ = true; + } + + std::vector values; + try { + qi::AnyValue anyvalues = p_memory_.call("getListData", keys_); + tools::fromAnyValueToFloatVector(anyvalues, values); + } catch (const std::exception& e) { + std::cerr << "Exception caught in BatteryConverter: " << e.what() << std::endl; + return; + } + + msg_.header.stamp = ros::Time::now(); + msg_.charge = (int)(values[0] * 100.0); + msg_.voltage = values[1]; + msg_.current = values[2]; + msg_.hatch_open = (bool)values[3]; + msg_.charging = ((int)values[4]) == 8; + msg_.connected_to_charging_station = (bool)values[5]; + + for_each( message_actions::MessageAction action, actions ) + { + callbacks_[action]( msg_ ); + } +} + +void BatteryConverter::reset( ) +{ + if (is_subscribed_) + { +// p_battery_.call("unsubscribe", "ROS"); + is_subscribed_ = false; + } +} + +} // publisher +} //naoqi diff --git a/src/converters/battery.hpp b/src/converters/battery.hpp new file mode 100644 index 00000000..589c3664 --- /dev/null +++ b/src/converters/battery.hpp @@ -0,0 +1,74 @@ +/* + * 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 BATTERY_CONVERTER_HPP +#define BATTERY_CONVERTER_HPP + +/* +* LOCAL includes +*/ +#include "converter_base.hpp" +#include + +/* +* ROS includes +*/ +#include + +namespace naoqi +{ +namespace converter +{ + +class BatteryConverter : public BaseConverter +{ + + typedef boost::function Callback_t; + + +public: + BatteryConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ); + + ~BatteryConverter(); + + void reset( ); + + void registerCallback( message_actions::MessageAction action, Callback_t cb ); + + void callAll( const std::vector& actions ); + + +private: + std::map callbacks_; + + /** Battery (Proxy) configurations */ + qi::AnyObject p_battery_; + /** Memory (Proxy) configurations */ + qi::AnyObject p_memory_; + /** Key describeing whether we are subscribed to the ALBattery module */ + bool is_subscribed_; + + /** The memory keys of the battery */ + std::vector keys_; + /** Pre-filled messges that are sent */ + nao_interaction_msgs::BatteryInfo msg_; +}; + +} //publisher +} //naoqi + +#endif diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index dcc2facc..d400486c 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -41,6 +41,7 @@ #include "converters/memory/string.hpp" #include "converters/log.hpp" #include "converters/odom.hpp" +#include "converters/battery.hpp" /* * PUBLISHERS @@ -584,6 +585,9 @@ void Driver::registerDefaultConverter() bool odom_enabled = boot_config_.get( "converters.odom.enabled", true); size_t odom_frequency = boot_config_.get( "converters.odom.frequency", 10); + bool battery_enabled = boot_config_.get( "converters.battery.enabled", true); + size_t battery_frequency = boot_config_.get( "converters.battery.frequency", 10); + bool bumper_enabled = boot_config_.get( "converters.bumper.enabled", true); bool hand_enabled = boot_config_.get( "converters.touch_hand.enabled", true); bool head_enabled = boot_config_.get( "converters.touch_head.enabled", true); From 2abeed00a523eb9e9e8ed7bb318a9d4e15942ac6 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Fri, 30 Sep 2016 13:52:17 +0100 Subject: [PATCH 10/44] Some minor renaming --- src/event/people.hpp | 10 +++++----- src/naoqi_driver.cpp | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/event/people.hpp b/src/event/people.hpp index 0e08a565..78a931a9 100644 --- a/src/event/people.hpp +++ b/src/event/people.hpp @@ -128,14 +128,14 @@ class FaceDetectedEventRegister: public PeopleEventRegister keys, const float& frequency, const qi::SessionPtr& session ) : PeopleEventRegister(name, keys, frequency, session) {} }; -class PersonCharacteristicsEventRegister: public PeopleEventRegister +class PersonDetectedEventRegister: public PeopleEventRegister { public: - PersonCharacteristicsEventRegister( const std::string& name, const std::vector keys, const float& frequency, const qi::SessionPtr& session ) : PeopleEventRegister(name, keys, frequency, session) {} + PersonDetectedEventRegister( 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) -//QI_REGISTER_OBJECT(PersonCharacteristicsEventRegister, peopleCallback) +//QI_REGISTER_OBJECT(PersonDetectedEventRegister, peopleCallback) static bool _qiregisterPeopleEventRegisterFaceDetected() { ::qi::ObjectTypeBuilder > b; @@ -145,13 +145,13 @@ static bool _qiregisterPeopleEventRegisterFaceDetected() { } static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterPeopleEventRegisterFaceDetected(); -static bool _qiregisterPeopleEventRegisterPersonCharacteristics() { +static bool _qiregisterPeopleEventRegisterPersonDetected() { ::qi::ObjectTypeBuilder > b; QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, PeopleEventRegister, peopleCallback) b.registerType(); return true; } -static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterPeopleEventRegisterPersonCharacteristics(); +static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterPeopleEventRegisterPersonDetected(); } //naoqi diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index d400486c..8ce8edfd 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -868,8 +868,8 @@ void Driver::registerDefaultConverter() { std::vector people_events; people_events.push_back("PeoplePerception/PeopleDetected"); - boost::shared_ptr event_register = - boost::make_shared( "people_detected", people_events, 0, sessionPtr_ ); + boost::shared_ptr event_register = + boost::make_shared( "people_detected", people_events, 0, sessionPtr_ ); insertEventConverter("people_detected", event_register); if (keep_looping) { event_map_.find("people_detected")->second.startProcess(); From 3222a03ebfdd030091a0b0233588a2c754878d5c Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Fri, 30 Sep 2016 14:30:30 +0100 Subject: [PATCH 11/44] Adding odometry based on: https://github.com/ros-naoqi/naoqi_driver/pull/36 --- CMakeLists.txt | 1 + share/boot_config.json | 5 +++++ src/naoqi_driver.cpp | 33 +++++++++++++++++++++++++++++---- 3 files changed, 35 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c64fbdd6..99bfdd5c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ set( src/converters/log.cpp src/converters/odom.cpp src/converters/battery.cpp + src/converters/odom.cpp ) set( TOOLS_SRC diff --git a/share/boot_config.json b/share/boot_config.json index 5ee70cb0..4c86c336 100644 --- a/share/boot_config.json +++ b/share/boot_config.json @@ -75,6 +75,11 @@ "enabled" : true, "frequency" : 10 }, + "odom": + { + "enabled" : true, + "frequency" : 15 + }, "audio": { "enabled" : true diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 8ce8edfd..5719be9f 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -40,8 +40,8 @@ #include "converters/memory/int.hpp" #include "converters/memory/string.hpp" #include "converters/log.hpp" -#include "converters/odom.hpp" #include "converters/battery.hpp" +#include "converters/odom.hpp" /* * PUBLISHERS @@ -582,12 +582,12 @@ 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 odom_enabled = boot_config_.get( "converters.odom.enabled", true); - size_t odom_frequency = boot_config_.get( "converters.odom.frequency", 10); - bool battery_enabled = boot_config_.get( "converters.battery.enabled", true); size_t battery_frequency = boot_config_.get( "converters.battery.frequency", 10); + bool odom_enabled = boot_config_.get( "converters.odom.enabled", true); + size_t odom_frequency = boot_config_.get( "converters.odom.frequency", 10); + bool bumper_enabled = boot_config_.get( "converters.bumper.enabled", true); bool hand_enabled = boot_config_.get( "converters.touch_hand.enabled", true); bool head_enabled = boot_config_.get( "converters.touch_head.enabled", true); @@ -765,6 +765,31 @@ void Driver::registerDefaultConverter() usc->registerCallback( message_actions::LOG, boost::bind(&recorder::SonarRecorder::bufferize, usr, _1) ); registerConverter( usc, usp, usr ); } + + if(robot_ == robot::PEPPER) { + if ( battery_enabled ) + { + boost::shared_ptr > bp = boost::make_shared >( "battery" ); + boost::shared_ptr > br = boost::make_shared >( "battery" ); + boost::shared_ptr bc = boost::make_shared( "battery", battery_frequency, sessionPtr_ ); + bc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, bp, _1) ); + bc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, br, _1) ); + bc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, br, _1) ); + registerConverter( bc, bp, br ); + } + } + + /** Odom */ + if ( odom_enabled ) + { + boost::shared_ptr > lp = boost::make_shared >( "odom" ); + boost::shared_ptr > lr = boost::make_shared >( "odom" ); + boost::shared_ptr lc = boost::make_shared( "odom", odom_frequency, sessionPtr_ ); + lc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, lp, _1) ); + lc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, lr, _1) ); + lc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, lr, _1) ); + registerConverter( lc, lp, lr ); + } if ( audio_enabled ) { /** Audio */ From f6f97c876d8e005e256d145e099356165d5954a1 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Mon, 3 Oct 2016 13:41:41 +0100 Subject: [PATCH 12/44] Add waving detection --- src/event/people.cpp | 32 ++++++++++++++++++++++---------- src/event/people.hpp | 2 +- 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/event/people.cpp b/src/event/people.cpp index 6474de43..feec24b9 100644 --- a/src/event/people.cpp +++ b/src/event/people.cpp @@ -46,6 +46,7 @@ PeopleEventRegister::PeopleEventRegister( const std::string& name, const std: p_people_( session->service("ALPeoplePerception") ), p_gaze_( session->service("ALGazeAnalysis") ), p_face_( session->service("ALFaceCharacteristics") ), + p_waving_( session->service("ALWavingDetection") ), session_(session), isStarted_(false), isPublishing_(false), @@ -54,6 +55,7 @@ PeopleEventRegister::PeopleEventRegister( const std::string& name, const std: prefix("PeoplePerception/Person/") { memory_keys.push_back("/IsFaceDetected"); + memory_keys.push_back("/IsWaving"); memory_keys.push_back("/GazeDirection"); memory_keys.push_back("/HeadAngles"); memory_keys.push_back("/IsLookingAtRobot"); @@ -119,6 +121,8 @@ void PeopleEventRegister::startProcess() p_gaze_.call("subscribe", "ROS"); std::cout< Face : Start"<("subscribe", "ROS"); + std::cout< Waving : Start"<("subscribe", "ROS"); } std::cout << serviceName << " : Start" << std::endl; } @@ -142,6 +146,8 @@ void PeopleEventRegister::stopProcess() p_gaze_.call("unsubscribe", "ROS"); std::cout< Face : Stop"<("unsubscribe", "ROS"); + std::cout< Waving : Stop"<("unsubscribe", "ROS"); } for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it) { p_memory_.call("unsubscribeToEvent",it->c_str(), serviceName); @@ -339,22 +345,28 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu pd.person.position.position = toCartesian(pd.person.distance, pd.person.pitch, pd.person.yaw); pd.person.position.orientation.w = 1.0; - if(data.size() != 9) { + if(data.size() != keys.size()) { msg.person_array.push_back(pd); ROS_DEBUG("Could not retrieve any face information"); continue; } try { - pd.person.face_detected = (bool)data[0].content().asInt32(); + pd.person.face_detected = (bool)data[0].content().asInt32(); } catch(...) { ROS_DEBUG("Error retreiving face detected"); } + try { + pd.person.is_waving = (bool)data[1].content().asInt32(); + } catch(...) { + ROS_INFO("Error retreiving if waving"); + } + if(pd.person.face_detected) { /* Gaze Analysis */ try { - qi::AnyReference gaze = data[1].content(); + qi::AnyReference gaze = data[2].content(); if(gaze.kind() == qi::TypeKind_List && gaze.size() == 2) { qi::AnyReference g, yaw, pitch; @@ -377,7 +389,7 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu ROS_DEBUG_STREAM("Error retrieving gaze angle: " << e.what()); } try { - qi::AnyReference head_angle = data[2].content(); + qi::AnyReference head_angle = data[3].content(); if(head_angle.kind() == qi::TypeKind_List && head_angle.size() == 3) { qi::AnyReference yaw, pitch, roll; @@ -401,15 +413,15 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu ROS_DEBUG_STREAM("Error retrieving head angle: " << e.what()); } try { - pd.gaze.looking_at_robot = (bool)data[3].content().asInt32(); - pd.gaze.looking_at_robot_score = data[4].content().asFloat(); + pd.gaze.looking_at_robot = (bool)data[4].content().asInt32(); + pd.gaze.looking_at_robot_score = data[5].content().asFloat(); } catch(std::runtime_error& e) { ROS_DEBUG_STREAM("Error retrieving looking at robot: " << e.what()); } /* Face Charcteristics */ try { - qi::AnyReference age_props = data[5].content(); + qi::AnyReference age_props = data[6].content(); if(age_props.kind() == qi::TypeKind_List && age_props.size() == 2) { qi::AnyReference age, conf; @@ -428,7 +440,7 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu ROS_DEBUG_STREAM("Error retrieving age: " << e.what()); } try { - qi::AnyReference gender_props = data[6].content(); + qi::AnyReference gender_props = data[7].content(); if(gender_props.kind() == qi::TypeKind_List && gender_props.size() == 2) { qi::AnyReference gender, conf; @@ -447,7 +459,7 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu ROS_DEBUG_STREAM("Error retrieving gender: " << e.what()); } try { - qi::AnyReference smile_props = data[7].content(); + qi::AnyReference smile_props = data[8].content(); if(smile_props.kind() == qi::TypeKind_List && smile_props.size() == 2) { qi::AnyReference smile_degree, conf; @@ -466,7 +478,7 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu ROS_DEBUG_STREAM("Error retrieving smile: " << e.what()); } try { - qi::AnyReference expression_props = data[8].content(); + qi::AnyReference expression_props = data[9].content(); if(expression_props.kind() == qi::TypeKind_List && expression_props.size() == 5) { qi::AnyReference neutral, happy, surprised, angry, sad; diff --git a/src/event/people.hpp b/src/event/people.hpp index 78a931a9..df130e14 100644 --- a/src/event/people.hpp +++ b/src/event/people.hpp @@ -103,7 +103,7 @@ class PeopleEventRegister: public boost::enable_shared_from_this > recorder_; qi::SessionPtr session_; - qi::AnyObject p_memory_, p_people_, p_gaze_, p_face_; + qi::AnyObject p_memory_, p_people_, p_gaze_, p_face_, p_waving_; unsigned int serviceId; std::string name_; From 9d8ef7955d3c71f2e56df189edd422999c2860be Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Mon, 3 Oct 2016 13:54:22 +0100 Subject: [PATCH 13/44] Adding animated speech subscriber based on https://github.com/ros-naoqi/naoqi_driver/pull/65 --- CMakeLists.txt | 1 + src/naoqi_driver.cpp | 3 +- src/subscribers/animated_speech.cpp | 48 +++++++++++++++++++++++ src/subscribers/animated_speech.hpp | 60 +++++++++++++++++++++++++++++ 4 files changed, 111 insertions(+), 1 deletion(-) create mode 100644 src/subscribers/animated_speech.cpp create mode 100644 src/subscribers/animated_speech.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 99bfdd5c..6eba7ed1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,6 +44,7 @@ set( src/subscribers/teleop.cpp src/subscribers/moveto.cpp src/subscribers/speech.cpp + src/subscribers/animated_speech.cpp ) set( diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 5719be9f..c9fd5b56 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -65,7 +65,7 @@ #include "subscribers/teleop.hpp" #include "subscribers/moveto.hpp" #include "subscribers/speech.hpp" - +#include "subscribers/animated_speech.hpp" /* * SERVICES @@ -935,6 +935,7 @@ void Driver::registerDefaultSubscriber() registerSubscriber( boost::make_shared("teleop", "/cmd_vel", "/joint_angles", sessionPtr_) ); registerSubscriber( boost::make_shared("moveto", "/move_base_simple/goal", sessionPtr_, tf2_buffer_) ); registerSubscriber( boost::make_shared("speech", "/speech", sessionPtr_) ); + registerSubscriber( boost::make_shared("animated_speech", "/animated_speech", sessionPtr_) ); } void Driver::registerService( service::Service srv ) diff --git a/src/subscribers/animated_speech.cpp b/src/subscribers/animated_speech.cpp new file mode 100644 index 00000000..933b929c --- /dev/null +++ b/src/subscribers/animated_speech.cpp @@ -0,0 +1,48 @@ +/* + * 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 "animated_speech.hpp" + + +namespace naoqi +{ +namespace subscriber +{ + + AnimatedSpeechSubscriber::AnimatedSpeechSubscriber( const std::string& name, const std::string& animated_speech_topic, const qi::SessionPtr& session ): + animated_speech_topic_(animated_speech_topic), + BaseSubscriber( name, animated_speech_topic, session ), + p_tts_( session->service("ALAnimatedSpeech") ) + {} + + void AnimatedSpeechSubscriber::reset( ros::NodeHandle& nh ) + { + sub_animated_speech_ = nh.subscribe( animated_speech_topic_, 10, &AnimatedSpeechSubscriber::animated_speech_callback, this ); + + is_initialized_ = true; + } + + void AnimatedSpeechSubscriber::animated_speech_callback( const std_msgs::StringConstPtr& string_msg ) + { + p_tts_.async("say", string_msg->data); + } + +}// subscriber +}// naoqi diff --git a/src/subscribers/animated_speech.hpp b/src/subscribers/animated_speech.hpp new file mode 100644 index 00000000..cf927894 --- /dev/null +++ b/src/subscribers/animated_speech.hpp @@ -0,0 +1,60 @@ +/* + * 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 ANIMATED_SPEECH_SUBSCRIBER_HPP +#define ANIMATED_SPEECH_SUBSCRIBER_HPP + +/* + * LOCAL includes + */ +#include "subscriber_base.hpp" + +/* + * ROS includes + */ +#include +#include + +namespace naoqi +{ +namespace subscriber +{ + + class AnimatedSpeechSubscriber: public BaseSubscriber + { + public: + AnimatedSpeechSubscriber( const std::string& name, const std::string& speech_topic, const qi::SessionPtr& session ); + ~AnimatedSpeechSubscriber(){} + + void reset( ros::NodeHandle& nh ); + void animated_speech_callback( const std_msgs::StringConstPtr& speech_msg ); + + private: + + std::string animated_speech_topic_; + + qi::AnyObject p_tts_; + ros::Subscriber sub_animated_speech_; + + + + }; // class AnimatedSpeech + +} // subscriber +}// naoqi +#endif From 76173835534f02c4cd63c5e93f3a3728e8b32535 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Mon, 3 Oct 2016 16:30:08 +0100 Subject: [PATCH 14/44] Adding animation player that either takes a tag or the full path of the animation --- CMakeLists.txt | 1 + src/naoqi_driver.cpp | 2 + src/subscribers/play_animation.cpp | 51 +++++++++++++++++++++++++ src/subscribers/play_animation.hpp | 60 ++++++++++++++++++++++++++++++ 4 files changed, 114 insertions(+) create mode 100644 src/subscribers/play_animation.cpp create mode 100644 src/subscribers/play_animation.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6eba7ed1..fd952c3d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,6 +45,7 @@ set( src/subscribers/moveto.cpp src/subscribers/speech.cpp src/subscribers/animated_speech.cpp + src/subscribers/play_animation.cpp ) set( diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index c9fd5b56..6898493a 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -66,6 +66,7 @@ #include "subscribers/moveto.hpp" #include "subscribers/speech.hpp" #include "subscribers/animated_speech.hpp" +#include "subscribers/play_animation.hpp" /* * SERVICES @@ -936,6 +937,7 @@ void Driver::registerDefaultSubscriber() registerSubscriber( boost::make_shared("moveto", "/move_base_simple/goal", sessionPtr_, tf2_buffer_) ); registerSubscriber( boost::make_shared("speech", "/speech", sessionPtr_) ); registerSubscriber( boost::make_shared("animated_speech", "/animated_speech", sessionPtr_) ); + registerSubscriber( boost::make_shared("play_animation", "/play_animation", sessionPtr_) ); } void Driver::registerService( service::Service srv ) diff --git a/src/subscribers/play_animation.cpp b/src/subscribers/play_animation.cpp new file mode 100644 index 00000000..9851cead --- /dev/null +++ b/src/subscribers/play_animation.cpp @@ -0,0 +1,51 @@ +/* + * 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 "play_animation.hpp" + + +namespace naoqi +{ +namespace subscriber +{ + +PlayAnimationSubscriber::PlayAnimationSubscriber( const std::string& name, const std::string& pa_topic, const qi::SessionPtr& session ): + pa_topic_(pa_topic), + BaseSubscriber( name, pa_topic, session ), + p_animation_player_( session->service("ALAnimationPlayer") ) +{} + +void PlayAnimationSubscriber::reset( ros::NodeHandle& nh ) +{ + sub_pa_ = nh.subscribe( pa_topic_, 10, &PlayAnimationSubscriber::pa_callback, this ); + + is_initialized_ = true; +} + +void PlayAnimationSubscriber::pa_callback( const std_msgs::StringConstPtr& string_msg ) +{ + if(string_msg->data.find("/") != std::string::npos) + p_animation_player_.async("run", string_msg->data); + else + p_animation_player_.async("runTag", string_msg->data); +} + +} //publisher +} // naoqi diff --git a/src/subscribers/play_animation.hpp b/src/subscribers/play_animation.hpp new file mode 100644 index 00000000..81c932c4 --- /dev/null +++ b/src/subscribers/play_animation.hpp @@ -0,0 +1,60 @@ +/* + * 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 PLAY_ANIMATION_SUBSCRIBER_HPP +#define PLAY_ANIMATION_SUBSCRIBER_HPP + +/* + * LOCAL includes + */ +#include "subscriber_base.hpp" + +/* + * ROS includes + */ +#include +#include + +namespace naoqi +{ +namespace subscriber +{ + +class PlayAnimationSubscriber: public BaseSubscriber +{ +public: + PlayAnimationSubscriber( const std::string& name, const std::string& pa_topic, const qi::SessionPtr& session ); + ~PlayAnimationSubscriber(){} + + void reset( ros::NodeHandle& nh ); + void pa_callback( const std_msgs::StringConstPtr& speech_msg ); + +private: + + std::string pa_topic_; + + qi::AnyObject p_animation_player_; + ros::Subscriber sub_pa_; + + + +}; // class Speech + +} // subscriber +}// naoqi +#endif From 9987b0957cf2aebca05ad29f623ef5613655067d Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Mon, 3 Oct 2016 17:10:18 +0100 Subject: [PATCH 15/44] Minor changes based on feedback by @Natalia --- share/boot_config.json | 8 ++++---- src/event/people.cpp | 6 +++--- src/tools/from_any_value.cpp | 12 ++++++------ 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/share/boot_config.json b/share/boot_config.json index 4c86c336..10214b60 100644 --- a/share/boot_config.json +++ b/share/boot_config.json @@ -76,10 +76,10 @@ "frequency" : 10 }, "odom": - { - "enabled" : true, - "frequency" : 15 - }, + { + "enabled" : true, + "frequency" : 15 + }, "audio": { "enabled" : true diff --git a/src/event/people.cpp b/src/event/people.cpp index feec24b9..f0968d76 100644 --- a/src/event/people.cpp +++ b/src/event/people.cpp @@ -247,7 +247,7 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu } catch(std::runtime_error& e) { - std::cout << "Cannot retrieve facedetect" << std::endl; + ROS_DEBUG_STREAM("Cannot retrieve facedetect: " << e.what()); return; } if ( faces.face_info.size() == 0 ) return; @@ -317,7 +317,7 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu } catch(std::runtime_error& e) { - std::cout << "Cannot retrieve persondetected: " << e.what() << std::endl; + ROS_DEBUG_STREAM("Cannot retrieve persondetected: " << e.what()); return; } @@ -360,7 +360,7 @@ void PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValu try { pd.person.is_waving = (bool)data[1].content().asInt32(); } catch(...) { - ROS_INFO("Error retreiving if waving"); + ROS_DEBUG("Error retreiving if waving"); } if(pd.person.face_detected) { diff --git a/src/tools/from_any_value.cpp b/src/tools/from_any_value.cpp index 1c8291c7..31869055 100644 --- a/src/tools/from_any_value.cpp +++ b/src/tools/from_any_value.cpp @@ -584,7 +584,7 @@ NaoqiFaceDetected fromAnyValueToNaoqiFaceDetected(qi::AnyValue& value){ qi::AnyReference ref; if ( anyref.size() != 5 ) { - return result; + throw std::runtime_error("AnyValue does not have the expected size to be transformed to face detected"); } /** TimeStamp_ **/ ref = anyref[0].content(); @@ -610,7 +610,7 @@ NaoqiFaceDetected fromAnyValueToNaoqiFaceDetected(qi::AnyValue& value){ //CameraPose_InTorsoFrame, ref = anyref[2].content(); - for (int i = 0; i < 6; i++ ){ + for (int i = 0; i < ref.size(); i++ ){ if(ref[i].content().kind() == qi::TypeKind_Float) { result.camera_pose_in_torso_frame[i] = ref[i].content().asFloat(); @@ -623,7 +623,7 @@ NaoqiFaceDetected fromAnyValueToNaoqiFaceDetected(qi::AnyValue& value){ } //CameraPose_InRobotFrame, ref = anyref[3].content(); - for (int i = 0; i < 6; i++ ){ + for (int i = 0; i < ref.size(); i++ ){ if(ref[i].content().kind() == qi::TypeKind_Float) { result.camera_pose_in_robot_frame[i] = ref[i].content().asFloat(); @@ -715,7 +715,7 @@ NaoqiPersonDetected fromAnyValueToNaoqiPersonDetected(qi::AnyValue& value) { qi::AnyReference ref; if ( anyref.size() != 5 ) { - return result; + throw std::runtime_error("AnyValue does not have the expected size to be transformed to person detected"); } /** TimeStamp_ **/ ref = anyref[0].content(); @@ -741,7 +741,7 @@ NaoqiPersonDetected fromAnyValueToNaoqiPersonDetected(qi::AnyValue& value) { //CameraPose_InTorsoFrame, ref = anyref[2].content(); - for (int i = 0; i < 6; i++ ){ + for (int i = 0; i < ref.size(); i++ ){ if(ref[i].content().kind() == qi::TypeKind_Float) { result.camera_pose_in_torso_frame[i] = ref[i].content().asFloat(); @@ -754,7 +754,7 @@ NaoqiPersonDetected fromAnyValueToNaoqiPersonDetected(qi::AnyValue& value) { } //CameraPose_InRobotFrame, ref = anyref[3].content(); - for (int i = 0; i < 6; i++ ){ + for (int i = 0; i < ref.size(); i++ ){ if(ref[i].content().kind() == qi::TypeKind_Float) { result.camera_pose_in_robot_frame[i] = ref[i].content().asFloat(); From 7ffbf2e72855b911d61734de7f552d321e0db338 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Wed, 5 Oct 2016 10:37:31 +0100 Subject: [PATCH 16/44] Adding sound source localisation --- CMakeLists.txt | 2 + share/boot_config.json | 6 + src/converters/sound.cpp | 68 +++++++++++ src/converters/sound.hpp | 67 +++++++++++ src/event/sound.cpp | 251 +++++++++++++++++++++++++++++++++++++++ src/event/sound.hpp | 130 ++++++++++++++++++++ src/naoqi_driver.cpp | 24 ++++ 7 files changed, 548 insertions(+) create mode 100644 src/converters/sound.cpp create mode 100644 src/converters/sound.hpp create mode 100644 src/event/sound.cpp create mode 100644 src/event/sound.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fd952c3d..86eb467f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,7 @@ set( src/converters/odom.cpp src/converters/battery.cpp src/converters/odom.cpp + src/converters/sound.cpp ) set( TOOLS_SRC @@ -72,6 +73,7 @@ set( src/event/audio.cpp src/event/touch.cpp src/event/people.cpp + src/event/sound.cpp ) # use catkin if qibuild is not found diff --git a/share/boot_config.json b/share/boot_config.json index 10214b60..59d3510c 100644 --- a/share/boot_config.json +++ b/share/boot_config.json @@ -84,6 +84,12 @@ { "enabled" : true }, + "sound": + { + "enabled" : true, + "energy" : true, + "sensitivity" : 0.9 + }, "bumper": { "enabled" : true diff --git a/src/converters/sound.cpp b/src/converters/sound.cpp new file mode 100644 index 00000000..3ca89dd5 --- /dev/null +++ b/src/converters/sound.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 "sound.hpp" + +/* +* BOOST includes +*/ +#include +#define for_each BOOST_FOREACH + +namespace naoqi{ + +namespace converter{ + +template +SoundEventConverter::SoundEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session) + : BaseConverter >(name, frequency, session) +{ +} + +template +SoundEventConverter::~SoundEventConverter() { +} + +template +void SoundEventConverter::reset() +{ +} + +template +void SoundEventConverter::registerCallback( const message_actions::MessageAction action, Callback_t cb ) +{ + callbacks_[action] = cb; +} + +template +void SoundEventConverter::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 SoundEventConverter; +} + +} diff --git a/src/converters/sound.hpp b/src/converters/sound.hpp new file mode 100644 index 00000000..ef3c364b --- /dev/null +++ b/src/converters/sound.hpp @@ -0,0 +1,67 @@ +/* + * 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 SOUND_EVENT_CONVERTER_HPP +#define SOUND_EVENT_CONVERTER_HPP + +/* +* LOCAL includes +*/ +#include "converter_base.hpp" +#include + +/* +* ROS includes +*/ +#include +/* +* ALDEBARAN includes +*/ +#include + +namespace naoqi{ + +namespace converter{ + +template +class SoundEventConverter : public BaseConverter > +{ + + typedef boost::function Callback_t; + +public: + SoundEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session); + + ~SoundEventConverter(); + + 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/event/sound.cpp b/src/event/sound.cpp new file mode 100644 index 00000000..53767f23 --- /dev/null +++ b/src/event/sound.cpp @@ -0,0 +1,251 @@ +/* + * 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 "sound.hpp" +#include "../tools/from_any_value.hpp" + +namespace naoqi +{ + +template +SoundEventRegister::SoundEventRegister() +{ +} + +template +SoundEventRegister::SoundEventRegister(const std::string& name, std::vector keys, const float& frequency, const qi::SessionPtr& session ) + : serviceId(0), + keys_(keys), + p_memory_(session->service("ALMemory")), + p_sound_loc_(session->service("ALSoundLocalization")), + 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) ); + +} + +template +SoundEventRegister::~SoundEventRegister() +{ + stopProcess(); +} + +template +void SoundEventRegister::resetPublisher(ros::NodeHandle& nh) +{ + publisher_->reset(nh); +} + +template +void SoundEventRegister::resetRecorder( boost::shared_ptr gr ) +{ + recorder_->reset(gr, converter_->frequency()); +} + +template +void SoundEventRegister::setEnergyComputation(bool run) { + p_sound_loc_.call("setParameter", "EnergyComputation", run); +} + +template +void SoundEventRegister::setSensitivity(float level) { + p_sound_loc_.call("setParameter", "Sensitivity", level); +} + +template +void SoundEventRegister::startProcess() +{ + boost::mutex::scoped_lock start_lock(mutex_); + if (!isStarted_) + { + if(!serviceId) + { + 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, "soundCallback"); + } + std::cout << serviceName << " : Start" << std::endl; + } + isStarted_ = true; + } +} + +template +void SoundEventRegister::stopProcess() +{ + boost::mutex::scoped_lock stop_lock(mutex_); + if (isStarted_) + { + std::string serviceName = std::string("ROS-Driver-") + keys_[0]; + if(serviceId){ + for(std::vector::const_iterator it = keys_.begin(); it != keys_.end(); ++it) { + p_memory_.call("unsubscribeToEvent",it->c_str(), serviceName); + } + session_->unregisterService(serviceId); + serviceId = 0; + } + std::cout << serviceName << " : Stop" << std::endl; + isStarted_ = false; + } +} + +template +void SoundEventRegister::writeDump(const ros::Time& time) +{ + if (isStarted_) + { + recorder_->writeDump(time); + } +} + +template +void SoundEventRegister::setBufferDuration(float duration) +{ + recorder_->setBufferDuration(duration); +} + +template +void SoundEventRegister::isRecording(bool state) +{ + boost::mutex::scoped_lock rec_lock(mutex_); + isRecording_ = state; +} + +template +void SoundEventRegister::isPublishing(bool state) +{ + boost::mutex::scoped_lock pub_lock(mutex_); + isPublishing_ = state; +} + +template +void SoundEventRegister::isDumping(bool state) +{ + boost::mutex::scoped_lock dump_lock(mutex_); + isDumping_ = state; +} + +template +void SoundEventRegister::registerCallback() +{ +} + +template +void SoundEventRegister::unregisterCallback() +{ +} + +template +void SoundEventRegister::soundCallback(std::string &key, qi::AnyValue &value, qi::AnyValue &message) { + T msg = T(); + + soundCallbackMessage(key, value, 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 SoundEventRegister::soundCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::AudioSourceLocalization &msg) { + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = "Head"; + + qi::AnyReferenceVector anyref; + try{ + anyref = value.asListValuePtr(); + } + catch(std::runtime_error& e) + { + ROS_INFO_STREAM("Could not transform AnyValue into list: " << e.what()); + } + + if(anyref.size() != 4) { + ROS_INFO("Could not retrieve sound location"); + return; + } + + qi::AnyReference ref = anyref[1].content(); + + if(ref.kind() == qi::TypeKind_List && ref.size() == 4) { + qi::AnyReference azi, ele, conf, ener; + azi = ref[0].content(); + ele = ref[1].content(); + conf = ref[2].content(); + ener = ref[3].content(); + if(azi.kind() == qi::TypeKind_Float && ele.kind() == qi::TypeKind_Float + && conf.kind() == qi::TypeKind_Float && ener.kind() == qi::TypeKind_Float) { + msg.azimuth.data = azi.asFloat(); + msg.elevation.data = ele.asFloat(); + msg.confidence.data = conf.asFloat(); + msg.energy.data = ener.asFloat(); + } else{ + ROS_INFO("Could not retrieve azi/ele/conf/ener"); + } + } else { + ROS_INFO("Could not retrieve sound location"); + } +} + +// http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor +template class SoundEventRegister; + +}//namespace diff --git a/src/event/sound.hpp b/src/event/sound.hpp new file mode 100644 index 00000000..2f6a8fdb --- /dev/null +++ b/src/event/sound.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 SOUND_EVENT_REGISTER_HPP +#define SOUND_EVENT_REGISTER_HPP + +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +// Converter +#include "../src/converters/sound.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 SoundEventRegister: public boost::enable_shared_from_this > +{ + +public: + + /** + * @brief Constructor for recorder interface + */ + SoundEventRegister(); + SoundEventRegister( const std::string& name, std::vector keys, const float& frequency, const qi::SessionPtr& session ); + ~SoundEventRegister(); + + 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 processRemote(int nbOfChannels, int samplesByChannel, qi::AnyValue altimestamp, qi::AnyValue buffer); + void soundCallback(std::string &key, qi::AnyValue &value, qi::AnyValue &message); + void soundCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::AudioSourceLocalization &msg); + + void setEnergyComputation(bool run); + void setSensitivity(float level); + +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_, p_sound_loc_; + unsigned int serviceId; + + boost::mutex mutex_; + + bool isStarted_; + bool isPublishing_; + bool isRecording_; + bool isDumping_; + +protected: + std::vector keys_; + +}; // class + +class SoundLocalizedEventRegister: public SoundEventRegister +{ +public: + SoundLocalizedEventRegister( const std::string& name, std::vector keys, const float& frequency, const qi::SessionPtr& session ) : SoundEventRegister(name, keys, frequency, session) {} +}; + +//QI_REGISTER_OBJECT(SoundLocalizedEventRegister, soundCallback) + +static bool _qiregisterAudioEventRegisterSoundLocalized() { + ::qi::ObjectTypeBuilder > b; + QI_VAARGS_APPLY(__QI_REGISTER_ELEMENT, SoundEventRegister, soundCallback) + b.registerType(); + return true; + } +static bool BOOST_PP_CAT(__qi_registration, __LINE__) = _qiregisterAudioEventRegisterSoundLocalized(); + +} //naoqi + +#endif diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 6898493a..14762784 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -42,6 +42,7 @@ #include "converters/log.hpp" #include "converters/battery.hpp" #include "converters/odom.hpp" +#include "converters/sound.hpp" /* * PUBLISHERS @@ -90,6 +91,7 @@ #include "event/audio.hpp" #include "event/touch.hpp" #include "event/people.hpp" +#include "event/sound.hpp" /* * STATIC FUNCTIONS INCLUDE @@ -541,6 +543,11 @@ void Driver::registerDefaultConverter() bool audio_enabled = boot_config_.get( "converters.audio.enabled", true); size_t audio_frequency = boot_config_.get( "converters.audio.frequency", 1); + bool sound_enabled = boot_config_.get( "converters.sound.enabled", true); + size_t sound_frequency = boot_config_.get( "converters.sound.frequency", 1); + bool sound_energy = boot_config_.get( "converters.sound.energy", true); + float sound_sensitivity = boot_config_.get( "converters.sound.sensitivity", 0.9); + bool logs_enabled = boot_config_.get( "converters.logs.enabled", true); size_t logs_frequency = boot_config_.get( "converters.logs.frequency", 10); @@ -804,6 +811,23 @@ void Driver::registerDefaultConverter() event_map_.find("audio")->second.isPublishing(true); } } + + if( sound_enabled ) { + /** Sound **/ + std::vector sound_events; + sound_events.push_back("ALSoundLocalization/SoundLocated"); + boost::shared_ptr event_register = + boost::make_shared( "sound_localized", sound_events, 0, sessionPtr_ ); + event_register->setEnergyComputation(sound_energy); + event_register->setSensitivity(sound_sensitivity); + insertEventConverter("sound_localized", event_register); + if (keep_looping) { + event_map_.find("sound_localized")->second.startProcess(); + } + if (publish_enabled_) { + event_map_.find("sound_localized")->second.isPublishing(true); + } + } /** TOUCH **/ if ( bumper_enabled ) From 7c02ab3e18bb1a7963ffdcae2b84a1dd570774f8 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Wed, 5 Oct 2016 10:42:07 +0100 Subject: [PATCH 17/44] Changing error output to debug level --- src/event/sound.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/event/sound.cpp b/src/event/sound.cpp index 53767f23..e820cc46 100644 --- a/src/event/sound.cpp +++ b/src/event/sound.cpp @@ -215,11 +215,11 @@ void SoundEventRegister::soundCallbackMessage(std::string &key, qi::AnyValue } catch(std::runtime_error& e) { - ROS_INFO_STREAM("Could not transform AnyValue into list: " << e.what()); + ROS_DEBUG_STREAM("Could not transform AnyValue into list: " << e.what()); } if(anyref.size() != 4) { - ROS_INFO("Could not retrieve sound location"); + ROS_DEBUG("Could not retrieve sound location"); return; } @@ -238,10 +238,10 @@ void SoundEventRegister::soundCallbackMessage(std::string &key, qi::AnyValue msg.confidence.data = conf.asFloat(); msg.energy.data = ener.asFloat(); } else{ - ROS_INFO("Could not retrieve azi/ele/conf/ener"); + ROS_DEBUG("Could not retrieve azi/ele/conf/ener"); } } else { - ROS_INFO("Could not retrieve sound location"); + ROS_DEBUG("Could not retrieve sound location"); } } From 17171f42f3aa522a25fc7d575e925be9d1455ca2 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Wed, 5 Oct 2016 17:22:23 +0100 Subject: [PATCH 18/44] Adding basic behavior manager functionaility via services --- CMakeLists.txt | 1 + CMakeLists_catkin.txt | 1 + package.xml | 1 + src/naoqi_driver.cpp | 5 ++ src/services/behavior_manager.cpp | 67 +++++++++++++++++++++++ src/services/behavior_manager.hpp | 88 +++++++++++++++++++++++++++++++ 6 files changed, 163 insertions(+) create mode 100644 src/services/behavior_manager.cpp create mode 100644 src/services/behavior_manager.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 86eb467f..9dedcde7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,6 +52,7 @@ set( set( SERVICES_SRC src/services/robot_config.cpp + src/services/behavior_manager.cpp ) set( diff --git a/CMakeLists_catkin.txt b/CMakeLists_catkin.txt index 7498e259..9dce6d55 100644 --- a/CMakeLists_catkin.txt +++ b/CMakeLists_catkin.txt @@ -18,6 +18,7 @@ find_package(catkin COMPONENTS rosbag_storage rosgraph_msgs sensor_msgs + std_srvs tf2_geometry_msgs tf2_msgs tf2_ros diff --git a/package.xml b/package.xml index b9a2b03e..053438fb 100644 --- a/package.xml +++ b/package.xml @@ -26,6 +26,7 @@ rosbag_storage rosgraph_msgs sensor_msgs + std_srvs tf2_geometry_msgs tf2_msgs tf2_ros diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 14762784..86ed13ff 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -73,6 +73,7 @@ * SERVICES */ #include "services/robot_config.hpp" +#include "services/behavior_manager.hpp" /* * RECORDERS @@ -973,6 +974,10 @@ void Driver::registerService( service::Service srv ) void Driver::registerDefaultServices() { registerService( boost::make_shared("robot config service", "/naoqi_driver/get_robot_config", sessionPtr_) ); + registerService( boost::make_shared("getInstalledBehaviors", "/naoqi_driver/get_installed_behaviors", sessionPtr_) ); + registerService( boost::make_shared("getRunningBehaviors", "/naoqi_driver/get_running_behaviors", sessionPtr_) ); + registerService( boost::make_shared("startBehavior", "/naoqi_driver/start_behaviour", sessionPtr_) ); + registerService( boost::make_shared("stopBehavior", "/naoqi_driver/stop_behaviour", sessionPtr_) ); } std::vector Driver::getAvailableConverters() diff --git a/src/services/behavior_manager.cpp b/src/services/behavior_manager.cpp new file mode 100644 index 00000000..227d4976 --- /dev/null +++ b/src/services/behavior_manager.cpp @@ -0,0 +1,67 @@ +/* + * 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 "behavior_manager.hpp" +#include "../helpers/driver_helpers.hpp" + +namespace naoqi +{ +namespace service +{ + +void BehaviorManagerInfoService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &BehaviorManagerInfoService::callback, this); +} + +bool BehaviorManagerInfoService::callback(nao_interaction_msgs::BehaviorManagerInfoRequest& req, nao_interaction_msgs::BehaviorManagerInfoResponse& resp) { + resp.behaviors = p_bm_.call >(name_); + return true; +} + +void BehaviorManagerControlService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &BehaviorManagerControlService::callback, this); +} + +bool BehaviorManagerControlService::callback(nao_interaction_msgs::BehaviorManagerControlRequest& req, nao_interaction_msgs::BehaviorManagerControlResponse& resp) { + resp.success = true; + if(req.name.compare("ALL")==0 && name_.compare("stopBehavior")==0) { // Stop everything if ALL is given as behavior name + p_bm_.call("stopAllBehaviors"); + } else if(p_bm_.call("isBehaviorInstalled", req.name)) { + if(name_.compare("startBehavior") == 0) { + if(p_bm_.call("isBehaviorRunning", req.name)) { + return false; // To make clear that it is successfully started but the service call failed because it was already running + } else { + p_bm_.call(name_, req.name); + } + } else { + if(!p_bm_.call("isBehaviorRunning", req.name)) { + return false; + } else { + p_bm_.call(name_, req.name); + } + } + } else { + resp.success = false; + return false; + } + return true; +} + +} +} diff --git a/src/services/behavior_manager.hpp b/src/services/behavior_manager.hpp new file mode 100644 index 00000000..6d3f2c0d --- /dev/null +++ b/src/services/behavior_manager.hpp @@ -0,0 +1,88 @@ +/* + * 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 BEHAVIOR_MANAGER_SERVICE_HPP +#define BEHAVIOR_MANAGER_SERVICE_HPP + +#include + +#include +#include + +#include +#include +#include + +namespace naoqi +{ +namespace service +{ + +class BehaviorManagerService +{ +public: + BehaviorManagerService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ) : + name_(name), + topic_(topic), + session_(session), + p_bm_(session->service("ALBehaviorManager")) {} + + ~BehaviorManagerService(){}; + + std::string name() const + { + return name_; + } + + std::string topic() const + { + return topic_; + } + + virtual void reset( ros::NodeHandle& nh )=0; + +protected: + const std::string name_; + const std::string topic_; + + const qi::SessionPtr& session_; + qi::AnyObject p_bm_; + ros::ServiceServer service_; +}; + +class BehaviorManagerInfoService : public BehaviorManagerService +{ +public: + BehaviorManagerInfoService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : BehaviorManagerService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::BehaviorManagerInfoRequest& req, nao_interaction_msgs::BehaviorManagerInfoResponse& resp); + +}; + +class BehaviorManagerControlService : public BehaviorManagerService +{ +public: + BehaviorManagerControlService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : BehaviorManagerService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::BehaviorManagerControlRequest& req, nao_interaction_msgs::BehaviorManagerControlResponse& resp); + +}; + +} // service +} // naoqi +#endif From b8f48daea4bc4f4000c3f93157dbe3cec4c90fd2 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Wed, 5 Oct 2016 16:13:44 +0100 Subject: [PATCH 19/44] Adding services for learnHome and goToHome --- CMakeLists.txt | 2 +- src/naoqi_driver.cpp | 7 ++-- src/services/localization.cpp | 46 ++++++++++++++++++++++++ src/services/localization.hpp | 68 +++++++++++++++++++++++++++++++++++ 4 files changed, 120 insertions(+), 3 deletions(-) create mode 100644 src/services/localization.cpp create mode 100644 src/services/localization.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9dedcde7..4362fb0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,6 @@ set( src/converters/memory/string.cpp src/converters/sonar.cpp src/converters/log.cpp - src/converters/odom.cpp src/converters/battery.cpp src/converters/odom.cpp src/converters/sound.cpp @@ -53,6 +52,7 @@ set( SERVICES_SRC src/services/robot_config.cpp src/services/behavior_manager.cpp + src/services/localization.cpp ) set( diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 86ed13ff..51742c4e 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -74,6 +74,7 @@ */ #include "services/robot_config.hpp" #include "services/behavior_manager.hpp" +#include "services/localization.hpp" /* * RECORDERS @@ -594,8 +595,8 @@ void Driver::registerDefaultConverter() bool battery_enabled = boot_config_.get( "converters.battery.enabled", true); size_t battery_frequency = boot_config_.get( "converters.battery.frequency", 10); - bool odom_enabled = boot_config_.get( "converters.odom.enabled", true); - size_t odom_frequency = boot_config_.get( "converters.odom.frequency", 10); + bool odom_enabled = boot_config_.get( "converters.odom.enabled", true); + size_t odom_frequency = boot_config_.get( "converters.odom.frequency", 10); bool bumper_enabled = boot_config_.get( "converters.bumper.enabled", true); bool hand_enabled = boot_config_.get( "converters.touch_hand.enabled", true); @@ -978,6 +979,8 @@ void Driver::registerDefaultServices() registerService( boost::make_shared("getRunningBehaviors", "/naoqi_driver/get_running_behaviors", sessionPtr_) ); registerService( boost::make_shared("startBehavior", "/naoqi_driver/start_behaviour", sessionPtr_) ); registerService( boost::make_shared("stopBehavior", "/naoqi_driver/stop_behaviour", sessionPtr_) ); + registerService( boost::make_shared("learnHome", "/naoqi_driver/learn_home", sessionPtr_) ); + registerService( boost::make_shared("goToHome", "/naoqi_driver/go_to_home", sessionPtr_) ); } std::vector Driver::getAvailableConverters() diff --git a/src/services/localization.cpp b/src/services/localization.cpp new file mode 100644 index 00000000..5c32ee62 --- /dev/null +++ b/src/services/localization.cpp @@ -0,0 +1,46 @@ +/* + * 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 "localization.hpp" +#include "../helpers/driver_helpers.hpp" + +namespace naoqi +{ +namespace service +{ + +LocalizationService::LocalizationService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ) + : name_(name), + topic_(topic), + session_(session), + p_localization_(session->service("ALLocalization")) +{} + +void LocalizationService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &LocalizationService::callback, this); +} + +bool LocalizationService::callback(std_srvs::EmptyRequest &req, std_srvs::EmptyResponse &resp ) +{ + p_localization_.call(name_); + return true; +} + + +} +} diff --git a/src/services/localization.hpp b/src/services/localization.hpp new file mode 100644 index 00000000..c8d31e21 --- /dev/null +++ b/src/services/localization.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 LOCALIZATION_SERVICE_HPP +#define LOCALIZATION_SERVICE_HPP + +#include + +#include +#include + +#include +#include + +namespace naoqi +{ +namespace service +{ + +class LocalizationService +{ +public: + LocalizationService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ); + + ~LocalizationService(){}; + + std::string name() const + { + return name_; + } + + std::string topic() const + { + return topic_; + } + + void reset( ros::NodeHandle& nh ); + + bool callback( std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp ); + + +private: + const std::string name_; + const std::string topic_; + + const qi::SessionPtr& session_; + qi::AnyObject p_localization_; + ros::ServiceServer service_; +}; + +} // service +} // naoqi +#endif From 5b6d061980a19ae48f2772f3b05c817ba85803ab Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Wed, 12 Oct 2016 15:33:36 +0200 Subject: [PATCH 20/44] Update README.rst --- README.rst | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/README.rst b/README.rst index 95c01686..073aa7f8 100644 --- a/README.rst +++ b/README.rst @@ -17,13 +17,12 @@ How it works ============ The **naoqi_driver** module is a NAOqi module that also acts -as a ROS node. As there is no **roscore** on the robot, it -needs to be given the IP of the **roscore** in order to be +as a ROS node. It needs to be given the IP of the **roscore** in order to be registered as a node in the ROS processing graph. Usually, -you will start your **roscore** on your local desktop. +you will start your **roscore** on your local desktop +(or on a robot in case of instaling ROS there). -Once connected, normal ROS communication is happening between -your robot, running NAOqi OS, and your desktop, running ROS. +Once connected, ROS communicates with NAOqi OS running on your robot. For further information, you can go `here `_ or build the doc: From 61c8daf0b97c8e3540680281e1aac3caaa2cf9d3 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Mon, 21 Nov 2016 11:50:24 +0000 Subject: [PATCH 21/44] Adding ALtracker services. Currently only Face, People, and Sound tracking are supported. --- CMakeLists.txt | 1 + src/naoqi_driver.cpp | 6 +++ src/services/tracking.cpp | 88 ++++++++++++++++++++++++++++++ src/services/tracking.hpp | 110 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 205 insertions(+) create mode 100644 src/services/tracking.cpp create mode 100644 src/services/tracking.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4362fb0e..cfcce8ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,6 +53,7 @@ set( src/services/robot_config.cpp src/services/behavior_manager.cpp src/services/localization.cpp + src/services/tracking.cpp ) set( diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 51742c4e..241f2df6 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -75,6 +75,7 @@ #include "services/robot_config.hpp" #include "services/behavior_manager.hpp" #include "services/localization.hpp" +#include "services/tracking.hpp" /* * RECORDERS @@ -981,6 +982,11 @@ void Driver::registerDefaultServices() registerService( boost::make_shared("stopBehavior", "/naoqi_driver/stop_behaviour", sessionPtr_) ); registerService( boost::make_shared("learnHome", "/naoqi_driver/learn_home", sessionPtr_) ); registerService( boost::make_shared("goToHome", "/naoqi_driver/go_to_home", sessionPtr_) ); + registerService( boost::make_shared("stopTracker", "/naoqi_driver/tracker/stop_tracker", sessionPtr_) ); + registerService( boost::make_shared("unregisterAllTargets", "/naoqi_driver/tracker/unregister_all_targets", sessionPtr_) ); + registerService( boost::make_shared("setMode", "/naoqi_driver/tracker/set_mode", sessionPtr_) ); + registerService( boost::make_shared("track", "/naoqi_driver/tracker/track", sessionPtr_) ); + registerService( boost::make_shared("registerTarget", "/naoqi_driver/tracker/register_target", sessionPtr_) ); } std::vector Driver::getAvailableConverters() diff --git a/src/services/tracking.cpp b/src/services/tracking.cpp new file mode 100644 index 00000000..2996a286 --- /dev/null +++ b/src/services/tracking.cpp @@ -0,0 +1,88 @@ +/* + * 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 "tracking.hpp" +#include "../helpers/driver_helpers.hpp" + +namespace naoqi +{ +namespace service +{ + +void TrackerSetTargetService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &TrackerSetTargetService::callback, this); +} + +bool TrackerSetTargetService::callback(nao_interaction_msgs::SetTrackerTargetRequest &req, nao_interaction_msgs::SetTrackerTargetResponse &resp) +{ + if(req.target.compare(req.FACE) == 0) { + p_tracker_.call(name_, req.target, req.values[0]); + } else if(req.target.compare(req.PEOPLE) == 0 || req.target.compare(req.SOUND) == 0) { + p_tracker_.call(name_, req.target, req.values); + } else { + ROS_ERROR_STREAM("Unknown target '" << req.target <<"'"); + return false; + } + return true; +} + +void TrackerStartTrackingService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &TrackerStartTrackingService::callback, this); +} + +bool TrackerStartTrackingService::callback(nao_interaction_msgs::StartTrackerRequest& req, nao_interaction_msgs::StartTrackerResponse& resp) +{ + if(req.target.compare(req.FACE) == 0 || req.target.compare(req.PEOPLE) == 0 || req.target.compare(req.SOUND) == 0) { + p_tracker_.call(name_, req.target); + } else { + ROS_ERROR_STREAM("Unknown target '" << req.target <<"'"); + return false; + } + return true; +} + +void TrackerSetModeService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &TrackerSetModeService::callback, this); +} + +bool TrackerSetModeService::callback(nao_interaction_msgs::SetTrackerModeRequest& req, nao_interaction_msgs::SetTrackerModeResponse& resp) +{ + if(req.mode.compare(req.HEAD) == 0 || req.mode.compare(req.WHOLEBODY) == 0 || req.mode.compare(req.MOVE) == 0) { + p_tracker_.call(name_, req.mode); + } else { + ROS_ERROR_STREAM("Unknown mode '" << req.mode <<"'"); + return false; + } + return true; +} + +void TrackerEmptyServices::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &TrackerEmptyServices::callback, this); +} + +bool TrackerEmptyServices::callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) +{ + p_tracker_.call(name_); + return true; +} + +} +} diff --git a/src/services/tracking.hpp b/src/services/tracking.hpp new file mode 100644 index 00000000..d94aede5 --- /dev/null +++ b/src/services/tracking.hpp @@ -0,0 +1,110 @@ +/* + * 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 TRACKER_SERVICE_HPP +#define TRACKER_SERVICE_HPP + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace naoqi +{ +namespace service +{ + +class TrackerService +{ +public: + TrackerService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ): + name_(name), + topic_(topic), + session_(session), + p_tracker_(session->service("ALTracker")) + {} + + ~TrackerService(){} + + std::string name() const + { + return name_; + } + + std::string topic() const + { + return topic_; + } + + virtual void reset( ros::NodeHandle& nh )=0; + +protected: + const std::string name_; + const std::string topic_; + + const qi::SessionPtr& session_; + qi::AnyObject p_tracker_; + ros::ServiceServer service_; +}; + +class TrackerSetTargetService : public TrackerService +{ +public: + TrackerSetTargetService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : TrackerService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::SetTrackerTargetRequest& req, nao_interaction_msgs::SetTrackerTargetResponse& resp); + +}; + +class TrackerStartTrackingService : public TrackerService +{ +public: + TrackerStartTrackingService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : TrackerService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::StartTrackerRequest& req, nao_interaction_msgs::StartTrackerResponse& resp); + +}; + +class TrackerSetModeService : public TrackerService +{ +public: + TrackerSetModeService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : TrackerService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::SetTrackerModeRequest& req, nao_interaction_msgs::SetTrackerModeResponse& resp); + +}; + +class TrackerEmptyServices : public TrackerService +{ +public: + TrackerEmptyServices(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : TrackerService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp); + +}; + +} // service +} // naoqi +#endif From 9e0f103124082f62cb4ba2b9db53d18f06df9687 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Wed, 30 Nov 2016 09:23:31 +0000 Subject: [PATCH 22/44] Removing duplicate odom --- src/naoqi_driver.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 241f2df6..73647b97 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -789,18 +789,6 @@ void Driver::registerDefaultConverter() registerConverter( bc, bp, br ); } } - - /** Odom */ - if ( odom_enabled ) - { - boost::shared_ptr > lp = boost::make_shared >( "odom" ); - boost::shared_ptr > lr = boost::make_shared >( "odom" ); - boost::shared_ptr lc = boost::make_shared( "odom", odom_frequency, sessionPtr_ ); - lc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, lp, _1) ); - lc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, lr, _1) ); - lc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, lr, _1) ); - registerConverter( lc, lp, lr ); - } if ( audio_enabled ) { /** Audio */ From f23c8a2effbc46ae6eacd981e6543f4753cec371 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Fri, 25 Nov 2016 08:43:50 +0000 Subject: [PATCH 23/44] Adding basic breathing control Adding more functionality from ALLocalization Introducing sub-namespaces for all new services to make the easier to distinguish --- CMakeLists.txt | 1 + src/naoqi_driver.cpp | 20 +++++--- src/services/breathing.cpp | 48 +++++++++++++++++++ src/services/breathing.hpp | 80 ++++++++++++++++++++++++++++++++ src/services/localization.cpp | 64 ++++++++++++++++++++++---- src/services/localization.hpp | 86 ++++++++++++++++++++++++++++++++--- 6 files changed, 276 insertions(+), 23 deletions(-) create mode 100644 src/services/breathing.cpp create mode 100644 src/services/breathing.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index cfcce8ea..0c0762a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,6 +54,7 @@ set( src/services/behavior_manager.cpp src/services/localization.cpp src/services/tracking.cpp + src/services/breathing.cpp ) set( diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 73647b97..6c5e248b 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -76,6 +76,7 @@ #include "services/behavior_manager.hpp" #include "services/localization.hpp" #include "services/tracking.hpp" +#include "services/breathing.hpp" /* * RECORDERS @@ -964,17 +965,24 @@ void Driver::registerService( service::Service srv ) void Driver::registerDefaultServices() { registerService( boost::make_shared("robot config service", "/naoqi_driver/get_robot_config", sessionPtr_) ); - registerService( boost::make_shared("getInstalledBehaviors", "/naoqi_driver/get_installed_behaviors", sessionPtr_) ); - registerService( boost::make_shared("getRunningBehaviors", "/naoqi_driver/get_running_behaviors", sessionPtr_) ); - registerService( boost::make_shared("startBehavior", "/naoqi_driver/start_behaviour", sessionPtr_) ); - registerService( boost::make_shared("stopBehavior", "/naoqi_driver/stop_behaviour", sessionPtr_) ); - registerService( boost::make_shared("learnHome", "/naoqi_driver/learn_home", sessionPtr_) ); - registerService( boost::make_shared("goToHome", "/naoqi_driver/go_to_home", sessionPtr_) ); + registerService( boost::make_shared("getInstalledBehaviors", "/naoqi_driver/behaviour_manager/get_installed_behaviors", sessionPtr_) ); + registerService( boost::make_shared("getRunningBehaviors", "/naoqi_driver/behaviour_manager/get_running_behaviors", sessionPtr_) ); + registerService( boost::make_shared("startBehavior", "/naoqi_driver/behaviour_manager/start_behaviour", sessionPtr_) ); + registerService( boost::make_shared("stopBehavior", "/naoqi_driver/behaviour_manager/stop_behaviour", sessionPtr_) ); + registerService( boost::make_shared("ALLocalization-learnHome", "/naoqi_driver/localization/learn_home", sessionPtr_) ); + registerService( boost::make_shared("ALLocalization-goToHome", "/naoqi_driver/localization/go_to_home", sessionPtr_) ); + registerService( boost::make_shared("ALLocalization-stopAll", "/naoqi_driver/localization/stop_all", sessionPtr_) ); + registerService( boost::make_shared("ALLocalization-isDataAvailable", "/naoqi_driver/localization/is_data_available", sessionPtr_) ); + registerService( boost::make_shared("ALLocalization-getMessageFromErrorCode", "/naoqi_driver/localization/get_message_from_error_code", sessionPtr_) ); + registerService( boost::make_shared("ALLocalization-clear", "/naoqi_driver/localization/clear", sessionPtr_) ); + registerService( boost::make_shared("ALLocalization-load", "/naoqi_driver/localization/load", sessionPtr_) ); + registerService( boost::make_shared("ALLocalization-save", "/naoqi_driver/localization/save", sessionPtr_) ); registerService( boost::make_shared("stopTracker", "/naoqi_driver/tracker/stop_tracker", sessionPtr_) ); registerService( boost::make_shared("unregisterAllTargets", "/naoqi_driver/tracker/unregister_all_targets", sessionPtr_) ); registerService( boost::make_shared("setMode", "/naoqi_driver/tracker/set_mode", sessionPtr_) ); registerService( boost::make_shared("track", "/naoqi_driver/tracker/track", sessionPtr_) ); registerService( boost::make_shared("registerTarget", "/naoqi_driver/tracker/register_target", sessionPtr_) ); + registerService( boost::make_shared("setBreathEnabled", "/naoqi_driver/motion/set_breath_enabled", sessionPtr_) ); } std::vector Driver::getAvailableConverters() diff --git a/src/services/breathing.cpp b/src/services/breathing.cpp new file mode 100644 index 00000000..bf031588 --- /dev/null +++ b/src/services/breathing.cpp @@ -0,0 +1,48 @@ +/* + * 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 "breathing.hpp" +#include "../helpers/driver_helpers.hpp" + +namespace naoqi +{ +namespace service +{ + +void EnableBreathingService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &EnableBreathingService::callback, this); +} + +bool EnableBreathingService::callback(nao_interaction_msgs::SetBreathEnabledRequest& req, nao_interaction_msgs::SetBreathEnabledResponse& resp) +{ + if(req.chain_name.compare(req.HEAD) == 0 + || req.chain_name.compare(req.BODY) == 0 + || req.chain_name.compare(req.ARMS) == 0 + || req.chain_name.compare(req.LEGS) == 0 + || req.chain_name.compare(req.LARM) == 0 + || req.chain_name.compare(req.RARM) == 0) { + p_motion_.call(name_, req.chain_name, req.enable); + } else { + ROS_ERROR_STREAM("Unknown chain_name '" << req.chain_name <<"'"); + return false; + } + return true; +} + +} +} diff --git a/src/services/breathing.hpp b/src/services/breathing.hpp new file mode 100644 index 00000000..cc03f841 --- /dev/null +++ b/src/services/breathing.hpp @@ -0,0 +1,80 @@ +/* + * 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 BREATHING_SERVICE_HPP +#define BREATHING_SERVICE_HPP + +#include +#include + +#include +#include + +#include +#include + +namespace naoqi +{ +namespace service +{ + +class BreathingService +{ +public: + BreathingService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ): + name_(name), + topic_(topic), + session_(session), + p_motion_(session->service("ALMotion")) + {} + + ~BreathingService(){} + + std::string name() const + { + return name_; + } + + std::string topic() const + { + return topic_; + } + + virtual void reset( ros::NodeHandle& nh )=0; + +protected: + const std::string name_; + const std::string topic_; + + const qi::SessionPtr& session_; + qi::AnyObject p_motion_; + ros::ServiceServer service_; +}; + +class EnableBreathingService : public BreathingService +{ +public: + EnableBreathingService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : BreathingService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::SetBreathEnabledRequest& req, nao_interaction_msgs::SetBreathEnabledResponse& resp); + +}; + +} // service +} // naoqi +#endif diff --git a/src/services/localization.cpp b/src/services/localization.cpp index 5c32ee62..46684051 100644 --- a/src/services/localization.cpp +++ b/src/services/localization.cpp @@ -23,24 +23,68 @@ namespace naoqi namespace service { -LocalizationService::LocalizationService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ) - : name_(name), - topic_(topic), - session_(session), - p_localization_(session->service("ALLocalization")) -{} +void LocalizationEmptyService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &LocalizationEmptyService::callback, this); +} + +bool LocalizationEmptyService::callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) +{ + p_localization_.call(func_); + return true; +} + +// ############## + +void LocalizationTriggerService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &LocalizationTriggerService::callback, this); +} + +bool LocalizationTriggerService::callback(nao_interaction_msgs::LocalizationTriggerRequest& req, nao_interaction_msgs::LocalizationTriggerResponse& resp) +{ + resp.result = p_localization_.call(func_); + return true; +} + +// ############## -void LocalizationService::reset( ros::NodeHandle& nh ) +void LocalizationTriggerStringService::reset( ros::NodeHandle& nh ) { - service_ = nh.advertiseService(topic_, &LocalizationService::callback, this); + service_ = nh.advertiseService(topic_, &LocalizationTriggerStringService::callback, this); } -bool LocalizationService::callback(std_srvs::EmptyRequest &req, std_srvs::EmptyResponse &resp ) +bool LocalizationTriggerStringService::callback(nao_interaction_msgs::LocalizationTriggerStringRequest& req, nao_interaction_msgs::LocalizationTriggerStringResponse& resp) { - p_localization_.call(name_); + resp.result = p_localization_.call(func_, req.value); return true; } +// ############## + +void LocalizationCheckService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &LocalizationCheckService::callback, this); +} + +bool LocalizationCheckService::callback(nao_interaction_msgs::LocalizationCheckRequest& req, nao_interaction_msgs::LocalizationCheckResponse& resp) +{ + resp.result = p_localization_.call(func_); + return true; +} + +// ############## + +void LocalizationGetErrorMessageService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &LocalizationGetErrorMessageService::callback, this); +} + +bool LocalizationGetErrorMessageService::callback(nao_interaction_msgs::LocalizationGetErrorMessageRequest& req, nao_interaction_msgs::LocalizationGetErrorMessageResponse& resp) +{ + resp.error_message = p_localization_.call(func_, req.error_code); + return true; +} } } diff --git a/src/services/localization.hpp b/src/services/localization.hpp index c8d31e21..de783665 100644 --- a/src/services/localization.hpp +++ b/src/services/localization.hpp @@ -20,11 +20,18 @@ #define LOCALIZATION_SERVICE_HPP #include +#include +#include +#include #include #include #include +#include +#include +#include +#include #include namespace naoqi @@ -35,9 +42,16 @@ namespace service class LocalizationService { public: - LocalizationService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ); + LocalizationService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ) : + name_(name), + topic_(topic), + session_(session), + p_localization_(session->service("ALLocalization")) + { + func_ = split(name_, '-').back(); + } - ~LocalizationService(){}; + ~LocalizationService(){} std::string name() const { @@ -49,18 +63,76 @@ class LocalizationService return topic_; } - void reset( ros::NodeHandle& nh ); - - bool callback( std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp ); + virtual void reset( ros::NodeHandle& nh )=0; - -private: +protected: const std::string name_; + std::string func_; const std::string topic_; const qi::SessionPtr& session_; qi::AnyObject p_localization_; ros::ServiceServer service_; + + void split(const std::string &s, char delim, std::vector &elems) { + std::stringstream ss; + ss.str(s); + std::string item; + while (std::getline(ss, item, delim)) { + elems.push_back(item); + } + } + + std::vector split(const std::string &s, char delim) { + std::vector elems; + split(s, delim, elems); + return elems; + } +}; + +class LocalizationEmptyService : public LocalizationService +{ +public: + LocalizationEmptyService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : LocalizationService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp); + +}; + +class LocalizationTriggerService : public LocalizationService +{ +public: + LocalizationTriggerService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : LocalizationService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::LocalizationTriggerRequest& req, nao_interaction_msgs::LocalizationTriggerResponse& resp); + +}; + +class LocalizationTriggerStringService : public LocalizationService +{ +public: + LocalizationTriggerStringService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : LocalizationService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::LocalizationTriggerStringRequest& req, nao_interaction_msgs::LocalizationTriggerStringResponse& resp); + +}; + +class LocalizationCheckService : public LocalizationService +{ +public: + LocalizationCheckService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : LocalizationService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::LocalizationCheckRequest& req, nao_interaction_msgs::LocalizationCheckResponse& resp); + +}; + +class LocalizationGetErrorMessageService : public LocalizationService +{ +public: + LocalizationGetErrorMessageService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : LocalizationService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::LocalizationGetErrorMessageRequest& req, nao_interaction_msgs::LocalizationGetErrorMessageResponse& resp); + }; } // service From 7277f33062f06ef0c2ba1db03eefb676c450d46a Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Tue, 29 Nov 2016 08:22:43 +0000 Subject: [PATCH 24/44] Adding more functionality of the ALTracker module --- src/naoqi_driver.cpp | 12 ++++++---- src/services/tracking.cpp | 48 +++++++++++++++++++++++++++++++++++---- src/services/tracking.hpp | 40 +++++++++++++++++++++++++++++++- 3 files changed, 89 insertions(+), 11 deletions(-) diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 6c5e248b..ca6bb7f1 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -977,11 +977,13 @@ void Driver::registerDefaultServices() registerService( boost::make_shared("ALLocalization-clear", "/naoqi_driver/localization/clear", sessionPtr_) ); registerService( boost::make_shared("ALLocalization-load", "/naoqi_driver/localization/load", sessionPtr_) ); registerService( boost::make_shared("ALLocalization-save", "/naoqi_driver/localization/save", sessionPtr_) ); - registerService( boost::make_shared("stopTracker", "/naoqi_driver/tracker/stop_tracker", sessionPtr_) ); - registerService( boost::make_shared("unregisterAllTargets", "/naoqi_driver/tracker/unregister_all_targets", sessionPtr_) ); - registerService( boost::make_shared("setMode", "/naoqi_driver/tracker/set_mode", sessionPtr_) ); - registerService( boost::make_shared("track", "/naoqi_driver/tracker/track", sessionPtr_) ); - registerService( boost::make_shared("registerTarget", "/naoqi_driver/tracker/register_target", sessionPtr_) ); + registerService( boost::make_shared("ALTracker-stopTracker", "/naoqi_driver/tracker/stop_tracker", sessionPtr_) ); + registerService( boost::make_shared("ALTracker-unregisterAllTargets", "/naoqi_driver/tracker/unregister_all_targets", sessionPtr_) ); + registerService( boost::make_shared("ALTracker-setMode", "/naoqi_driver/tracker/set_mode", sessionPtr_) ); + registerService( boost::make_shared("ALTracker-track", "/naoqi_driver/tracker/track", sessionPtr_) ); + registerService( boost::make_shared("ALTracker-registerTarget", "/naoqi_driver/tracker/register_target", sessionPtr_) ); + registerService( boost::make_shared("ALTracker-pointAt", "/naoqi_driver/tracker/point_at", sessionPtr_) ); + registerService( boost::make_shared("ALTracker-lookAt", "/naoqi_driver/tracker/look_at", sessionPtr_) ); registerService( boost::make_shared("setBreathEnabled", "/naoqi_driver/motion/set_breath_enabled", sessionPtr_) ); } diff --git a/src/services/tracking.cpp b/src/services/tracking.cpp index 2996a286..048ad2bc 100644 --- a/src/services/tracking.cpp +++ b/src/services/tracking.cpp @@ -31,9 +31,9 @@ void TrackerSetTargetService::reset( ros::NodeHandle& nh ) bool TrackerSetTargetService::callback(nao_interaction_msgs::SetTrackerTargetRequest &req, nao_interaction_msgs::SetTrackerTargetResponse &resp) { if(req.target.compare(req.FACE) == 0) { - p_tracker_.call(name_, req.target, req.values[0]); + p_tracker_.call(func_, req.target, req.values[0]); } else if(req.target.compare(req.PEOPLE) == 0 || req.target.compare(req.SOUND) == 0) { - p_tracker_.call(name_, req.target, req.values); + p_tracker_.call(func_, req.target, req.values); } else { ROS_ERROR_STREAM("Unknown target '" << req.target <<"'"); return false; @@ -49,7 +49,7 @@ void TrackerStartTrackingService::reset( ros::NodeHandle& nh ) bool TrackerStartTrackingService::callback(nao_interaction_msgs::StartTrackerRequest& req, nao_interaction_msgs::StartTrackerResponse& resp) { if(req.target.compare(req.FACE) == 0 || req.target.compare(req.PEOPLE) == 0 || req.target.compare(req.SOUND) == 0) { - p_tracker_.call(name_, req.target); + p_tracker_.call(func_, req.target); } else { ROS_ERROR_STREAM("Unknown target '" << req.target <<"'"); return false; @@ -65,7 +65,7 @@ void TrackerSetModeService::reset( ros::NodeHandle& nh ) bool TrackerSetModeService::callback(nao_interaction_msgs::SetTrackerModeRequest& req, nao_interaction_msgs::SetTrackerModeResponse& resp) { if(req.mode.compare(req.HEAD) == 0 || req.mode.compare(req.WHOLEBODY) == 0 || req.mode.compare(req.MOVE) == 0) { - p_tracker_.call(name_, req.mode); + p_tracker_.call(func_, req.mode); } else { ROS_ERROR_STREAM("Unknown mode '" << req.mode <<"'"); return false; @@ -80,9 +80,47 @@ void TrackerEmptyServices::reset( ros::NodeHandle& nh ) bool TrackerEmptyServices::callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) { - p_tracker_.call(name_); + p_tracker_.call(func_); return true; } +void TrackerPointAtService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &TrackerPointAtService::callback, this); +} + +bool TrackerPointAtService::callback(nao_interaction_msgs::TrackerPointAtRequest &req, nao_interaction_msgs::TrackerPointAtResponse &resp) +{ + if(req.effector.compare(req.EFFECTOR_ARMS) == 0 || req.effector.compare(req.EFFECTOR_LARM) == 0 || req.effector.compare(req.EFFECTOR_RARM) == 0) { + if(req.frame == req.FRAME_ROBOT || req.frame == req.FRAME_TORSO || req.frame == req.FRAME_WORLD) { + std::vector target; target.push_back(req.target.x); target.push_back(req.target.y); target.push_back(req.target.z); + p_tracker_.call(func_, req.effector, target, req.frame, req.max_speed_fraction); + return true; + } else { + ROS_ERROR_STREAM("Unknown frame '" << req.frame <<"'"); + } + } else { + ROS_ERROR_STREAM("Unknown effector '" << req.effector <<"'"); + } + return false; +} + +void TrackerLookAtService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &TrackerLookAtService::callback, this); +} + +bool TrackerLookAtService::callback(nao_interaction_msgs::TrackerLookAtRequest &req, nao_interaction_msgs::TrackerLookAtResponse &resp) +{ + if(req.frame == req.FRAME_ROBOT || req.frame == req.FRAME_TORSO || req.frame == req.FRAME_WORLD) { + std::vector target; target.push_back(req.target.x); target.push_back(req.target.y); target.push_back(req.target.z); + p_tracker_.call(func_, target, req.frame, req.max_speed_fraction, req.use_whole_body); + return true; + } else { + ROS_ERROR_STREAM("Unknown frame '" << req.frame <<"'"); + } + return false; +} + } } diff --git a/src/services/tracking.hpp b/src/services/tracking.hpp index d94aede5..13087b31 100644 --- a/src/services/tracking.hpp +++ b/src/services/tracking.hpp @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include namespace naoqi @@ -44,7 +46,9 @@ class TrackerService topic_(topic), session_(session), p_tracker_(session->service("ALTracker")) - {} + { + func_ = split(name_, '-').back(); + } ~TrackerService(){} @@ -62,11 +66,27 @@ class TrackerService protected: const std::string name_; + std::string func_; const std::string topic_; const qi::SessionPtr& session_; qi::AnyObject p_tracker_; ros::ServiceServer service_; + + void split(const std::string &s, char delim, std::vector &elems) { + std::stringstream ss; + ss.str(s); + std::string item; + while (std::getline(ss, item, delim)) { + elems.push_back(item); + } + } + + std::vector split(const std::string &s, char delim) { + std::vector elems; + split(s, delim, elems); + return elems; + } }; class TrackerSetTargetService : public TrackerService @@ -105,6 +125,24 @@ class TrackerEmptyServices : public TrackerService }; +class TrackerPointAtService : public TrackerService +{ +public: + TrackerPointAtService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : TrackerService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::TrackerPointAtRequest& req, nao_interaction_msgs::TrackerPointAtResponse& resp); + +}; + +class TrackerLookAtService : public TrackerService +{ +public: + TrackerLookAtService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : TrackerService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::TrackerLookAtRequest& req, nao_interaction_msgs::TrackerLookAtResponse& resp); + +}; + } // service } // naoqi #endif From ea21316c80fad02ddc87960c58e0994871164141 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Wed, 30 Nov 2016 08:55:47 +0000 Subject: [PATCH 25/44] Adding animated speech and text to speech as service to be able to have a blocking call. --- CMakeLists.txt | 2 + src/naoqi_driver.cpp | 4 ++ src/services/animated_speech.cpp | 38 ++++++++++++ src/services/animated_speech.hpp | 100 +++++++++++++++++++++++++++++++ src/services/text_to_speech.cpp | 38 ++++++++++++ src/services/text_to_speech.hpp | 100 +++++++++++++++++++++++++++++++ 6 files changed, 282 insertions(+) create mode 100644 src/services/animated_speech.cpp create mode 100644 src/services/animated_speech.hpp create mode 100644 src/services/text_to_speech.cpp create mode 100644 src/services/text_to_speech.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c0762a5..0cfe2167 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,6 +55,8 @@ set( src/services/localization.cpp src/services/tracking.cpp src/services/breathing.cpp + src/services/text_to_speech.cpp + src/services/animated_speech.cpp ) set( diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index ca6bb7f1..e5f65b5c 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -77,6 +77,8 @@ #include "services/localization.hpp" #include "services/tracking.hpp" #include "services/breathing.hpp" +#include "services/text_to_speech.hpp" +#include "services/animated_speech.hpp" /* * RECORDERS @@ -985,6 +987,8 @@ void Driver::registerDefaultServices() registerService( boost::make_shared("ALTracker-pointAt", "/naoqi_driver/tracker/point_at", sessionPtr_) ); registerService( boost::make_shared("ALTracker-lookAt", "/naoqi_driver/tracker/look_at", sessionPtr_) ); registerService( boost::make_shared("setBreathEnabled", "/naoqi_driver/motion/set_breath_enabled", sessionPtr_) ); + registerService( boost::make_shared("ALTextToSpeech-say", "/naoqi_driver/tts/say", sessionPtr_) ); + registerService( boost::make_shared("ALAnimatedSpeech-say", "/naoqi_driver/animated_speech/say", sessionPtr_) ); } std::vector Driver::getAvailableConverters() diff --git a/src/services/animated_speech.cpp b/src/services/animated_speech.cpp new file mode 100644 index 00000000..530e7018 --- /dev/null +++ b/src/services/animated_speech.cpp @@ -0,0 +1,38 @@ +/* + * 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 "animated_speech.hpp" +#include "../helpers/driver_helpers.hpp" + +namespace naoqi +{ +namespace service +{ + +void AnimatedSpeechSayService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &AnimatedSpeechSayService::callback, this); +} + +bool AnimatedSpeechSayService::callback(nao_interaction_msgs::SayRequest& req, nao_interaction_msgs::SayResponse& resp) +{ + p_tts_.call(func_, req.text); + return true; +} + +} +} diff --git a/src/services/animated_speech.hpp b/src/services/animated_speech.hpp new file mode 100644 index 00000000..b7ae340b --- /dev/null +++ b/src/services/animated_speech.hpp @@ -0,0 +1,100 @@ +/* + * 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 ANIMATED_SPEECH_SERVICE_HPP +#define ANIMATED_SPEECH_SERVICE_HPP + +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace naoqi +{ +namespace service +{ + +class AnimatedSpeechService +{ +public: + AnimatedSpeechService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ) : + name_(name), + topic_(topic), + session_(session), + p_tts_(session->service("ALAnimatedSpeech")) + { + func_ = split(name_, '-').back(); + } + + ~AnimatedSpeechService(){} + + std::string name() const + { + return name_; + } + + std::string topic() const + { + return topic_; + } + + virtual void reset( ros::NodeHandle& nh )=0; + +protected: + const std::string name_; + std::string func_; + const std::string topic_; + + const qi::SessionPtr& session_; + qi::AnyObject p_tts_; + ros::ServiceServer service_; + + void split(const std::string &s, char delim, std::vector &elems) { + std::stringstream ss; + ss.str(s); + std::string item; + while (std::getline(ss, item, delim)) { + elems.push_back(item); + } + } + + std::vector split(const std::string &s, char delim) { + std::vector elems; + split(s, delim, elems); + return elems; + } +}; + +class AnimatedSpeechSayService : public AnimatedSpeechService +{ +public: + AnimatedSpeechSayService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : AnimatedSpeechService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::SayRequest& req, nao_interaction_msgs::SayResponse& resp); + +}; + +} // service +} // naoqi +#endif diff --git a/src/services/text_to_speech.cpp b/src/services/text_to_speech.cpp new file mode 100644 index 00000000..352cd61e --- /dev/null +++ b/src/services/text_to_speech.cpp @@ -0,0 +1,38 @@ +/* + * 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 "text_to_speech.hpp" +#include "../helpers/driver_helpers.hpp" + +namespace naoqi +{ +namespace service +{ + +void TextToSpeechSayService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &TextToSpeechSayService::callback, this); +} + +bool TextToSpeechSayService::callback(nao_interaction_msgs::SayRequest& req, nao_interaction_msgs::SayResponse& resp) +{ + p_tts_.call(func_, req.text); + return true; +} + +} +} diff --git a/src/services/text_to_speech.hpp b/src/services/text_to_speech.hpp new file mode 100644 index 00000000..fc2f5ef8 --- /dev/null +++ b/src/services/text_to_speech.hpp @@ -0,0 +1,100 @@ +/* + * 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 TEXT_TO_SPEECH_SERVICE_HPP +#define TEXT_TO_SPEECH_SERVICE_HPP + +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace naoqi +{ +namespace service +{ + +class TextToSpeechService +{ +public: + TextToSpeechService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ) : + name_(name), + topic_(topic), + session_(session), + p_tts_(session->service("ALTextToSpeech")) + { + func_ = split(name_, '-').back(); + } + + ~TextToSpeechService(){} + + std::string name() const + { + return name_; + } + + std::string topic() const + { + return topic_; + } + + virtual void reset( ros::NodeHandle& nh )=0; + +protected: + const std::string name_; + std::string func_; + const std::string topic_; + + const qi::SessionPtr& session_; + qi::AnyObject p_tts_; + ros::ServiceServer service_; + + void split(const std::string &s, char delim, std::vector &elems) { + std::stringstream ss; + ss.str(s); + std::string item; + while (std::getline(ss, item, delim)) { + elems.push_back(item); + } + } + + std::vector split(const std::string &s, char delim) { + std::vector elems; + split(s, delim, elems); + return elems; + } +}; + +class TextToSpeechSayService : public TextToSpeechService +{ +public: + TextToSpeechSayService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : TextToSpeechService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::SayRequest& req, nao_interaction_msgs::SayResponse& resp); + +}; + +} // service +} // naoqi +#endif From d468da2e017a40f7823f82103e023f7e39706803 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Thu, 8 Dec 2016 15:56:01 +0000 Subject: [PATCH 26/44] Renaming breathing to motion control and adding a move_to service that allows to make blocking calls in addition to the fire and forget topic that already exists. Also adding basic robot posture control. --- CMakeLists.txt | 3 +- src/naoqi_driver.cpp | 10 ++- src/services/breathing.cpp | 48 ------------- src/services/breathing.hpp | 80 --------------------- src/services/motion.cpp | 104 ++++++++++++++++++++++++++++ src/services/motion.hpp | 122 +++++++++++++++++++++++++++++++++ src/services/robot_posture.cpp | 56 +++++++++++++++ src/services/robot_posture.hpp | 110 +++++++++++++++++++++++++++++ 8 files changed, 402 insertions(+), 131 deletions(-) delete mode 100644 src/services/breathing.cpp delete mode 100644 src/services/breathing.hpp create mode 100644 src/services/motion.cpp create mode 100644 src/services/motion.hpp create mode 100644 src/services/robot_posture.cpp create mode 100644 src/services/robot_posture.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0cfe2167..62ecb304 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,9 +54,10 @@ set( src/services/behavior_manager.cpp src/services/localization.cpp src/services/tracking.cpp - src/services/breathing.cpp + src/services/motion.cpp src/services/text_to_speech.cpp src/services/animated_speech.cpp + src/services/robot_posture.cpp ) set( diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index e5f65b5c..1e289822 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -76,9 +76,10 @@ #include "services/behavior_manager.hpp" #include "services/localization.hpp" #include "services/tracking.hpp" -#include "services/breathing.hpp" +#include "services/motion.hpp" #include "services/text_to_speech.hpp" #include "services/animated_speech.hpp" +#include "services/robot_posture.hpp" /* * RECORDERS @@ -986,9 +987,14 @@ void Driver::registerDefaultServices() registerService( boost::make_shared("ALTracker-registerTarget", "/naoqi_driver/tracker/register_target", sessionPtr_) ); registerService( boost::make_shared("ALTracker-pointAt", "/naoqi_driver/tracker/point_at", sessionPtr_) ); registerService( boost::make_shared("ALTracker-lookAt", "/naoqi_driver/tracker/look_at", sessionPtr_) ); - registerService( boost::make_shared("setBreathEnabled", "/naoqi_driver/motion/set_breath_enabled", sessionPtr_) ); + registerService( boost::make_shared("ALMotion-setBreathEnabled", "/naoqi_driver/motion/set_breath_enabled", sessionPtr_) ); + registerService( boost::make_shared("ALMotion-wakeUp", "/naoqi_driver/motion/wake_up", sessionPtr_) ); + registerService( boost::make_shared("ALMotion-rest", "/naoqi_driver/motion/rest", sessionPtr_) ); + registerService( boost::make_shared("ALMotion-moveTo", "/naoqi_driver/motion/move_to", sessionPtr_, tf2_buffer_) ); registerService( boost::make_shared("ALTextToSpeech-say", "/naoqi_driver/tts/say", sessionPtr_) ); registerService( boost::make_shared("ALAnimatedSpeech-say", "/naoqi_driver/animated_speech/say", sessionPtr_) ); + registerService( boost::make_shared("ALRobotPosture-stopMove", "/naoqi_driver/robot_posture/stop_all", sessionPtr_) ); + registerService( boost::make_shared("ALRobotPosture-goToPosture", "/naoqi_driver/robot_posture/go_to_posture", sessionPtr_) ); } std::vector Driver::getAvailableConverters() diff --git a/src/services/breathing.cpp b/src/services/breathing.cpp deleted file mode 100644 index bf031588..00000000 --- a/src/services/breathing.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* - * 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 "breathing.hpp" -#include "../helpers/driver_helpers.hpp" - -namespace naoqi -{ -namespace service -{ - -void EnableBreathingService::reset( ros::NodeHandle& nh ) -{ - service_ = nh.advertiseService(topic_, &EnableBreathingService::callback, this); -} - -bool EnableBreathingService::callback(nao_interaction_msgs::SetBreathEnabledRequest& req, nao_interaction_msgs::SetBreathEnabledResponse& resp) -{ - if(req.chain_name.compare(req.HEAD) == 0 - || req.chain_name.compare(req.BODY) == 0 - || req.chain_name.compare(req.ARMS) == 0 - || req.chain_name.compare(req.LEGS) == 0 - || req.chain_name.compare(req.LARM) == 0 - || req.chain_name.compare(req.RARM) == 0) { - p_motion_.call(name_, req.chain_name, req.enable); - } else { - ROS_ERROR_STREAM("Unknown chain_name '" << req.chain_name <<"'"); - return false; - } - return true; -} - -} -} diff --git a/src/services/breathing.hpp b/src/services/breathing.hpp deleted file mode 100644 index cc03f841..00000000 --- a/src/services/breathing.hpp +++ /dev/null @@ -1,80 +0,0 @@ -/* - * 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 BREATHING_SERVICE_HPP -#define BREATHING_SERVICE_HPP - -#include -#include - -#include -#include - -#include -#include - -namespace naoqi -{ -namespace service -{ - -class BreathingService -{ -public: - BreathingService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ): - name_(name), - topic_(topic), - session_(session), - p_motion_(session->service("ALMotion")) - {} - - ~BreathingService(){} - - std::string name() const - { - return name_; - } - - std::string topic() const - { - return topic_; - } - - virtual void reset( ros::NodeHandle& nh )=0; - -protected: - const std::string name_; - const std::string topic_; - - const qi::SessionPtr& session_; - qi::AnyObject p_motion_; - ros::ServiceServer service_; -}; - -class EnableBreathingService : public BreathingService -{ -public: - EnableBreathingService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : BreathingService(name, topic, session) {} - void reset(ros::NodeHandle& nh); - bool callback(nao_interaction_msgs::SetBreathEnabledRequest& req, nao_interaction_msgs::SetBreathEnabledResponse& resp); - -}; - -} // service -} // naoqi -#endif diff --git a/src/services/motion.cpp b/src/services/motion.cpp new file mode 100644 index 00000000..d060e2a4 --- /dev/null +++ b/src/services/motion.cpp @@ -0,0 +1,104 @@ +/* + * 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 "motion.hpp" +#include "../helpers/driver_helpers.hpp" +#include "../helpers/transform_helpers.hpp" + +namespace naoqi +{ +namespace service +{ + +void MotionEmptyService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &MotionEmptyService::callback, this); +} + +bool MotionEmptyService::callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) +{ + p_motion_.call(func_); + return true; +} + +void EnableBreathingService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &EnableBreathingService::callback, this); +} + +bool EnableBreathingService::callback(nao_interaction_msgs::SetBreathEnabledRequest& req, nao_interaction_msgs::SetBreathEnabledResponse& resp) +{ + if(req.chain_name.compare(req.HEAD) == 0 + || req.chain_name.compare(req.BODY) == 0 + || req.chain_name.compare(req.ARMS) == 0 + || req.chain_name.compare(req.LEGS) == 0 + || req.chain_name.compare(req.LARM) == 0 + || req.chain_name.compare(req.RARM) == 0) { + p_motion_.call(func_, req.chain_name, req.enable); + } else { + ROS_ERROR_STREAM("Unknown chain_name '" << req.chain_name <<"'"); + return false; + } + return true; +} + +void MoveToService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &MoveToService::callback, this); +} + +bool MoveToService::callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp) +{ + if ( req.pose.header.frame_id == "base_footprint" ) + { + double yaw = helpers::transform::getYaw(req.pose.pose); + + std::cout << "going to move x: " << req.pose.pose.position.x << " y: " << req.pose.pose.position.y << " z: " << req.pose.pose.position.z << " yaw: " << yaw << std::endl; + p_motion_.call(func_, req.pose.pose.position.x, req.pose.pose.position.y, yaw); + } + else{ + geometry_msgs::PoseStamped pose_msg_bf; + //geometry_msgs::TransformStamped tf_trans; + //tf_listenerPtr_->waitForTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(5) ); + bool canTransform = tf2_buffer_->canTransform("base_footprint", req.pose.header.frame_id, ros::Time(0), ros::Duration(2) ); + if (!canTransform) { + std::cout << "Cannot transform from " << req.pose.header.frame_id << " to base_footprint" << std::endl; + return false; + } + try + { + //tf_listenerPtr_->lookupTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), tf_trans); + //std::cout << "got a transform " << tf_trans.getOrigin().x() << std::endl; + tf2_buffer_->transform( req.pose, pose_msg_bf, "base_footprint", ros::Time(0), req.pose.header.frame_id ); + double yaw = helpers::transform::getYaw(pose_msg_bf.pose); + std::cout << "odom to move x: " << pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y << " z: " << pose_msg_bf.pose.position.z << " yaw: " << yaw << std::endl; + p_motion_.call(func_, pose_msg_bf.pose.position.x, pose_msg_bf.pose.position.y, yaw ); + } catch( const tf2::LookupException& e) + { + std::cout << e.what() << std::endl; + std::cout << "moveto position in frame_id " << req.pose.header.frame_id << "is not supported in any other base frame than basefootprint" << std::endl; + } + catch( const tf2::ExtrapolationException& e) + { + std::cout << "received an error on the time lookup" << std::endl; + } + } + return true; +} + +} +} diff --git a/src/services/motion.hpp b/src/services/motion.hpp new file mode 100644 index 00000000..d8093ea4 --- /dev/null +++ b/src/services/motion.hpp @@ -0,0 +1,122 @@ +/* + * 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 BREATHING_SERVICE_HPP +#define BREATHING_SERVICE_HPP + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace naoqi +{ +namespace service +{ + +class MotionService +{ +public: + MotionService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ): + name_(name), + topic_(topic), + session_(session), + p_motion_(session->service("ALMotion")) + { + func_ = split(name_, '-').back(); + } + + ~MotionService(){} + + std::string name() const + { + return name_; + } + + std::string topic() const + { + return topic_; + } + + virtual void reset( ros::NodeHandle& nh )=0; + +protected: + const std::string name_; + std::string func_; + const std::string topic_; + + const qi::SessionPtr& session_; + qi::AnyObject p_motion_; + ros::ServiceServer service_; + + void split(const std::string &s, char delim, std::vector &elems) { + std::stringstream ss; + ss.str(s); + std::string item; + while (std::getline(ss, item, delim)) { + elems.push_back(item); + } + } + + std::vector split(const std::string &s, char delim) { + std::vector elems; + split(s, delim, elems); + return elems; + } +}; + +class MotionEmptyService : public MotionService +{ +public: + MotionEmptyService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : MotionService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp); + +}; + +class EnableBreathingService : public MotionService +{ +public: + EnableBreathingService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : MotionService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::SetBreathEnabledRequest& req, nao_interaction_msgs::SetBreathEnabledResponse& resp); + +}; + +class MoveToService : public MotionService +{ +public: + MoveToService(const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer) : MotionService(name, topic, session), tf2_buffer_(tf2_buffer) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp); + +private: + boost::shared_ptr tf2_buffer_; + +}; + +} // service +} // naoqi +#endif diff --git a/src/services/robot_posture.cpp b/src/services/robot_posture.cpp new file mode 100644 index 00000000..5ca84a9b --- /dev/null +++ b/src/services/robot_posture.cpp @@ -0,0 +1,56 @@ +/* + * 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 "robot_posture.hpp" +#include "../helpers/driver_helpers.hpp" + +namespace naoqi +{ +namespace service +{ + +void RobotPostureEmptyService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &RobotPostureEmptyService::callback, this); +} + +bool RobotPostureEmptyService::callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) +{ + p_posture_.call(func_); + return true; +} + +// ############## + +void RobotPostureGoToService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &RobotPostureGoToService::callback, this); +} + +bool RobotPostureGoToService::callback(nao_interaction_msgs::GoToPostureRequest& req, nao_interaction_msgs::GoToPostureResponse& resp) +{ + if(req.posture_name.compare(req.STAND_INIT) == 0 || req.posture_name.compare(req.STAND_ZERO) == 0 || req.posture_name.compare(req.CROUCH) == 0) { + p_posture_.call(func_, req.posture_name, req.speed); + } else { + ROS_ERROR_STREAM("Unknown posture '" << req.posture_name <<"'"); + return false; + } + return true; +} + +} +} diff --git a/src/services/robot_posture.hpp b/src/services/robot_posture.hpp new file mode 100644 index 00000000..cb71b61b --- /dev/null +++ b/src/services/robot_posture.hpp @@ -0,0 +1,110 @@ +/* + * 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 ROBOT_POSTURE_SERVICE_HPP +#define ROBOT_POSTURE_SERVICE_HPP + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace naoqi +{ +namespace service +{ + +class RobotPostureService +{ +public: + RobotPostureService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ) : + name_(name), + topic_(topic), + session_(session), + p_posture_(session->service("ALRobotPosture")) + { + func_ = split(name_, '-').back(); + } + + ~RobotPostureService(){} + + std::string name() const + { + return name_; + } + + std::string topic() const + { + return topic_; + } + + virtual void reset( ros::NodeHandle& nh )=0; + +protected: + const std::string name_; + std::string func_; + const std::string topic_; + + const qi::SessionPtr& session_; + qi::AnyObject p_posture_; + ros::ServiceServer service_; + + void split(const std::string &s, char delim, std::vector &elems) { + std::stringstream ss; + ss.str(s); + std::string item; + while (std::getline(ss, item, delim)) { + elems.push_back(item); + } + } + + std::vector split(const std::string &s, char delim) { + std::vector elems; + split(s, delim, elems); + return elems; + } +}; + +class RobotPostureEmptyService : public RobotPostureService +{ +public: + RobotPostureEmptyService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : RobotPostureService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp); + +}; + +class RobotPostureGoToService : public RobotPostureService +{ +public: + RobotPostureGoToService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : RobotPostureService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::GoToPostureRequest& req, nao_interaction_msgs::GoToPostureResponse& resp); + +}; + +} // service +} // naoqi +#endif From 0b2e0b84747bdb44f200e265bbf730b5fb2dd8cc Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Wed, 25 Jan 2017 20:34:39 +0100 Subject: [PATCH 27/44] fixing camera_info_definitions.hpp --- src/converters/camera_info_definitions.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/converters/camera_info_definitions.hpp b/src/converters/camera_info_definitions.hpp index 2f313af0..73a65492 100644 --- a/src/converters/camera_info_definitions.hpp +++ b/src/converters/camera_info_definitions.hpp @@ -174,7 +174,7 @@ inline sensor_msgs::CameraInfo createCameraInfoDEPTHVGA() cam_info_msg.K = boost::array{{ 525, 0, 319.5000000, 0, 525, 239.5000000000000, 0, 0, 1 }}; cam_info_msg.distortion_model = "plumb_bob"; - cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0); + cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0).convert_to_container >(); cam_info_msg.R = boost::array{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }}; @@ -195,7 +195,7 @@ inline sensor_msgs::CameraInfo createCameraInfoDEPTHQVGA() cam_info_msg.K = boost::array{{ 285.6768151355473, 0.0, 161.1691935721059, 0.0, 285.5373311462721, 127.602991640182, 0.0, 0.0, 1.0 }}; cam_info_msg.distortion_model = "plumb_bob"; - cam_info_msg.D = boost::assign::list_of(0.016936081367960803)(-0.06749793149686571)(-0.0003138996585151219)(-0.0006992075603777127)(0); + cam_info_msg.D = boost::assign::list_of(0.016936081367960803)(-0.06749793149686571)(-0.0003138996585151219)(-0.000699207560377712)(0).convert_to_container >(); cam_info_msg.R = boost::array{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }}; @@ -215,7 +215,7 @@ inline sensor_msgs::CameraInfo createCameraInfoDEPTHQQVGA() cam_info_msg.K = boost::array{{ 525/4.0f, 0, 319.5000000/4.0f, 0, 525/4.0f, 239.5000000000000/4.0f, 0, 0, 1 }}; cam_info_msg.distortion_model = "plumb_bob"; - cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0); + cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0).convert_to_container >(); cam_info_msg.R = boost::array{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }}; From a554329e8bc1fcd11fd355b8edabe09b2db2a0ea Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Fri, 3 Feb 2017 18:59:01 +0100 Subject: [PATCH 28/44] adding Navigation service --- CMakeLists.txt | 2 + src/naoqi_driver.cpp | 7 ++- src/services/navigation.cpp | 83 ++++++++++++++++++++++++ src/services/navigation.hpp | 112 +++++++++++++++++++++++++++++++++ src/subscribers/navigateto.cpp | 88 ++++++++++++++++++++++++++ src/subscribers/navigateto.hpp | 56 +++++++++++++++++ 6 files changed, 346 insertions(+), 2 deletions(-) create mode 100644 src/services/navigation.cpp create mode 100644 src/services/navigation.hpp create mode 100644 src/subscribers/navigateto.cpp create mode 100644 src/subscribers/navigateto.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 62ecb304..4f9013a3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,7 @@ set( SUBSCRIBER_SRC src/subscribers/teleop.cpp src/subscribers/moveto.cpp + src/subscribers/navigateto.cpp src/subscribers/speech.cpp src/subscribers/animated_speech.cpp src/subscribers/play_animation.cpp @@ -55,6 +56,7 @@ set( src/services/localization.cpp src/services/tracking.cpp src/services/motion.cpp + src/services/navigation.cpp src/services/text_to_speech.cpp src/services/animated_speech.cpp src/services/robot_posture.cpp diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 1e289822..59bac91f 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -65,6 +65,7 @@ */ #include "subscribers/teleop.hpp" #include "subscribers/moveto.hpp" +#include "subscribers/navigateto.hpp" #include "subscribers/speech.hpp" #include "subscribers/animated_speech.hpp" #include "subscribers/play_animation.hpp" @@ -77,6 +78,7 @@ #include "services/localization.hpp" #include "services/tracking.hpp" #include "services/motion.hpp" +#include "services/navigation.hpp" #include "services/text_to_speech.hpp" #include "services/animated_speech.hpp" #include "services/robot_posture.hpp" @@ -702,7 +704,6 @@ void Driver::registerDefaultConverter() registerConverter( bcc, bcp, bcr ); } - if(robot_ == robot::PEPPER) { /** Depth Camera */ @@ -953,7 +954,8 @@ void Driver::registerDefaultSubscriber() if (!subscribers_.empty()) return; registerSubscriber( boost::make_shared("teleop", "/cmd_vel", "/joint_angles", sessionPtr_) ); - registerSubscriber( boost::make_shared("moveto", "/move_base_simple/goal", sessionPtr_, tf2_buffer_) ); + //registerSubscriber( boost::make_shared("moveto", "/move_base_simple/goal", sessionPtr_, tf2_buffer_) ); + registerSubscriber( boost::make_shared("navigateto", "/move_base_simple/goal", sessionPtr_, tf2_buffer_) ); registerSubscriber( boost::make_shared("speech", "/speech", sessionPtr_) ); registerSubscriber( boost::make_shared("animated_speech", "/animated_speech", sessionPtr_) ); registerSubscriber( boost::make_shared("play_animation", "/play_animation", sessionPtr_) ); @@ -991,6 +993,7 @@ void Driver::registerDefaultServices() registerService( boost::make_shared("ALMotion-wakeUp", "/naoqi_driver/motion/wake_up", sessionPtr_) ); registerService( boost::make_shared("ALMotion-rest", "/naoqi_driver/motion/rest", sessionPtr_) ); registerService( boost::make_shared("ALMotion-moveTo", "/naoqi_driver/motion/move_to", sessionPtr_, tf2_buffer_) ); + registerService( boost::make_shared("ALNavigation-navigateTo", "/naoqi_driver/navigation/navigate_to", sessionPtr_, tf2_buffer_) ); registerService( boost::make_shared("ALTextToSpeech-say", "/naoqi_driver/tts/say", sessionPtr_) ); registerService( boost::make_shared("ALAnimatedSpeech-say", "/naoqi_driver/animated_speech/say", sessionPtr_) ); registerService( boost::make_shared("ALRobotPosture-stopMove", "/naoqi_driver/robot_posture/stop_all", sessionPtr_) ); diff --git a/src/services/navigation.cpp b/src/services/navigation.cpp new file mode 100644 index 00000000..b1852808 --- /dev/null +++ b/src/services/navigation.cpp @@ -0,0 +1,83 @@ +/* + * 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 "navigation.hpp" +#include "../helpers/driver_helpers.hpp" +#include "../helpers/transform_helpers.hpp" + +namespace naoqi +{ +namespace service +{ + +void NavigationEmptyService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &NavigationEmptyService::callback, this); +} + +bool NavigationEmptyService::callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) +{ + p_navigation_.call(func_); + return true; +} + +void NavigateToService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &NavigateToService::callback, this); +} + +bool NavigateToService::callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp) +{ + if ( req.pose.header.frame_id == "base_footprint" ) + { + double yaw = helpers::transform::getYaw(req.pose.pose); + + std::cout << "going to navigate x: " << req.pose.pose.position.x << " y: " << req.pose.pose.position.y << " z: " << req.pose.pose.position.z << " yaw: " << yaw << std::endl; + p_navigation_.call(func_, req.pose.pose.position.x, req.pose.pose.position.y, yaw); + } + else{ + geometry_msgs::PoseStamped pose_msg_bf; + //geometry_msgs::TransformStamped tf_trans; + //tf_listenerPtr_->waitForTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(5) ); + bool canTransform = tf2_buffer_->canTransform("base_footprint", req.pose.header.frame_id, ros::Time(0), ros::Duration(2) ); + if (!canTransform) { + std::cout << "Cannot transform from " << req.pose.header.frame_id << " to base_footprint" << std::endl; + return false; + } + try + { + //tf_listenerPtr_->lookupTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), tf_trans); + //std::cout << "got a transform " << tf_trans.getOrigin().x() << std::endl; + tf2_buffer_->transform( req.pose, pose_msg_bf, "base_footprint", ros::Time(0), req.pose.header.frame_id ); + double yaw = helpers::transform::getYaw(pose_msg_bf.pose); + std::cout << "odom to navigate x: " << pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y << " z: " << pose_msg_bf.pose.position.z << " yaw: " << yaw << std::endl; + p_navigation_.call(func_, pose_msg_bf.pose.position.x, pose_msg_bf.pose.position.y, yaw ); + } catch( const tf2::LookupException& e) + { + std::cout << e.what() << std::endl; + std::cout << "navigateto position in frame_id " << req.pose.header.frame_id << "is not supported in any other base frame than basefootprint" << std::endl; + } + catch( const tf2::ExtrapolationException& e) + { + std::cout << "received an error on the time lookup" << std::endl; + } + } + return true; +} + +} +} diff --git a/src/services/navigation.hpp b/src/services/navigation.hpp new file mode 100644 index 00000000..d04706ba --- /dev/null +++ b/src/services/navigation.hpp @@ -0,0 +1,112 @@ +/* + * 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 NAVIGATION_SERVICE_HPP +#define NAVIGATION_SERVICE_HPP + +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace naoqi +{ +namespace service +{ + +class NavigationService +{ +public: + NavigationService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ): + name_(name), + topic_(topic), + session_(session), + p_navigation_(session->service("ALNavigation")) + { + func_ = split(name_, '-').back(); + } + + ~NavigationService(){} + + std::string name() const + { + return name_; + } + + std::string topic() const + { + return topic_; + } + + virtual void reset( ros::NodeHandle& nh )=0; + +protected: + const std::string name_; + std::string func_; + const std::string topic_; + + const qi::SessionPtr& session_; + qi::AnyObject p_navigation_; + ros::ServiceServer service_; + + void split(const std::string &s, char delim, std::vector &elems) { + std::stringstream ss; + ss.str(s); + std::string item; + while (std::getline(ss, item, delim)) { + elems.push_back(item); + } + } + + std::vector split(const std::string &s, char delim) { + std::vector elems; + split(s, delim, elems); + return elems; + } +}; + +class NavigationEmptyService : public NavigationService +{ +public: + NavigationEmptyService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : NavigationService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp); + +}; + +class NavigateToService : public NavigationService +{ +public: + NavigateToService(const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer) : NavigationService(name, topic, session), tf2_buffer_(tf2_buffer) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp); + +private: + boost::shared_ptr tf2_buffer_; + +}; + +} // service +} // naoqi +#endif diff --git a/src/subscribers/navigateto.cpp b/src/subscribers/navigateto.cpp new file mode 100644 index 00000000..3061e09f --- /dev/null +++ b/src/subscribers/navigateto.cpp @@ -0,0 +1,88 @@ +/* + * 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 "navigateto.hpp" + +/* + * ROS includes + */ +//#include +#include + +#include "../helpers/transform_helpers.hpp" + +namespace naoqi +{ +namespace subscriber +{ + +NavigatetoSubscriber::NavigatetoSubscriber( const std::string& name, const std::string& topic, const qi::SessionPtr& session, + const boost::shared_ptr& tf2_buffer): + BaseSubscriber( name, topic, session ), + p_navigation_( session->service("ALNavigation") ), + tf2_buffer_( tf2_buffer ) +{} + +void NavigatetoSubscriber::reset( ros::NodeHandle& nh ) +{ + sub_navigateto_ = nh.subscribe( topic_, 10, &NavigatetoSubscriber::callback, this ); + is_initialized_ = true; +} + +void NavigatetoSubscriber::callback( const geometry_msgs::PoseStampedConstPtr& pose_msg ) +{ + if ( pose_msg->header.frame_id == "base_footprint" ) + { + double yaw = helpers::transform::getYaw(pose_msg->pose); + + std::cout << "going to navigate x: " << pose_msg->pose.position.x << " y: " << pose_msg->pose.position.y << " z: " << pose_msg->pose.position.z << " yaw: " << yaw << std::endl; + p_navigation_.async("navigateTo", pose_msg->pose.position.x, pose_msg->pose.position.y, yaw); + } + else{ + geometry_msgs::PoseStamped pose_msg_bf; + //geometry_msgs::TransformStamped tf_trans; + //tf_listenerPtr_->waitForTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(5) ); + bool canTransform = tf2_buffer_->canTransform("base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(2) ); + if (!canTransform) { + std::cout << "Cannot transform from " << pose_msg->header.frame_id << " to base_footprint" << std::endl; + return; + } + try + { + //tf_listenerPtr_->lookupTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), tf_trans); + //std::cout << "got a transform " << tf_trans.getOrigin().x() << std::endl; + tf2_buffer_->transform( *pose_msg, pose_msg_bf, "base_footprint", ros::Time(0), pose_msg->header.frame_id ); + double yaw = helpers::transform::getYaw(pose_msg_bf.pose); + std::cout << "odom to navigate x: " << pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y << " z: " << pose_msg_bf.pose.position.z << " yaw: " << yaw << std::endl; + p_navigation_.async("navigateTo", pose_msg_bf.pose.position.x, pose_msg_bf.pose.position.y, yaw ); + } catch( const tf2::LookupException& e) + { + std::cout << e.what() << std::endl; + std::cout << "navigateto position in frame_id " << pose_msg->header.frame_id << "is not supported in any other base frame than basefootprint" << std::endl; + } + catch( const tf2::ExtrapolationException& e) + { + std::cout << "received an error on the time lookup" << std::endl; + } + } +} + +} //publisher +} // naoqi diff --git a/src/subscribers/navigateto.hpp b/src/subscribers/navigateto.hpp new file mode 100644 index 00000000..9a1d70d6 --- /dev/null +++ b/src/subscribers/navigateto.hpp @@ -0,0 +1,56 @@ +/* + * 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 NAVIGATETO_SUBSCRIBER_HPP +#define NAVIGATETO_SUBSCRIBER_HPP + +/* + * LOCAL includes + */ +#include "subscriber_base.hpp" + +/* + * ROS includes + */ +#include +#include +#include + +namespace naoqi +{ +namespace subscriber +{ + +class NavigatetoSubscriber: public BaseSubscriber +{ +public: + NavigatetoSubscriber( const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer ); + ~NavigatetoSubscriber(){} + + void reset( ros::NodeHandle& nh ); + void callback( const geometry_msgs::PoseStampedConstPtr& pose_msg ); + +private: + qi::AnyObject p_navigation_; + ros::Subscriber sub_navigateto_; + boost::shared_ptr tf2_buffer_; +}; // class Teleop + +} // subscriber +}// naoqi +#endif From 9dac1f4a0618df344f22336e7799e02b97588323 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Wed, 15 Feb 2017 19:52:40 +0100 Subject: [PATCH 29/44] adding navigateToInMap service --- src/naoqi_driver.cpp | 1 + src/services/navigation.cpp | 51 +++++++++++++++++++++++++++++++++++++ src/services/navigation.hpp | 12 +++++++++ 3 files changed, 64 insertions(+) diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 59bac91f..54a04e14 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -994,6 +994,7 @@ void Driver::registerDefaultServices() registerService( boost::make_shared("ALMotion-rest", "/naoqi_driver/motion/rest", sessionPtr_) ); registerService( boost::make_shared("ALMotion-moveTo", "/naoqi_driver/motion/move_to", sessionPtr_, tf2_buffer_) ); registerService( boost::make_shared("ALNavigation-navigateTo", "/naoqi_driver/navigation/navigate_to", sessionPtr_, tf2_buffer_) ); + registerService( boost::make_shared("ALNavigation-navigateToInMap", "/naoqi_driver/navigation/navigate_to_in_map", sessionPtr_, tf2_buffer_) ); registerService( boost::make_shared("ALTextToSpeech-say", "/naoqi_driver/tts/say", sessionPtr_) ); registerService( boost::make_shared("ALAnimatedSpeech-say", "/naoqi_driver/animated_speech/say", sessionPtr_) ); registerService( boost::make_shared("ALRobotPosture-stopMove", "/naoqi_driver/robot_posture/stop_all", sessionPtr_) ); diff --git a/src/services/navigation.cpp b/src/services/navigation.cpp index b1852808..7f44af33 100644 --- a/src/services/navigation.cpp +++ b/src/services/navigation.cpp @@ -79,5 +79,56 @@ bool NavigateToService::callback(nao_interaction_msgs::GoToPoseRequest& req, nao return true; } +void NavigateToInMapService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &NavigateToInMapService::callback, this); + pose.reserve(3); + pose.resize(3); +} + +bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp) +{ + if ( req.pose.header.frame_id == "odom" ) + { + double yaw = helpers::transform::getYaw(req.pose.pose); + std::cout << "odom to navigate x: " << req.pose.pose.position.x << " y: " << req.pose.pose.position.y << " z: " << req.pose.pose.position.z << " yaw: " << yaw << std::endl; + pose[0] = req.pose.pose.position.x; + pose[1] = req.pose.pose.position.y; + pose[2] = yaw; + p_navigation_.call(func_, pose); + } + else + { + geometry_msgs::PoseStamped pose_msg_bf; + //geometry_msgs::TransformStamped tf_trans; + //tf_listenerPtr_->waitForTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(5) ); + bool canTransform = tf2_buffer_->canTransform("base_footprint", req.pose.header.frame_id, ros::Time(0), ros::Duration(2) ); + if (!canTransform) { + std::cout << "Cannot transform from " << req.pose.header.frame_id << " to base_footprint" << std::endl; + return false; + } + try + { + tf2_buffer_->transform( req.pose, pose_msg_bf, "base_footprint", ros::Time(0), req.pose.header.frame_id ); + double yaw = helpers::transform::getYaw(pose_msg_bf.pose); + std::cout << "going to navigate x: " << pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y << " z: " << pose_msg_bf.pose.position.z << " yaw: " << yaw << std::endl; + pose[0] = pose_msg_bf.pose.position.x; + pose[1] = pose_msg_bf.pose.position.y; + pose[2] = yaw; + p_navigation_.call(func_, pose); + + } catch( const tf2::LookupException& e) + { + std::cout << e.what() << std::endl; + std::cout << "navigateto position in frame_id " << req.pose.header.frame_id << "is not supported in any other base frame than basefootprint" << std::endl; + } + catch( const tf2::ExtrapolationException& e) + { + std::cout << "received an error on the time lookup" << std::endl; + } + } + return true; +} + } } diff --git a/src/services/navigation.hpp b/src/services/navigation.hpp index d04706ba..8f278c74 100644 --- a/src/services/navigation.hpp +++ b/src/services/navigation.hpp @@ -107,6 +107,18 @@ class NavigateToService : public NavigationService }; +class NavigateToInMapService : public NavigationService +{ +public: + NavigateToInMapService(const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer) : NavigationService(name, topic, session), tf2_buffer_(tf2_buffer) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp); + +private: + boost::shared_ptr tf2_buffer_; + std::vector pose; +}; + } // service } // naoqi #endif From 2de37ecc93723623908907feedcb09cd294d12e8 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Mon, 20 Feb 2017 11:01:12 +0100 Subject: [PATCH 30/44] fixing navigation class init --- src/services/navigation.hpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/services/navigation.hpp b/src/services/navigation.hpp index 8f278c74..04366516 100644 --- a/src/services/navigation.hpp +++ b/src/services/navigation.hpp @@ -110,7 +110,12 @@ class NavigateToService : public NavigationService class NavigateToInMapService : public NavigationService { public: - NavigateToInMapService(const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer) : NavigationService(name, topic, session), tf2_buffer_(tf2_buffer) {} + NavigateToInMapService(const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer) : NavigationService(name, topic, session), tf2_buffer_(tf2_buffer) + { + pose.reserve(3); + pose.resize(3); + } + void reset(ros::NodeHandle& nh); bool callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp); @@ -119,6 +124,18 @@ class NavigateToInMapService : public NavigationService std::vector pose; }; +class FindFreeZoneService : public NavigationService +{ +public: + FindFreeZoneService(const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer) : NavigationService(name, topic, session), tf2_buffer_(tf2_buffer) {} + + void reset(ros::NodeHandle& nh); + bool callback(const float& radius, const float& constraint); + +private: + boost::shared_ptr tf2_buffer_; +}; + } // service } // naoqi #endif From 24ef1d0bd832a90f4af8d9d20cf9956f3a6b8e0c Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Mon, 20 Feb 2017 11:04:52 +0100 Subject: [PATCH 31/44] fixing navigation class --- src/services/navigation.cpp | 4 +--- src/services/navigation.hpp | 14 +------------- 2 files changed, 2 insertions(+), 16 deletions(-) diff --git a/src/services/navigation.cpp b/src/services/navigation.cpp index 7f44af33..da34a0fa 100644 --- a/src/services/navigation.cpp +++ b/src/services/navigation.cpp @@ -1,5 +1,5 @@ /* - * Copyright 2015 Aldebaran + * Copyright 2017 SoftBank Robotics Europe * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -82,8 +82,6 @@ bool NavigateToService::callback(nao_interaction_msgs::GoToPoseRequest& req, nao void NavigateToInMapService::reset( ros::NodeHandle& nh ) { service_ = nh.advertiseService(topic_, &NavigateToInMapService::callback, this); - pose.reserve(3); - pose.resize(3); } bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp) diff --git a/src/services/navigation.hpp b/src/services/navigation.hpp index 04366516..f196dc2f 100644 --- a/src/services/navigation.hpp +++ b/src/services/navigation.hpp @@ -1,5 +1,5 @@ /* - * Copyright 2015 Aldebaran + * Copyright 2017 SoftBank Robotics Europe * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -124,18 +124,6 @@ class NavigateToInMapService : public NavigationService std::vector pose; }; -class FindFreeZoneService : public NavigationService -{ -public: - FindFreeZoneService(const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer) : NavigationService(name, topic, session), tf2_buffer_(tf2_buffer) {} - - void reset(ros::NodeHandle& nh); - bool callback(const float& radius, const float& constraint); - -private: - boost::shared_ptr tf2_buffer_; -}; - } // service } // naoqi #endif From 932516d145dd3e62cbd33651ab08209f461eb96d Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Thu, 9 Mar 2017 16:09:02 +0100 Subject: [PATCH 32/44] adding explorationService --- src/naoqi_driver.cpp | 1 + src/services/navigation.cpp | 17 +++++++++++++++++ src/services/navigation.hpp | 9 +++++++++ 3 files changed, 27 insertions(+) diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 54a04e14..90d9ca44 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -999,6 +999,7 @@ void Driver::registerDefaultServices() registerService( boost::make_shared("ALAnimatedSpeech-say", "/naoqi_driver/animated_speech/say", sessionPtr_) ); registerService( boost::make_shared("ALRobotPosture-stopMove", "/naoqi_driver/robot_posture/stop_all", sessionPtr_) ); registerService( boost::make_shared("ALRobotPosture-goToPosture", "/naoqi_driver/robot_posture/go_to_posture", sessionPtr_) ); + registerService( boost::make_shared("ALNavigation-explore", "/naoqi_driver/navigation/explore", sessionPtr_) ); } std::vector Driver::getAvailableConverters() diff --git a/src/services/navigation.cpp b/src/services/navigation.cpp index da34a0fa..39ad5a05 100644 --- a/src/services/navigation.cpp +++ b/src/services/navigation.cpp @@ -128,5 +128,22 @@ bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req return true; } +void ExploreService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &ExploreService::callback, this); +} + +bool ExploreService::callback(nao_interaction_msgs::ExploreRequest& req, nao_interaction_msgs::ExploreResponse& resp) //int radius +{ + bool res(false); + int error_code = p_navigation_.call(func_, req.radius); + if (error_code != 0) + ROS_ERROR("Exploration failed."); + else + res = true; + + return res; +} + } } diff --git a/src/services/navigation.hpp b/src/services/navigation.hpp index f196dc2f..6e685fe7 100644 --- a/src/services/navigation.hpp +++ b/src/services/navigation.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -124,6 +125,14 @@ class NavigateToInMapService : public NavigationService std::vector pose; }; +class ExploreService : public NavigationService +{ +public: + ExploreService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : NavigationService(name, topic, session) {} + void reset(ros::NodeHandle& nh); + bool callback(nao_interaction_msgs::ExploreRequest& req, nao_interaction_msgs::ExploreResponse& resp); +}; + } // service } // naoqi #endif From 0ca5a10dfadeb6e5bbe32ef92fa55ea1d7025042 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Thu, 9 Mar 2017 16:33:36 +0100 Subject: [PATCH 33/44] adding saving map to Explore service --- src/services/navigation.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/services/navigation.cpp b/src/services/navigation.cpp index 39ad5a05..42e5b4d2 100644 --- a/src/services/navigation.cpp +++ b/src/services/navigation.cpp @@ -135,6 +135,7 @@ void ExploreService::reset( ros::NodeHandle& nh ) bool ExploreService::callback(nao_interaction_msgs::ExploreRequest& req, nao_interaction_msgs::ExploreResponse& resp) //int radius { + //explore bool res(false); int error_code = p_navigation_.call(func_, req.radius); if (error_code != 0) @@ -142,6 +143,15 @@ bool ExploreService::callback(nao_interaction_msgs::ExploreRequest& req, nao_int else res = true; + //stop exploration if it did not stop yet + p_navigation_.call("stopExploration"); + + //save exploration + if (res) + resp.path_to_map = p_navigation_.call("saveExploration"); + else + resp.path_to_map = ""; + return res; } From de2a64f6e41faa9f4125c6f4d434d626979bce35 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Fri, 10 Mar 2017 09:38:12 +0100 Subject: [PATCH 34/44] fixing NavigateToInMapService --- src/services/navigation.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/src/services/navigation.cpp b/src/services/navigation.cpp index 42e5b4d2..c7567489 100644 --- a/src/services/navigation.cpp +++ b/src/services/navigation.cpp @@ -89,7 +89,10 @@ bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req if ( req.pose.header.frame_id == "odom" ) { double yaw = helpers::transform::getYaw(req.pose.pose); - std::cout << "odom to navigate x: " << req.pose.pose.position.x << " y: " << req.pose.pose.position.y << " z: " << req.pose.pose.position.z << " yaw: " << yaw << std::endl; + std::cout << "odom to navigate x: " << req.pose.pose.position.x + << " y: " << req.pose.pose.position.y + << " z: " << req.pose.pose.position.z + << " yaw: " << yaw << std::endl; pose[0] = req.pose.pose.position.x; pose[1] = req.pose.pose.position.y; pose[2] = yaw; @@ -98,18 +101,19 @@ bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req else { geometry_msgs::PoseStamped pose_msg_bf; - //geometry_msgs::TransformStamped tf_trans; - //tf_listenerPtr_->waitForTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(5) ); - bool canTransform = tf2_buffer_->canTransform("base_footprint", req.pose.header.frame_id, ros::Time(0), ros::Duration(2) ); + bool canTransform = tf2_buffer_->canTransform("odom", req.pose.header.frame_id, ros::Time(0), ros::Duration(2) ); if (!canTransform) { - std::cout << "Cannot transform from " << req.pose.header.frame_id << " to base_footprint" << std::endl; + std::cout << "Cannot transform from " << req.pose.header.frame_id << " to odom" << std::endl; return false; } try { - tf2_buffer_->transform( req.pose, pose_msg_bf, "base_footprint", ros::Time(0), req.pose.header.frame_id ); + tf2_buffer_->transform( req.pose, pose_msg_bf, "odom", ros::Time(0), req.pose.header.frame_id ); double yaw = helpers::transform::getYaw(pose_msg_bf.pose); - std::cout << "going to navigate x: " << pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y << " z: " << pose_msg_bf.pose.position.z << " yaw: " << yaw << std::endl; + std::cout << "going to navigate x: " << pose_msg_bf.pose.position.x + << " y: " << pose_msg_bf.pose.position.y + << " z: " << pose_msg_bf.pose.position.z + << " yaw: " << yaw << std::endl; pose[0] = pose_msg_bf.pose.position.x; pose[1] = pose_msg_bf.pose.position.y; pose[2] = yaw; @@ -118,7 +122,8 @@ bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req } catch( const tf2::LookupException& e) { std::cout << e.what() << std::endl; - std::cout << "navigateto position in frame_id " << req.pose.header.frame_id << "is not supported in any other base frame than basefootprint" << std::endl; + std::cout << "navigateto position in frame_id " << req.pose.header.frame_id + << "is not supported in any other base frame than odom" << std::endl; } catch( const tf2::ExtrapolationException& e) { @@ -133,7 +138,7 @@ void ExploreService::reset( ros::NodeHandle& nh ) service_ = nh.advertiseService(topic_, &ExploreService::callback, this); } -bool ExploreService::callback(nao_interaction_msgs::ExploreRequest& req, nao_interaction_msgs::ExploreResponse& resp) //int radius +bool ExploreService::callback(nao_interaction_msgs::ExploreRequest& req, nao_interaction_msgs::ExploreResponse& resp) { //explore bool res(false); From 821760ba99d03abb050712e164b1f7c020a72e58 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Fri, 10 Mar 2017 18:01:21 +0100 Subject: [PATCH 35/44] fixing NavigateToInMapService --- src/services/navigation.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/services/navigation.cpp b/src/services/navigation.cpp index c7567489..edb70c8d 100644 --- a/src/services/navigation.cpp +++ b/src/services/navigation.cpp @@ -84,12 +84,13 @@ void NavigateToInMapService::reset( ros::NodeHandle& nh ) service_ = nh.advertiseService(topic_, &NavigateToInMapService::callback, this); } -bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp) +bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req, + nao_interaction_msgs::GoToPoseResponse& resp) { - if ( req.pose.header.frame_id == "odom" ) + if ( req.pose.header.frame_id == "map" ) { double yaw = helpers::transform::getYaw(req.pose.pose); - std::cout << "odom to navigate x: " << req.pose.pose.position.x + std::cout << "map to navigate x: " << req.pose.pose.position.x << " y: " << req.pose.pose.position.y << " z: " << req.pose.pose.position.z << " yaw: " << yaw << std::endl; @@ -101,14 +102,14 @@ bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req else { geometry_msgs::PoseStamped pose_msg_bf; - bool canTransform = tf2_buffer_->canTransform("odom", req.pose.header.frame_id, ros::Time(0), ros::Duration(2) ); + bool canTransform = tf2_buffer_->canTransform("map", req.pose.header.frame_id, ros::Time(0), ros::Duration(2) ); if (!canTransform) { - std::cout << "Cannot transform from " << req.pose.header.frame_id << " to odom" << std::endl; + std::cout << "Cannot transform from " << req.pose.header.frame_id << " to map" << std::endl; return false; } try { - tf2_buffer_->transform( req.pose, pose_msg_bf, "odom", ros::Time(0), req.pose.header.frame_id ); + tf2_buffer_->transform( req.pose, pose_msg_bf, "map", ros::Time(0), req.pose.header.frame_id ); double yaw = helpers::transform::getYaw(pose_msg_bf.pose); std::cout << "going to navigate x: " << pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y @@ -123,7 +124,7 @@ bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req { std::cout << e.what() << std::endl; std::cout << "navigateto position in frame_id " << req.pose.header.frame_id - << "is not supported in any other base frame than odom" << std::endl; + << "is not supported in any other base frame than map" << std::endl; } catch( const tf2::ExtrapolationException& e) { From 092f775d12db2c29348e7aba307ac0260616ef47 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Fri, 10 Mar 2017 18:05:48 +0100 Subject: [PATCH 36/44] updating ExploreService --- src/services/navigation.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/services/navigation.cpp b/src/services/navigation.cpp index edb70c8d..2b245ad9 100644 --- a/src/services/navigation.cpp +++ b/src/services/navigation.cpp @@ -144,8 +144,16 @@ bool ExploreService::callback(nao_interaction_msgs::ExploreRequest& req, nao_int //explore bool res(false); int error_code = p_navigation_.call(func_, req.radius); + + //stop exploration if it did not stop yet + p_navigation_.call("stopExploration"); + if (error_code != 0) + { ROS_ERROR("Exploration failed."); + resp.path_to_map = ""; + return res; + } else res = true; @@ -153,10 +161,7 @@ bool ExploreService::callback(nao_interaction_msgs::ExploreRequest& req, nao_int p_navigation_.call("stopExploration"); //save exploration - if (res) - resp.path_to_map = p_navigation_.call("saveExploration"); - else - resp.path_to_map = ""; + resp.path_to_map = p_navigation_.call("saveExploration"); return res; } From c4d472d6630f292adcc8180dd7e02474f88b42a3 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Fri, 10 Mar 2017 18:09:03 +0100 Subject: [PATCH 37/44] adding relocalization service --- CMakeLists.txt | 1 + src/naoqi_driver.cpp | 3 +- src/subscribers/relocalizeinmap.cpp | 74 +++++++++++++++++++++++++++++ src/subscribers/relocalizeinmap.hpp | 60 +++++++++++++++++++++++ 4 files changed, 137 insertions(+), 1 deletion(-) create mode 100644 src/subscribers/relocalizeinmap.cpp create mode 100644 src/subscribers/relocalizeinmap.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4f9013a3..196eb6eb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,6 +47,7 @@ set( src/subscribers/speech.cpp src/subscribers/animated_speech.cpp src/subscribers/play_animation.cpp + src/subscribers/relocalizeinmap.cpp ) set( diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 90d9ca44..6b0a62de 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -69,6 +69,7 @@ #include "subscribers/speech.hpp" #include "subscribers/animated_speech.hpp" #include "subscribers/play_animation.hpp" +#include "subscribers/relocalizeinmap.hpp" /* * SERVICES @@ -954,11 +955,11 @@ void Driver::registerDefaultSubscriber() if (!subscribers_.empty()) return; registerSubscriber( boost::make_shared("teleop", "/cmd_vel", "/joint_angles", sessionPtr_) ); - //registerSubscriber( boost::make_shared("moveto", "/move_base_simple/goal", sessionPtr_, tf2_buffer_) ); registerSubscriber( boost::make_shared("navigateto", "/move_base_simple/goal", sessionPtr_, tf2_buffer_) ); registerSubscriber( boost::make_shared("speech", "/speech", sessionPtr_) ); registerSubscriber( boost::make_shared("animated_speech", "/animated_speech", sessionPtr_) ); registerSubscriber( boost::make_shared("play_animation", "/play_animation", sessionPtr_) ); + registerSubscriber( boost::make_shared("relocalizeInMap", "/initialpose", sessionPtr_, tf2_buffer_) ); } void Driver::registerService( service::Service srv ) diff --git a/src/subscribers/relocalizeinmap.cpp b/src/subscribers/relocalizeinmap.cpp new file mode 100644 index 00000000..a9a92580 --- /dev/null +++ b/src/subscribers/relocalizeinmap.cpp @@ -0,0 +1,74 @@ +/* + * 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 "relocalizeinmap.hpp" + +/* + * ROS includes + */ +//#include +#include + +#include "../helpers/transform_helpers.hpp" + +namespace naoqi +{ +namespace subscriber +{ + +RelocalizeSubscriber::RelocalizeSubscriber( const std::string& name, + const std::string& topic, + const qi::SessionPtr& session, + const boost::shared_ptr& tf2_buffer): + BaseSubscriber( name, topic, session ), + p_navigation_( session->service("ALNavigation") ), + tf2_buffer_( tf2_buffer ) +{ + pose.reserve(3); + pose.resize(3); +} + +void RelocalizeSubscriber::reset( ros::NodeHandle& nh ) +{ + sub_relocalize_ = nh.subscribe( topic_, 10, &RelocalizeSubscriber::callback, this ); + is_initialized_ = true; +} + +void RelocalizeSubscriber::callback( const geometry_msgs::PoseWithCovarianceStamped& pose_msg ) +{ + if ( pose_msg.header.frame_id == "map" ) + { + double yaw = helpers::transform::getYaw(pose_msg.pose.pose); + + std::cout << "going to " << name_ + << " x: " << pose_msg.pose.pose.position.x + << " y: " << pose_msg.pose.pose.position.y + << " z: " << pose_msg.pose.pose.position.z + << " yaw: " << yaw << std::endl; + + pose[0] = pose_msg.pose.pose.position.x; + pose[1] = pose_msg.pose.pose.position.y; + pose[2] = yaw; + p_navigation_.async(name_, pose); + } +} + +} //publisher +} // naoqi diff --git a/src/subscribers/relocalizeinmap.hpp b/src/subscribers/relocalizeinmap.hpp new file mode 100644 index 00000000..ec7f4068 --- /dev/null +++ b/src/subscribers/relocalizeinmap.hpp @@ -0,0 +1,60 @@ +/* + * 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 RELOCALIZE_SUBSCRIBER_HPP +#define RELOCALIZE_SUBSCRIBER_HPP + +/* + * LOCAL includes + */ +#include "subscriber_base.hpp" + +/* + * ROS includes + */ +#include +#include +#include + +namespace naoqi +{ +namespace subscriber +{ + +class RelocalizeSubscriber: public BaseSubscriber +{ +public: + RelocalizeSubscriber( const std::string& name, + const std::string& topic, + const qi::SessionPtr& session, + const boost::shared_ptr& tf2_buffer ); + ~RelocalizeSubscriber(){} + + void reset( ros::NodeHandle& nh ); + void callback( const geometry_msgs::PoseWithCovarianceStamped& pose_msg ); + +private: + qi::AnyObject p_navigation_; + ros::Subscriber sub_relocalize_; + boost::shared_ptr tf2_buffer_; + std::vector pose; +}; + +} // subscriber +}// naoqi +#endif From c3bcbf19309b5e300340d2814d11bbc6d034c533 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Mon, 13 Mar 2017 14:59:26 +0100 Subject: [PATCH 38/44] adding navigateToInMap to navigateto subscriber --- src/subscribers/navigateto.cpp | 57 +++++++++++++++++++++++++--------- 1 file changed, 43 insertions(+), 14 deletions(-) diff --git a/src/subscribers/navigateto.cpp b/src/subscribers/navigateto.cpp index 3061e09f..9c9baccc 100644 --- a/src/subscribers/navigateto.cpp +++ b/src/subscribers/navigateto.cpp @@ -52,30 +52,59 @@ void NavigatetoSubscriber::callback( const geometry_msgs::PoseStampedConstPtr& p { double yaw = helpers::transform::getYaw(pose_msg->pose); - std::cout << "going to navigate x: " << pose_msg->pose.position.x << " y: " << pose_msg->pose.position.y << " z: " << pose_msg->pose.position.z << " yaw: " << yaw << std::endl; - p_navigation_.async("navigateTo", pose_msg->pose.position.x, pose_msg->pose.position.y, yaw); + std::cout << "going to navigate x: " << pose_msg->pose.position.x + << " y: " << pose_msg->pose.position.y + << " z: " << pose_msg->pose.position.z + << " yaw: " << yaw << std::endl; + p_navigation_.async("navigateTo", + pose_msg->pose.position.x, + pose_msg->pose.position.y, + yaw); } - else{ + else if (pose_msg->header.frame_id == "map") + { + double yaw = helpers::transform::getYaw(pose_msg->pose); + std::cout << "map to navigate x: " << pose_msg->pose.position.x + << " y: " << pose_msg->pose.position.y + << " z: " << pose_msg->pose.position.z + << " yaw: " << yaw << std::endl; + std::vector pose(3); + pose[0] = pose_msg->pose.position.x; + pose[1] = pose_msg->pose.position.y; + pose[2] = yaw; + p_navigation_.call("navigateToInMap", pose); + } + else + { geometry_msgs::PoseStamped pose_msg_bf; - //geometry_msgs::TransformStamped tf_trans; - //tf_listenerPtr_->waitForTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(5) ); - bool canTransform = tf2_buffer_->canTransform("base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(2) ); - if (!canTransform) { - std::cout << "Cannot transform from " << pose_msg->header.frame_id << " to base_footprint" << std::endl; + bool canTransform = tf2_buffer_->canTransform("base_footprint", + pose_msg->header.frame_id, + ros::Time(0), + ros::Duration(2) ); + if (!canTransform) + { + std::cout << "Cannot transform from " << pose_msg->header.frame_id + << " to base_footprint" << std::endl; return; } try { - //tf_listenerPtr_->lookupTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), tf_trans); - //std::cout << "got a transform " << tf_trans.getOrigin().x() << std::endl; tf2_buffer_->transform( *pose_msg, pose_msg_bf, "base_footprint", ros::Time(0), pose_msg->header.frame_id ); double yaw = helpers::transform::getYaw(pose_msg_bf.pose); - std::cout << "odom to navigate x: " << pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y << " z: " << pose_msg_bf.pose.position.z << " yaw: " << yaw << std::endl; - p_navigation_.async("navigateTo", pose_msg_bf.pose.position.x, pose_msg_bf.pose.position.y, yaw ); - } catch( const tf2::LookupException& e) + std::cout << "odom to navigate x: " << pose_msg_bf.pose.position.x + << " y: " << pose_msg_bf.pose.position.y + << " z: " << pose_msg_bf.pose.position.z + << " yaw: " << yaw << std::endl; + p_navigation_.async("navigateTo", + pose_msg_bf.pose.position.x, + pose_msg_bf.pose.position.y, + yaw); + } + catch( const tf2::LookupException& e) { std::cout << e.what() << std::endl; - std::cout << "navigateto position in frame_id " << pose_msg->header.frame_id << "is not supported in any other base frame than basefootprint" << std::endl; + std::cout << "navigateto position in frame_id " << pose_msg->header.frame_id + << "is not supported in other base frame than basefootprint" << std::endl; } catch( const tf2::ExtrapolationException& e) { From e778b2dcf7710d7c62c7375e1e4a3db4cfa5b94b Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Tue, 4 Apr 2017 16:15:11 +0200 Subject: [PATCH 39/44] Remove duplicated odom parameter in boot_config.json --- share/boot_config.json | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/share/boot_config.json b/share/boot_config.json index 59d3510c..8e67d5f1 100644 --- a/share/boot_config.json +++ b/share/boot_config.json @@ -75,11 +75,6 @@ "enabled" : true, "frequency" : 10 }, - "odom": - { - "enabled" : true, - "frequency" : 15 - }, "audio": { "enabled" : true @@ -102,6 +97,11 @@ { "enabled" : true }, + "odom": + { + "enabled" : true, + "frequency" : 50 + }, "face": { "enabled" : true @@ -109,11 +109,6 @@ "people": { "enabled" : true - }, - "odom": - { - "enabled" : true, - "frequency" : 15 } } } From 66e3397d1121cee3aed6722974b79e4a229735b6 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Tue, 4 Apr 2017 19:32:46 +0200 Subject: [PATCH 40/44] update boot_config.json: remove the bottom camera by default --- share/boot_config.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/share/boot_config.json b/share/boot_config.json index 8e67d5f1..cb508382 100644 --- a/share/boot_config.json +++ b/share/boot_config.json @@ -11,7 +11,7 @@ }, "bottom_camera": { - "enabled" : true, + "enabled" : false, "resolution" : 1, "fps" : 10, "recorder_fps" : 5 From 52518436be77fb2763e9a5116a2bb2a430f18165 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Fri, 7 Apr 2017 10:08:20 +0200 Subject: [PATCH 41/44] fixing navigation in the map frame --- src/subscribers/navigateto.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/subscribers/navigateto.cpp b/src/subscribers/navigateto.cpp index 9c9baccc..eeaae3b7 100644 --- a/src/subscribers/navigateto.cpp +++ b/src/subscribers/navigateto.cpp @@ -72,7 +72,7 @@ void NavigatetoSubscriber::callback( const geometry_msgs::PoseStampedConstPtr& p pose[0] = pose_msg->pose.position.x; pose[1] = pose_msg->pose.position.y; pose[2] = yaw; - p_navigation_.call("navigateToInMap", pose); + p_navigation_.async("navigateToInMap", pose); } else { From 691f49a18cfe2540a20ccb4087b139f924d319fa Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Fri, 7 Apr 2017 13:59:12 +0200 Subject: [PATCH 42/44] cleaning the navigateto subscriber --- src/subscribers/navigateto.cpp | 21 ++++++++++++++------- src/subscribers/navigateto.hpp | 7 +++++-- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/subscribers/navigateto.cpp b/src/subscribers/navigateto.cpp index eeaae3b7..a38b08f0 100644 --- a/src/subscribers/navigateto.cpp +++ b/src/subscribers/navigateto.cpp @@ -23,7 +23,6 @@ /* * ROS includes */ -//#include #include #include "../helpers/transform_helpers.hpp" @@ -33,8 +32,10 @@ namespace naoqi namespace subscriber { -NavigatetoSubscriber::NavigatetoSubscriber( const std::string& name, const std::string& topic, const qi::SessionPtr& session, - const boost::shared_ptr& tf2_buffer): +NavigatetoSubscriber::NavigatetoSubscriber( const std::string& name, + const std::string& topic, + const qi::SessionPtr& session, + const boost::shared_ptr& tf2_buffer): BaseSubscriber( name, topic, session ), p_navigation_( session->service("ALNavigation") ), tf2_buffer_( tf2_buffer ) @@ -76,20 +77,26 @@ void NavigatetoSubscriber::callback( const geometry_msgs::PoseStampedConstPtr& p } else { + std::string frame = "base_footprint"; geometry_msgs::PoseStamped pose_msg_bf; - bool canTransform = tf2_buffer_->canTransform("base_footprint", + bool canTransform = tf2_buffer_->canTransform(frame, pose_msg->header.frame_id, ros::Time(0), ros::Duration(2) ); if (!canTransform) { std::cout << "Cannot transform from " << pose_msg->header.frame_id - << " to base_footprint" << std::endl; + << " to " << frame << std::endl; return; } try { - tf2_buffer_->transform( *pose_msg, pose_msg_bf, "base_footprint", ros::Time(0), pose_msg->header.frame_id ); + tf2_buffer_->transform( *pose_msg, + pose_msg_bf, + frame, + ros::Time(0), + pose_msg->header.frame_id ); + double yaw = helpers::transform::getYaw(pose_msg_bf.pose); std::cout << "odom to navigate x: " << pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y @@ -104,7 +111,7 @@ void NavigatetoSubscriber::callback( const geometry_msgs::PoseStampedConstPtr& p { std::cout << e.what() << std::endl; std::cout << "navigateto position in frame_id " << pose_msg->header.frame_id - << "is not supported in other base frame than basefootprint" << std::endl; + << " is not supported; use the " << frame << " frame" << std::endl; } catch( const tf2::ExtrapolationException& e) { diff --git a/src/subscribers/navigateto.hpp b/src/subscribers/navigateto.hpp index 9a1d70d6..47a8a36a 100644 --- a/src/subscribers/navigateto.hpp +++ b/src/subscribers/navigateto.hpp @@ -39,7 +39,10 @@ namespace subscriber class NavigatetoSubscriber: public BaseSubscriber { public: - NavigatetoSubscriber( const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer ); + NavigatetoSubscriber( const std::string& name, + const std::string& topic, + const qi::SessionPtr& session, + const boost::shared_ptr& tf2_buffer ); ~NavigatetoSubscriber(){} void reset( ros::NodeHandle& nh ); @@ -49,7 +52,7 @@ class NavigatetoSubscriber: public BaseSubscriber qi::AnyObject p_navigation_; ros::Subscriber sub_navigateto_; boost::shared_ptr tf2_buffer_; -}; // class Teleop +}; } // subscriber }// naoqi From 2e788e8b40413f4b0dec830e11e8de74371f8601 Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Fri, 7 Apr 2017 14:05:23 +0200 Subject: [PATCH 43/44] cleaning the relocalizeinmap subscriber --- src/subscribers/relocalizeinmap.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/subscribers/relocalizeinmap.cpp b/src/subscribers/relocalizeinmap.cpp index a9a92580..0354197f 100644 --- a/src/subscribers/relocalizeinmap.cpp +++ b/src/subscribers/relocalizeinmap.cpp @@ -53,6 +53,9 @@ void RelocalizeSubscriber::reset( ros::NodeHandle& nh ) void RelocalizeSubscriber::callback( const geometry_msgs::PoseWithCovarianceStamped& pose_msg ) { + //stop localization + p_navigation_.call("stopLocalization"); + if ( pose_msg.header.frame_id == "map" ) { double yaw = helpers::transform::getYaw(pose_msg.pose.pose); @@ -66,8 +69,16 @@ void RelocalizeSubscriber::callback( const geometry_msgs::PoseWithCovarianceStam pose[0] = pose_msg.pose.pose.position.x; pose[1] = pose_msg.pose.pose.position.y; pose[2] = yaw; - p_navigation_.async(name_, pose); + p_navigation_.call(name_, pose); + } + else + { + std::cout << name_ << " in frame " << pose_msg.header.frame_id + << " is not supported; use the map frame" << std::endl; } + + //start localization + p_navigation_.call("startLocalization"); } } //publisher From c6fa3e02180a2d07a259904a8bade2816f17dd9c Mon Sep 17 00:00:00 2001 From: Natalia Lyubova Date: Fri, 7 Apr 2017 18:07:06 +0200 Subject: [PATCH 44/44] cleaning services, adding services for loading map and relocalization --- src/naoqi_driver.cpp | 2 + src/services/navigation.cpp | 295 ++++++++++++++++++++++++++++-------- src/services/navigation.hpp | 90 +++++++++-- 3 files changed, 312 insertions(+), 75 deletions(-) diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 6b0a62de..1f32de8d 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -1001,6 +1001,8 @@ void Driver::registerDefaultServices() registerService( boost::make_shared("ALRobotPosture-stopMove", "/naoqi_driver/robot_posture/stop_all", sessionPtr_) ); registerService( boost::make_shared("ALRobotPosture-goToPosture", "/naoqi_driver/robot_posture/go_to_posture", sessionPtr_) ); registerService( boost::make_shared("ALNavigation-explore", "/naoqi_driver/navigation/explore", sessionPtr_) ); + registerService( boost::make_shared("ALNavigation-loadExploration", "/naoqi_driver/navigation/load_exploration", sessionPtr_) ); + registerService( boost::make_shared("ALNavigation-relocalizeInMap", "/naoqi_driver/navigation/relocalize_in_map", sessionPtr_, tf2_buffer_) ); } std::vector Driver::getAvailableConverters() diff --git a/src/services/navigation.cpp b/src/services/navigation.cpp index 2b245ad9..761af9c0 100644 --- a/src/services/navigation.cpp +++ b/src/services/navigation.cpp @@ -29,7 +29,8 @@ void NavigationEmptyService::reset( ros::NodeHandle& nh ) service_ = nh.advertiseService(topic_, &NavigationEmptyService::callback, this); } -bool NavigationEmptyService::callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp) +bool NavigationEmptyService::callback(std_srvs::EmptyRequest& req, + std_srvs::EmptyResponse& resp) { p_navigation_.call(func_); return true; @@ -40,91 +41,76 @@ void NavigateToService::reset( ros::NodeHandle& nh ) service_ = nh.advertiseService(topic_, &NavigateToService::callback, this); } -bool NavigateToService::callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp) +bool NavigateToService::callback(nao_interaction_msgs::GoToPoseRequest& req, + nao_interaction_msgs::GoToPoseResponse& resp) { if ( req.pose.header.frame_id == "base_footprint" ) { double yaw = helpers::transform::getYaw(req.pose.pose); - std::cout << "going to navigate x: " << req.pose.pose.position.x << " y: " << req.pose.pose.position.y << " z: " << req.pose.pose.position.z << " yaw: " << yaw << std::endl; - p_navigation_.call(func_, req.pose.pose.position.x, req.pose.pose.position.y, yaw); - } - else{ - geometry_msgs::PoseStamped pose_msg_bf; - //geometry_msgs::TransformStamped tf_trans; - //tf_listenerPtr_->waitForTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), ros::Duration(5) ); - bool canTransform = tf2_buffer_->canTransform("base_footprint", req.pose.header.frame_id, ros::Time(0), ros::Duration(2) ); - if (!canTransform) { - std::cout << "Cannot transform from " << req.pose.header.frame_id << " to base_footprint" << std::endl; - return false; - } - try - { - //tf_listenerPtr_->lookupTransform( "/base_footprint", pose_msg->header.frame_id, ros::Time(0), tf_trans); - //std::cout << "got a transform " << tf_trans.getOrigin().x() << std::endl; - tf2_buffer_->transform( req.pose, pose_msg_bf, "base_footprint", ros::Time(0), req.pose.header.frame_id ); - double yaw = helpers::transform::getYaw(pose_msg_bf.pose); - std::cout << "odom to navigate x: " << pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y << " z: " << pose_msg_bf.pose.position.z << " yaw: " << yaw << std::endl; - p_navigation_.call(func_, pose_msg_bf.pose.position.x, pose_msg_bf.pose.position.y, yaw ); - } catch( const tf2::LookupException& e) - { - std::cout << e.what() << std::endl; - std::cout << "navigateto position in frame_id " << req.pose.header.frame_id << "is not supported in any other base frame than basefootprint" << std::endl; - } - catch( const tf2::ExtrapolationException& e) - { - std::cout << "received an error on the time lookup" << std::endl; - } - } - return true; -} - -void NavigateToInMapService::reset( ros::NodeHandle& nh ) -{ - service_ = nh.advertiseService(topic_, &NavigateToInMapService::callback, this); -} + std::cout << "going to navigate x: " << req.pose.pose.position.x + << " y: " << req.pose.pose.position.y + << " z: " << req.pose.pose.position.z + << " yaw: " << yaw << std::endl; -bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req, - nao_interaction_msgs::GoToPoseResponse& resp) -{ - if ( req.pose.header.frame_id == "map" ) + p_navigation_.async(func_, + req.pose.pose.position.x, + req.pose.pose.position.y, + yaw); + } + else if (req.pose.header.frame_id == "map") { double yaw = helpers::transform::getYaw(req.pose.pose); std::cout << "map to navigate x: " << req.pose.pose.position.x << " y: " << req.pose.pose.position.y << " z: " << req.pose.pose.position.z << " yaw: " << yaw << std::endl; + + std::vector pose(3); pose[0] = req.pose.pose.position.x; pose[1] = req.pose.pose.position.y; pose[2] = yaw; - p_navigation_.call(func_, pose); + p_navigation_.async("navigateToInMap", pose); } else { + std::string frame = "base_footprint"; geometry_msgs::PoseStamped pose_msg_bf; - bool canTransform = tf2_buffer_->canTransform("map", req.pose.header.frame_id, ros::Time(0), ros::Duration(2) ); - if (!canTransform) { - std::cout << "Cannot transform from " << req.pose.header.frame_id << " to map" << std::endl; + bool canTransform = tf2_buffer_->canTransform(frame, + req.pose.header.frame_id, + ros::Time(0), + ros::Duration(2) ); + if (!canTransform) + { + std::cout << "Cannot transform from " << req.pose.header.frame_id + << " to " << frame << std::endl; return false; } try { - tf2_buffer_->transform( req.pose, pose_msg_bf, "map", ros::Time(0), req.pose.header.frame_id ); + tf2_buffer_->transform( req.pose, + pose_msg_bf, + frame, + ros::Time(0), + req.pose.header.frame_id ); + double yaw = helpers::transform::getYaw(pose_msg_bf.pose); - std::cout << "going to navigate x: " << pose_msg_bf.pose.position.x + std::cout << "odom to navigate x: " + << pose_msg_bf.pose.position.x << " y: " << pose_msg_bf.pose.position.y << " z: " << pose_msg_bf.pose.position.z << " yaw: " << yaw << std::endl; - pose[0] = pose_msg_bf.pose.position.x; - pose[1] = pose_msg_bf.pose.position.y; - pose[2] = yaw; - p_navigation_.call(func_, pose); - } catch( const tf2::LookupException& e) + p_navigation_.async(func_, + pose_msg_bf.pose.position.x, + pose_msg_bf.pose.position.y, + yaw); + } + catch( const tf2::LookupException& e) { std::cout << e.what() << std::endl; std::cout << "navigateto position in frame_id " << req.pose.header.frame_id - << "is not supported in any other base frame than map" << std::endl; + << " is not supported; use the " << frame << " frame" << std::endl; } catch( const tf2::ExtrapolationException& e) { @@ -134,28 +120,94 @@ bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req return true; } +void NavigateToInMapService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &NavigateToInMapService::callback, this); +} + +bool NavigateToInMapService::callback(nao_interaction_msgs::GoToPoseRequest& req, + nao_interaction_msgs::GoToPoseResponse& resp) +{ + if ( req.pose.header.frame_id == "map" ) + { + double yaw = helpers::transform::getYaw(req.pose.pose); + std::cout << "map to navigate x: " << req.pose.pose.position.x + << " y: " << req.pose.pose.position.y + << " z: " << req.pose.pose.position.z + << " yaw: " << yaw << std::endl; + + pose[0] = req.pose.pose.position.x; + pose[1] = req.pose.pose.position.y; + pose[2] = yaw; + p_navigation_.async(func_, pose); + } + else + { + std::string frame("map"); + geometry_msgs::PoseStamped pose_msg_bf; + bool canTransform = tf2_buffer_->canTransform(frame, + req.pose.header.frame_id, + ros::Time(0), + ros::Duration(2)); + + if (!canTransform) { + std::cout << "Cannot transform from " << req.pose.header.frame_id + << " to " << frame << " frame" << std::endl; + return false; + } + + try + { + tf2_buffer_->transform( req.pose, pose_msg_bf, + frame, + ros::Time(0), + req.pose.header.frame_id ); + + double yaw = helpers::transform::getYaw(pose_msg_bf.pose); + std::cout << "going to navigate x: " << pose_msg_bf.pose.position.x + << " y: " << pose_msg_bf.pose.position.y + << " z: " << pose_msg_bf.pose.position.z + << " yaw: " << yaw << std::endl; + + pose[0] = pose_msg_bf.pose.position.x; + pose[1] = pose_msg_bf.pose.position.y; + pose[2] = yaw; + p_navigation_.async(func_, pose); + } + catch( const tf2::LookupException& e) + { + std::cout << e.what() << std::endl; + std::cout << "navigateto position in frame_id " << req.pose.header.frame_id + << "is not supported; use the " << frame << " frame" << std::endl; + } + catch( const tf2::ExtrapolationException& e) + { + std::cout << "received an error on the time lookup" << std::endl; + } + } + return true; +} + void ExploreService::reset( ros::NodeHandle& nh ) { service_ = nh.advertiseService(topic_, &ExploreService::callback, this); } -bool ExploreService::callback(nao_interaction_msgs::ExploreRequest& req, nao_interaction_msgs::ExploreResponse& resp) +bool ExploreService::callback(nao_interaction_msgs::ExploreRequest& req, + nao_interaction_msgs::ExploreResponse& resp) { //explore - bool res(false); - int error_code = p_navigation_.call(func_, req.radius); - - //stop exploration if it did not stop yet - p_navigation_.call("stopExploration"); + bool res(true); + resp.path_to_map = ""; + ROS_INFO_STREAM("Starting exploration in " << req.radius << " meters"); + int error_code = p_navigation_.call(func_, req.radius); if (error_code != 0) { - ROS_ERROR("Exploration failed."); - resp.path_to_map = ""; - return res; + ROS_ERROR_STREAM(func_ << " failed."); + return false; } - else - res = true; + ROS_INFO("Finished exploration"); //stop exploration if it did not stop yet p_navigation_.call("stopExploration"); @@ -163,8 +215,119 @@ bool ExploreService::callback(nao_interaction_msgs::ExploreRequest& req, nao_int //save exploration resp.path_to_map = p_navigation_.call("saveExploration"); + //stop localization + p_navigation_.call("stopLocalization"); + + //load exploration + res = p_navigation_.call("loadExploration", resp.path_to_map); + + //relocalize + if (!res) + { + ROS_ERROR("The explored map cannot be loaded."); + return res; + } + + ROS_INFO("Now, you need to move your robot to zero in the map and \ + relocalize with RelocalizeInMapService with (0,0,0)"); + return res; } +void LoadExplorationService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &LoadExplorationService::callback, this); +} + +bool LoadExplorationService::callback(nao_interaction_msgs::LoadExplorationRequest& req, + nao_interaction_msgs::LoadExplorationResponse& resp) +{ + //stop localization + p_navigation_.call("stopLocalization"); + + //load exploration + resp.result = p_navigation_.call(func_, req.path_to_map); + + //relocalize + if (!resp.result) + ROS_ERROR("The explored map cannot be loaded."); + + return resp.result; +} + +void RelocalizeInMapService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &RelocalizeInMapService::callback, this); +} + +bool RelocalizeInMapService::callback(nao_interaction_msgs::RelocalizeInMapRequest& req, + nao_interaction_msgs::RelocalizeInMapResponse& resp) +{ + resp.result = true; + + //stop localization + p_navigation_.call("stopLocalization"); + + if ( req.pose.header.frame_id == "map" ) + { + //relocalize + pose[0] = req.pose.pose.position.x; + pose[1] = req.pose.pose.position.y; + pose[2] = helpers::transform::getYaw(req.pose.pose); + p_navigation_.call(func_, pose); + } + else + { + std::string frame("map"); + geometry_msgs::PoseStamped pose_msg_bf; + bool canTransform = tf2_buffer_->canTransform(frame, + req.pose.header.frame_id, + ros::Time(0), + ros::Duration(2)); + + if (!canTransform) { + std::cout << "Cannot transform from " << req.pose.header.frame_id + << " to " << frame << std::endl; + resp.result = false; + return false; + } + try + { + tf2_buffer_->transform( req.pose, pose_msg_bf, + frame, + ros::Time(0), + req.pose.header.frame_id ); + + double yaw = helpers::transform::getYaw(pose_msg_bf.pose); + std::cout << "going to navigate x: " << pose_msg_bf.pose.position.x + << " y: " << pose_msg_bf.pose.position.y + << " z: " << pose_msg_bf.pose.position.z + << " yaw: " << yaw << std::endl; + + pose[0] = pose_msg_bf.pose.position.x; + pose[1] = pose_msg_bf.pose.position.y; + pose[2] = yaw; + p_navigation_.async(func_, pose); + } + catch( const tf2::LookupException& e) + { + std::cout << e.what() << std::endl; + std::cout << "navigateto position in frame_id " << req.pose.header.frame_id + << "is not supported; use the " << frame << " frame" << std::endl; + resp.result = false; + } + catch( const tf2::ExtrapolationException& e) + { + std::cout << "received an error on the time lookup" << std::endl; + resp.result = false; + } + } + + //start localization + p_navigation_.call("startLocalization"); + + return resp.result; +} + } } diff --git a/src/services/navigation.hpp b/src/services/navigation.hpp index 6e685fe7..0cd712b8 100644 --- a/src/services/navigation.hpp +++ b/src/services/navigation.hpp @@ -28,6 +28,8 @@ #include #include #include +#include +#include #include #include @@ -39,7 +41,9 @@ namespace service class NavigationService { public: - NavigationService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ): + NavigationService(const std::string& name, + const std::string& topic, + const qi::SessionPtr& session): name_(name), topic_(topic), session_(session), @@ -90,18 +94,34 @@ class NavigationService class NavigationEmptyService : public NavigationService { public: - NavigationEmptyService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : NavigationService(name, topic, session) {} + NavigationEmptyService(const std::string& name, + const std::string& topic, + const qi::SessionPtr& session): + NavigationService(name, topic, session) + {} + void reset(ros::NodeHandle& nh); - bool callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp); + + bool callback(std_srvs::EmptyRequest& req, + std_srvs::EmptyResponse& resp); }; class NavigateToService : public NavigationService { public: - NavigateToService(const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer) : NavigationService(name, topic, session), tf2_buffer_(tf2_buffer) {} + NavigateToService(const std::string& name, + const std::string& topic, + const qi::SessionPtr& session, + const boost::shared_ptr& tf2_buffer): + NavigationService(name, topic, session), + tf2_buffer_(tf2_buffer) + {} + void reset(ros::NodeHandle& nh); - bool callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp); + + bool callback(nao_interaction_msgs::GoToPoseRequest& req, + nao_interaction_msgs::GoToPoseResponse& resp); private: boost::shared_ptr tf2_buffer_; @@ -111,14 +131,21 @@ class NavigateToService : public NavigationService class NavigateToInMapService : public NavigationService { public: - NavigateToInMapService(const std::string& name, const std::string& topic, const qi::SessionPtr& session, const boost::shared_ptr& tf2_buffer) : NavigationService(name, topic, session), tf2_buffer_(tf2_buffer) + NavigateToInMapService(const std::string& name, + const std::string& topic, + const qi::SessionPtr& session, + const boost::shared_ptr& tf2_buffer): + NavigationService(name, topic, session), + tf2_buffer_(tf2_buffer) { pose.reserve(3); pose.resize(3); } void reset(ros::NodeHandle& nh); - bool callback(nao_interaction_msgs::GoToPoseRequest& req, nao_interaction_msgs::GoToPoseResponse& resp); + + bool callback(nao_interaction_msgs::GoToPoseRequest& req, + nao_interaction_msgs::GoToPoseResponse& resp); private: boost::shared_ptr tf2_buffer_; @@ -128,9 +155,54 @@ class NavigateToInMapService : public NavigationService class ExploreService : public NavigationService { public: - ExploreService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : NavigationService(name, topic, session) {} + ExploreService(const std::string& name, + const std::string& topic, + const qi::SessionPtr& session): + NavigationService(name, topic, session) + {} + void reset(ros::NodeHandle& nh); - bool callback(nao_interaction_msgs::ExploreRequest& req, nao_interaction_msgs::ExploreResponse& resp); + + bool callback(nao_interaction_msgs::ExploreRequest& req, + nao_interaction_msgs::ExploreResponse& resp); +}; + +class RelocalizeInMapService : public NavigationService +{ +public: + RelocalizeInMapService(const std::string& name, + const std::string& topic, + const qi::SessionPtr& session, + const boost::shared_ptr& tf2_buffer): + NavigationService(name, topic, session), + tf2_buffer_(tf2_buffer) + { + pose.reserve(3); + pose.resize(3); + } + + void reset(ros::NodeHandle& nh); + + bool callback(nao_interaction_msgs::RelocalizeInMapRequest& req, + nao_interaction_msgs::RelocalizeInMapResponse& resp); + +private: + boost::shared_ptr tf2_buffer_; + std::vector pose; +}; + +class LoadExplorationService : public NavigationService +{ +public: + LoadExplorationService(const std::string& name, + const std::string& topic, + const qi::SessionPtr& session): + NavigationService(name, topic, session) {} + + void reset(ros::NodeHandle& nh); + + bool callback(nao_interaction_msgs::LoadExplorationRequest& req, + nao_interaction_msgs::LoadExplorationResponse& resp); }; } // service