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