From abe61ef9eb17360c33964bb57d97b00424c03b60 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 27 Oct 2017 22:00:46 +0200 Subject: [PATCH] fixup --- include/moveit_task_constructor/introspection.h | 14 ++++++-------- src/introspection.cpp | 16 ++++------------ 2 files changed, 10 insertions(+), 20 deletions(-) diff --git a/include/moveit_task_constructor/introspection.h b/include/moveit_task_constructor/introspection.h index 2191afeb..c3e50f57 100644 --- a/include/moveit_task_constructor/introspection.h +++ b/include/moveit_task_constructor/introspection.h @@ -14,13 +14,12 @@ MOVEIT_CLASS_FORWARD(Task) MOVEIT_CLASS_FORWARD(SolutionBase) class Introspection { + // publish Task state and (new) Solutions ros::Publisher task_publisher_; ros::Publisher solution_publisher_; - // TODO add services to provide InterfaceState and Solution - ros::ServiceServer interfaceState_service_; - ros::ServiceServer solution_service_; - - ros::NodeHandle n; + // services to provide InterfaceState and Solution + ros::ServiceServer get_interface_state_service_; + ros::ServiceServer get_solution_service_; public: Introspection(const std::string &task_topic = "task", @@ -43,11 +42,10 @@ public: } void operator()(const SolutionBase &s) { publishSolution(s); } - //advertise services - void generateInterfaceService(); - void generateSolutionService(); + // get interface state bool getInterfaceState(moveit_task_constructor::GetInterfaceState::Request &req, moveit_task_constructor::GetInterfaceState::Response &res); + // get solution bool getSolution(moveit_task_constructor::GetSolution::Request &req, moveit_task_constructor::GetSolution::Response &res); }; diff --git a/src/introspection.cpp b/src/introspection.cpp index 32d0c90c..30a9f1b8 100644 --- a/src/introspection.cpp +++ b/src/introspection.cpp @@ -13,6 +13,10 @@ Introspection::Introspection(const std::string &task_topic, ros::NodeHandle n; task_publisher_ = n.advertise(task_topic, 1); solution_publisher_ = n.advertise<::moveit_task_constructor::Solution>(solution_topic, 1, true); + + n = ros::NodeHandle("~"); // services are advertised in private namespace + get_interface_state_service_ = n.advertiseService("get_interface_state", &Introspection::getInterfaceState, this); + get_solution_service_ = n.advertiseService("get_solution", &Introspection::getSolution, this); } Introspection &Introspection::instance() @@ -34,18 +38,6 @@ void Introspection::publishSolution(const SolutionBase &s) publishSolution(msg); } -void Introspection::generateInterfaceService() -{ - Introspection i; - interfaceState_service_ = n.advertiseService("get_interface_state", &Introspection::getInterfaceState, &i); -} - -void Introspection::generateSolutionService() -{ - Introspection i; - solution_service_ = n.advertiseService("get_solution", &Introspection::getSolution, &i); -} - bool Introspection::getInterfaceState(moveit_task_constructor::GetInterfaceState::Request &req, moveit_task_constructor::GetInterfaceState::Response &res) {