added simple nan runtime check

lbr-stack · Nov 20, 2023 · 9add600 · 9add600
1 parent 091aca5
commit 9add600
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 0 deletions.
diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp
@@ -1,6 +1,7 @@
 #ifndef LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_
 #define LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_
 
+#include <cmath>
 #include <limits>
 #include <map>
 #include <memory>

diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp
@@ -1,6 +1,7 @@
 #ifndef LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_
 #define LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_
 
+#include <cmath>
 #include <functional>
 #include <limits>
 #include <memory>

diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/lbr_state_broadcaster.cpp
@@ -32,6 +32,10 @@ controller_interface::CallbackReturn LBRStateBroadcaster::on_init() {
 
 controller_interface::return_type LBRStateBroadcaster::update(const rclcpp::Time & /*time*/,
                                                               const rclcpp::Duration & /*period*/) {
+  // check any for nan
+  if (std::isnan(state_interface_map_[joint_names_[0]][hardware_interface::HW_IF_POSITION])) {
+    return controller_interface::return_type::OK;
+  }
   for (const auto &state_interface : state_interfaces_) {
     state_interface_map_[state_interface.get_prefix_name()][state_interface.get_interface_name()] =
         state_interface.get_value();

diff --git a/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp b/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp
@@ -55,6 +55,10 @@ controller_interface::CallbackReturn LBRVirtualFTBroadcaster::on_init() {
 controller_interface::return_type
 LBRVirtualFTBroadcaster::update(const rclcpp::Time & /*time*/,
                                 const rclcpp::Duration & /*period*/) {
+  // check any for nan
+  if (std::isnan(joint_position_interfaces_[0].get().get_value())) {
+    return controller_interface::return_type::OK;
+  }
   for (std::size_t i = 0; i < joint_names_.size(); ++i) {
     joint_positions_(i) = joint_position_interfaces_[i].get().get_value();
     external_joint_torques_(i) = external_joint_torque_interfaces_[i].get().get_value();