Let sensors also export state interfaces of joints (#1885)

Author: christophfroehlich

hardware_interface/CMakeLists.txt

@@ -21,6 +21,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_gen_version_h REQUIRED)
+find_package(backward_ros REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -123,6 +123,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
     info_ = hardware_info;
+    parse_state_interface_descriptions(info_.joints, joint_state_interfaces_);
     parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_);
     return CallbackReturn::SUCCESS;
   };
@@ -179,7 +180,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
 
     std::vector<StateInterface::ConstSharedPtr> state_interfaces;
     state_interfaces.reserve(
-      unlisted_interface_descriptions.size() + sensor_state_interfaces_.size());
+      unlisted_interface_descriptions.size() + sensor_state_interfaces_.size() +
+      joint_state_interfaces_.size());
 
     // add InterfaceDescriptions and create StateInterfaces from the descriptions and add to maps.
     for (const auto & description : unlisted_interface_descriptions)
@@ -201,6 +203,14 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
       state_interfaces.push_back(std::const_pointer_cast<const StateInterface>(state_interface));
     }
 
+    for (const auto & [name, descr] : joint_state_interfaces_)
+    {
+      auto state_interface = std::make_shared<StateInterface>(descr);
+      sensor_states_map_.insert(std::make_pair(name, state_interface));
+      joint_states_.push_back(state_interface);
+      state_interfaces.push_back(std::const_pointer_cast<const StateInterface>(state_interface));
+    }
+
     return state_interfaces;
   }
 
@@ -274,10 +284,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
 protected:
   HardwareInfo info_;
   // interface names to InterfaceDescription
+  std::unordered_map<std::string, InterfaceDescription> joint_state_interfaces_;
   std::unordered_map<std::string, InterfaceDescription> sensor_state_interfaces_;
   std::unordered_map<std::string, InterfaceDescription> unlisted_state_interfaces_;
 
   // Exported Command- and StateInterfaces in order they are listed in the hardware description.
+  std::vector<StateInterface::SharedPtr> joint_states_;
   std::vector<StateInterface::SharedPtr> sensor_states_;
   std::vector<StateInterface::SharedPtr> unlisted_states_;
 

hardware_interface/package.xml

@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_gen_version_h</buildtool_depend>
 
+  <depend>backward_ros</depend>
   <depend>control_msgs</depend>
   <depend>lifecycle_msgs</depend>
   <depend>pluginlib</depend>

hardware_interface/src/resource_manager.cpp

@@ -615,11 +615,10 @@ class ResourceStorage
     auto interfaces = hardware.export_state_interfaces();
     const auto interface_names = add_state_interfaces(interfaces);
 
-    RCLCPP_WARN(
-      get_logger(),
+    RCLCPP_WARN_EXPRESSION(
+      get_logger(), interface_names.empty(),
       "Importing state interfaces for the hardware '%s' returned no state interfaces.",
       hardware.get_name().c_str());
-
     hardware_info_map_[hardware.get_name()].state_interfaces = interface_names;
     available_state_interfaces_.reserve(
       available_state_interfaces_.capacity() + interface_names.size());

hardware_interface/test/test_component_interfaces.cpp

@@ -271,7 +271,7 @@ class DummySensor : public hardware_interface::SensorInterface
     // We can read some voltage level
     std::vector<hardware_interface::StateInterface> state_interfaces;
     state_interfaces.emplace_back(
-      hardware_interface::StateInterface("joint1", "voltage", &voltage_level_));
+      hardware_interface::StateInterface("sens1", "voltage", &voltage_level_));
 
     return state_interfaces;
   }
