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