execution.cpp

#include "bt/execution.h"

Execution::Execution(const std::string& name, const BT::NodeConfiguration& config)
    : BT::StatefulActionNode(name, config) {}


/*
BT::NodeStatus Execution::tick() {
    ROS_INFO("davor");
    if (it_ != ets_.solution.sub_trajectory.end()){
        mgi_reference_->execute(it_->trajectory);
        publisher_reference_->publish(it_->scene_diff);
        it_++;
        ROS_INFO("true");
        return BT::NodeStatus::RUNNING;
    } else {
        ROS_INFO("false");
        return BT::NodeStatus::SUCCESS;
    }
};
*/

BT::NodeStatus Execution::onStart(){
    if (it_ != ets_.solution.sub_trajectory.end()){
        std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
        for (auto& e : it_->scene_diff.allowed_collision_matrix.entry_names) ROS_INFO("%s", e.c_str());
	    for (auto& e : it_->scene_diff.allowed_collision_matrix.entry_values) {
		for (auto en: e.enabled){
			ROS_INFO("%f", en);
            }
        }
        executions_->insert_or_assign(mr_reference_->name(), pair);
        it_++;
        return BT::NodeStatus::RUNNING;
    } else {
        return BT::NodeStatus::SUCCESS;
    }
 }

    // If onStart() returned RUNNING, we will keep calling
    // this method until it return something different from RUNNING
BT::NodeStatus Execution::onRunning(){
    if (it_ != ets_.solution.sub_trajectory.end()){
        std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
        for (auto& e : it_->scene_diff.allowed_collision_matrix.entry_names) ROS_INFO("%s", e.c_str());
	    for (auto& e : it_->scene_diff.allowed_collision_matrix.entry_values) {
		for (auto en: e.enabled){
			ROS_INFO("%f", en);
            }
        }
        executions_->insert_or_assign(mr_reference_->name(), pair);
        it_++;
        return BT::NodeStatus::RUNNING;
    } else {
        return BT::NodeStatus::SUCCESS;
    }
}

    // callback to execute if the action was aborted by another node
void Execution::onHalted(){}

void Execution::init(std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) {
    mr_reference_= mr_reference;
    publisher_reference_ = publisher_reference;
    ets_ = ets;
    it_ = ets_.solution.sub_trajectory.begin();
    executions_ = executions;
}