Exposing memory publishers and adding update memory topic

CMakeLists.txt

@@ -41,6 +41,7 @@ set(
   src/subscribers/teleop.cpp
   src/subscribers/moveto.cpp
   src/subscribers/speech.cpp
+  src/subscribers/memory.cpp
   )
 
 set(

share/mem_config.json

@@ -0,0 +1,5 @@
+{
+	"memKeys": [],
+        "topic": "",
+	"frequency": 0
+}

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 )
 {

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
@@ -145,6 +145,7 @@ void Driver::init()
   registerDefaultConverter();
   registerDefaultSubscriber();
   registerDefaultServices();
+  addMemoryConverters(helpers::filesystem::getMemConfigFile());
   startRosLoop();
 }
 
@@ -871,6 +872,7 @@ void Driver::registerDefaultSubscriber()
   registerSubscriber( boost::make_shared<naoqi::subscriber::TeleopSubscriber>("teleop", "/cmd_vel", "/joint_angles", sessionPtr_) );
   registerSubscriber( boost::make_shared<naoqi::subscriber::MovetoSubscriber>("moveto", "/move_base_simple/goal", sessionPtr_, tf2_buffer_) );
   registerSubscriber( boost::make_shared<naoqi::subscriber::SpeechSubscriber>("speech", "/speech", sessionPtr_) );
+  registerSubscriber( boost::make_shared<naoqi::subscriber::MemorySubscriber>("update_memory", "/update_memory", sessionPtr_) );
 }
 
 void Driver::registerService( service::Service srv )
@@ -1156,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 <YourROSCoreIP>" << 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);

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<qi::AnyValue> memData;
+  for(int i = 0; i < msg->strings.size(); i++){
+    std::vector<qi::AnyValue> 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<qi::AnyValue> 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<qi::AnyValue> 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<void>("insertListData", qi::AnyValue::from(memData));
+}
+
+} //publisher
+} // naoqi

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 <naoqi_bridge_msgs/MemoryList.h>
+
+/*
+ * ROS includes
+ */
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+
+namespace naoqi
+{
+namespace subscriber
+{
+
+class MemorySubscriber: public BaseSubscriber<MemorySubscriber>
+{
+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