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

Add new get_value API for Handles and Interfaces #1976

Open
wants to merge 11 commits into
base: master
Choose a base branch
from
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
{
if (existing_axes_[i])
{
forces[i] = state_interfaces_[interface_counter].get().get_value();
forces[i] = state_interfaces_[interface_counter].get().get_value<double>().value();
++interface_counter;
}
}
Expand All @@ -123,7 +123,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
{
if (existing_axes_[i + FORCES_SIZE])
{
torques[i] = state_interfaces_[torque_interface_counter].get().get_value();
torques[i] = state_interfaces_[torque_interface_counter].get().get_value<double>().value();
++torque_interface_counter;
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
std::array<double, 4> orientation;
for (auto i = 0u; i < orientation.size(); ++i)
{
orientation[i] = state_interfaces_[i].get().get_value();
orientation[i] = state_interfaces_[i].get().get_value<double>().value();
}
return orientation;
}
Expand All @@ -69,7 +69,8 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
const std::size_t interface_offset{4};
for (auto i = 0u; i < angular_velocity.size(); ++i)
{
angular_velocity[i] = state_interfaces_[interface_offset + i].get().get_value();
angular_velocity[i] =
state_interfaces_[interface_offset + i].get().get_value<double>().value();
}
return angular_velocity;
}
Expand All @@ -86,7 +87,8 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
const std::size_t interface_offset{7};
for (auto i = 0u; i < linear_acceleration.size(); ++i)
{
linear_acceleration[i] = state_interfaces_[interface_offset + i].get().get_value();
linear_acceleration[i] =
state_interfaces_[interface_offset + i].get().get_value<double>().value();
}
return linear_acceleration;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ class PoseSensor : public SemanticComponentInterface<geometry_msgs::msg::Pose>
std::array<double, 3> position;
for (auto i = 0u; i < position.size(); ++i)
{
position[i] = state_interfaces_[i].get().get_value();
position[i] = state_interfaces_[i].get().get_value<double>().value();
}
return position;
}
Expand All @@ -67,7 +67,7 @@ class PoseSensor : public SemanticComponentInterface<geometry_msgs::msg::Pose>
const std::size_t interface_offset{3};
for (auto i = 0u; i < orientation.size(); ++i)
{
orientation[i] = state_interfaces_[interface_offset + i].get().get_value();
orientation[i] = state_interfaces_[interface_offset + i].get().get_value<double>().value();
}
return orientation;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ class RangeSensor : public SemanticComponentInterface<sensor_msgs::msg::Range>
*
* \return value of the range in meters
*/
float get_range() const { return state_interfaces_[0].get().get_value(); }
float get_range() const { return state_interfaces_[0].get().get_value<double>().value(); }

/// Return Range message with range in meters
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ class SemanticComponentInterface
// insert all the values
for (auto i = 0u; i < state_interfaces_.size(); ++i)
{
values.emplace_back(state_interfaces_[i].get().get_value());
values.emplace_back(state_interfaces_[i].get().get_value<double>().value());
}
return true;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces)
EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME);
EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state");

EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE);
EXPECT_EQ(
exported_state_interfaces[0]->get_value<double>().value(), EXPORTED_STATE_INTERFACE_VALUE);
}

TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
Expand All @@ -72,7 +73,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME);
EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf");

EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE);
EXPECT_EQ(reference_interfaces[0]->get_value<double>().value(), INTERFACE_VALUE);
}

TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name)
Expand Down Expand Up @@ -120,7 +121,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
EXPECT_FALSE(controller.is_in_chained_mode());

// Fail setting chained mode
EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE);
EXPECT_EQ(reference_interfaces[0]->get_value<double>().value(), INTERFACE_VALUE);

EXPECT_FALSE(controller.set_chained_mode(true));
EXPECT_FALSE(controller.is_in_chained_mode());
Expand All @@ -129,11 +130,13 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
EXPECT_FALSE(controller.is_in_chained_mode());

// Success setting chained mode
reference_interfaces[0]->set_value(0.0);
(void)reference_interfaces[0]->set_value(0.0);

EXPECT_TRUE(controller.set_chained_mode(true));
EXPECT_TRUE(controller.is_in_chained_mode());
EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE);
EXPECT_EQ(
exported_state_interfaces[0]->get_value<double>().value(),
EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE);

controller.configure();
EXPECT_TRUE(controller.set_chained_mode(false));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -101,21 +101,23 @@ controller_interface::return_type TestChainableController::update_and_write_comm

