Adding people perception and other things

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

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

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 <http://ros-naoqi.github.io/naoqi_driver/>`_ or build the doc:

launch/register_depth.launch

@@ -0,0 +1,29 @@
+<launch>
+<!--  <node pkg="tf" name="depth_to_top_camera_frame_publisher" type="static_transform_publisher" args="0.039 0.044 0.035 0.000 0.000 0.000 CameraTop_optical_frame CameraDepth_optical_frame 1"/>
+  <node pkg="tf" name="depth_to_top_camera_frame_publisher" type="static_transform_publisher" args="-0.05 0.12 0.0 0.0 0.0 -0.07 CameraTop_optical_frame CameraDepth_optical_frame 1"/>
+
+-->
+  <node pkg="tf" name="depth_to_top_camera_frame_publisher" type="static_transform_publisher" args="-0.035 0.03 0.035 0.0 0.0 -0.07 CameraTop_optical_frame CameraDepth_optical_frame 1"/>
+
+  <node pkg="image_proc" type="image_proc" name="rgb_processing" output="screen" ns="/naoqi_driver_node/camera/front" />
+
+  <node pkg="nodelet" type="nodelet" name="register_depth" output="screen"
+        args="standalone depth_image_proc/register" >
+    <remap from="rgb/camera_info" to="/naoqi_driver_node/camera/front/camera_info"/>
+    <remap from="depth/camera_info" to="/naoqi_driver_node/camera/depth/camera_info"/>
+    <remap from="depth/image_rect" to="/naoqi_driver_node/camera/depth/image_raw"/>
+    <remap from="depth_registered/camera_info" to="/naoqi_driver_node/camera/depth_registered/camera_info"/>
+    <remap from="depth_registered/image_rect" to="/naoqi_driver_node/camera/depth_registered/image_rect"/>
+  </node>
+
+  <node pkg="nodelet" type="nodelet" name="cloudify" output="screen"
+        args="standalone depth_image_proc/point_cloud_xyzrgb" >
+    <remap from="rgb/camera_info" to="/naoqi_driver_node/camera/front/camera_info"/>
+    <remap from="rgb/image_rect_color" to="/naoqi_driver_node/camera/front/image_rect_color"/>
+    <remap from="depth_registered/image_rect" to="/naoqi_driver_node/camera/depth_registered/image_rect"/>
+    <remap from="depth_registered/points" to="/naoqi_driver_node/camera/depth_registered/points"/>
+  </node>
+</launch>
+
+
+

package.xml

@@ -18,13 +18,15 @@
   <build_depend>image_transport</build_depend>
   <build_depend>kdl_parser</build_depend>
   <build_depend version_gte="0.0.4">naoqi_bridge_msgs</build_depend>
+  <build_depend>nao_interaction_msgs</build_depend>
   <build_depend>naoqi_libqi</build_depend>
   <build_depend>naoqi_libqicore</build_depend>
   <build_depend>orocos_kdl</build_depend>
   <build_depend>robot_state_publisher</build_depend>
   <build_depend>rosbag_storage</build_depend>
   <build_depend>rosgraph_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
   <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>tf2_msgs</build_depend>
   <build_depend>tf2_ros</build_depend>
@@ -36,6 +38,7 @@
   <run_depend>image_transport</run_depend>
   <run_depend>kdl_parser</run_depend>
   <run_depend version_gte="0.0.4">naoqi_bridge_msgs</run_depend>
+  <run_depend>nao_interaction_msgs</run_depend>
   <run_depend>naoqi_libqi</run_depend>
   <run_depend>naoqi_libqicore</run_depend>
   <run_depend>orocos_kdl</run_depend>

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
     }
   }
 }

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 <boost/foreach.hpp>
+#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<std::string> 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<std::string>::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<message_actions::MessageAction>& actions )
+{
+  if (!is_subscribed_)
+  {
+    is_subscribed_ = true;
+  }
+
+  std::vector<float> values;
+  try {
+      qi::AnyValue anyvalues = p_memory_.call<qi::AnyValue>("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<void>("unsubscribe", "ROS");
+    is_subscribed_ = false;
+  }
+}
+
+} // publisher
+} //naoqi

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 <naoqi_driver/message_actions.h>
+
+/*
+* ROS includes
+*/
+#include <nao_interaction_msgs/BatteryInfo.h>
+
+namespace naoqi
+{
+namespace converter
+{
+
+class BatteryConverter : public BaseConverter<BatteryConverter>
+{
+
+  typedef boost::function<void(nao_interaction_msgs::BatteryInfo&)> 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<message_actions::MessageAction>& actions );
+
+
+private:
+  std::map<message_actions::MessageAction, Callback_t> 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<std::string> keys_;
+  /** Pre-filled messges that are sent */
+  nao_interaction_msgs::BatteryInfo msg_;
+};
+
+} //publisher
+} //naoqi
+
+#endif