diff --git a/CMakeLists.txt b/CMakeLists.txt index 671ca98e..196eb6eb 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 @@ -19,7 +20,9 @@ set( src/converters/memory/string.cpp src/converters/sonar.cpp src/converters/log.cpp + src/converters/battery.cpp src/converters/odom.cpp + src/converters/sound.cpp ) set( TOOLS_SRC @@ -40,12 +43,24 @@ 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 + src/subscribers/relocalizeinmap.cpp ) set( SERVICES_SRC src/services/robot_config.cpp + src/services/behavior_manager.cpp + 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 ) set( @@ -66,6 +81,8 @@ set( src/event/basic.hpp 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/CMakeLists_catkin.txt b/CMakeLists_catkin.txt index 3f42cc14..9dce6d55 100644 --- a/CMakeLists_catkin.txt +++ b/CMakeLists_catkin.txt @@ -11,12 +11,14 @@ find_package(catkin COMPONENTS image_transport kdl_parser naoqi_bridge_msgs + nao_interaction_msgs naoqi_libqi naoqi_libqicore robot_state_publisher rosbag_storage rosgraph_msgs sensor_msgs + std_srvs tf2_geometry_msgs tf2_msgs tf2_ros 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: 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index aecd7151..053438fb 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 @@ -25,6 +26,7 @@ rosbag_storage rosgraph_msgs sensor_msgs + std_srvs tf2_geometry_msgs tf2_msgs tf2_ros @@ -36,6 +38,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..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 @@ -70,10 +70,21 @@ "enabled" : true, "frequency" : 10 }, + "battery": + { + "enabled" : true, + "frequency" : 10 + }, "audio": { "enabled" : true }, + "sound": + { + "enabled" : true, + "energy" : true, + "sensitivity" : 0.9 + }, "bumper": { "enabled" : true @@ -89,7 +100,15 @@ "odom": { "enabled" : true, - "frequency" : 15 + "frequency" : 50 + }, + "face": + { + "enabled" : true + }, + "people": + { + "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/converters/camera_info_definitions.hpp b/src/converters/camera_info_definitions.hpp index 7d935f3a..73a65492 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).convert_to_container >(); 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.000699207560377712)(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{{ 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).convert_to_container >(); cam_info_msg.R = boost::array{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }}; diff --git a/src/converters/people.cpp b/src/converters/people.cpp new file mode 100644 index 00000000..f5ef665a --- /dev/null +++ b/src/converters/people.cpp @@ -0,0 +1,69 @@ +/* + * Copyright 2015 Aldebaran + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +/* +* LOCAL includes +*/ +#include "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; +template class PeopleEventConverter; +} + +} diff --git a/src/converters/people.hpp b/src/converters/people.hpp new file mode 100644 index 00000000..9f2e04d8 --- /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 +#include +/* +* ALDEBARAN includes +*/ +#include + +namespace naoqi{ + +namespace converter{ + +template +class PeopleEventConverter : public BaseConverter > +{ + + typedef boost::function Callback_t; + +public: + PeopleEventConverter(const std::string& name, const float& frequency, const qi::SessionPtr& session); + + ~PeopleEventConverter(); + + virtual void reset(); + + void registerCallback( const message_actions::MessageAction action, Callback_t cb ); + + void callAll(const std::vector& actions, T& msg); + +private: + /** Registered Callbacks **/ + std::map callbacks_; + T msg_; +}; + +} + +} + +#endif // PEOPLE_CONVERTER_HPP diff --git a/src/converters/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/people.cpp b/src/event/people.cpp new file mode 100644 index 00000000..f0968d76 --- /dev/null +++ b/src/event/people.cpp @@ -0,0 +1,522 @@ +/* + * 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 + +#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")), + 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), + isRecording_(false), + isDumping_(false), + 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"); + 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 ); + + 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"); + } + 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< Waving : Start"<("subscribe", "ROS"); + } + 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){ + 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"); + 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); + } + 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(); + + peopleCallbackMessage(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 PeopleEventRegister::peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::FaceDetectedArray &msg) +{ + tools::NaoqiFaceDetected faces; + try { + faces = tools::fromAnyValueToNaoqiFaceDetected(value); + } + catch(std::runtime_error& e) + { + ROS_DEBUG_STREAM("Cannot retrieve facedetect: " << e.what()); + return; + } + if ( faces.face_info.size() == 0 ) return; + + msg.header.frame_id = "CameraTop_optical_frame"; + msg.header.stamp = ros::Time::now(); + + 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; + + 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.face_array.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, nao_interaction_msgs::PersonDetectedArray &msg) +{ + tools::NaoqiPersonDetected people; + try { + people = tools::fromAnyValueToNaoqiPersonDetected(value); + } + catch(std::runtime_error& e) + { + ROS_DEBUG_STREAM("Cannot retrieve persondetected: " << e.what()); + return; + } + + msg.header.frame_id = "CameraDepth_optical_frame"; + 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); + 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::PersonDetected pd; + pd.face.gender = -1; // Since 0 = female, initialising as -1 to disambigute no data from femal. + + /* People Perception */ + try { + 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() != 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(); + } catch(...) { + ROS_DEBUG("Error retreiving face detected"); + } + + try { + pd.person.is_waving = (bool)data[1].content().asInt32(); + } catch(...) { + ROS_DEBUG("Error retreiving if waving"); + } + + if(pd.person.face_detected) { + /* Gaze Analysis */ + try { + qi::AnyReference gaze = data[2].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()); + 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"); + } + } 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[3].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()); + 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"); + } + } else { + ROS_DEBUG("Could not retrieve head angle"); + } + } catch(std::runtime_error& e) { + ROS_DEBUG_STREAM("Error retrieving head angle: " << e.what()); + } + try { + 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[6].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) { + pd.face.age = (int)age.asFloat(); + pd.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[7].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) { + pd.face.gender = (int)gender.asFloat(); + pd.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[8].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) { + pd.face.smile_degree = smile_degree.asFloat(); + pd.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[9].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) { + 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"); + } + } 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.person_array.push_back(pd); + } +} + +// http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor +template class PeopleEventRegister; +template class PeopleEventRegister; + +}//namespace diff --git a/src/event/people.hpp b/src/event/people.hpp new file mode 100644 index 00000000..df130e14 --- /dev/null +++ b/src/event/people.hpp @@ -0,0 +1,158 @@ +/* + * 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 + +#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, qi::AnyValue &value, nao_interaction_msgs::FaceDetectedArray &msg); + void peopleCallbackMessage(std::string &key, qi::AnyValue &value, nao_interaction_msgs::PersonDetectedArray &msg); + +private: + void registerCallback(); + void unregisterCallback(); + void onEvent(); + geometry_msgs::Point toCartesian(float dist, float azi, float inc); + template + std::string num_to_str(N num) { + std::stringstream ss; + ss << num; + return ss.str(); + } + +private: + boost::shared_ptr > converter_; + boost::shared_ptr > publisher_; + //boost::shared_ptr > recorder_; + + qi::SessionPtr session_; + qi::AnyObject p_memory_, p_people_, p_gaze_, p_face_, p_waving_; + unsigned int serviceId; + std::string name_; + + boost::mutex mutex_; + + bool isStarted_; + bool isPublishing_; + bool isRecording_; + bool isDumping_; + + std::string prefix; + std::vector memory_keys; + +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) {} +}; + +class PersonDetectedEventRegister: public PeopleEventRegister +{ +public: + 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) + 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/event/sound.cpp b/src/event/sound.cpp new file mode 100644 index 00000000..e820cc46 --- /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_DEBUG_STREAM("Could not transform AnyValue into list: " << e.what()); + } + + if(anyref.size() != 4) { + ROS_DEBUG("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_DEBUG("Could not retrieve azi/ele/conf/ener"); + } + } else { + ROS_DEBUG("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 43b805c6..1f32de8d 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" @@ -39,7 +40,9 @@ #include "converters/memory/int.hpp" #include "converters/memory/string.hpp" #include "converters/log.hpp" +#include "converters/battery.hpp" #include "converters/odom.hpp" +#include "converters/sound.hpp" /* * PUBLISHERS @@ -62,13 +65,24 @@ */ #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" +#include "subscribers/relocalizeinmap.hpp" /* * SERVICES */ #include "services/robot_config.hpp" +#include "services/behavior_manager.hpp" +#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" /* * RECORDERS @@ -86,6 +100,8 @@ #include "event/basic.hpp" #include "event/audio.hpp" #include "event/touch.hpp" +#include "event/people.hpp" +#include "event/sound.hpp" /* * STATIC FUNCTIONS INCLUDE @@ -537,6 +553,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); @@ -563,6 +584,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 @@ -578,12 +600,18 @@ 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); + + 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 * callAll method will be triggered (because InfoPublisher is considered to always have subscribers, isSubscribed always @@ -677,7 +705,6 @@ void Driver::registerDefaultConverter() registerConverter( bcc, bcp, bcr ); } - if(robot_ == robot::PEPPER) { /** Depth Camera */ @@ -755,6 +782,19 @@ 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 ); + } + } if ( audio_enabled ) { /** Audio */ @@ -768,6 +808,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 ) @@ -839,6 +896,35 @@ void Driver::registerDefaultConverter() registerConverter( lc, lp, lr ); } + /** PEOPLE **/ + if ( face_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); + } + } + 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); + } + } } @@ -869,8 +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 ) @@ -882,6 +971,38 @@ 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/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("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("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("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_) ); + 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/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/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 diff --git a/src/services/localization.cpp b/src/services/localization.cpp new file mode 100644 index 00000000..46684051 --- /dev/null +++ b/src/services/localization.cpp @@ -0,0 +1,90 @@ +/* + * 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 +{ + +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 LocalizationTriggerStringService::reset( ros::NodeHandle& nh ) +{ + service_ = nh.advertiseService(topic_, &LocalizationTriggerStringService::callback, this); +} + +bool LocalizationTriggerStringService::callback(nao_interaction_msgs::LocalizationTriggerStringRequest& req, nao_interaction_msgs::LocalizationTriggerStringResponse& resp) +{ + 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 new file mode 100644 index 00000000..de783665 --- /dev/null +++ b/src/services/localization.hpp @@ -0,0 +1,140 @@ +/* + * 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 +#include + +#include +#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 ) : + name_(name), + topic_(topic), + session_(session), + p_localization_(session->service("ALLocalization")) + { + func_ = split(name_, '-').back(); + } + + ~LocalizationService(){} + + 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_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 +} // 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/navigation.cpp b/src/services/navigation.cpp new file mode 100644 index 00000000..761af9c0 --- /dev/null +++ b/src/services/navigation.cpp @@ -0,0 +1,333 @@ +/* + * 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. + * 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_.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_.async("navigateToInMap", pose); + } + else + { + std::string frame = "base_footprint"; + 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; + 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 << "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(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; 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 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) +{ + //explore + 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_STREAM(func_ << " failed."); + return false; + } + ROS_INFO("Finished exploration"); + + //stop exploration if it did not stop yet + p_navigation_.call("stopExploration"); + + //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 new file mode 100644 index 00000000..0cd712b8 --- /dev/null +++ b/src/services/navigation.hpp @@ -0,0 +1,210 @@ +/* + * 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. + * 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 +#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_; + +}; + +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) + { + pose.reserve(3); + pose.resize(3); + } + + 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; +}; + +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); +}; + +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 +} // 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 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 diff --git a/src/services/tracking.cpp b/src/services/tracking.cpp new file mode 100644 index 00000000..048ad2bc --- /dev/null +++ b/src/services/tracking.cpp @@ -0,0 +1,126 @@ +/* + * 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(func_, req.target, req.values[0]); + } else if(req.target.compare(req.PEOPLE) == 0 || req.target.compare(req.SOUND) == 0) { + p_tracker_.call(func_, 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(func_, 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(func_, 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(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 new file mode 100644 index 00000000..13087b31 --- /dev/null +++ b/src/services/tracking.hpp @@ -0,0 +1,148 @@ +/* + * 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 +#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")) + { + func_ = split(name_, '-').back(); + } + + ~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_; + 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 +{ +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); + +}; + +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 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 diff --git a/src/subscribers/navigateto.cpp b/src/subscribers/navigateto.cpp new file mode 100644 index 00000000..a38b08f0 --- /dev/null +++ b/src/subscribers/navigateto.cpp @@ -0,0 +1,124 @@ +/* + * 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 "../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 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_.async("navigateToInMap", pose); + } + else + { + std::string frame = "base_footprint"; + geometry_msgs::PoseStamped pose_msg_bf; + 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 " << frame << std::endl; + return; + } + try + { + 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 + << " 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; use the " << frame << " frame" << 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..47a8a36a --- /dev/null +++ b/src/subscribers/navigateto.hpp @@ -0,0 +1,59 @@ +/* + * 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_; +}; + +} // subscriber +}// naoqi +#endif 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 diff --git a/src/subscribers/relocalizeinmap.cpp b/src/subscribers/relocalizeinmap.cpp new file mode 100644 index 00000000..0354197f --- /dev/null +++ b/src/subscribers/relocalizeinmap.cpp @@ -0,0 +1,85 @@ +/* + * 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 ) +{ + //stop localization + p_navigation_.call("stopLocalization"); + + 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_.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 +} // 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 diff --git a/src/tools/from_any_value.cpp b/src/tools/from_any_value.cpp index 960f6e7f..31869055 100644 --- a/src/tools/from_any_value.cpp +++ b/src/tools/from_any_value.cpp @@ -181,6 +181,604 @@ 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 ) { + throw std::runtime_error("AnyValue does not have the expected size to be transformed to face detected"); + } + /** 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 < ref.size(); 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 < ref.size(); 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; +} + +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 ) { + throw std::runtime_error("AnyValue does not have the expected size to be transformed to person detected"); + } + /** 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 < ref.size(); 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 < ref.size(); 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..5b6e0d2c 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,10 @@ namespace tools { 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 new file mode 100644 index 00000000..587483d3 --- /dev/null +++ b/src/tools/naoqi_facedetected.hpp @@ -0,0 +1,118 @@ +/* + * 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 NaoqiPersonInfo{ + int id; + float distance_to_camera; + float pitch_angle_in_image; + float yaw_angle_in_image; +}; + + +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; +}; + +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; +}; + +} + +} + +#endif // NAOQI_FACEDETECTED_HPP