mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
introspection service generators are added
This commit is contained in:
parent
f6c3c7b27f
commit
3fa87be8ef
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user