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) MOVEIT_CLASS_FORWARD(SolutionBase)
class Introspection { class Introspection {
// publish Task state and (new) Solutions
ros::Publisher task_publisher_; ros::Publisher task_publisher_;
ros::Publisher solution_publisher_; ros::Publisher solution_publisher_;
// TODO add services to provide InterfaceState and Solution // services to provide InterfaceState and Solution
ros::ServiceServer interfaceState_service_; ros::ServiceServer get_interface_state_service_;
ros::ServiceServer solution_service_; ros::ServiceServer get_solution_service_;
ros::NodeHandle n;
public: public:
Introspection(const std::string &task_topic = "task", Introspection(const std::string &task_topic = "task",
@ -43,11 +42,10 @@ public:
} }
void operator()(const SolutionBase &s) { publishSolution(s); } void operator()(const SolutionBase &s) { publishSolution(s); }
//advertise services // get interface state
void generateInterfaceService();
void generateSolutionService();
bool getInterfaceState(moveit_task_constructor::GetInterfaceState::Request &req, bool getInterfaceState(moveit_task_constructor::GetInterfaceState::Request &req,
moveit_task_constructor::GetInterfaceState::Response &res); moveit_task_constructor::GetInterfaceState::Response &res);
// get solution
bool getSolution(moveit_task_constructor::GetSolution::Request &req, bool getSolution(moveit_task_constructor::GetSolution::Request &req,
moveit_task_constructor::GetSolution::Response &res); moveit_task_constructor::GetSolution::Response &res);
}; };

View File

@ -13,6 +13,10 @@ Introspection::Introspection(const std::string &task_topic,
ros::NodeHandle n; ros::NodeHandle n;
task_publisher_ = n.advertise<moveit_task_constructor::Task>(task_topic, 1); task_publisher_ = n.advertise<moveit_task_constructor::Task>(task_topic, 1);
solution_publisher_ = n.advertise<::moveit_task_constructor::Solution>(solution_topic, 1, true); 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() Introspection &Introspection::instance()
@ -34,18 +38,6 @@ void Introspection::publishSolution(const SolutionBase &s)
publishSolution(msg); 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, bool Introspection::getInterfaceState(moveit_task_constructor::GetInterfaceState::Request &req,
moveit_task_constructor::GetInterfaceState::Response &res) moveit_task_constructor::GetInterfaceState::Response &res)
{ {