diff --git a/src/tst_util.cc b/src/tst_util.cc new file mode 100644 index 0000000000000000000000000000000000000000..bccf31b48dfc44796d628df655c1bb0347c36f3f --- /dev/null +++ b/src/tst_util.cc @@ -0,0 +1,451 @@ +#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; +} + + + diff --git a/src/tst_util.h b/src/tst_util.h new file mode 100644 index 0000000000000000000000000000000000000000..e882006bfa8086c10226365a736a1bae3890dea0 --- /dev/null +++ b/src/tst_util.h @@ -0,0 +1,58 @@ +#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