diff --git a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h index 6699835d..ce9552cf 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h +++ b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h @@ -176,6 +176,13 @@ class CartesianControllerBase : public controller_interface::ControllerInterface return false; } + /** + * @brief Helper method to check the controller's state during input callbacks + * + * @return True if the controller is active, false otherwise + */ + bool isActive() const {return m_active;}; + KDL::Chain m_robot_chain; std::shared_ptr m_forward_kinematics_solver; diff --git a/cartesian_force_controller/src/cartesian_force_controller.cpp b/cartesian_force_controller/src/cartesian_force_controller.cpp index e6bc90e4..ae3fd2c5 100644 --- a/cartesian_force_controller/src/cartesian_force_controller.cpp +++ b/cartesian_force_controller/src/cartesian_force_controller.cpp @@ -209,6 +209,11 @@ void CartesianForceController::setFtSensorReferenceFrame(const std::string & new void CartesianForceController::targetWrenchCallback( const geometry_msgs::msg::WrenchStamped::SharedPtr wrench) { + if (!this->isActive()) + { + return; + } + if (std::isnan(wrench->wrench.force.x) || std::isnan(wrench->wrench.force.y) || std::isnan(wrench->wrench.force.z) || std::isnan(wrench->wrench.torque.x) || std::isnan(wrench->wrench.torque.y) || std::isnan(wrench->wrench.torque.z)) @@ -230,6 +235,11 @@ void CartesianForceController::targetWrenchCallback( void CartesianForceController::ftSensorWrenchCallback( const geometry_msgs::msg::WrenchStamped::SharedPtr wrench) { + if (!this->isActive()) + { + return; + } + if (std::isnan(wrench->wrench.force.x) || std::isnan(wrench->wrench.force.y) || std::isnan(wrench->wrench.force.z) || std::isnan(wrench->wrench.torque.x) || std::isnan(wrench->wrench.torque.y) || std::isnan(wrench->wrench.torque.z)) diff --git a/cartesian_motion_controller/src/cartesian_motion_controller.cpp b/cartesian_motion_controller/src/cartesian_motion_controller.cpp index 00443a11..45037759 100644 --- a/cartesian_motion_controller/src/cartesian_motion_controller.cpp +++ b/cartesian_motion_controller/src/cartesian_motion_controller.cpp @@ -193,6 +193,11 @@ ctrl::Vector6D CartesianMotionController::computeMotionError() void CartesianMotionController::targetFrameCallback( const geometry_msgs::msg::PoseStamped::SharedPtr target) { + if (!this->isActive()) + { + return; + } + if (std::isnan(target->pose.position.x) || std::isnan(target->pose.position.y) || std::isnan(target->pose.position.z) || std::isnan(target->pose.orientation.x) || std::isnan(target->pose.orientation.y) || std::isnan(target->pose.orientation.z) ||