Added check in BT for empty tool paths (#144)

ros-industrial-consortium · Oct 9, 2024 · d0cb593 · d0cb593
1 parent 3a937da
commit d0cb593
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/snp_application/src/bt/snp_bt_ros_nodes.cpp b/snp_application/src/bt/snp_bt_ros_nodes.cpp
@@ -137,6 +137,12 @@ BT::NodeStatus GenerateToolPathsServiceNode::onResponseReceived(const typename R
     return BT::NodeStatus::FAILURE;
   }
 
+  if (response->tool_paths.empty())
+  {
+    config().blackboard->set(ERROR_MESSAGE_KEY, "Tool paths are empty! Check tool path planner parameters.");
+    return BT::NodeStatus::FAILURE;
+  }
+
   // Copy the tool paths
   std::vector<snp_msgs::msg::ToolPath> tool_paths = response->tool_paths;