Don't update MockComponent's state interfaces if command interfaces are not finite (#2786)

(cherry picked from commit bafd793)

Author: christophfroehlich

hardware_interface/src/mock_components/generic_system.cpp

@@ -485,7 +485,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
         if (joint_command.get()->get_interface_name() == hardware_interface::HW_IF_POSITION)
         {
           const std::string & name = joint_command.get()->get_name();
-          if (has_state(name))
+          if (has_state(name) && std::isfinite(get_command(name)))
           {
             set_state(
               name, get_command(name) + (custom_interface_with_following_offset_.empty()
@@ -523,7 +523,10 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
         get_command(
           joint_state.get()->get_prefix_name() + "/" + hardware_interface::HW_IF_POSITION) +
         position_state_following_offset_;
-      set_state(full_interface_name, cmd);
+      if (std::isfinite(cmd))
+      {
+        set_state(full_interface_name, cmd);
+      }
     }
   }
 

hardware_interface/test/mock_components/test_generic_system.cpp

@@ -1032,6 +1032,28 @@ void generic_system_functional_test(
   EXPECT_TRUE(std::isnan(j2p_c.get_optional().value()));
   EXPECT_TRUE(std::isnan(j2v_c.get_optional().value()));
 
+  // read() does not change values until commands are set (i.e, isfinite())
+  ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK);
+  EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value());
+  EXPECT_EQ(0.0, j1v_s.get_optional().value());
+  EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value());
+  EXPECT_EQ(0.0, j2v_s.get_optional().value());
+  EXPECT_TRUE(std::isnan(j1p_c.get_optional().value()));
+  EXPECT_TRUE(std::isnan(j1v_c.get_optional().value()));
+  EXPECT_TRUE(std::isnan(j2p_c.get_optional().value()));
+  EXPECT_TRUE(std::isnan(j2v_c.get_optional().value()));
+
+  // write() does not change values until commands are set (i.e, isfinite())
+  ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK);
+  EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value());
+  EXPECT_EQ(0.0, j1v_s.get_optional().value());
+  EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value());
+  EXPECT_EQ(0.0, j2v_s.get_optional().value());
+  EXPECT_TRUE(std::isnan(j1p_c.get_optional().value()));
+  EXPECT_TRUE(std::isnan(j1v_c.get_optional().value()));
+  EXPECT_TRUE(std::isnan(j2p_c.get_optional().value()));
+  EXPECT_TRUE(std::isnan(j2v_c.get_optional().value()));
+
   // set some new values in commands
   ASSERT_TRUE(j1p_c.set_value(0.11));
   ASSERT_TRUE(j1v_c.set_value(0.22));