Moving exec stuff to delegation util.

src/tst_util.cc

+#include "tst_util.h"
+
+#include "ros/ros.h"
+
+#include "lrs_srvs_exec/TSTCreateExecutor.h"
+#include "lrs_srvs_exec/TSTDestroyExecutor.h"
+#include "lrs_srvs_exec/TSTExecutorCheckFromTni.h"
+#include "lrs_srvs_exec/TSTExecutorExpand.h"
+#include "lrs_srvs_exec/TSTExecutorIsDelegationExpandable.h"
+#include "lrs_srvs_exec/TSTExecutorGetConstraints.h"
+#include "lrs_srvs_exec/TSTExecutorGetConstraintsFromTni.h"
+
+#include "lrs_srvs_tst/TSTInitPlanningProcess.h"
+#include "lrs_srvs_tst/TSTAdvancePlanningProcess.h"
+
+#include "nodelet/loader.h"
+#include "nodeletutil.h"
+
+using namespace std;
+
+bool handle_as_seq_wrt_constraints(std::string type) {
+  if (
+      (type == "seq") ||
+      (type == "loop") ||
+      (type == "try") ||
+      (type == "scan-from-above") ||
+      (type == "scan-map-team") ||
+      (type == "handle-scan-spec-queue") ||
+      (type == "scan-inspect") ||
+      (type == "request")
+      ) {
+    return true;
+  } else {
+    return false;
+  }
+}
+
+bool handle_as_seq_wrt_delegation(std::string type) {
+  // Expandable nodes are handled correctly automatically.  This is for nodes like seq, conc, loop
+  if (
+      (type == "seq") ||
+      (type == "loop") ||
+      (type == "try") ||
+      (type == "conc")
+      ) {
+    return true;
+  } else {
+    return false;
+  }
+}
+
+bool load_exec_nodelet_if_needed (std::string type) {
+#if 0  
+  std::string manager = "execmanager";
+  std::string name = type;
+  std::replace (name.begin(), name.end(), '-', '_');
+    
+  ROS_ERROR("load_exec_nodelet_if_needed: %s - %s", name.c_str(), manager.c_str());
+
+  nodelet::Loader nl;
+
+  std::vector<std::string> names = nl.listLoadedNodelets();
+  for (auto n : names) {
+    ROS_ERROR("LOADED NODELET: %s", n.c_str());
+  }
+
+  auto it = std::find(names.begin(), names.end(), name);
+  if (it == names.end ()) {
+    ROS_ERROR("nodelet not found: LOAD IT");
+  } else {
+    ROS_ERROR("nodelet found: ALREADY LOADED");
+  }
+#endif
+  return true;
+}
+
+bool create_executor (std::string type, int tstid, bool prepare_flag) {
+  bool res = true;
+
+  ros::NodeHandle n;
+  ostringstream os;
+  replace (type.begin(), type.end(), '-', '_');  
+  os << "tst_executor/" << type << "/create_executor";
+  ros::ServiceClient client = n.serviceClient<lrs_srvs_exec::TSTCreateExecutor>(os.str());
+  load_nodelet_for_service(client, type);
+  
+  if (!client.exists()) {
+    ROS_ERROR("create_executor: service call does not exist, loading of nodelets failed or something else went wrong.");
+    return false;
+  }
+
+  
+  lrs_srvs_exec::TSTCreateExecutor srv;
+  srv.request.id = tstid;
+  srv.request.run_prepare = prepare_flag;
+  if (client.call(srv)) {
+    res = srv.response.success;
+    if (!res) {
+      ROS_ERROR("Failed to create executor: %s %d - %d", type.c_str(), tstid, srv.response.error);
+    }
+  } else {
+    ROS_ERROR("tstutil: Failed to create executor: %s %d", type.c_str(), tstid);
+    res = false;
+  }
+
+  return res;
+}
+
+bool destroy_executor (std::string type, int tstid) {
+  bool res = true;
+
+  ros::NodeHandle n;
+  ostringstream os;
+  replace (type.begin(), type.end(), '-', '_');  
+  os << "tst_executor/" << type << "/destroy_executor";
+  ros::ServiceClient client = n.serviceClient<lrs_srvs_exec::TSTDestroyExecutor>(os.str());
+  load_nodelet_for_service(client, type);
+  
+  if (!client.exists()) {
+    ROS_ERROR("create_executor: service call does not exist, loading of nodelets failed or something else went wrong.");
+    return false;
+  }
+  
+  lrs_srvs_exec::TSTDestroyExecutor srv;
+  srv.request.id = tstid;
+  if (client.call(srv)) {
+    res = srv.response.success;
+    if (!res) {
+      ROS_ERROR("Failed to destroy executor: %s %d - %d", type.c_str(), tstid, srv.response.error);
+    }
+  } else {
+    ROS_ERROR("tstutil: Failed to destroy executor: %s - %d", type.c_str(), tstid);
+    res = false;
+  }
+  return res;
+}
+
+
+bool executor_check (lrs_msgs_tst::TSTNodeInfo tni, std::vector<std::string> & path_uuids) {
+ bool res = false;
+
+  ros::NodeHandle n;
+  ostringstream os;
+  std::string type = tni.type;
+  replace (type.begin(), type.end(), '-', '_');
+  os << "tst_executor/" << type << "/executor_check";
+  
+  ros::ServiceClient client = n.serviceClient<lrs_srvs_exec::TSTExecutorCheckFromTni>(os.str());
+  load_nodelet_for_service(client, type);
+  
+  if (!client.exists()) {
+    ROS_ERROR("create_executor: service call does not exist, loading of nodelets failed or something else went wrong.");
+    return false;
+  }
+  
+  lrs_srvs_exec::TSTExecutorCheckFromTni srv;
+  srv.request.tni = tni;
+  if (client.call(srv)) {
+    if (srv.response.success) {
+      path_uuids = srv.response.path_uuids;
+      res = true;
+    } else {
+      ROS_ERROR("tstutil: Failed check in executor: %s - %d - %s", 
+                tni.type.c_str(), srv.response.error, srv.response.reason.c_str());
+      res = false;
+    }
+  } else {
+    ROS_ERROR("tstutil.cc executor_check: Failed to call executor: %s", tni.type.c_str());
+    res = false;
+  }
+  return res;
+}
+
+#if 0
+bool executor_check (std::string tsttype, int tstid) {
+ bool res = false;
+
+  ros::NodeHandle n;
+  ostringstream os;
+  std::string type = tsttype;
+  replace (type.begin(), type.end(), '-', '_');
+  os << "tst_executor/" << type << "/executor_check";
+  ros::ServiceClient client = n.serviceClient<lrs_srvs_exec::TSTExecutorCheck>(os.str());
+  lrs_srvs_exec::TSTExecutorCheck srv;
+  srv.request.id = tstid;
+  if (client.call(srv)) {
+    if (srv.response.success) {
+      res = true;
+    } else {
+      ROS_INFO("tstutil: Failed check in executor: %d - %d", tstid, srv.response.error);
+      res = false;
+    }
+  } else {
+    ROS_ERROR("tstutil.cc executor_check: Failed to call executor: %d", tstid);
+    res = false;
+  }
+  return res;
+}
+#endif
+
+#if 0
+bool executor_lock_resources_check (std::string execns, std::string tstns, int tstid) {
+  bool res = false;
+
+  ros::NodeHandle n;
+  ostringstream os;
+  os << execns << "/tst_executor/" << "executor_lock_resources_check";
+  ros::ServiceClient client = n.serviceClient<lrs_srvs_exec::TSTExecutorCheck>(os.str());
+  lrs_srvs_exec::TSTExecutorCheck srv;
+  srv.request.ns = tstns;
+  srv.request.id = tstid;
+  if (client.call(srv)) {
+    if (srv.response.success) {
+      res = true;
+    } else {
+      ROS_ERROR("Failed check lock resources in executor: %s %s %d - %d", 
+                execns.c_str(), tstns.c_str(), tstid,
+                srv.response.error);
+      res = false;
+    }
+  } else {
+    ROS_ERROR("tstutil.cc executor_lock_resources_check: Failed to call executor: %s %s %d", 
+              execns.c_str(), tstns.c_str(), tstid);
+    res = false;
+  }
+  return res;
+}
+#endif
+
+bool executor_is_delegation_expandable (std::string type) {
+  bool res = false;
+
+  ros::NodeHandle n;
+  ostringstream os;
+  replace (type.begin(), type.end(), '-', '_');  
+  os << "tst_executor/" << type << "/executor_is_delegation_expandable";
+  ros::ServiceClient client = n.serviceClient<lrs_srvs_exec::TSTExecutorIsDelegationExpandable>(os.str());
+  load_nodelet_for_service(client, type);
+  
+  if (!client.exists()) {
+    ROS_ERROR("create_executor: service call does not exist, loading of nodelets failed or something else went wrong.");
+    return false;
+  }
+  
+  lrs_srvs_exec::TSTExecutorIsDelegationExpandable srv;
+  if (client.call(srv)) {
+    if (srv.response.success) {
+      res = srv.response.delegation_expandable_flag;
+    }
+    if (!srv.response.success) {
+      ROS_ERROR("Failed to get is_delegation_expandable in executor: %s - %d", 
+                type.c_str(), srv.response.error);
+      res = false;
+    }
+  } else {
+    ROS_ERROR("tstutil.cc executor_is_delegation_expandable: Failed to call executor: %s", type.c_str());
+    res = false;
+  }
+  return res;
+}
+
+
+int executor_expand (std::string nodetype, int tstid, int free_id,
+                     std::vector<std::string> possible_units, 
+                     int expansion_try, int & expansion_can_be_tried, std::string & fail_reason) {
+  int res = 0;
+
+  ros::NodeHandle n;
+  ostringstream os;
+  replace (nodetype.begin(), nodetype.end(), '-', '_');  
+  os << "tst_executor/" << nodetype << "/executor_expand";
+  ros::ServiceClient client = n.serviceClient<lrs_srvs_exec::TSTExecutorExpand>(os.str());
+
+  load_nodelet_for_service(client, nodetype);
+  
+  if (!client.exists()) {
+    ROS_ERROR("create_executor: service call does not exist, loading of nodelets failed or something else went wrong.");
+    fail_reason = "executor_expand: create_executor: service call does not exist, loading of nodelets failed or something else went wrong.";
+    return -1;
+  }
+  
+  
+  lrs_srvs_exec::TSTExecutorExpand srv;
+  srv.request.id = tstid;
+  srv.request.free_id = free_id;
+  srv.request.possible_units = possible_units;
+  srv.request.expansion_try = expansion_try;
+  if (client.call(srv)) {
+    if (srv.response.success) {
+      res = srv.response.free_id;
+      expansion_can_be_tried = srv.response.expansion_can_be_tried;
+    }
+    if (!srv.response.success) {
+      ROS_ERROR("Failed to expand in executor: %s %d - %d", nodetype.c_str(), tstid, srv.response.error);
+      fail_reason = "executor_expand: Failed to expand in executor";
+      expansion_can_be_tried = -1;
+      res = 0;
+    }
+  } else {
+    ROS_ERROR("Failed to expand in executor: %s %d", nodetype.c_str(), tstid);
+    fail_reason = "executor_expand: Failed to call expand in executor";    
+    expansion_can_be_tried = -2;
+    res = 0;
+  }
+  return res;
+}
+
+#if 0
+bool executor_get_constraints (std::string execns, std::string tstns, 
+                               int tstid, std::string & csp_json) {
+  bool res = true;
+
+  ros::NodeHandle n;
+  ostringstream os;
+  os << execns + "/tst_executor/" << "executor_get_constraints";
+  ros::ServiceClient client = n.serviceClient<lrs_srvs_exec::TSTExecutorGetConstraints>(os.str());
+  lrs_srvs_exec::TSTExecutorGetConstraints srv;
+  srv.request.ns = tstns;
+  srv.request.id = tstid;
+  if (client.call(srv)) {
+    res = srv.response.success;
+    if (res) {
+      csp_json = srv.response.csp_json;
+    } else {
+      ROS_ERROR("Failed to get executor constraints: %s %s %d - %d", 
+                execns.c_str(), tstns.c_str(), tstid, srv.response.error);
+    }
+  } else {
+    ROS_ERROR("Failed to get executor constraints: %s %s %d", 
+              os.str().c_str(), tstns.c_str(), tstid);
+    res = false;
+  }
+  return res;
+}
+#endif
+
+bool executor_get_constraints (std::string execns, lrs_msgs_tst::TSTNodeInfo tni, 
+                               std::string & csp_json) {
+  bool res = true;
+
+  ros::NodeHandle n;
+  ostringstream os;
+  std::string type = tni.type;
+  replace (type.begin(), type.end(), '-', '_');
+  os << execns + "/tst_executor/" << type << "/executor_get_constraints";
+  ros::ServiceClient client = n.serviceClient<lrs_srvs_exec::TSTExecutorGetConstraintsFromTni>(os.str());
+  load_nodelet_for_service(client, type);
+
+  if (!client.exists()) {
+    ROS_ERROR("create_executor: service call does not exist, loading of nodelets failed or something else went wrong.");
+    return false;
+  }
+  
+  lrs_srvs_exec::TSTExecutorGetConstraintsFromTni srv;
+  srv.request.tni = tni;
+  if (client.call(srv)) {
+    res = srv.response.success;
+    if (res) {
+      csp_json = srv.response.csp_json;
+    } else {
+      ROS_ERROR("Failed to get executor constraints: %s %s - %d", 
+                execns.c_str(), type.c_str(), srv.response.error);
+    }
+  } else {
+    ROS_ERROR("Failed to get executor constraints: %s %s", 
+              os.str().c_str(), type.c_str());
+    res = false;
+  }
+  return res;
+
+}
+
+int init_planning_process (std::string domain, 
+                           std::string problem, 
+                           std::string ns, int id,
+			   std::vector<std::string> goal_units,
+			   std::vector<std::string> del_units,
+                           int max_iter, int max_cpu_ms, bool use_mp, int & free_id) {
+  int res = -1;
+
+  ros::NodeHandle n;
+  ostringstream os;
+
+  string this_ns = ros::names::clean (ros::this_node::getNamespace());
+
+  os << this_ns << "/tfpop/init_planning_process";
+  ros::ServiceClient client = 
+    n.serviceClient<lrs_srvs_tst::TSTInitPlanningProcess>(os.str());
+  lrs_srvs_tst::TSTInitPlanningProcess srv;
+  srv.request.domain = domain;
+  srv.request.problem = problem;
+  srv.request.ns = ns;
+  srv.request.id = id;
+  srv.request.goal_units = goal_units;
+  srv.request.del_units = del_units;
+  srv.request.max_iter = max_iter;
+  srv.request.max_cpu_ms = max_cpu_ms;
+  srv.request.use_mp = use_mp;
+  srv.request.free_id = free_id;
+
+  if (client.call(srv)) {
+    if (srv.response.success) {
+      res = srv.response.ppid;
+      free_id = srv.response.free_id;
+    } else {
+      ROS_ERROR("Failed to init_planning_process: %s %d - %d", ns.c_str(), id, srv.response.error);
+    }
+  } else {
+    ROS_ERROR("Call to init_planning_process failed: %s", ns.c_str());
+  }
+
+  return res;
+}
+
+bool advance_planning_process (int ppid, bool del_ok, bool & finished, bool & solved, 
+                               bool & delegate, int & free_id) {
+  bool res = true;
+
+  ros::NodeHandle n;
+  ostringstream os;
+  string this_ns = ros::names::clean (ros::this_node::getNamespace());
+
+  os << this_ns << "/tfpop/advance_planning_process";
+  ros::ServiceClient client = 
+    n.serviceClient<lrs_srvs_tst::TSTAdvancePlanningProcess>(os.str());
+  lrs_srvs_tst::TSTAdvancePlanningProcess srv;
+  srv.request.ppid = ppid;
+  srv.request.del_ok = del_ok;
+  srv.request.free_id = free_id;
+
+  if (client.call(srv)) {
+    if (srv.response.success) {
+      finished = srv.response.finished;
+      solved = srv.response.solved;
+      delegate = srv.response.delegate;
+      free_id = srv.response.free_id;
+      res = srv.response.success;
+    } else {
+      ROS_ERROR("Call to advance_planning_process returned success=false: ppid%d - error%d", ppid, srv.response.error);
+      res = false;
+    }
+  } else {
+    ROS_ERROR("Call to advance_planning_process failed: %d", ppid);
+    res = false;
+  }
+
+  return res;
+}
+
+
+

