introspection service generators are added

This commit is contained in:
eirtech 2017-10-27 14:45:47 +02:00 committed by Robert Haschke
parent f6c3c7b27f
commit 3fa87be8ef
2 changed files with 46 additions and 0 deletions

View File

@ -2,8 +2,11 @@
#include <moveit/macros/class_forward.h>
#include <ros/publisher.h>
#include <ros/service.h>
#include <moveit_task_constructor/Task.h>
#include <moveit_task_constructor/Solution.h>
#include <moveit_task_constructor/GetInterfaceState.h>
#include <moveit_task_constructor/GetSolution.h>
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);

View File

@ -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<InterfaceState>::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<SolutionBase>::instance()[req.solution_id];
solution.fillMessage(msg);
res.solution = msg;
return true;
}
void publishAllPlans(const Task &task, bool wait) {
Task::SolutionProcessor processor