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 8bfc3ec..554df4a 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 @@ -33,11 +33,32 @@ namespace joint_state_topic_hardware_interface { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +struct VelocityLimits +{ + explicit VelocityLimits() = default; + explicit VelocityLimits(const hardware_interface::InterfaceInfo& info) + { + if (not info.min.empty()) + { + min = std::stod(info.min); + } + if (not info.max.empty()) + { + max = std::stod(info.max); + } + } + + double min = -std::numeric_limits::infinity(); + double max = std::numeric_limits::infinity(); +}; + class JointStateTopicSystem : public hardware_interface::SystemInterface { public: CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + std::vector export_state_interfaces() override; std::vector export_command_interfaces() override; @@ -70,6 +91,11 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface std::vector> joint_commands_; std::vector> joint_states_; + bool enable_command_limiting_ = false; + std::vector nonlimited_velocity_commands_; + std::vector limited_velocity_commands_; + std::vector velocity_limits_; + // If the difference between the current joint state and joint command is less than this value, // the joint command will not be published. 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 5a7ff3a..cf081e3 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 @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -57,6 +58,9 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware // Initialize storage for all joints' standard interfaces, regardless of their existence and set all values to nan joint_commands_.resize(standard_interfaces_.size()); joint_states_.resize(standard_interfaces_.size()); + velocity_limits_.resize(get_hardware_info().joints.size()); + limited_velocity_commands_.resize(get_hardware_info().joints.size()); + nonlimited_velocity_commands_.resize(get_hardware_info().joints.size()); for (auto i = 0u; i < standard_interfaces_.size(); i++) { joint_commands_[i].resize(get_hardware_info().joints.size(), 0.0); @@ -67,7 +71,7 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { const auto& component = get_hardware_info().joints[i]; - for (const auto& interface : component.state_interfaces) + for (const auto& interface : component.command_interfaces) { auto it = std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name); // If interface name is found in the interfaces list @@ -80,6 +84,7 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware joint_states_[index][i] = std::stod(interface.initial_value); joint_commands_[index][i] = std::stod(interface.initial_value); } + velocity_limits_[i] = VelocityLimits(interface); } } } @@ -135,14 +140,28 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware // if the values on the `joint_states_topic` are wrapped between -2*pi and 2*pi (like they are in Isaac Sim) // sum the total joint rotation returned on the `joint_states_` interface - if (get_hardware_parameter("sum_wrapped_joint_states", "false") == "true") + sum_wrapped_joint_states_ = + hardware_interface::parse_bool(get_hardware_parameter("sum_wrapped_joint_states", "false")); + // Clamp the command values to the min/max defined in the tag for each joint + enable_command_limiting_ = hardware_interface::parse_bool(get_hardware_parameter("enable_command_limiting", "false")); + + if (enable_command_limiting_) { - sum_wrapped_joint_states_ = true; + RCLCPP_WARN(get_node()->get_logger(), "** Joint command limiting is enabled **"); } return CallbackReturn::SUCCESS; } +CallbackReturn JointStateTopicSystem::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) +{ + REGISTER_ROS2_CONTROL_INTROSPECTION("nonlimited_left_wheel_velocity", &nonlimited_velocity_commands_[0]); + REGISTER_ROS2_CONTROL_INTROSPECTION("nonlimited_right_wheel_velocity", &nonlimited_velocity_commands_[1]); + REGISTER_ROS2_CONTROL_INTROSPECTION("limited_left_wheel_velocity", &limited_velocity_commands_[0]); + REGISTER_ROS2_CONTROL_INTROSPECTION("limited_right_wheel_velocity", &limited_velocity_commands_[1]); + return CallbackReturn::SUCCESS; +} + std::vector JointStateTopicSystem::export_state_interfaces() { std::vector state_interfaces; @@ -269,7 +288,17 @@ hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time& } else if (interface.name == hardware_interface::HW_IF_VELOCITY) { - joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]); + nonlimited_velocity_commands_[i] = joint_commands_[VELOCITY_INTERFACE_INDEX][i]; + if (enable_command_limiting_) + { + limited_velocity_commands_[i] = + std::clamp(nonlimited_velocity_commands_[i], velocity_limits_[i].min, velocity_limits_[i].max); + joint_state.velocity.push_back(limited_velocity_commands_[i]); + } + else + { + joint_state.velocity.push_back(nonlimited_velocity_commands_[i]); + } } else if (interface.name == hardware_interface::HW_IF_EFFORT) {