From 6028717a5efcf5d0cb419dee256cb653e3645197 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Mon, 17 Mar 2025 18:10:38 +0300
Subject: [PATCH] fix(MoveToPose): Add input validation for setGoal function

- Added include for rclcpp logging
- Modified setGoal() to validate inputs using getInput()
- Added fatal error logging when input retrieval fails
- Return false if any input is missing or invalid
---
 rbs_bt_executor/src/MoveToPose.cpp | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/rbs_bt_executor/src/MoveToPose.cpp b/rbs_bt_executor/src/MoveToPose.cpp
index f36d1cd..c81a4af 100644
--- a/rbs_bt_executor/src/MoveToPose.cpp
+++ b/rbs_bt_executor/src/MoveToPose.cpp
@@ -3,6 +3,7 @@
 #include <behaviortree_cpp/tree_node.h>
 #include <behaviortree_ros2/plugins.hpp>
 #include <behaviortree_ros2/ros_node_params.hpp>
+#include <rclcpp/logging.hpp>
 
 using namespace BT;
 using MoveitSendPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
@@ -26,9 +27,18 @@ public:
   bool setGoal(RosActionNode::Goal &goal) override {
     RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
                 this->action_name_.c_str());
-    getInput("robot_name", goal.robot_name);
-    getInput("pose", m_target_pose);
-    getInput("duration", goal.duration);
+    if (!getInput("robot_name", goal.robot_name)) {
+      RCLCPP_FATAL(this->logger(), "[%s] Could not get robot_name from input port", name().c_str());
+      return false;
+    }
+    if (!getInput("pose", m_target_pose)) {
+      RCLCPP_FATAL(this->logger(), "[%s] Could not get target_pose from input port", name().c_str());
+      return false;
+    }
+    if (!getInput("duration", goal.duration)) {
+      RCLCPP_FATAL(this->logger(), "[%s] Could not get duration from input port", name().c_str());
+      return false;
+    }
     goal.target_pose = *m_target_pose;
     return true;
   }