From 1b24667f7a94af2ffd41346d1c56af3ce3c39f26 Mon Sep 17 00:00:00 2001 From: EnejiOhieku Date: Tue, 24 Mar 2026 19:45:54 +0100 Subject: [PATCH] Added trigger_joint_command_threshold_enabled_ to ensure messages ae always sent if the trigger_joint_command_threshold param is not set --- joint_state_topic_hardware_interface/README.md | 2 +- .../joint_state_topic_hardware_interface.hpp | 1 + .../src/joint_state_topic_hardware_interface.cpp | 3 ++- 3 files changed, 4 insertions(+), 2 deletions(-) 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; }