Go to the documentation of this file.
4 : BT::StatefulActionNode(name, config) {}
8 if (
it_ !=
ets_.solution.sub_trajectory.end()){
9 std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(
it_->trajectory,
it_->scene_diff);
12 return BT::NodeStatus::RUNNING;
14 return BT::NodeStatus::SUCCESS;
19 if (
it_ !=
ets_.solution.sub_trajectory.end()){
20 std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(
it_->trajectory,
it_->scene_diff);
23 return BT::NodeStatus::RUNNING;
25 return BT::NodeStatus::SUCCESS;
30 void Execution::init(std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions ,
AbstractRobotDecorator* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) {
33 it_ =
ets_.solution.sub_trajectory.begin();
std::vector< moveit_task_constructor_msgs::SubTrajectory >::iterator it_
next iteration of ExecuteTaskSolutionGoal
Execution(const std::string &name, const BT::NodeConfiguration &config)
constructor
void onHalted() override
Function called on halt.
BT::NodeStatus onRunning() override
Function called while node is in running state.
BT::NodeStatus onStart() override
Function called at first execution.
std::string & name() override
Redirects name call to next_.
Abstract Robot Decorator.
void init(std::map< std::string, std::pair< moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene >> *executions_, AbstractRobotDecorator *mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal &ets_)
Initialize Node.
AbstractRobotDecorator * mr_reference_
Reference to the robot.
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_
ExecuteTaskSolutionGoal, which is a reference to trajetory and planningscene.
std::map< std::string, std::pair< moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene > > * executions_
trajetory and planningscene mapped to robot