diff --git a/joint_state_topic_hardware_interface/README.md b/joint_state_topic_hardware_interface/README.md
index 7f14d19..1670166 100644
--- a/joint_state_topic_hardware_interface/README.md
+++ b/joint_state_topic_hardware_interface/README.md
@@ -10,7 +10,7 @@ The `joint_state_topic_hardware_interface` has a few `ros2_control` urdf tags to
* joint_commands_topic: (default: "/robot_joint_command"). Example: `/my_topic_joint_commands`.
* joint_states_topic: (default: "/robot_joint_states"). Example: `/my_topic_joint_states`.
-* trigger_joint_command_threshold: (default: 1e-5). Used to avoid spamming the joint command topic when the difference between the current joint state and the joint command is smaller than this value, set to -1 to always send the joint command. Example: `0.001`.
+* trigger_joint_command_threshold: Used to avoid spamming the joint command topic when the difference between the current joint state and the joint command is smaller than this value. If not set, the system will always send the joint command. Example: `0.001`.
* sum_wrapped_joint_states: (default: "false"). Used to track the total rotation for joint states the values reported on the `joint_commands_topic` wrap from 2*pi to -2*pi when rotating in the positive direction. (Isaac Sim only reports joint states from 2*pi to -2*pi) Example: `true`.
### Per-joint Parameters
diff --git a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp
index 51482a8..3c55d2e 100644
--- a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp
+++ b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp
@@ -50,6 +50,7 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface
// If the difference between the current joint state and joint command is less than this value,
// the joint command will not be published.
+ bool trigger_joint_command_threshold_enabled_{ false };
double trigger_joint_command_threshold_ = 1e-5;
};
diff --git a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp
index 86733b3..4f7c2a0 100644
--- a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp
+++ b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp
@@ -67,6 +67,7 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware
if (auto it = get_hardware_info().hardware_parameters.find("trigger_joint_command_threshold");
it != get_hardware_info().hardware_parameters.end())
{
+ trigger_joint_command_threshold_enabled_ = true;
trigger_joint_command_threshold_ = std::stod(it->second);
}
@@ -191,7 +192,7 @@ hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time&
get_command(joints[i].name + "/" + interface.name));
}
}
- if (diff <= trigger_joint_command_threshold_)
+ if (trigger_joint_command_threshold_enabled_ && diff <= trigger_joint_command_threshold_)
{
return hardware_interface::return_type::OK;
}