robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

fix(MoveToPose): Add input validation for setGoal function

Browse source 
- 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
This commit is contained in:
Ilya Uraev 2025-03-17 18:10:38 +03:00
parent c87ace262d
commit 6028717a5e
1 changed files with 13 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

16
rbs_bt_executor/src/MoveToPose.cpp
View file

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