Skip to content
Snippets Groups Projects
Commit 703fb181 authored by KingMaZito's avatar KingMaZito
Browse files

...

parent 0c43d354
No related branches found
No related tags found
No related merge requests found
...@@ -27,8 +27,8 @@ BT::NodeStatus Execution::onStart(){ ...@@ -27,8 +27,8 @@ BT::NodeStatus Execution::onStart(){
for (auto& e : it_->scene_diff.allowed_collision_matrix.entry_values) { for (auto& e : it_->scene_diff.allowed_collision_matrix.entry_values) {
for (auto en: e.enabled){ for (auto en: e.enabled){
ROS_INFO("%f", en); ROS_INFO("%f", en);
} }
} }
executions_->insert_or_assign(mr_reference_->name(), pair); executions_->insert_or_assign(mr_reference_->name(), pair);
it_++; it_++;
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
...@@ -42,6 +42,12 @@ BT::NodeStatus Execution::onStart(){ ...@@ -42,6 +42,12 @@ BT::NodeStatus Execution::onStart(){
BT::NodeStatus Execution::onRunning(){ BT::NodeStatus Execution::onRunning(){
if (it_ != ets_.solution.sub_trajectory.end()){ if (it_ != ets_.solution.sub_trajectory.end()){
std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff); 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); executions_->insert_or_assign(mr_reference_->name(), pair);
it_++; it_++;
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment