Skip to content

Commit

Permalink
remove old functionallity and continue adapting tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@mamueluth
mamueluth committed Aug 2, 2024
1 parent 6303e83 commit 395c50c
Show file tree
Hide file tree
Showing 32 changed files with 1,264 additions and 934 deletions.
25 changes: 13 additions & 12 deletions controller_interface/include/controller_interface/chainable_controller_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,9 +80,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase
* exported. The method has the same meaning as `export_state_interfaces` method from
* hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
*
* \returns list of StateInterfaces that other controller can use as their inputs.
* \returns list of StateInterfacesDescriptions that other controller can use as their inputs.
*/
virtual std::vector<hardware_interface::StateInterface> on_export_state_interfaces();
virtual std::vector<hardware_interface::InterfaceDescription>
export_state_interface_descriptions() = 0;

/// Virtual method that each chainable controller should implement to export its read/write
/// chainable interfaces.
Expand All @@ -91,9 +92,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase
* exported. The method has the same meaning as `export_command_interface` method from
* hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
*
* \returns list of CommandInterfaces that other controller can use as their outputs.
* \returns list of CommandInterfacesDescriptions that other controller can use as their outputs.
*/
virtual std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces();
virtual std::vector<hardware_interface::InterfaceDescription>
export_reference_interface_descriptions() = 0;

/// Virtual method that each chainable controller should implement to switch chained mode.
/**
Expand Down Expand Up @@ -133,18 +135,17 @@ class ChainableControllerInterface : public ControllerInterfaceBase
virtual return_type update_and_write_commands(
const rclcpp::Time & time, const rclcpp::Duration & period) = 0;

/// Storage of values for state interfaces
// interface_names are in order they have been exported
std::vector<std::string> exported_state_interface_names_;
std::vector<double> state_interfaces_values_;
// storage for the exported StateInterfaces
std::unordered_map<std::string, std::shared_ptr<hardware_interface::StateInterface>>
exported_state_interfaces_;

/// Storage of values for reference interfaces
// interface_names are in order they have been exported
std::vector<std::string> exported_reference_interface_names_;
// BEGIN (Handle export change): for backward compatibility
std::vector<double> reference_interfaces_;
std::unordered_map<std::string, std::reference_wrapper<double>> ref_interface_to_value_;
// END
// storage for the exported CommandInterfaces
std::unordered_map<std::string, std::shared_ptr<hardware_interface::CommandInterface>>
reference_interfaces_ptrs_;
reference_interfaces_;

private:
/// A flag marking if a chainable controller is currently preceded by another controller.
Expand Down
4 changes: 2 additions & 2 deletions controller_interface/include/semantic_components/force_torque_sensor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,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>();
++interface_counter;
}
}
Expand All @@ -126,7 +126,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
{
if (existing_axes_[i])
{
torques_[i - 3] = state_interfaces_[torque_interface_counter].get().get_value();
torques_[i - 3] = state_interfaces_[torque_interface_counter].get().get_value<double>();
++torque_interface_counter;
}
}
Expand Down
6 changes: 3 additions & 3 deletions controller_interface/include/semantic_components/imu_sensor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
size_t interface_offset = 0;
for (size_t i = 0; 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>();
}
return orientation_;
}
Expand All @@ -75,7 +75,7 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
size_t interface_offset = orientation_.size();
for (size_t i = 0; 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>();
}
return angular_velocity_;
}
Expand All @@ -91,7 +91,7 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
size_t interface_offset = orientation_.size() + angular_velocity_.size();
for (size_t i = 0; 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>();
}
return linear_acceleration_;
}
Expand Down
2 changes: 1 addition & 1 deletion controller_interface/include/semantic_components/semantic_component_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ class SemanticComponentInterface
// insert all the values
for (size_t i = 0; i < state_interfaces_.size(); ++i)
{
values.emplace_back(state_interfaces_[i].get().get_value());
values.emplace_back(state_interfaces_[i].get().get_value<double>());
}
return true;
}
Expand Down
149 changes: 72 additions & 77 deletions controller_interface/src/chainable_controller_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,101 +47,122 @@ return_type ChainableControllerInterface::update(
std::vector<std::shared_ptr<hardware_interface::StateInterface>>
ChainableControllerInterface::export_state_interfaces()
{
auto state_interfaces = on_export_state_interfaces();
auto state_interfaces_descr = export_state_interface_descriptions();
std::vector<std::shared_ptr<hardware_interface::StateInterface>> state_interfaces_ptrs_vec;
state_interfaces_ptrs_vec.reserve(state_interfaces.size());
state_interfaces_ptrs_vec.reserve(state_interfaces_descr.size());

// check if the names of the controller state interfaces begin with the controller's name
for (auto & interface : state_interfaces)
for (auto & descr : state_interfaces_descr)
{
if (interface.get_prefix_name() != get_node()->get_name())
if (descr.prefix_name != get_node()->get_name())
{
std::string error_msg =
"The prefix of the interface '" + interface.get_prefix_name() +
"The prefix of the interface description'" + descr.prefix_name +
"' does not equal the controller's name '" + get_node()->get_name() +
"'. This is mandatory for state interfaces. No state interface will be exported. Please "
"correct and recompile the controller with name '" +
get_node()->get_name() + "' and try again.";
throw std::runtime_error(error_msg);
}

state_interfaces_ptrs_vec.push_back(
std::make_shared<hardware_interface::StateInterface>(interface));
auto state_interface = std::make_shared<hardware_interface::StateInterface>(descr);
const auto inteface_name = state_interface->get_name();
// check the exported interface name is unique
auto [it, succ] = exported_state_interfaces_.insert({inteface_name, state_interface});
// either we have name duplicate which we want to avoid under all circumstances since interfaces
// need to be uniquely identify able or something else really went wrong. In any case abort and
// inform cm by throwing exception
if (!succ)
{
std::string error_msg =
"Could not insert StateInterface<" + inteface_name +
"> into exported_state_interfaces_ map. Check if you export duplicates. The "
"map returned iterator with interface_name<" +
it->second->get_name() +
">. If its a duplicate adjust exportation of InterfacesDescription so that all the "
"interface names are unique.";
exported_state_interfaces_.clear();
exported_state_interface_names_.clear();
state_interfaces_ptrs_vec.clear();
throw std::runtime_error(error_msg);
}
exported_state_interface_names_.push_back(inteface_name);
state_interfaces_ptrs_vec.push_back(state_interface);
}

return state_interfaces_ptrs_vec;
}

std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
ChainableControllerInterface::export_reference_interfaces()
{
auto reference_interfaces = on_export_reference_interfaces();
std::vector<std::shared_ptr<hardware_interface::CommandInterface>> reference_interfaces_ptrs_vec;
reference_interfaces_ptrs_vec.reserve(reference_interfaces.size());
std::vector<std::shared_ptr<hardware_interface::CommandInterface>> reference_interfaces_ptrs_vec;
reference_interfaces_ptrs_vec.reserve(reference_interfaces.size());

// BEGIN (Handle export change): for backward compatibility
// BEGIN (Handle export change): for backward compatibility
// check if the "reference_interfaces_" variable is resized to number of interfaces
if (reference_interfaces_.size() != reference_interfaces.size())
if (exported_state_interfaces_.size() != state_interfaces_descr.size())
{
// TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the
// framework
std::string error_msg =
"The internal storage for reference values 'reference_interfaces_' variable has size '" +
std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" +
std::to_string(reference_interfaces.size()) +
"' equal to the number of exported reference interfaces. Please correct and recompile the "
"controller with name '" +
get_node()->get_name() + "' and try again.";
throw std::runtime_error(error_msg);
std::string error_msg =
"The internal storage for reference values 'reference_interfaces_' variable has size '" +
std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" +
std::to_string(reference_interfaces.size()) +
"The internal storage for reference ptrs 'exported_state_interfaces_' variable has size '" +
std::to_string(exported_state_interfaces_.size()) +
"', but it is expected to have the size '" + std::to_string(state_interfaces_descr.size()) +
"' equal to the number of exported reference interfaces. Please correct and recompile the "
"controller with name '" +
get_node()->get_name() + "' and try again.";
throw std::runtime_error(error_msg);
}
// END

return state_interfaces_ptrs_vec;
}

std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
ChainableControllerInterface::export_reference_interfaces()
{
auto reference_interface_descr = export_reference_interface_descriptions();
std::vector<std::shared_ptr<hardware_interface::CommandInterface>> reference_interfaces_ptrs_vec;
reference_interfaces_ptrs_vec.reserve(reference_interface_descr.size());

// check if the names of the reference interfaces begin with the controller's name
const auto ref_interface_size = reference_interfaces.size();
for (auto & interface : reference_interfaces)
for (auto & descr : reference_interface_descr)
{
auto & interface = reference_interfaces[i];
if (interface.get_prefix_name() != get_node()->get_name())
if (descr.prefix_name != get_node()->get_name())
{
std::string error_msg = "The name of the interface " + interface.get_name() +
std::string error_msg = "The name of the interface descr " + descr.prefix_name +
" does not begin with the controller's name. This is mandatory for "
"reference interfaces. Please "
"correct and recompile the controller with name " +
get_node()->get_name() + " and try again.";
throw std::runtime_error(error_msg);
}

std::shared_ptr<hardware_interface::CommandInterface> interface_ptr =
std::make_shared<hardware_interface::CommandInterface>(std::move(interface));
reference_interfaces_ptrs_vec.push_back(interface_ptr);
reference_interfaces_ptrs_.insert(std::make_pair(interface_ptr->get_name(), interface_ptr));
auto reference_interface = std::make_shared<hardware_interface::CommandInterface>(descr);
const auto inteface_name = reference_interface->get_name();
// check the exported interface name is unique
auto [it, succ] = reference_interfaces_.insert({inteface_name, reference_interface});
// either we have name duplicate which we want to avoid under all circumstances since interfaces
// need to be uniquely identify able or something else really went wrong. In any case abort and
// inform cm by throwing exception
if (!succ)
{
std::string error_msg =
"Could not insert Reference interface<" + inteface_name +
"> into reference_interfaces_ map. Check if you export duplicates. The "
"map returned iterator with interface_name<" +
it->second->get_name() +
">. If its a duplicate adjust exportation of InterfacesDescription so that all the "
"interface names are unique.";
reference_interfaces_.clear();
exported_reference_interface_names_.clear();
reference_interfaces_ptrs_vec.clear();
throw std::runtime_error(error_msg);
}
exported_reference_interface_names_.push_back(inteface_name);
reference_interfaces_ptrs_vec.push_back(reference_interface);
}

if (reference_interfaces_ptrs_.size() != ref_interface_size)
if (reference_interfaces_.size() != reference_interface_descr.size())
{
std::string error_msg =
"The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" +
std::to_string(reference_interfaces_ptrs_.size()) +
"', but it is expected to have the size '" + std::to_string(ref_interface_size) +
"The internal storage for reference ptrs 'reference_interfaces_' variable has size '" +
std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" +
std::to_string(reference_interface_descr.size()) +
"' equal to the number of exported reference interfaces. Please correct and recompile the "
"controller with name '" +
get_node()->get_name() + "' and try again.";
throw std::runtime_error(error_msg);
}

return reference_interfaces_ptrs_vec;
return reference_interfaces_ptrs_vec;
}

bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
Expand Down Expand Up @@ -173,30 +194,4 @@ bool ChainableControllerInterface::is_in_chained_mode() const { return in_chaine

bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; }

std::vector<hardware_interface::StateInterface>
ChainableControllerInterface::on_export_state_interfaces()
{
state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0);
std::vector<hardware_interface::StateInterface> state_interfaces;
for (size_t i = 0; i < exported_state_interface_names_.size(); ++i)
{
state_interfaces.emplace_back(
get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]);
}
return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
ChainableControllerInterface::on_export_reference_interfaces()
{
reference_interfaces_.resize(exported_reference_interface_names_.size(), 0.0);
std::vector<hardware_interface::CommandInterface> reference_interfaces;
for (size_t i = 0; i < exported_reference_interface_names_.size(); ++i)
{
reference_interfaces.emplace_back(hardware_interface::CommandInterface(
get_node()->get_name(), exported_reference_interface_names_[i], &reference_interfaces_[i]));
}
return reference_interfaces;
}

} // namespace controller_interface
Loading

0 comments on commit 395c50c

Please sign in to comment.