Skip to content

Commit

Permalink
Only process input callbacks for active controllers
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
stefanscherzinger committed Apr 8, 2024
1 parent 4b5daab commit db22061
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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<KDL::TreeFkSolverPos_recursive> m_forward_kinematics_solver;
Expand Down
10 changes: 10 additions & 0 deletions cartesian_force_controller/src/cartesian_force_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand All @@ -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))
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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) ||
Expand Down

0 comments on commit db22061

Please sign in to comment.