continue adapting controllers

ackermann_steering_controller/src/ackermann_steering_controller.cpp

@@ -63,11 +63,13 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
  else
  {
  const double traction_right_wheel_value =
- state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
+ state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value<double>();
  const double traction_left_wheel_value =
- state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
- const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
- const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
+ state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value<double>();
+ const double steering_right_position =
+ state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value<double>();
+ const double steering_left_position =
+ state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value<double>();
  if (
  std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
  std::isfinite(steering_right_position) && std::isfinite(steering_left_position))

bicycle_steering_controller/src/bicycle_steering_controller.cpp

@@ -60,8 +60,8 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
  }
  else
  {
- const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
- const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
+ const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value<double>();
+ const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value<double>();
  if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position))
  {
  if (params_.position_feedback)

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -453,15 +453,15 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto
  {
  for (size_t index = 0; index < dof_; ++index)
  {
- trajectory_point_interface[index] = joint_interface[index].get().get_value<double>();
+ trajectory_point_interface[index] = joint_interface[index].get().get_value();
  }
  };
 
  auto interface_has_values = [](const auto & joint_interface)
  {
  return std::find_if(
  joint_interface.begin(), joint_interface.end(), [](const auto & interface)
- { return std::isnan(interface.get().get_value<double>()); }) == joint_interface.end();
+ { return std::isnan(interface.get().get_value()); }) == joint_interface.end();
  };
 
  // Assign values from the command interfaces as state. Therefore needs check for both.
@@ -523,15 +523,15 @@ bool JointTrajectoryController::read_commands_from_command_interfaces(
  {
  for (size_t index = 0; index < dof_; ++index)
  {
- trajectory_point_interface[index] = joint_interface[index].get().get_value<double>();
+ trajectory_point_interface[index] = joint_interface[index].get().get_value();
  }
  };
 
  auto interface_has_values = [](const auto & joint_interface)
  {
  return std::find_if(
  joint_interface.begin(), joint_interface.end(), [](const auto & interface)
- { return std::isnan(interface.get().get_value<double>()); }) == joint_interface.end();
+ { return std::isnan(interface.get().get_value()); }) == joint_interface.end();
  };
 
  // Assign values from the command interfaces as command.

pid_controller/test/test_pid_controller.hpp

@@ -26,6 +26,7 @@
 #include <utility>
 #include <vector>
 
+#include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 #include "pid_controller/pid_controller.hpp"
@@ -70,7 +71,7 @@ class TestablePidController : public pid_controller::PidController
  controller_interface::CallbackReturn on_activate(
  const rclcpp_lifecycle::State & previous_state) override
  {
- auto ref_itfs = on_export_reference_interfaces();
+ auto ref_itfs = export_state_interface_descriptions();
  return pid_controller::PidController::on_activate(previous_state);
  }
 
@@ -111,8 +112,6 @@ class PidControllerFixture : public ::testing::Test
  dof_names_ = {"joint1"};
  command_interface_ = "position";
  state_interfaces_ = {"position"};
- dof_state_values_ = {1.1};
- dof_command_values_ = {101.101};
  reference_and_state_dof_names_ = {"joint1state"};
 
  // initialize controller
@@ -144,8 +143,10 @@ class PidControllerFixture : public ::testing::Test
 
  for (size_t i = 0; i < dof_names_.size(); ++i)
  {
- command_itfs_.emplace_back(hardware_interface::CommandInterface(
- dof_names_[i], command_interface_, &dof_command_values_[i]));
+ command_itfs_.emplace_back(
+ hardware_interface::CommandInterface(hardware_interface::InterfaceDescription(
+ dof_names_[i],
+ hardware_interface::InterfaceInfo(command_interface_, "double", "101.101"))));
  command_ifs.emplace_back(command_itfs_.back());
  }
 
@@ -158,7 +159,8 @@ class PidControllerFixture : public ::testing::Test
  for (const auto & dof_name : dof_names_)
  {
  state_itfs_.emplace_back(
- hardware_interface::StateInterface(dof_name, interface, &dof_state_values_[index]));
+ hardware_interface::StateInterface(hardware_interface::InterfaceDescription(
+ dof_name, hardware_interface::InterfaceInfo(interface, "double", "1.1"))));
  state_ifs.emplace_back(state_itfs_.back());
  ++index;
  }
@@ -264,8 +266,6 @@ class PidControllerFixture : public ::testing::Test
  std::vector<std::string> dof_names_;
  std::string command_interface_;
  std::vector<std::string> state_interfaces_;
- std::vector<double> dof_state_values_;
- std::vector<double> dof_command_values_;
  std::vector<std::string> reference_and_state_dof_names_;
 
  std::vector<hardware_interface::StateInterface> state_itfs_;

pid_controller/test/test_pid_controller_preceding.cpp

@@ -54,15 +54,15 @@ TEST_F(PidControllerTest, check_exported_interfaces)
  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
  auto cmd_if_conf = controller_->command_interface_configuration();
- ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size());
+ ASSERT_EQ(cmd_if_conf.names.size(), command_itfs_.size());
  for (size_t i = 0; i < cmd_if_conf.names.size(); ++i)
  {
  EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_);
  }
  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
 
  auto state_if_conf = controller_->state_interface_configuration();
- ASSERT_EQ(state_if_conf.names.size(), dof_state_values_.size());
+ ASSERT_EQ(state_if_conf.names.size(), state_itfs_.size());
  size_t si_index = 0;
  for (const auto & interface : state_interfaces_)
  {
@@ -76,17 +76,17 @@ TEST_F(PidControllerTest, check_exported_interfaces)
 
  // check ref itfs
  auto ref_if_conf = controller_->export_reference_interfaces();
- ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size());
+ ASSERT_EQ(ref_if_conf.size(), state_itfs_.size());
  size_t ri_index = 0;
  for (const auto & interface : state_interfaces_)
  {
  for (const auto & dof_name : reference_and_state_dof_names_)
  {
  const std::string ref_itf_name =
  std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface;
- EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name);
- EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name());
- EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface);
+ EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name);
+ EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name());
+ EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface);
  ++ri_index;
  }
  }

steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp

@@ -104,6 +104,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
  std::unique_ptr<ControllerStatePublisherOdom> rt_odom_state_publisher_;
  std::unique_ptr<ControllerStatePublisherTf> rt_tf_odom_state_publisher_;
 
+ std::vector<hardware_interface::InterfaceDescription> export_state_interface_descriptions()
+ override;
  // override methods from ChainableControllerInterface
  std::vector<hardware_interface::InterfaceDescription> export_reference_interface_descriptions()
  override;

steering_controllers_library/src/steering_controllers_library.cpp

@@ -311,6 +311,12 @@ SteeringControllersLibrary::state_interface_configuration() const
  return state_interfaces_config;
 }
 
+std::vector<hardware_interface::InterfaceDescription>
+SteeringControllersLibrary::export_state_interface_descriptions()
+{
+ return {};
+}
+
 std::vector<hardware_interface::InterfaceDescription>
 SteeringControllersLibrary::export_reference_interface_descriptions()
 {
@@ -368,16 +374,16 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f
  {
  if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z))
  {
- reference_interfaces_ptrs_[lin_ref_itf_]->set_value(current_ref->twist.linear.x);
- reference_interfaces_ptrs_[ang_ref_itf_]->set_value(current_ref->twist.angular.z);
+ reference_interfaces_[lin_ref_itf_]->set_value(current_ref->twist.linear.x);
+ reference_interfaces_[ang_ref_itf_]->set_value(current_ref->twist.angular.z);
  }
  }
  else
  {
  if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z))
  {
- reference_interfaces_ptrs_[lin_ref_itf_]->set_value(0.0);
- reference_interfaces_ptrs_[ang_ref_itf_]->set_value(0.0);
+ reference_interfaces_[lin_ref_itf_]->set_value(0.0);
+ reference_interfaces_[ang_ref_itf_]->set_value(0.0);
  current_ref->twist.linear.x = std::numeric_limits<double>::quiet_NaN();
  current_ref->twist.angular.z = std::numeric_limits<double>::quiet_NaN();
  }
