Added trigger_joint_command_threshold_enabled_

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: `<param name="joint_commands_topic">/my_topic_joint_commands</param>`.
 * joint_states_topic: (default: "/robot_joint_states"). Example: `<param name="joint_states_topic">/my_topic_joint_states</param>`.
-* 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: `<param name="trigger_joint_command_threshold">0.001</param>`.
+* 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: `<param name="trigger_joint_command_threshold">0.001</param>`.
 * 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: `<param name="sum_wrapped_joint_states">true</param>`.
 
 ### Per-joint Parameters

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;
 };
 

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;
   }