stop on nan in generic system

StoglRobotics-forks · Jun 3, 2024 · cbe0340 · cbe0340
1 parent a18bbc2
commit cbe0340
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp
@@ -515,6 +515,11 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
             joint_states_[ACCELERATION_INTERFACE_INDEX][j] =
               (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds();
           }
+          else
+          {
+            joint_states_[VELOCITY_INTERFACE_INDEX][j] = 0.0;
+            joint_states_[ACCELERATION_INTERFACE_INDEX][j] = 0.0;
+          }
           break;
         }
         case POSITION_INTERFACE_INDEX: