Skip to content
Snippets Groups Projects
Commit 525d0639 authored by KingMaZito's avatar KingMaZito
Browse files

...

parent 703fb181
No related branches found
No related tags found
No related merge requests found
...@@ -23,14 +23,9 @@ BT::NodeStatus Execution::tick() { ...@@ -23,14 +23,9 @@ BT::NodeStatus Execution::tick() {
BT::NodeStatus Execution::onStart(){ BT::NodeStatus Execution::onStart(){
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_++;
ROS_INFO("oihinin");
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
} else { } else {
return BT::NodeStatus::SUCCESS; return BT::NodeStatus::SUCCESS;
...@@ -42,14 +37,10 @@ BT::NodeStatus Execution::onStart(){ ...@@ -42,14 +37,10 @@ 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_++;
ROS_INFO("oihinin");
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
} else { } else {
return BT::NodeStatus::SUCCESS; return BT::NodeStatus::SUCCESS;
......
This diff is collapsed.
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment