diff --git a/src/bt/execution.cpp b/src/bt/execution.cpp index 39b261171e171f6b31c1ac725c5fdfd15c358da6..aa2434ec7b7a6ee32fab8e6a3eb19ed7c8577a36 100644 --- a/src/bt/execution.cpp +++ b/src/bt/execution.cpp @@ -27,8 +27,8 @@ BT::NodeStatus Execution::onStart(){ 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; @@ -42,6 +42,12 @@ BT::NodeStatus Execution::onStart(){ 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;