Skip to content

Commit

Permalink
add a remapped integration configuration method to ControllerSpec
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor committed Aug 9, 2024
1 parent 0b09028 commit 85aa876
Showing 1 changed file with 44 additions and 0 deletions.
44 changes: 44 additions & 0 deletions controller_manager/include/controller_manager/controller_spec.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,50 @@ struct ControllerSpec
hardware_interface::ControllerInfo info;
controller_interface::ControllerInterfaceBaseSharedPtr c;
std::shared_ptr<rclcpp::Time> next_update_cycle_time;

controller_interface::InterfaceConfiguration get_remapped_command_interface_configuration() const
{
return get_remapped_interface_configuration(
c->command_interface_configuration(), c->get_command_interfaces_remap());
}

controller_interface::InterfaceConfiguration get_remapped_state_interface_configuration() const
{
return get_remapped_interface_configuration(
c->state_interface_configuration(), c->get_state_interfaces_remap());
}

private:
controller_interface::InterfaceConfiguration get_remapped_interface_configuration(
const controller_interface::InterfaceConfiguration & interface_cfg,
const std::map<std::string, std::string> & remap) const
{
if (interface_cfg.type != controller_interface::interface_configuration_type::INDIVIDUAL)
{
return interface_cfg;
}
else
{
if (c->get_command_interfaces_remap().empty())
{
return interface_cfg;
}
else
{
controller_interface::InterfaceConfiguration remapped_cmd_itf_cfg = interface_cfg;
for (auto & [key, value] : remap)
{
auto it =
std::find(remapped_cmd_itf_cfg.names.begin(), remapped_cmd_itf_cfg.names.end(), key);
if (it != remapped_cmd_itf_cfg.names.end())
{
*it = value;
}
}
return remapped_cmd_itf_cfg;
}
}
}
};

struct ControllerChainSpec
Expand Down

0 comments on commit 85aa876

Please sign in to comment.