Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Let sensors also export state interfaces of joints #1885

Merged
merged 10 commits into from
Dec 9, 2024
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
Expand Down Expand Up @@ -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)
Expand All @@ -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;
}

Expand Down Expand Up @@ -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_;

Expand Down
5 changes: 2 additions & 3 deletions hardware_interface/src/resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down
145 changes: 130 additions & 15 deletions hardware_interface/test/test_component_interfaces.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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
Expand All @@ -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;
}

Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -948,29 +1007,85 @@ 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("sens1", state_interfaces[position]->get_prefix_name());
EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value()));
}

// Not updated because is is UNCONFIGURED
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_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_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::Logger logger = rclcpp::get_logger("test_sensor_component");
auto state = sensor_hw.initialize(sensor_res, logger, nullptr);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_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, position] = test_components::vector_contains(state_interfaces, "sens1/voltage");
EXPECT_TRUE(contains);
EXPECT_EQ("sens1/voltage", state_interfaces[position]->get_name());
EXPECT_EQ("voltage", state_interfaces[position]->get_interface_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/position");
EXPECT_TRUE(contains);
EXPECT_EQ("joint1/position", state_interfaces[position]->get_name());
EXPECT_EQ("position", state_interfaces[position]->get_interface_name());
EXPECT_EQ("joint1", 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;
auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").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()));
EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->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());
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
Expand Down Expand Up @@ -1708,7 +1823,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);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Loading