Move hw activate (#585)

Author: KmakD

husarion_ugv_controller/launch/controller.launch.py

@@ -17,15 +17,8 @@
 
 from husarion_ugv_utils.logging import limit_log_level_to_info
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    GroupAction,
-    IncludeLaunchDescription,
-    RegisterEventHandler,
-    Shutdown,
-)
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Shutdown
 from launch.conditions import UnlessCondition
-from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     EnvironmentVariable,
@@ -200,54 +193,20 @@ def generate_launch_description():
         limit_log_level_to_info("rcl", log_level),
     ]
 
-    joint_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster", *spawner_common_args],
-        namespace=namespace,
-        emulate_tty=True,
-    )
-
-    drive_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["drive_controller", *spawner_common_args],
-        namespace=namespace,
-        emulate_tty=True,
-    )
-
-    imu_broadcaster_spawner = Node(
+    controllers_spawner = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["imu_broadcaster", *spawner_common_args],
+        arguments=[
+            "joint_state_broadcaster",
+            "drive_controller",
+            "imu_broadcaster",
+            "--activate-as-group",
+            *spawner_common_args,
+        ],
         namespace=namespace,
         emulate_tty=True,
     )
 
-    # Launch spawner one after another
-    # when spawning without delay ros2_control_node sometimes crashed
-    delay_drive_controller_spawner = RegisterEventHandler(
-        event_handler=OnProcessExit(
-            target_action=joint_state_broadcaster_spawner,
-            on_exit=[drive_controller_spawner],
-        ),
-    )
-
-    delay_imu_broadcaster_spawner = RegisterEventHandler(
-        event_handler=OnProcessExit(
-            target_action=drive_controller_spawner,
-            on_exit=[imu_broadcaster_spawner],
-        ),
-    )
-
-    spawners = GroupAction(
-        actions=[
-            joint_state_broadcaster_spawner,
-            delay_drive_controller_spawner,
-            delay_imu_broadcaster_spawner,
-        ]
-    )
-
     actions = [
         declare_common_dir_path_arg,
         declare_robot_model_arg,  # robot_model is used by wheel_type
@@ -259,7 +218,7 @@ def generate_launch_description():
         SetParameter(name="use_sim_time", value=use_sim),
         load_urdf,
         control_node,
-        spawners,
+        controllers_spawner,
     ]
 
     return LaunchDescription(actions)

husarion_ugv_hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp

@@ -94,6 +94,7 @@ class UGVSystem : public hardware_interface::SystemInterface
   void ConfigureRobotDriver();
   virtual void DefineRobotDriver() = 0;
   virtual void ConfigureEStop();  // virtual for mocking
+  void ResetEStop();
 
   void UpdateMotorsState();
   void UpdateDriverState();

husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp

@@ -80,22 +80,6 @@ CallbackReturn UGVSystem::on_configure(const rclcpp_lifecycle::State &)
     return CallbackReturn::ERROR;
   }
 
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn UGVSystem::on_cleanup(const rclcpp_lifecycle::State &)
-{
-  robot_driver_->Deinitialize();
-  robot_driver_.reset();
-
-  gpio_controller_.reset();
-  e_stop_.reset();
-
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn UGVSystem::on_activate(const rclcpp_lifecycle::State &)
-{
   std::fill(hw_commands_velocities_.begin(), hw_commands_velocities_.end(), 0.0);
   std::fill(hw_states_positions_.begin(), hw_states_positions_.end(), 0.0);
   std::fill(hw_states_velocities_.begin(), hw_states_velocities_.end(), 0.0);
@@ -139,7 +123,7 @@ CallbackReturn UGVSystem::on_activate(const rclcpp_lifecycle::State &)
   auto e_stop_reset_qos = rclcpp::ServicesQoS();
   e_stop_reset_qos.keep_last(1);
   system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
-    "hardware/e_stop_reset", std::bind(&EStopInterface::ResetEStop, e_stop_), 2,
+    "hardware/e_stop_reset", std::bind(&UGVSystem::ResetEStop, this), 2,
     rclcpp::CallbackGroupType::MutuallyExclusive, e_stop_reset_qos);
 
   system_ros_interface_->AddDiagnosticTask(
@@ -159,6 +143,24 @@ CallbackReturn UGVSystem::on_activate(const rclcpp_lifecycle::State &)
   return CallbackReturn::SUCCESS;
 }
 
+CallbackReturn UGVSystem::on_cleanup(const rclcpp_lifecycle::State &)
+{
+  robot_driver_->Deinitialize();
+  robot_driver_.reset();
+
+  gpio_controller_.reset();
+
+  system_ros_interface_.reset();
+  e_stop_.reset();
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn UGVSystem::on_activate(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+
 CallbackReturn UGVSystem::on_deactivate(const rclcpp_lifecycle::State &)
 {
   try {
@@ -168,8 +170,6 @@ CallbackReturn UGVSystem::on_deactivate(const rclcpp_lifecycle::State &)
     return CallbackReturn::ERROR;
   }
 
-  system_ros_interface_.reset();
-
   return CallbackReturn::SUCCESS;
 }
 
@@ -263,9 +263,10 @@ return_type UGVSystem::read(const rclcpp::Time & time, const rclcpp::Duration &
 
 return_type UGVSystem::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
 {
+  const auto lifecycle_state = this->get_lifecycle_state().id();
   const bool e_stop = e_stop_->ReadEStopState();
 
-  if (!e_stop) {
+  if (lifecycle_state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && !e_stop) {
     HandleRobotDriverWriteOperation([this] {
       const auto speed_cmds = GetSpeedCommands();
       robot_driver_->SendSpeedCommands(speed_cmds);
@@ -456,6 +457,18 @@ void UGVSystem::ConfigureEStop()
   RCLCPP_INFO(logger_, "Successfully configured E-Stop");
 }
 
+void UGVSystem::ResetEStop()
+{
+  const auto lifecycle_state = this->get_lifecycle_state().id();
+
+  if (lifecycle_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
+    throw std::runtime_error(
+      "Can't reset E-Stop when the hardware interface is not in ACTIVE state.");
+  }
+
+  e_stop_->ResetEStop();
+}
+
 void UGVSystem::UpdateMotorsState()
 {
   try {

husarion_ugv_hardware_interfaces/test/unit/robot_system/test_lynx_system.cpp

@@ -86,6 +86,8 @@ class TestLynxSystem : public ::testing::Test
 public:
   TestLynxSystem()
   {
+    rclcpp::init(0, nullptr);
+
     lynx_system_ = std::make_shared<LynxSystemWrapper>();
 
     hardware_info_ = husarion_ugv_hardware_interfaces_test::GenerateDefaultHardwareInfo();
@@ -95,7 +97,7 @@ class TestLynxSystem : public ::testing::Test
     lynx_system_->on_configure(rclcpp_lifecycle::State());
   }
 
-  ~TestLynxSystem() {}
+  ~TestLynxSystem() { rclcpp::shutdown(); }
 
 protected:
   std::shared_ptr<LynxSystemWrapper> lynx_system_;

husarion_ugv_hardware_interfaces/test/unit/robot_system/test_panther_system.cpp

@@ -100,6 +100,8 @@ class TestPantherSystem : public ::testing::Test
 public:
   TestPantherSystem()
   {
+    rclcpp::init(0, nullptr);
+
     panther_system_ = std::make_shared<PantherSystemWrapper>();
 
     hardware_info_ = husarion_ugv_hardware_interfaces_test::GenerateDefaultHardwareInfo();
@@ -110,7 +112,7 @@ class TestPantherSystem : public ::testing::Test
     panther_system_->on_configure(rclcpp_lifecycle::State());
   }
 
-  ~TestPantherSystem() {}
+  ~TestPantherSystem() { rclcpp::shutdown(); }
 
 protected:
   std::shared_ptr<PantherSystemWrapper> panther_system_;

husarion_ugv_hardware_interfaces/test/unit/robot_system/test_ugv_system.cpp

@@ -67,12 +67,6 @@ class MockUGVSystem : public husarion_ugv_hardware_interfaces::UGVSystem
   MOCK_METHOD(void, DiagnoseErrors, (diagnostic_updater::DiagnosticStatusWrapper &), (override));
   MOCK_METHOD(void, DiagnoseStatus, (diagnostic_updater::DiagnosticStatusWrapper &), (override));
 
-  void DefaultDefineRobotDriver()
-  {
-    robot_driver_ =
-      std::make_shared<husarion_ugv_hardware_interfaces_test::MockRobotDriver::NiceMock>();
-  }
-
   void SetHwCommandVelocity(const std::vector<double> & velocity)
   {
     hw_commands_velocities_ = velocity;
@@ -140,27 +134,36 @@ TEST_F(TestUGVSystem, OnInit)
 
 TEST_F(TestUGVSystem, OnConfigure)
 {
+  rclcpp::init(0, nullptr);
+
   ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
 
-  EXPECT_CALL(*ugv_system_, DefineRobotDriver()).WillOnce(::testing::Invoke([&]() {
-    ugv_system_->DefaultDefineRobotDriver();
-  }));
+  EXPECT_CALL(*ugv_system_, DefineRobotDriver()).Times(1);
   EXPECT_CALL(*ugv_system_, ConfigureGPIOController()).Times(1);
   EXPECT_CALL(*ugv_system_, ConfigureEStop()).Times(1);
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Activate()).Times(1);
+  EXPECT_CALL(*ugv_system_->GetMockGPIOController(), QueryControlInterfaceIOStates()).Times(1);
+  EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1);
   auto callback_return = ugv_system_->on_configure(rclcpp_lifecycle::State());
 
   EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+
+  rclcpp::shutdown();
 }
 
 TEST_F(TestUGVSystem, OnCleanup)
 {
+  rclcpp::init(0, nullptr);
+
   ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
   ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
 
   EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Deinitialize()).Times(1);
   auto callback_return = ugv_system_->on_cleanup(rclcpp_lifecycle::State());
 
   EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+
+  rclcpp::shutdown();
 }
 
 TEST_F(TestUGVSystem, OnActivate)
@@ -169,10 +172,6 @@ TEST_F(TestUGVSystem, OnActivate)
 
   ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
   ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
-
-  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), Activate()).Times(1);
-  EXPECT_CALL(*ugv_system_->GetMockGPIOController(), QueryControlInterfaceIOStates()).Times(1);
-  EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1);
   auto callback_return = ugv_system_->on_activate(rclcpp_lifecycle::State());
 
   EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
@@ -198,6 +197,8 @@ TEST_F(TestUGVSystem, OnDeactivate)
 
 TEST_F(TestUGVSystem, OnShutdown)
 {
+  rclcpp::init(0, nullptr);
+
   ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
   ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
 
@@ -206,10 +207,14 @@ TEST_F(TestUGVSystem, OnShutdown)
   auto callback_return = ugv_system_->on_shutdown(rclcpp_lifecycle::State());
 
   EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+
+  rclcpp::shutdown();
 }
 
 TEST_F(TestUGVSystem, OnError)
 {
+  rclcpp::init(0, nullptr);
+
   ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
   ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
 
@@ -218,6 +223,8 @@ TEST_F(TestUGVSystem, OnError)
   auto callback_return = ugv_system_->on_error(rclcpp_lifecycle::State());
 
   EXPECT_EQ(callback_return, hardware_interface::CallbackReturn::SUCCESS);
+
+  rclcpp::shutdown();
 }
 
 TEST_F(TestUGVSystem, ExportStateInterfacesInitialValues)
@@ -359,19 +366,42 @@ TEST_F(TestUGVSystem, Write)
 
   const auto velocity = std::vector<float>{1.0, 2.0, 3.0, 4.0};
 
+  // System is deactivated, no speed commands sent
   ASSERT_NO_THROW(ugv_system_->on_init(hardware_info_));
   ASSERT_NO_THROW(ugv_system_->on_configure(rclcpp_lifecycle::State()));
-  ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State()));
 
-  EXPECT_CALL(*ugv_system_, GetSpeedCommands()).WillOnce(::testing::Return(velocity));
   EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1);
-  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), SendSpeedCommands(velocity)).Times(1);
+  EXPECT_CALL(*ugv_system_, GetSpeedCommands()).Times(0);
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), SendSpeedCommands(velocity)).Times(0);
 
   auto callback_return = ugv_system_->write(
     rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration(0, 0));
 
   EXPECT_EQ(callback_return, hardware_interface::return_type::OK);
 
+  // System is activated, speed commands sent
+  ugv_system_->set_lifecycle_state(
+    rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "test_active"));
+
+  ASSERT_NO_THROW(ugv_system_->on_activate(rclcpp_lifecycle::State()));
+
+  EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).Times(1);
+  EXPECT_CALL(*ugv_system_, GetSpeedCommands()).WillOnce(::testing::Return(velocity));
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), SendSpeedCommands(velocity)).Times(1);
+
+  callback_return = ugv_system_->write(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration(0, 0));
+
+  EXPECT_EQ(callback_return, hardware_interface::return_type::OK);
+
+  // System is activated, but e-stop was triggered
+  EXPECT_CALL(*ugv_system_->GetMockEStop(), ReadEStopState()).WillOnce(::testing::Return(true));
+  EXPECT_CALL(*ugv_system_, GetSpeedCommands()).Times(0);
+  EXPECT_CALL(*ugv_system_->GetMockRobotDriver(), SendSpeedCommands(velocity)).Times(0);
+
+  callback_return = ugv_system_->write(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration(0, 0));
+
+  EXPECT_EQ(callback_return, hardware_interface::return_type::OK);
+
   rclcpp::shutdown();
 }