@@ -332,15 +332,72 @@ class DummySensorDefault : public hardware_interface::SensorInterface
 
   CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override
   {
-    for (const auto & [name, descr] : sensor_state_interfaces_)
+    set_state("sens1/voltage", 0.0);
+    read_calls_ = 0;
+    return CallbackReturn::SUCCESS;
+  }
+
+  std::string get_name() const override { return "DummySensorDefault"; }
+
+  hardware_interface::return_type read(
+    const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
+  {
+    ++read_calls_;
+    if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS)
+    {
+      return hardware_interface::return_type::ERROR;
+    }
+
+    // no-op, static value
+    set_state("sens1/voltage", voltage_level_hw_value_);
+    return hardware_interface::return_type::OK;
+  }
+
+  CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override
+  {
+    if (!recoverable_error_happened_)
+    {
+      recoverable_error_happened_ = true;
+      return CallbackReturn::SUCCESS;
+    }
+    else
+    {
+      return CallbackReturn::ERROR;
+    }
+    return CallbackReturn::FAILURE;
+  }
+
+private:
+  double voltage_level_hw_value_ = 0x666;
+
+  // Helper variables to initiate error on read
+  int read_calls_ = 0;
+  bool recoverable_error_happened_ = false;
+};
+
+class DummySensorJointDefault : public hardware_interface::SensorInterface
+{
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
+  {
+    if (
+      hardware_interface::SensorInterface::on_init(info) !=
+      hardware_interface::CallbackReturn::SUCCESS)
     {
-      set_state(name, 0.0);
+      return hardware_interface::CallbackReturn::ERROR;
     }
+
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override
+  {
+    set_state("joint1/position", 10.0);
+    set_state("sens1/voltage", 0.0);
     read_calls_ = 0;
     return CallbackReturn::SUCCESS;
   }
 
-  std::string get_name() const override { return "DummySensorDefault"; }
+  std::string get_name() const override { return "DummySensorJointDefault"; }
 
   hardware_interface::return_type read(
     const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
@@ -352,7 +409,8 @@ class DummySensorDefault : public hardware_interface::SensorInterface
     }
 
     // no-op, static value
-    set_state("joint1/voltage", voltage_level_hw_value_);
+    set_state("joint1/position", position_hw_value_);
+    set_state("sens1/voltage", voltage_level_hw_value_);
     return hardware_interface::return_type::OK;
   }
 
@@ -371,6 +429,7 @@ class DummySensorDefault : public hardware_interface::SensorInterface
   }
 
 private:
+  double position_hw_value_ = 0x777;
   double voltage_level_hw_value_ = 0x666;
 
   // Helper variables to initiate error on read
@@ -907,9 +966,9 @@ TEST(TestComponentInterfaces, dummy_sensor)
 
   auto state_interfaces = sensor_hw.export_state_interfaces();
   ASSERT_EQ(1u, state_interfaces.size());
-  EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name());
+  EXPECT_EQ("sens1/voltage", state_interfaces[0]->get_name());
   EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name());
-  EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name());
+  EXPECT_EQ("sens1", state_interfaces[0]->get_prefix_name());
   EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value()));
 
   // Not updated because is is UNCONFIGURED
@@ -948,29 +1007,83 @@ TEST(TestComponentInterfaces, dummy_sensor_default)
   auto state_interfaces = sensor_hw.export_state_interfaces();
   ASSERT_EQ(1u, state_interfaces.size());
   {
-    auto [contains, position] =
-      test_components::vector_contains(state_interfaces, "joint1/voltage");
+    auto [contains, position] = test_components::vector_contains(state_interfaces, "sens1/voltage");
     EXPECT_TRUE(contains);
-    EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name());
+    EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name());
     EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name());
-    EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name());
+    EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name());
     EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value()));
   }
 
   // Not updated because is is UNCONFIGURED
-  auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second;
+  auto si_sens1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second;
   sensor_hw.read(TIME, PERIOD);
-  EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_vol]->get_value()));
+  EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value()));
 
   // Updated because is is INACTIVE
   state = sensor_hw.configure();
   EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id());
   EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label());