@@ -397,12 +403,12 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
  // TODO(destogl): add limiter for the velocities
 
  if (
- !std::isnan(reference_interfaces_ptrs_[lin_ref_itf_]->get_value<double>()) &&
- !std::isnan(reference_interfaces_ptrs_[ang_ref_itf_]->get_value<double>()))
+ !std::isnan(reference_interfaces_[lin_ref_itf_]->get_value<double>()) &&
+ !std::isnan(reference_interfaces_[ang_ref_itf_]->get_value<double>()))
  {
  // store (for open loop odometry) and set commands
- last_linear_velocity_ = reference_interfaces_ptrs_[lin_ref_itf_]->get_value<double>();
- last_angular_velocity_ = reference_interfaces_ptrs_[ang_ref_itf_]->get_value<double>();
+ last_linear_velocity_ = reference_interfaces_[lin_ref_itf_]->get_value<double>();
+ last_angular_velocity_ = reference_interfaces_[ang_ref_itf_]->get_value<double>();
 
  auto [traction_commands, steering_commands] =
  odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop);
@@ -486,30 +492,30 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
  if (params_.position_feedback)
  {
  controller_state_publisher_->msg_.traction_wheels_position.push_back(
- state_interfaces_[i].get_value());
+ state_interfaces_[i].get_value<double>());
  }
  else
  {
  controller_state_publisher_->msg_.traction_wheels_velocity.push_back(
- state_interfaces_[i].get_value());
+ state_interfaces_[i].get_value<double>());
  }
  controller_state_publisher_->msg_.linear_velocity_command.push_back(
- command_interfaces_[i].get_value());
+ command_interfaces_[i].get_value<double>());
  }
 
  for (size_t i = 0; i < number_of_steering_wheels; ++i)
  {
  controller_state_publisher_->msg_.steer_positions.push_back(
- state_interfaces_[number_of_traction_wheels + i].get_value());
+ state_interfaces_[number_of_traction_wheels + i].get_value<double>());
  controller_state_publisher_->msg_.steering_angle_command.push_back(
- command_interfaces_[number_of_traction_wheels + i].get_value());
+ command_interfaces_[number_of_traction_wheels + i].get_value<double>());
  }
 
  controller_state_publisher_->unlockAndPublish();
  }
 
- reference_interfaces_ptrs_[lin_ref_itf_]->set_value(std::numeric_limits<double>::quiet_NaN());
- reference_interfaces_ptrs_[ang_ref_itf_]->set_value(std::numeric_limits<double>::quiet_NaN());
+ reference_interfaces_[lin_ref_itf_]->set_value(std::numeric_limits<double>::quiet_NaN());
+ reference_interfaces_[ang_ref_itf_]->set_value(std::numeric_limits<double>::quiet_NaN());
 
  return controller_interface::return_type::OK;
 }

steering_controllers_library/test/test_steering_controllers_library.cpp

@@ -91,9 +91,11 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
  ASSERT_FALSE(controller_->is_in_chained_mode());
 
- for (const auto & interface : controller_->reference_interfaces_)
+ for (const auto & interface : controller_->ordered_reference_interfaces_)
  {
- EXPECT_TRUE(std::isnan(interface));
+ EXPECT_TRUE(interface->holds_value());
+ EXPECT_TRUE(interface->operator bool());
+ EXPECT_TRUE(std::isnan(interface->get_value<double>()));
  }
 
  // set command statically
@@ -127,24 +129,25 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
  controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
  controller_interface::return_type::OK);
 
- EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
- EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1]));
- for (const auto & interface : controller_->reference_interfaces_)
+ for (const auto & interface : controller_->ordered_reference_interfaces_)
  {
- EXPECT_TRUE(std::isnan(interface));
+ EXPECT_TRUE(interface->holds_value());
+ EXPECT_TRUE(interface->operator bool());
+ EXPECT_TRUE(std::isnan(interface->get_value<double>()));
  }
  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x));
  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z));
 
- EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
- for (const auto & interface : controller_->reference_interfaces_)
+ for (const auto & interface : controller_->ordered_reference_interfaces_)
  {
- EXPECT_TRUE(std::isnan(interface));
+ EXPECT_TRUE(interface->holds_value());
+ EXPECT_TRUE(interface->operator bool());
+ EXPECT_TRUE(std::isnan(interface->get_value<double>()));
  }
 
  for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i)
  {
- EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0);
+ EXPECT_EQ(controller_->command_interfaces_[i].get_value<double>(), 0.0);
  }
 
  // case 2 position_feedback = true
@@ -168,24 +171,25 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
  controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
  controller_interface::return_type::OK);
 
- EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
- EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1]));
- for (const auto & interface : controller_->reference_interfaces_)
+ for (const auto & interface : controller_->ordered_reference_interfaces_)
  {
- EXPECT_TRUE(std::isnan(interface));
+ EXPECT_TRUE(interface->holds_value());
+ EXPECT_TRUE(interface->operator bool());
+ EXPECT_TRUE(std::isnan(interface->get_value<double>()));
  }
  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x));
  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z));
 
- EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
- for (const auto & interface : controller_->reference_interfaces_)
+ for (const auto & interface : controller_->ordered_reference_interfaces_)
  {
- EXPECT_TRUE(std::isnan(interface));
+ EXPECT_TRUE(interface->holds_value());
+ EXPECT_TRUE(interface->operator bool());
+ EXPECT_TRUE(std::isnan(interface->get_value<double>()));
  }
 
  for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i)
  {
- EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0);
+ EXPECT_EQ(controller_->command_interfaces_[i].get_value<double>(), 0);
  }
 }
 

steering_controllers_library/test/test_steering_controllers_library.hpp

@@ -15,8 +15,6 @@
 #ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_
 #define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_
 
-#include <gmock/gmock.h>
-
 #include <chrono>
 #include <memory>
 #include <string>
@@ -88,7 +86,7 @@ class TestableSteeringControllersLibrary
  controller_interface::CallbackReturn on_activate(
  const rclcpp_lifecycle::State & previous_state) override
  {
- auto ref_itfs = on_export_reference_interfaces();
+ auto ref_itf_descriptions = export_reference_interface_descriptions();
  return steering_controllers_library::SteeringControllersLibrary::on_activate(previous_state);
  }
 

tricycle_steering_controller/src/tricycle_steering_controller.cpp

@@ -61,10 +61,10 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period
  else
  {
  const double traction_right_wheel_value =
- state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
+ state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value<double>();
  const double traction_left_wheel_value =
- state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
- const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
+ state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value<double>();
+ const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value<double>();
  if (
  std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
  std::isfinite(steering_position))

tricycle_steering_controller/test/test_tricycle_steering_controller.cpp

@@ -75,7 +75,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces)
  EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
 
  // check ref itfs
- auto ref_if_conf = controller_->export_reference_interfaces();
+ auto ref_if_conf = controller_->export_reference_interface_descriptions();
  ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size());
  for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
  {

tricycle_steering_controller/test/test_tricycle_steering_controller.hpp

@@ -79,7 +79,7 @@ class TestableTricycleSteeringController
  controller_interface::CallbackReturn on_activate(
  const rclcpp_lifecycle::State & previous_state) override
  {
- auto ref_itfs = on_export_reference_interfaces();
+ auto ref_itfs = export_reference_interface_descriptions();
  return tricycle_steering_controller::TricycleSteeringController::on_activate(previous_state);
  }