Added basic support for ALPeoplePerception

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

CMakeLists_catkin.txt

@@ -19,6 +19,7 @@ find_package(catkin COMPONENTS
   rosgraph_msgs
   sensor_msgs
   tf2_geometry_msgs
+  people_msgs
   tf2_msgs
   tf2_ros
 )

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,

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 <sstream>
+#include <string>
+/*
+* LOCAL includes
+*/
+#include "people.hpp"
+#include "../tools/from_any_value.hpp"
+/*
+* BOOST includes
+*/
+#include <boost/foreach.hpp>
+#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<message_actions::MessageAction>& actions )
+{
+
+  std::vector<int> people_ids;
+
+  msg_.people = std::vector<people_msgs::Person>();
+  // Retreive all id's of people.
+  try {
+    qi::AnyValue anyvalues = p_memory_.call<qi::AnyValue>("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<float> 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<qi::AnyValue>("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<people_msgs::Person>();
+  }
+
+} //converter
+
+} // naoqi

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 <naoqi_driver/message_actions.h>
+
+/*
+* ROS includes
+*/
+#include <people_msgs/People.h>
+#include <people_msgs/Person.h>
+#include <geometry_msgs/Point.h>
+
+namespace naoqi
+{
+namespace converter
+{
+
+class PeopleConverter : public BaseConverter<PeopleConverter>
+{
+
+  typedef boost::function<void(people_msgs::People&)> 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<message_actions::MessageAction>& actions );
+
+  void reset( );
+
+private:
+
+  qi::AnyObject p_memory_;
+
+  std::map<message_actions::MessageAction, Callback_t> callbacks_;
+
+  people_msgs::People msg_;
+
+}; // class
+
+} //publisher
+} // naoqi
+
+#endif

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<publisher::BasicPublisher<people_msgs::People> > pp = boost::make_shared<publisher::BasicPublisher<people_msgs::People> >( "people" );
+    boost::shared_ptr<recorder::BasicRecorder<people_msgs::People> > pr = boost::make_shared<recorder::BasicRecorder<people_msgs::People> >( "people" );
+    boost::shared_ptr<converter::PeopleConverter> pc = boost::make_shared<converter::PeopleConverter>( "people", people_frequency, sessionPtr_ );
+    pc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher<people_msgs::People>::publish, pp, _1) );
+    pc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder<people_msgs::People>::write, pr, _1) );
+    pc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder<people_msgs::People>::bufferize, pr, _1) );
+    registerConverter( pc, pp, pr );
+  }
+
+
   /** Sonar */
   if ( sonar_enabled )
   {

src/tools/from_any_value.cpp

@@ -200,6 +200,25 @@ std::vector<float> fromAnyValueToFloatVector(qi::AnyValue& value, std::vector<fl
   return result;
 }
 
+std::vector<int> fromAnyValueToIntVector(qi::AnyValue& value, std::vector<int>& result){
+  qi::AnyReferenceVector anyrefs = value.asListValuePtr();
+
+  for(int i=0; i<anyrefs.size();i++)
+  {
+    try
+    {
+      result.push_back(anyrefs[i].content().toInt());
+    }
+    catch(std::runtime_error& e)
+    {
+      result.push_back(-1.0);
+      std::cout << e.what() << "=> set to -1" << std::endl;
+    }
+  }
+  return result;
+}
+
+
 std::vector<std::string> fromAnyValueToStringVector(qi::AnyValue& value, std::vector<std::string>& result){
   qi::AnyReferenceVector anyrefs = value.asListValuePtr();
 

src/tools/from_any_value.hpp

@@ -37,6 +37,7 @@ NaoqiImage fromAnyValueToNaoqiImage(qi::AnyValue& value);
 std::vector<std::string> fromAnyValueToStringVector(qi::AnyValue& value, std::vector<std::string>& result);
 
 std::vector<float> fromAnyValueToFloatVector(qi::AnyValue& value, std::vector<float>& result);
+std::vector<int> fromAnyValueToIntVector(qi::AnyValue& value, std::vector<int>& result);
 
 void fromAnyValueToFloatVectorVector(
         qi::AnyValue &value,