-  EXPECT_EQ(0.0, state_interfaces[si_joint1_vol]->get_value());
+  EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value());
+
+  // It can read now
+  sensor_hw.read(TIME, PERIOD);
+  EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value());
+}
+
+TEST(TestComponentInterfaces, dummy_sensor_default_joint)
+{
+  hardware_interface::Sensor sensor_hw(
+    std::make_unique<test_components::DummySensorJointDefault>());
+
+  const std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head) +
+    ros2_control_test_assets::valid_urdf_ros2_control_joint_voltage_sensor +
+    ros2_control_test_assets::urdf_tail;
+  const std::vector<hardware_interface::HardwareInfo> control_resources =
+    hardware_interface::parse_control_resources_from_urdf(urdf_to_test);
+  const hardware_interface::HardwareInfo sensor_res = control_resources[0];
+  rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_system_components");
+  auto state =
+    sensor_hw.initialize(sensor_res, node->get_logger(), node->get_node_clock_interface());
+  ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
+  ASSERT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
+
+  auto state_interfaces = sensor_hw.export_state_interfaces();
+  ASSERT_EQ(2u, state_interfaces.size());
+
+  auto [contains_sens1_vol, si_sens1_vol] =
+    test_components::vector_contains(state_interfaces, "sens1/voltage");
+  ASSERT_TRUE(contains_sens1_vol);
+  EXPECT_EQ("sens1/voltage", state_interfaces[si_sens1_vol]->get_name());
+  EXPECT_EQ("voltage", state_interfaces[si_sens1_vol]->get_interface_name());
+  EXPECT_EQ("sens1", state_interfaces[si_sens1_vol]->get_prefix_name());
+  EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value()));
+
+  auto [contains_joint1_pos, si_joint1_pos] =
+    test_components::vector_contains(state_interfaces, "joint1/position");
+  ASSERT_TRUE(contains_joint1_pos);
+  EXPECT_EQ("joint1/position", state_interfaces[si_joint1_pos]->get_name());
+  EXPECT_EQ("position", state_interfaces[si_joint1_pos]->get_interface_name());
+  EXPECT_EQ("joint1", state_interfaces[si_joint1_pos]->get_prefix_name());
+  EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value()));
+
+  // Not updated because is is UNCONFIGURED
+  sensor_hw.read(TIME, PERIOD);
+  EXPECT_TRUE(std::isnan(state_interfaces[si_sens1_vol]->get_value()));
+  EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value()));
+
+  // Updated because is is INACTIVE
+  state = sensor_hw.configure();
+  ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id());
+  ASSERT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label());
+  EXPECT_EQ(0.0, state_interfaces[si_sens1_vol]->get_value());
+  EXPECT_EQ(10.0, state_interfaces[si_joint1_pos]->get_value());
 
   // It can read now
   sensor_hw.read(TIME, PERIOD);
-  EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value());
+  EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_value());
+  EXPECT_EQ(0x777, state_interfaces[si_joint1_pos]->get_value());
 }
 
 // BEGIN (Handle export change): for backward compatibility
@@ -1708,7 +1821,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior)
   EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
 
   // activate again and expect reset values
-  auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second;
+  auto si_joint1_vol = test_components::vector_contains(state_interfaces, "sens1/voltage").second;
   state = sensor_hw.configure();
   EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0);
 

hardware_interface/test/test_component_interfaces_custom_export.cpp

@@ -245,21 +245,20 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export)
   auto state_interfaces = sensor_hw.export_state_interfaces();
   ASSERT_EQ(2u, state_interfaces.size());
   {
-    auto [contains, position] =
-      test_components::vector_contains(state_interfaces, "joint1/voltage");
-    EXPECT_TRUE(contains);
-    EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name());
+    auto [contains, position] = test_components::vector_contains(state_interfaces, "sens1/voltage");
+    ASSERT_TRUE(contains);
+    EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name());
     EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name());
-    EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name());
+    EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name());
     EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value()));
   }
   {
     auto [contains, position] =
-      test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface");
-    EXPECT_TRUE(contains);
-    EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name());
+      test_components::vector_contains(state_interfaces, "sens1/some_unlisted_interface");
+    ASSERT_TRUE(contains);
+    EXPECT_EQ("sens1/some_unlisted_interface", state_interfaces[position]->get_name());
     EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name());
-    EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name());
+    EXPECT_EQ("sens1", state_interfaces[position]->get_prefix_name());
   }
 }
 

ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp

@@ -636,7 +636,24 @@ const auto valid_urdf_ros2_control_voltage_sensor_only =
       <plugin>ros2_control_demo_hardware/SingleJointVoltageSensor</plugin>
       <param name="example_param_read_for_sec">2</param>
     </hardware>
-    <sensor name="joint1">
+    <sensor name="sens1">
+      <state_interface name="voltage" initial_value="0.0"/>
+    </sensor>
+  </ros2_control>
+)";
+
+// Joint+Voltage Sensor
+const auto valid_urdf_ros2_control_joint_voltage_sensor =
+  R"(
+  <ros2_control name="SingleJointVoltage" type="sensor">
+    <hardware>
+      <plugin>ros2_control_demo_hardware/SingleJointVoltageSensor</plugin>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <joint name="joint1">
+      <state_interface name="position"/>
+    </joint>
+    <sensor name="sens1">
       <state_interface name="voltage" initial_value="0.0"/>
     </sensor>
   </ros2_control>