From db2206193a63854246152ef9f34176a27d8e4ea5 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 26 May 2023 23:37:13 +0200 Subject: [PATCH] Only process input callbacks for active controllers This is cleaner and easier to understand for user applications: - Controllers process input only when active - This avoids sudden jumps upon activation due to some values from earlier callbacks. - Controllers don't spam `NaN` warnings when inactive and working with fake hardware. --- .../cartesian_controller_base.h | 7 +++++++ .../src/cartesian_force_controller.cpp | 10 ++++++++++ .../src/cartesian_motion_controller.cpp | 5 +++++ 3 files changed, 22 insertions(+) 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) ||