Adding basic behavior manager functionaility via services

CMakeLists.txt

@@ -52,6 +52,7 @@ set(
 set(
   SERVICES_SRC
   src/services/robot_config.cpp
+  src/services/behavior_manager.cpp
   )
 
 set(

CMakeLists_catkin.txt

@@ -18,6 +18,7 @@ find_package(catkin COMPONENTS
   rosbag_storage
   rosgraph_msgs
   sensor_msgs
+  std_srvs
   tf2_geometry_msgs
   tf2_msgs
   tf2_ros

package.xml

@@ -26,6 +26,7 @@
   <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>

src/naoqi_driver.cpp

@@ -73,6 +73,7 @@
  * SERVICES
  */
 #include "services/robot_config.hpp"
+#include "services/behavior_manager.hpp"
 
 /*
  * RECORDERS
@@ -972,6 +973,10 @@ void Driver::registerService( service::Service srv )
 void Driver::registerDefaultServices()
 {
   registerService( boost::make_shared<service::RobotConfigService>("robot config service", "/naoqi_driver/get_robot_config", sessionPtr_) );
+  registerService( boost::make_shared<service::BehaviorManagerInfoService>("getInstalledBehaviors", "/naoqi_driver/get_installed_behaviors", sessionPtr_) );
+  registerService( boost::make_shared<service::BehaviorManagerInfoService>("getRunningBehaviors", "/naoqi_driver/get_running_behaviors", sessionPtr_) );
+  registerService( boost::make_shared<service::BehaviorManagerControlService>("startBehavior", "/naoqi_driver/start_behaviour", sessionPtr_) );
+  registerService( boost::make_shared<service::BehaviorManagerControlService>("stopBehavior", "/naoqi_driver/stop_behaviour", sessionPtr_) );
 }
 
 std::vector<std::string> Driver::getAvailableConverters()

src/services/behavior_manager.cpp

@@ -0,0 +1,67 @@
+/*
+ * 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 "behavior_manager.hpp"
+#include "../helpers/driver_helpers.hpp"
+
+namespace naoqi
+{
+namespace service
+{
+
+void BehaviorManagerInfoService::reset( ros::NodeHandle& nh )
+{
+  service_ = nh.advertiseService(topic_, &BehaviorManagerInfoService::callback, this);
+}
+
+bool BehaviorManagerInfoService::callback(nao_interaction_msgs::BehaviorManagerInfoRequest& req, nao_interaction_msgs::BehaviorManagerInfoResponse& resp) {
+  resp.behaviors = p_bm_.call<std::vector<std::string> >(name_);
+  return true;
+}
+
+void BehaviorManagerControlService::reset( ros::NodeHandle& nh )
+{
+  service_ = nh.advertiseService(topic_, &BehaviorManagerControlService::callback, this);
+}
+
+bool BehaviorManagerControlService::callback(nao_interaction_msgs::BehaviorManagerControlRequest& req, nao_interaction_msgs::BehaviorManagerControlResponse& resp) {
+  resp.success = true;
+  if(req.name.compare("ALL")==0 && name_.compare("stopBehavior")==0) { // Stop everything if ALL is given as behavior name
+    p_bm_.call<void>("stopAllBehaviors");
+  } else if(p_bm_.call<bool>("isBehaviorInstalled", req.name)) {
+    if(name_.compare("startBehavior") == 0) {
+      if(p_bm_.call<bool>("isBehaviorRunning", req.name)) {
+        return false; // To make clear that it is successfully started but the service call failed because it was already running
+      } else {
+        p_bm_.call<void>(name_, req.name);
+      }
+    } else {
+      if(!p_bm_.call<bool>("isBehaviorRunning", req.name)) {
+        return false;
+      } else {
+        p_bm_.call<void>(name_, req.name);
+      }
+    }
+  } else {
+    resp.success = false;
+    return false;
+  }
+  return true;
+}
+
+}
+}

src/services/behavior_manager.hpp

@@ -0,0 +1,88 @@
+/*
+ * 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 BEHAVIOR_MANAGER_SERVICE_HPP
+#define BEHAVIOR_MANAGER_SERVICE_HPP
+
+#include <iostream>
+
+#include <ros/node_handle.h>
+#include <ros/service_server.h>
+
+#include <nao_interaction_msgs/BehaviorManagerInfo.h>
+#include <nao_interaction_msgs/BehaviorManagerControl.h>
+#include <qi/session.hpp>
+
+namespace naoqi
+{
+namespace service
+{
+
+class BehaviorManagerService
+{
+public:
+  BehaviorManagerService( const std::string& name, const std::string& topic, const qi::SessionPtr& session ) :
+      name_(name),
+      topic_(topic),
+      session_(session),
+      p_bm_(session->service("ALBehaviorManager")) {}
+
+  ~BehaviorManagerService(){};
+
+  std::string name() const
+  {
+    return name_;
+  }
+
+  std::string topic() const
+  {
+    return topic_;
+  }
+
+  virtual void reset( ros::NodeHandle& nh )=0;
+
+protected:
+  const std::string name_;
+  const std::string topic_;
+
+  const qi::SessionPtr& session_;
+  qi::AnyObject p_bm_;
+  ros::ServiceServer service_;
+};
+
+class BehaviorManagerInfoService : public BehaviorManagerService
+{
+public:
+  BehaviorManagerInfoService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : BehaviorManagerService(name, topic, session) {}
+  void reset(ros::NodeHandle& nh);
+  bool callback(nao_interaction_msgs::BehaviorManagerInfoRequest& req, nao_interaction_msgs::BehaviorManagerInfoResponse& resp);
+
+};
+
+class BehaviorManagerControlService : public BehaviorManagerService
+{
+public:
+  BehaviorManagerControlService(const std::string& name, const std::string& topic, const qi::SessionPtr& session) : BehaviorManagerService(name, topic, session) {}
+  void reset(ros::NodeHandle& nh);
+  bool callback(nao_interaction_msgs::BehaviorManagerControlRequest& req, nao_interaction_msgs::BehaviorManagerControlResponse& resp);
+
+};
+
+} // service
+} // naoqi
+#endif