src/tst_util.h

+#ifndef _TST_UTIL_H
+#define _TST_UTIL_H
+
+#include <string>
+
+#include "lrs_msgs_tst/TSTNodeInfo.h"
+
+bool handle_as_seq_wrt_constraints(std::string type);
+bool handle_as_seq_wrt_delegation(std::string type);
+
+//
+// exec
+//
+
+bool load_exec_nodelet_if_needed (std::string type);
+
+bool create_executor (std::string type, int tstid, bool prepare_flag);
+
+bool destroy_executor (std::string type, int tstid);
+
+bool executor_check (lrs_msgs_tst::TSTNodeInfo tni, std::vector<std::string> & path_uuids);
+
+///bool executor_check (std::string tsttype, std::string tstns, int tstid);
+
+//bool executor_lock_resources_check (std::string execns, std::string tstns, int tstid);
+// Check that all resources that will be locked by the executor is unlocked.
+
+bool executor_is_delegation_expandable (std::string type);
+
+int executor_expand (std::string nodetype, int tstid, int free_id,
+                     std::vector<std::string> possible_units,
+                     int expansion_try, int & expansion_can_be_tried, std::string & fail_reason);
+// Return of 0 is failure, otherwise new free_id is returned.
+
+bool executor_get_constraints (std::string execns, std::string tstns, 
+                               int tstid, std::string & csp_json);
+
+bool executor_get_constraints (std::string execns, lrs_msgs_tst::TSTNodeInfo tni, 
+                               std::string & csp_json);
+
+//
+// tfpop
+//
+
+int init_planning_process (std::string domain, std::string problem, 
+                           std::string ns, int id,
+			   std::vector<std::string> goal_units,
+			   std::vector<std::string> del_units,
+                           int max_iter, int max_cpu_ms, bool use_mp,
+                           int & free_id);
+
+bool advance_planning_process (int ppid, bool del_ok, bool & finished, bool & solved, 
+                               bool & delegate, int & free_id);
+
+
+
+
+#endif