Cleanup wrong lifecycle transitions in tests and unnecessary checks (#1534)

christophfroehlich · web-flow · commit 6d681ef4dbe6 · 2025-02-13T12:16:10.000+01:00
diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp
@@ -141,8 +141,6 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
   rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
   rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED};
 
-  bool is_halted = false;
-
   bool reset();
   void halt();
 
diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
@@ -101,15 +101,6 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   auto logger = get_node()->get_logger();
-  if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
-  {
-    if (!is_halted)
-    {
-      halt();
-      is_halted = true;
-    }
-    return controller_interface::return_type::OK;
-  }
 
   const std::shared_ptr<TwistStamped> command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT());
 
@@ -149,15 +140,6 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   auto logger = get_node()->get_logger();
-  if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
-  {
-    if (!is_halted)
-    {
-      halt();
-      is_halted = true;
-    }
-    return controller_interface::return_type::OK;
-  }
 
   // command may be limited further by SpeedLimit,
   // without affecting the stored twist command
@@ -562,7 +544,6 @@ controller_interface::CallbackReturn DiffDriveController::on_activate(
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  is_halted = false;
   subscriber_is_active_ = true;
 
   RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active.");
@@ -573,11 +554,7 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate(
   const rclcpp_lifecycle::State &)
 {
   subscriber_is_active_ = false;
-  if (!is_halted)
-  {
-    halt();
-    is_halted = true;
-  }
+  halt();
   reset_buffers();
   registered_left_wheel_handles_.clear();
   registered_right_wheel_handles_.clear();
@@ -616,7 +593,6 @@ bool DiffDriveController::reset()
   subscriber_is_active_ = false;
   velocity_command_subscriber_.reset();
 
-  is_halted = false;
   return true;
 }
 
diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp
@@ -719,11 +719,16 @@ TEST_F(TestDiffDriveController, cleanup)
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
+  // should be moving
+  EXPECT_LT(0.0, left_wheel_vel_cmd_.get_value());
+  EXPECT_LT(0.0, right_wheel_vel_cmd_.get_value());
+
   state = controller_->get_node()->deactivate();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
-  ASSERT_EQ(
-    controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
-    controller_interface::return_type::OK);
+
+  // should be stopped
+  EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()";
+  EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()";
 
   state = controller_->get_node()->cleanup();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp
@@ -31,11 +31,6 @@ void ForwardCommandController::declare_parameters()
 
 controller_interface::CallbackReturn ForwardCommandController::read_parameters()
 {
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
   params_ = param_listener_->get_params();
 
   if (params_.joints.empty())
diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp
@@ -32,11 +32,6 @@ void MultiInterfaceForwardCommandController::declare_parameters()
 
 controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters()
 {
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
   params_ = param_listener_->get_params();
 
   if (params_.joint.empty())
diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp
@@ -158,11 +158,6 @@ controller_interface::return_type GpioCommandController::update(
 bool GpioCommandController::update_dynamic_map_parameters()
 {
   auto logger = get_node()->get_logger();
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(logger, "Error encountered during init");
-    return false;
-  }
   // update the dynamic map parameters
   param_listener_->refresh_dynamic_parameters();
   // get parameters from the listener in case they were updated
diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp
@@ -206,11 +206,6 @@ controller_interface::CallbackReturn GripperActionController<HardwareInterface>:
   const rclcpp_lifecycle::State &)
 {
   const auto logger = get_node()->get_logger();
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
   params_ = param_listener_->get_params();
 
   // Action status checking update rate
diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp
@@ -90,11 +90,6 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf
 controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
   params_ = param_listener_->get_params();
 
   if (use_all_available_interfaces())
diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp
@@ -149,23 +149,6 @@ void JointStateBroadcasterTest::assign_state_interfaces(
   state_broadcaster_->assign_interfaces({}, std::move(state_ifs));
 }
 
-TEST_F(JointStateBroadcasterTest, ConfigureErrorTest)
-{
-  // publishers not initialized yet
-  ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_);
-  ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
-
-  // configure failed
-  ASSERT_THROW(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), std::exception);
-
-  SetUpStateBroadcaster();
-  // check state remains unchanged
-
-  // publishers still not initialized
-  ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_);
-  ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
-}
-
 TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)
 {
   // publishers not initialized yet
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -144,10 +144,6 @@ JointTrajectoryController::state_interface_configuration() const
 controller_interface::return_type JointTrajectoryController::update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
-  {
-    return controller_interface::return_type::OK;
-  }
   auto logger = this->get_node()->get_logger();
   // update dynamic parameters
   if (param_listener_->is_old(params_))
@@ -655,12 +651,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
 {
   auto logger = get_node()->get_logger();
 
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(logger, "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
-
   // update the dynamic map parameters
   param_listener_->refresh_dynamic_parameters();
 
diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp
@@ -1020,9 +1020,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_acti
   auto state_ref = traj_controller_->get_state_reference();
   auto state = traj_controller_->get_state_feedback();
 
-  // run an update
-  updateControllerAsync(rclcpp::Duration::from_seconds(0.01));
-
   // There will be no active trajectory upon deactivation, so we can't use the expectCommandPoint
   // method.
   if (traj_controller_->has_position_command_interface())
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -46,39 +46,6 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_robot_description)
     SetUpTrajectoryControllerLocal({}, "<invalid_robot_description/>"));
 }
 
-TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  SetUpTrajectoryController(executor);
-  traj_controller_->get_node()->set_parameter(
-    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));
-
-  const auto state = traj_controller_->configure();
-  ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
-
-  // send msg
-  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
-  builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
-  // *INDENT-OFF*
-  std::vector<std::vector<double>> points{
-    {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
-  std::vector<std::vector<double>> points_velocities{
-    {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
-  // *INDENT-ON*
-  publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
-  traj_controller_->wait_for_trajectory(executor);
-
-  traj_controller_->update(
-    rclcpp::Time(static_cast<uint64_t>(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5));
-
-  // hw position == 0 because controller is not activated
-  EXPECT_EQ(0.0, joint_pos_[0]);
-  EXPECT_EQ(0.0, joint_pos_[1]);
-  EXPECT_EQ(0.0, joint_pos_[2]);
-
-  executor.cancel();
-}
-
 TEST_P(TrajectoryControllerTestParameterized, check_interface_names)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
@@ -148,7 +115,6 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup)
 
   auto state = traj_controller_->get_node()->deactivate();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
-  traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
 
   state = traj_controller_->get_node()->cleanup();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
@@ -223,8 +189,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
   std::vector<double> deactivated_positions{joint_pos_[0], joint_pos_[1], joint_pos_[2]};
   state = traj_controller_->get_node()->deactivate();
   ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
+
   // it should be holding the current point
-  traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1));
   expectHoldingPointDeactivated(deactivated_positions);
 
   // reactivate
diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp
@@ -226,11 +226,6 @@ controller_interface::CallbackReturn GripperActionController::on_configure(
   const rclcpp_lifecycle::State &)
 {
   const auto logger = get_node()->get_logger();
-  if (!param_listener_)
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
   params_ = param_listener_->get_params();
 
   // Action status checking update rate
diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp
@@ -67,23 +67,13 @@ TEST_F(GripperControllerTest, ParametersNotSet)
   this->SetUpController(
     "test_gripper_action_position_controller_no_parameters",
     controller_interface::return_type::ERROR);
-
-  // configure failed, 'joints' parameter not set
-  ASSERT_EQ(
-    this->controller_->on_configure(rclcpp_lifecycle::State()),
-    controller_interface::CallbackReturn::ERROR);
 }
 
 TEST_F(GripperControllerTest, JointParameterIsEmpty)
 {
   this->SetUpController(
     "test_gripper_action_position_controller_empty_joint",
     controller_interface::return_type::ERROR);
-
-  // configure failed, 'joints' is empty
-  ASSERT_EQ(
-    this->controller_->on_configure(rclcpp_lifecycle::State()),
-    controller_interface::CallbackReturn::ERROR);
 }
 
 TEST_F(GripperControllerTest, ConfigureParamsSuccess)
diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp
@@ -135,8 +135,6 @@ class TricycleController : public controller_interface::ControllerInterface
   TractionLimiter limiter_traction_;
   SteeringLimiter limiter_steering_;
 
-  bool is_halted = false;
-
   void reset_odometry(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<std_srvs::srv::Empty::Request> req,
diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp
@@ -88,16 +88,6 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const
 controller_interface::return_type TricycleController::update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
-  {
-    if (!is_halted)
-    {
-      halt();
-      is_halted = true;
-    }
-    return controller_interface::return_type::OK;
-  }
-
   // if the mutex is unable to lock, last_command_msg_ won't be updated
   received_velocity_msg_ptr_.try_get([this](const std::shared_ptr<TwistStamped> & msg)
                                      { last_command_msg_ = msg; });
@@ -382,7 +372,6 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &)
     return CallbackReturn::ERROR;
   }
 
-  is_halted = false;
   subscriber_is_active_ = true;
 
   RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active.");
@@ -392,6 +381,7 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &)
 CallbackReturn TricycleController::on_deactivate(const rclcpp_lifecycle::State &)
 {
   subscriber_is_active_ = false;
+  halt();
   return CallbackReturn::SUCCESS;
 }
 
@@ -438,7 +428,6 @@ bool TricycleController::reset()
   velocity_command_subscriber_.reset();
 
   received_velocity_msg_ptr_.set(nullptr);
-  is_halted = false;
   return true;
 }
 
diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp
@@ -282,9 +282,10 @@ TEST_F(TestTricycleController, cleanup)
 
   state = controller_->get_node()->deactivate();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
-  ASSERT_EQ(
-    controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
-    controller_interface::return_type::OK);
+
+  // should be stopped
+  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value());
+  EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value());
 
   state = controller_->get_node()->cleanup();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
@@ -333,11 +334,11 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters)
   // deactivated
   // wait so controller process the second point when deactivated
   std::this_thread::sleep_for(std::chrono::milliseconds(500));
-  state = controller_->get_node()->deactivate();
-  ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
+  state = controller_->get_node()->deactivate();
+  ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
 
   EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()";
   EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()";
