execution.cpp
Go to the documentation of this file.
1 #include "bt/execution.h"
2 
3 Execution::Execution(const std::string& name, const BT::NodeConfiguration& config)
4  : BT::StatefulActionNode(name, config) {}
5 
6 BT::NodeStatus Execution::onStart(){
7  ROS_INFO("PING");
8  if (it_ != ets_.solution.sub_trajectory.end()){
9  std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
10  executions_->insert_or_assign(mr_reference_->name(), pair);
11  it_++;
12  return BT::NodeStatus::RUNNING;
13  } else {
14  return BT::NodeStatus::SUCCESS;
15  }
16  }
17 
18 BT::NodeStatus Execution::onRunning(){
19  if (it_ != ets_.solution.sub_trajectory.end()){
20  std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
21  executions_->insert_or_assign(mr_reference_->name(), pair);
22  it_++;
23  return BT::NodeStatus::RUNNING;
24  } else {
25  return BT::NodeStatus::SUCCESS;
26  }
27 }
29 
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) {
31  mr_reference_= mr_reference;
32  ets_ = ets;
33  it_ = ets_.solution.sub_trajectory.begin();
34  executions_ = executions;
35 }
Execution::it_
std::vector< moveit_task_constructor_msgs::SubTrajectory >::iterator it_
next iteration of ExecuteTaskSolutionGoal
Definition: execution.h:28
Execution::Execution
Execution(const std::string &name, const BT::NodeConfiguration &config)
constructor
Definition: execution.cpp:3
Execution::onHalted
void onHalted() override
Function called on halt.
Definition: execution.cpp:28
Execution::onRunning
BT::NodeStatus onRunning() override
Function called while node is in running state.
Definition: execution.cpp:18
Execution::onStart
BT::NodeStatus onStart() override
Function called at first execution.
Definition: execution.cpp:6
AbstractRobotDecorator::name
std::string & name() override
Redirects name call to next_.
Definition: abstract_robot_decorator.h:47
AbstractRobotDecorator
Abstract Robot Decorator.
Definition: abstract_robot_decorator.h:22
Execution::init
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.
Definition: execution.cpp:30
Execution::mr_reference_
AbstractRobotDecorator * mr_reference_
Reference to the robot.
Definition: execution.h:26
Execution::ets_
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_
ExecuteTaskSolutionGoal, which is a reference to trajetory and planningscene.
Definition: execution.h:25
execution.h
Execution::executions_
std::map< std::string, std::pair< moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene > > * executions_
trajetory and planningscene mapped to robot
Definition: execution.h:27


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51