This commit is contained in:
Robert Haschke 2017-10-27 22:00:46 +02:00
parent 3fa87be8ef
commit abe61ef9eb
2 changed files with 10 additions and 20 deletions

View File

@ -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);
};

View File

@ -13,6 +13,10 @@ Introspection::Introspection(const std::string &task_topic,
ros::NodeHandle n;
task_publisher_ = n.advertise<moveit_task_constructor::Task>(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)
{