for (size_t i = 0; i < command_interfaces_.size(); ++i)
{
command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value());
(void)command_interfaces_[i].set_value(
reference_interfaces_[i] - state_interfaces_[i].get_value<double>().value());
}
// If there is a command interface then integrate and set it to the exported state interface data
for (size_t i = 0; i < exported_state_interface_names_.size() && i < command_interfaces_.size();
++i)
{
state_interfaces_values_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT;
state_interfaces_values_[i] =
command_interfaces_[i].get_value<double>().value() * CONTROLLER_DT;
}
// If there is no command interface and if there is a state interface then just forward the same
// value as in the state interface
for (size_t i = 0; i < exported_state_interface_names_.size() && i < state_interfaces_.size() &&
command_interfaces_.empty();
++i)
{
state_interfaces_values_[i] = state_interfaces_[i].get_value();
state_interfaces_values_[i] = state_interfaces_[i].get_value<double>().value();
}

return controller_interface::return_type::OK;
Expand Down Expand Up @@ -240,7 +242,7 @@ std::vector<double> TestChainableController::get_state_interface_data() const
std::vector<double> state_intr_data;
for (const auto & interface : state_interfaces_)
{
state_intr_data.push_back(interface.get_value());
state_intr_data.push_back(interface.get_value<double>().value());
}
return state_intr_data;
}
Expand Down
6 changes: 3 additions & 3 deletions controller_manager/test/test_controller/test_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ controller_interface::return_type TestController::update(
// set value to hardware to produce and test different behaviors there
if (!std::isnan(set_first_command_interface_value_to))
{
command_interfaces_[0].set_value(set_first_command_interface_value_to);
(void)command_interfaces_[0].set_value(set_first_command_interface_value_to);
// reset to be easier to test
set_first_command_interface_value_to = std::numeric_limits<double>::quiet_NaN();
}
Expand All @@ -90,7 +90,7 @@ controller_interface::return_type TestController::update(
RCLCPP_DEBUG(
get_node()->get_logger(), "Setting value of command interface '%s' to %f",
command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]);
command_interfaces_[i].set_value(external_commands_for_testing_[i]);
(void)command_interfaces_[i].set_value(external_commands_for_testing_[i]);
}
}

Expand Down Expand Up @@ -187,7 +187,7 @@ std::vector<double> TestController::get_state_interface_data() const
std::vector<double> state_intr_data;
for (const auto & interface : state_interfaces_)
{
state_intr_data.push_back(interface.get_value());
state_intr_data.push_back(interface.get_value<double>().value());
}
return state_intr_data;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -520,8 +520,12 @@ class TestControllerChainingWithControllerManager
// Command of DiffDrive controller are references of PID controllers
EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE);
EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE);
ASSERT_EQ(diff_drive_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_REF);
ASSERT_EQ(diff_drive_controller->command_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_REF);
ASSERT_EQ(
diff_drive_controller->command_interfaces_[0].get_value<double>().value(),
EXP_LEFT_WHEEL_REF);
ASSERT_EQ(
diff_drive_controller->command_interfaces_[1].get_value<double>().value(),
EXP_RIGHT_WHEEL_REF);
ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF);
ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF);

Expand All @@ -537,21 +541,32 @@ class TestControllerChainingWithControllerManager

EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE);
EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD);
ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD);
ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
ASSERT_EQ(
pid_left_wheel_controller->command_interfaces_[0].get_value<double>().value(),
EXP_LEFT_WHEEL_CMD);
ASSERT_EQ(
pid_left_wheel_controller->state_interfaces_[0].get_value<double>().value(),
EXP_LEFT_WHEEL_HW_STATE);
// DiffDrive uses the same state
ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
ASSERT_EQ(
diff_drive_controller->state_interfaces_[0].get_value<double>().value(),
EXP_LEFT_WHEEL_HW_STATE);
// The state doesn't change wrt to any data from the hardware calculation
ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X);
ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X);

EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE);
EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD);
ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD);
ASSERT_EQ(
pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
pid_right_wheel_controller->command_interfaces_[0].get_value<double>().value(),
EXP_RIGHT_WHEEL_CMD);
ASSERT_EQ(
pid_right_wheel_controller->state_interfaces_[0].get_value<double>().value(),
EXP_RIGHT_WHEEL_HW_STATE);
// DiffDrive uses the same state
ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
ASSERT_EQ(
diff_drive_controller->state_interfaces_[1].get_value<double>().value(),
EXP_RIGHT_WHEEL_HW_STATE);
// The state doesn't change wrt to any data from the hardware calculation
ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y);
ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y);
Expand Down Expand Up @@ -811,10 +826,16 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers)
EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE);
// 32 / 2
EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD);
ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD);
ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
ASSERT_EQ(
pid_left_wheel_controller->command_interfaces_[0].get_value<double>().value(),
EXP_LEFT_WHEEL_CMD);
ASSERT_EQ(
pid_left_wheel_controller->state_interfaces_[0].get_value<double>().value(),
EXP_LEFT_WHEEL_HW_STATE);
// DiffDrive uses the same state
ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
ASSERT_EQ(
diff_drive_controller->state_interfaces_[0].get_value<double>().value(),
EXP_LEFT_WHEEL_HW_STATE);
// The state doesn't change wrt to any data from the hardware calculation
ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X);
ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X);
Expand All @@ -823,13 +844,19 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers)
EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE);
// 128 / 2
EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD);
ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD);
ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
ASSERT_EQ(
pid_right_wheel_controller->command_interfaces_[0].get_value<double>().value(),
EXP_RIGHT_WHEEL_CMD);
ASSERT_EQ(
pid_right_wheel_controller->state_interfaces_[0].get_value<double>().value(),
EXP_RIGHT_WHEEL_HW_STATE);
ASSERT_EQ(odom_publisher_controller->internal_counter, 2u);
ASSERT_EQ(sensor_fusion_controller->internal_counter, 6u);
ASSERT_EQ(robot_localization_controller->internal_counter, 4u);
// DiffDrive uses the same state
ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
ASSERT_EQ(
diff_drive_controller->state_interfaces_[1].get_value<double>().value(),
EXP_RIGHT_WHEEL_HW_STATE);
// The state doesn't change wrt to any data from the hardware calculation
ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y);
ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y);
Expand Down Expand Up @@ -1986,19 +2013,31 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add
EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE);
// 32 / 2
EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD);
ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD);
ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
ASSERT_EQ(
pid_left_wheel_controller->command_interfaces_[0].get_value<double>().value(),
EXP_LEFT_WHEEL_CMD);
ASSERT_EQ(
pid_left_wheel_controller->state_interfaces_[0].get_value<double>().value(),
EXP_LEFT_WHEEL_HW_STATE);
// DiffDrive uses the same state
ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
ASSERT_EQ(
diff_drive_controller->state_interfaces_[0].get_value<double>().value(),
EXP_LEFT_WHEEL_HW_STATE);

// 128 - 0
EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE);
// 128 / 2
EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD);
ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD);
ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
ASSERT_EQ(
pid_right_wheel_controller->command_interfaces_[0].get_value<double>().value(),
EXP_RIGHT_WHEEL_CMD);
ASSERT_EQ(
pid_right_wheel_controller->state_interfaces_[0].get_value<double>().value(),
EXP_RIGHT_WHEEL_HW_STATE);
// DiffDrive uses the same state
ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
ASSERT_EQ(
diff_drive_controller->state_interfaces_[1].get_value<double>().value(),
EXP_RIGHT_WHEEL_HW_STATE);

// update all controllers at once and see that all have expected values --> also checks the order
// of controller execution
Expand Down
26 changes: 22 additions & 4 deletions hardware_interface/include/hardware_interface/handle.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <limits>
#include <memory>
#include <mutex>
#include <optional>
#include <shared_mutex>
#include <string>
#include <utility>
Expand Down Expand Up @@ -142,7 +143,9 @@ class Handle

const std::string & get_prefix_name() const { return prefix_name_; }

[[deprecated("Use bool get_value(double & value) instead to retrieve the value.")]]
[[deprecated(
"Use std::optional<T> get_value() or bool get_value(double & "
"value) instead to retrieve the value.")]]
double get_value() const
{
std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
Expand All @@ -157,7 +160,23 @@ class Handle
// END
}

[[nodiscard]] bool get_value(double & value) const
template <typename T = double>
[[nodiscard]] std::optional<T> get_value() const
{
std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
if (!lock.owns_lock())
{
return std::nullopt;
}
THROW_ON_NULLPTR(this->value_ptr_);
// TODO(saikishor): uncomment the following line when HANDLE_DATATYPE is used with multiple
// datatypes and remove the line below it.
// return value_ptr_ != nullptr ? *value_ptr_ : std::get<T>(value_);
return *value_ptr_;
}

template <typename T>
[[nodiscard]] bool get_value(T & value) const
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we actually need this format of get_value? Are we using this somewhere of do you have specific use-case where this has advantages over std::optional<T> get_value()?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Right now, we are not using it in ros2_control repositories, but this exists from many releases both in rolling nd jazzy. So, at least not to break API of people that might use it, I haven't removed it here

{
std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
if (!lock.owns_lock())
Expand All @@ -166,8 +185,7 @@ class Handle
}
// BEGIN (Handle export change): for backward compatibility
// TODO(Manuel) set value directly if old functionality is removed
THROW_ON_NULLPTR(value_ptr_);
value = *value_ptr_;
value = value_ptr_ != nullptr ? *value_ptr_ : std::get<T>(value_);
return true;
// END
}
Expand Down
Loading
Loading