From 22721379cf82a1229084a893774c24c12e9228e2 Mon Sep 17 00:00:00 2001 From: TVerloop Date: Tue, 13 Feb 2018 13:45:47 +0100 Subject: [PATCH] Added basic support for ALPeoplePerception --- CMakeLists.txt | 1 + CMakeLists_catkin.txt | 1 + share/boot_config.json | 7 ++- src/converters/people.cpp | 119 +++++++++++++++++++++++++++++++++++ src/converters/people.hpp | 66 +++++++++++++++++++ src/naoqi_driver.cpp | 19 +++++- src/tools/from_any_value.cpp | 19 ++++++ src/tools/from_any_value.hpp | 1 + 8 files changed, 231 insertions(+), 2 deletions(-) create mode 100644 src/converters/people.cpp create mode 100644 src/converters/people.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d6881c52..26dba2fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ set( src/converters/info.cpp src/converters/joint_state.cpp src/converters/laser.cpp + src/converters/people.cpp src/converters/memory_list.cpp src/converters/memory/bool.cpp src/converters/memory/int.cpp diff --git a/CMakeLists_catkin.txt b/CMakeLists_catkin.txt index 7debd6ae..3b039036 100644 --- a/CMakeLists_catkin.txt +++ b/CMakeLists_catkin.txt @@ -19,6 +19,7 @@ find_package(catkin COMPONENTS rosgraph_msgs sensor_msgs tf2_geometry_msgs + people_msgs tf2_msgs tf2_ros ) diff --git a/share/boot_config.json b/share/boot_config.json index fbfbdb46..ba46ac27 100644 --- a/share/boot_config.json +++ b/share/boot_config.json @@ -37,7 +37,7 @@ }, "logs": { - "enabled" : true, + "enabled" : false, "frequency" : 1 }, "diag": @@ -65,6 +65,11 @@ "enabled" : true, "frequency" : 10 }, + "people": + { + "enabled" : true, + "frequency" : 1 + }, "sonar": { "enabled" : true, diff --git a/src/converters/people.cpp b/src/converters/people.cpp new file mode 100644 index 00000000..72b2aa55 --- /dev/null +++ b/src/converters/people.cpp @@ -0,0 +1,119 @@ +/* + * 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 +/* +* LOCAL includes +*/ +#include "people.hpp" +#include "../tools/from_any_value.hpp" +/* +* BOOST includes +*/ +#include +#define for_each BOOST_FOREACH + +namespace naoqi +{ +namespace converter +{ + +static const char* peopleListMemoryKey = "PeoplePerception/PeopleList"; + +static const std::string personPositionMemoryKey[] = { + "PeoplePerception/Person/", + "/PositionInTorsoFrame" +}; + + + +PeopleConverter::PeopleConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ): + BaseConverter( name, frequency, session ), + p_memory_(session->service("ALMemory")) +{ + +} + +void PeopleConverter::registerCallback( message_actions::MessageAction action, Callback_t cb ) +{ + callbacks_[action] = cb; +} + +void PeopleConverter::callAll( const std::vector& actions ) +{ + + std::vector people_ids; + + msg_.people = std::vector(); + // Retreive all id's of people. + try { + qi::AnyValue anyvalues = p_memory_.call("getData", peopleListMemoryKey); + tools::fromAnyValueToIntVector(anyvalues, people_ids); + } catch (const std::exception& e) { + std::cerr << "Exception caught in PersonConverter: " << e.what() << std::endl; + return; + } + // Loop over all ids + for_each(unsigned int id, people_ids) + { + people_msgs::Person person; + //Set person name to id + person.name = std::to_string(id); + + std::vector position; + + try { + //Construct memory key from id and constant parts + std::stringstream ss; + ss << personPositionMemoryKey[0] << id << personPositionMemoryKey[1]; + //Getreive data from ALMemory + qi::AnyValue anyvalues = p_memory_.call("getData", ss.str()); + + tools::fromAnyValueToFloatVector(anyvalues, position); + // Set Person position to position relative to robot. + person.position.x = position[0]; + person.position.y = position[1]; + person.position.z = position[2]; + // Add person to the People message + msg_.people.push_back(person); + + } catch (const std::exception& e) { + std::cerr << "Exception caught in PersonConverter: " << e.what() << std::endl; + return; + } + + } + + // Set header timestamp + msg_.header.stamp = ros::Time::now(); + + for_each( message_actions::MessageAction action, actions ) + { + callbacks_[action](msg_); + } +} + + void PeopleConverter::reset( ) + { + msg_.header.frame_id = "torso"; + msg_.people = std::vector(); + } + +} //converter + +} // naoqi diff --git a/src/converters/people.hpp b/src/converters/people.hpp new file mode 100644 index 00000000..07e756a5 --- /dev/null +++ b/src/converters/people.hpp @@ -0,0 +1,66 @@ +/* + * 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_CONVERTER_HPP +#define PEOPLE_CONVERTER_HPP + +/* +* LOCAL includes +*/ +#include "converter_base.hpp" +#include + +/* +* ROS includes +*/ +#include +#include +#include + +namespace naoqi +{ +namespace converter +{ + +class PeopleConverter : public BaseConverter +{ + + typedef boost::function Callback_t; + +public: + PeopleConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ); + + void registerCallback( message_actions::MessageAction action, Callback_t cb ); + + void callAll( const std::vector& actions ); + + void reset( ); + +private: + + qi::AnyObject p_memory_; + + std::map callbacks_; + + people_msgs::People msg_; + +}; // class + +} //publisher +} // naoqi + +#endif diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index ff98f1d4..b9171a04 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -40,7 +40,7 @@ #include "converters/memory/string.hpp" #include "converters/log.hpp" #include "converters/odom.hpp" - +#include "converters/people.hpp" /* * PUBLISHERS */ @@ -584,6 +584,9 @@ void Driver::registerDefaultConverter() bool laser_enabled = boot_config_.get( "converters.laser.enabled", true); size_t laser_frequency = boot_config_.get( "converters.laser.frequency", 10); + bool people_enabled = boot_config_.get( "converters.people.enabled", true); + size_t people_frequency = boot_config_.get( "converters.people.frequency", 1); + bool sonar_enabled = boot_config_.get( "converters.sonar.enabled", true); size_t sonar_frequency = boot_config_.get( "converters.sonar.frequency", 10); @@ -742,6 +745,20 @@ void Driver::registerDefaultConverter() } } + /** People */ + if ( people_enabled ) + { + + boost::shared_ptr > pp = boost::make_shared >( "people" ); + boost::shared_ptr > pr = boost::make_shared >( "people" ); + boost::shared_ptr pc = boost::make_shared( "people", people_frequency, sessionPtr_ ); + pc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher::publish, pp, _1) ); + pc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder::write, pr, _1) ); + pc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder::bufferize, pr, _1) ); + registerConverter( pc, pp, pr ); + } + + /** Sonar */ if ( sonar_enabled ) { diff --git a/src/tools/from_any_value.cpp b/src/tools/from_any_value.cpp index a0cce4c0..8154982f 100644 --- a/src/tools/from_any_value.cpp +++ b/src/tools/from_any_value.cpp @@ -200,6 +200,25 @@ std::vector fromAnyValueToFloatVector(qi::AnyValue& value, std::vector fromAnyValueToIntVector(qi::AnyValue& value, std::vector& result){ + qi::AnyReferenceVector anyrefs = value.asListValuePtr(); + + for(int i=0; i set to -1" << std::endl; + } + } + return result; +} + + std::vector fromAnyValueToStringVector(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 62e30ce6..1f40b3f2 100644 --- a/src/tools/from_any_value.hpp +++ b/src/tools/from_any_value.hpp @@ -37,6 +37,7 @@ NaoqiImage fromAnyValueToNaoqiImage(qi::AnyValue& value); std::vector fromAnyValueToStringVector(qi::AnyValue& value, std::vector& result); std::vector fromAnyValueToFloatVector(qi::AnyValue& value, std::vector& result); +std::vector fromAnyValueToIntVector(qi::AnyValue& value, std::vector& result); void fromAnyValueToFloatVectorVector( qi::AnyValue &value,