From 703fb1812e5332fcda6d7a9bb2ef30e7976253b6 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Fri, 10 Feb 2023 01:36:55 +0100
Subject: [PATCH] ...
---
src/bt/execution.cpp | 10 ++++++++--
1 file changed, 8 insertions(+), 2 deletions(-)
diff --git a/src/bt/execution.cpp b/src/bt/execution.cpp
index 39b2611..aa2434e 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;
--
GitLab