From 53702f9dbdb6a99b0e719d07d76650d6304ef71e Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Thu, 15 Dec 2016 13:07:42 +0000 Subject: [PATCH 1/2] Enabling memory converters --- share/mem_config.json | 10 ++++++++++ src/helpers/filesystem_helpers.hpp | 15 +++++++++++++++ src/naoqi_driver.cpp | 15 ++++++++------- 3 files changed, 33 insertions(+), 7 deletions(-) create mode 100644 share/mem_config.json diff --git a/share/mem_config.json b/share/mem_config.json new file mode 100644 index 00000000..bd142bf4 --- /dev/null +++ b/share/mem_config.json @@ -0,0 +1,10 @@ +{ + "memKeys": [ + "Device/SubDeviceList/Battery/Charge/Sensor/Value", + "Device/SubDeviceList/Battery/Charge/Sensor/TotalVoltage", + "Device/SubDeviceList/Platform/ILS/Sensor/Value", + "Device/SubDeviceList/Battery/Charge/Sensor/Charging" + ], + "topic": "battery_test", + "frequency": 10 +} diff --git a/src/helpers/filesystem_helpers.hpp b/src/helpers/filesystem_helpers.hpp index aab19b72..522c1758 100644 --- a/src/helpers/filesystem_helpers.hpp +++ b/src/helpers/filesystem_helpers.hpp @@ -108,6 +108,21 @@ inline std::string& getBootConfigFile() #endif } +/** mem config loader */ +static const std::string mem_config_file_name = "mem_config.json"; +inline std::string& getMemConfigFile() +{ +#ifdef CATKIN_BUILD + static std::string path = ros::package::getPath("naoqi_driver")+"/share/"+mem_config_file_name; + std::cout << "found a catkin prefix " << path << std::endl; + return path; +#else + static std::string path = qi::path::findData( "/", mem_config_file_name ); + std::cout << "found a qibuild path " << path << std::endl; + return path; +#endif +} + /* URDF loader */ inline std::string& getURDF( std::string filename ) { diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 43b805c6..551f2adf 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -145,6 +145,7 @@ void Driver::init() registerDefaultConverter(); registerDefaultSubscriber(); registerDefaultServices(); + addMemoryConverters(helpers::filesystem::getMemConfigFile()); startRosLoop(); } @@ -1157,13 +1158,13 @@ void Driver::parseJsonFile(std::string filepath, boost::property_tree::ptree &pt void Driver::addMemoryConverters(std::string filepath){ // Check if the nodeHandle pointer is already initialized - if(!nhPtr_){ - std::cout << BOLDRED << "The connection with the ROS master does not seem to be initialized." << std::endl - << BOLDYELLOW << "Please run:" << RESETCOLOR << std::endl - << GREEN << "\t$ qicli call ROS-Driver.setMasterURI " << RESETCOLOR << std::endl - << BOLDYELLOW << "before trying to add converters" << RESETCOLOR << std::endl; - return; - } +// if(!nhPtr_){ +// std::cout << BOLDRED << "The connection with the ROS master does not seem to be initialized." << std::endl +// << BOLDYELLOW << "Please run:" << RESETCOLOR << std::endl +// << GREEN << "\t$ qicli call ROS-Driver.setMasterURI " << RESETCOLOR << std::endl +// << BOLDYELLOW << "before trying to add converters" << RESETCOLOR << std::endl; +// return; +// } // Open the file filepath and parse it boost::property_tree::ptree pt; From 345d32b7babb96af36be3d1fa1c39e9ee0cd5ae5 Mon Sep 17 00:00:00 2001 From: Christian Dondrup Date: Fri, 16 Dec 2016 09:48:25 +0000 Subject: [PATCH 2/2] Adding memory subscriber, removing example json content from config file, removing commented code. --- CMakeLists.txt | 1 + share/mem_config.json | 11 ++---- src/naoqi_driver.cpp | 12 ++----- src/subscribers/memory.cpp | 73 ++++++++++++++++++++++++++++++++++++++ src/subscribers/memory.hpp | 61 +++++++++++++++++++++++++++++++ 5 files changed, 140 insertions(+), 18 deletions(-) create mode 100644 src/subscribers/memory.cpp create mode 100644 src/subscribers/memory.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 671ca98e..3c167a79 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,7 @@ set( src/subscribers/teleop.cpp src/subscribers/moveto.cpp src/subscribers/speech.cpp + src/subscribers/memory.cpp ) set( diff --git a/share/mem_config.json b/share/mem_config.json index bd142bf4..c70938ad 100644 --- a/share/mem_config.json +++ b/share/mem_config.json @@ -1,10 +1,5 @@ { - "memKeys": [ - "Device/SubDeviceList/Battery/Charge/Sensor/Value", - "Device/SubDeviceList/Battery/Charge/Sensor/TotalVoltage", - "Device/SubDeviceList/Platform/ILS/Sensor/Value", - "Device/SubDeviceList/Battery/Charge/Sensor/Charging" - ], - "topic": "battery_test", - "frequency": 10 + "memKeys": [], + "topic": "", + "frequency": 0 } diff --git a/src/naoqi_driver.cpp b/src/naoqi_driver.cpp index 551f2adf..be5f6fbb 100644 --- a/src/naoqi_driver.cpp +++ b/src/naoqi_driver.cpp @@ -63,7 +63,7 @@ #include "subscribers/teleop.hpp" #include "subscribers/moveto.hpp" #include "subscribers/speech.hpp" - +#include "subscribers/memory.hpp" /* * SERVICES @@ -872,6 +872,7 @@ void Driver::registerDefaultSubscriber() registerSubscriber( boost::make_shared("teleop", "/cmd_vel", "/joint_angles", sessionPtr_) ); registerSubscriber( boost::make_shared("moveto", "/move_base_simple/goal", sessionPtr_, tf2_buffer_) ); registerSubscriber( boost::make_shared("speech", "/speech", sessionPtr_) ); + registerSubscriber( boost::make_shared("update_memory", "/update_memory", sessionPtr_) ); } void Driver::registerService( service::Service srv ) @@ -1157,15 +1158,6 @@ void Driver::parseJsonFile(std::string filepath, boost::property_tree::ptree &pt } void Driver::addMemoryConverters(std::string filepath){ - // Check if the nodeHandle pointer is already initialized -// if(!nhPtr_){ -// std::cout << BOLDRED << "The connection with the ROS master does not seem to be initialized." << std::endl -// << BOLDYELLOW << "Please run:" << RESETCOLOR << std::endl -// << GREEN << "\t$ qicli call ROS-Driver.setMasterURI " << RESETCOLOR << std::endl -// << BOLDYELLOW << "before trying to add converters" << RESETCOLOR << std::endl; -// return; -// } - // Open the file filepath and parse it boost::property_tree::ptree pt; parseJsonFile(filepath, pt); diff --git a/src/subscribers/memory.cpp b/src/subscribers/memory.cpp new file mode 100644 index 00000000..541f5ff3 --- /dev/null +++ b/src/subscribers/memory.cpp @@ -0,0 +1,73 @@ +/* + * 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 "memory.hpp" + + +namespace naoqi +{ +namespace subscriber +{ + +MemorySubscriber::MemorySubscriber( const std::string& name, const std::string& memory_topic, const qi::SessionPtr& session ): + memory_topic_(memory_topic), + BaseSubscriber( name, memory_topic, session ), + p_memory_( session->service("ALMemory") ) +{} + +void MemorySubscriber::reset( ros::NodeHandle& nh ) +{ + sub_memory_ = nh.subscribe( memory_topic_, 10, &MemorySubscriber::memory_callback, this ); + + is_initialized_ = true; +} + +void MemorySubscriber::memory_callback( const naoqi_bridge_msgs::MemoryListConstPtr &msg ) +{ + std::vector memData; + for(int i = 0; i < msg->strings.size(); i++){ + std::vector insert; + insert.push_back(qi::AnyValue::from(msg->strings[i].memoryKey)); + insert.push_back(qi::AnyValue::from(msg->strings[i].data)); + + memData.push_back(qi::AnyValue::from(insert)); + } + + for(int i = 0; i < msg->ints.size(); i++){ + std::vector insert; + insert.push_back(qi::AnyValue::from(msg->ints[i].memoryKey)); + insert.push_back(qi::AnyValue::from(msg->ints[i].data)); + + memData.push_back(qi::AnyValue::from(insert)); + } + + for(int i = 0; i < msg->floats.size(); i++){ + std::vector insert; + insert.push_back(qi::AnyValue::from(msg->floats[i].memoryKey)); + insert.push_back(qi::AnyValue::from(msg->floats[i].data)); + + memData.push_back(qi::AnyValue::from(insert)); + } + + p_memory_.async("insertListData", qi::AnyValue::from(memData)); +} + +} //publisher +} // naoqi diff --git a/src/subscribers/memory.hpp b/src/subscribers/memory.hpp new file mode 100644 index 00000000..10a8a3d7 --- /dev/null +++ b/src/subscribers/memory.hpp @@ -0,0 +1,61 @@ +/* + * 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 MEMORY_SUBSCRIBER_HPP +#define MEMORY_SUBSCRIBER_HPP + +/* + * LOCAL includes + */ +#include "subscriber_base.hpp" +#include + +/* + * ROS includes + */ +#include +#include + +namespace naoqi +{ +namespace subscriber +{ + +class MemorySubscriber: public BaseSubscriber +{ +public: + MemorySubscriber(const std::string& name, const std::string& memory_topic, const qi::SessionPtr& session ); + ~MemorySubscriber(){} + + void reset( ros::NodeHandle& nh ); + void memory_callback( const naoqi_bridge_msgs::MemoryListConstPtr &msg ); + +private: + + std::string memory_topic_; + + qi::AnyObject p_memory_; + ros::Subscriber sub_memory_; + + + +}; // class Memory + +} // subscriber +}// naoqi +#endif