Bugfix 🏎: Reset and test of command buffer for forwarding controllers.

forward_command_controller/src/forward_command_controller.cpp

@@ -15,6 +15,7 @@
 #include "forward_command_controller/forward_command_controller.hpp"
 
 #include <algorithm>
+#include <memory>
 #include <string>
 #include <utility>
 #include <vector>
@@ -144,12 +145,17 @@ CallbackReturn ForwardCommandController::on_activate(
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
   }
 
+  // reset command buffer if a command came through callback when controller was inactive
+  rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>(nullptr);
+
   return CallbackReturn::SUCCESS;
 }
 
 CallbackReturn ForwardCommandController::on_deactivate(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  // reset command buffer
+  rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>(nullptr);
   return CallbackReturn::SUCCESS;
 }
 

forward_command_controller/test/test_forward_command_controller.cpp

@@ -12,8 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <stddef.h>
-
 #include <functional>
 #include <memory>
 #include <string>
@@ -22,6 +20,8 @@
 
 #include "gmock/gmock.h"
 
+#include "test_forward_command_controller.hpp"
+
 #include "forward_command_controller/forward_command_controller.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
@@ -32,7 +32,6 @@
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/wait_set.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
-#include "test_forward_command_controller.hpp"
 
 using CallbackReturn = forward_command_controller::ForwardCommandController::CallbackReturn;
 using hardware_interface::LoanedCommandInterface;
@@ -291,3 +290,94 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest)
   ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0);
   ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0);
 }
+
+TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
+{
+  SetUpController();
+
+  controller_->get_node()->set_parameter({"joints", joint_names_});
+  controller_->get_node()->set_parameter({"interface_name", "position"});
+
+  // default values
+  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
+  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1);
+  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1);
+
+  auto node_state = controller_->configure();
+  ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+
+  node_state = controller_->activate();
+  ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+
+  auto command_msg = std::make_shared<std_msgs::msg::Float64MultiArray>();
+  command_msg->data = {10.0, 20.0, 30.0};
+
+  controller_->rt_command_ptr_.writeFromNonRT(command_msg);
+
+  // update successful
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  // check command in handle was set
+  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10);
+  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20);
+  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30);
+
+  node_state = controller_->deactivate();
+  ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+
+  // command ptr should be reset (nullptr) after deactivation - same check as in `update`
+  ASSERT_FALSE(
+    controller_->rt_command_ptr_.readFromNonRT() &&
+    *(controller_->rt_command_ptr_.readFromNonRT()));
+  ASSERT_FALSE(
+    controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT()));
+
+  // Controller is inactive but let's put some data into buffer (simulate callback when inactive)
+  command_msg = std::make_shared<std_msgs::msg::Float64MultiArray>();
+  command_msg->data = {5.5, 6.6, 7.7};
+
+  controller_->rt_command_ptr_.writeFromNonRT(command_msg);
+
+  // command ptr should be available and message should be there - same check as in `update`
+  ASSERT_TRUE(
+    controller_->rt_command_ptr_.readFromNonRT() &&
+    *(controller_->rt_command_ptr_.readFromNonRT()));
+  ASSERT_TRUE(
+    controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT()));
+
+  // Now activate again
+  node_state = controller_->activate();
+  ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+
+  // command ptr should be reset (nullptr) after activation - same check as in `update`
+  ASSERT_FALSE(
+    controller_->rt_command_ptr_.readFromNonRT() &&
+    *(controller_->rt_command_ptr_.readFromNonRT()));
+  ASSERT_FALSE(
+    controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT()));
+
+  // update successful
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  // values should not change
+  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10);
+  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20);
+  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30);
+
+  // set commands again
+  controller_->rt_command_ptr_.writeFromNonRT(command_msg);
+
+  // update successful
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  // check command in handle was set
+  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5);
+  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6);
+  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7);
+}

forward_command_controller/test/test_forward_command_controller.hpp

@@ -44,6 +44,7 @@ class FriendForwardCommandController : public forward_command_controller::Forwar
   FRIEND_TEST(ForwardCommandControllerTest, WrongCommandCheckTest);
   FRIEND_TEST(ForwardCommandControllerTest, NoCommandCheckTest);
   FRIEND_TEST(ForwardCommandControllerTest, CommandCallbackTest);
+  FRIEND_TEST(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess);
 };
 
 class ForwardCommandControllerTest : public ::testing::Test