diff --git a/include/moveit_task_constructor/introspection.h b/include/moveit_task_constructor/introspection.h index 4e6bf896..2191afeb 100644 --- a/include/moveit_task_constructor/introspection.h +++ b/include/moveit_task_constructor/introspection.h @@ -2,8 +2,11 @@ #include #include +#include #include #include +#include +#include namespace moveit { namespace task_constructor { @@ -14,6 +17,10 @@ class Introspection { 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; public: Introspection(const std::string &task_topic = "task", @@ -35,6 +42,14 @@ public: solution_publisher_.publish(msg); } void operator()(const SolutionBase &s) { publishSolution(s); } + + //advertise services + void generateInterfaceService(); + void generateSolutionService(); + bool getInterfaceState(moveit_task_constructor::GetInterfaceState::Request &req, + moveit_task_constructor::GetInterfaceState::Response &res); + bool getSolution(moveit_task_constructor::GetSolution::Request &req, + moveit_task_constructor::GetSolution::Response &res); }; void publishAllPlans(const Task &task, bool wait = true); diff --git a/src/introspection.cpp b/src/introspection.cpp index 41c2e006..32d0c90c 100644 --- a/src/introspection.cpp +++ b/src/introspection.cpp @@ -34,6 +34,37 @@ 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) +{ + const InterfaceState& state = Repository::instance()[req.state_id]; + moveit_msgs::PlanningScene msg; + state.scene()->getPlanningSceneDiffMsg(msg); + res.scene = msg; + return true; +} + +bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &req, + moveit_task_constructor::GetSolution::Response &res) +{ + ::moveit_task_constructor::Solution msg; + const SolutionBase& solution = Repository::instance()[req.solution_id]; + solution.fillMessage(msg); + res.solution = msg; + return true; +} void publishAllPlans(const Task &task, bool wait) { Task::SolutionProcessor processor