Skip to content

Commit f50f34b

Browse files
mamueluthmamueluth
committed
adjusted resource manager to work with shared_ptr adapt actuator,sensor
and system
1 parent b644fa4 commit f50f34b

File tree

10 files changed

+201
-22
lines changed

10 files changed

+201
-22
lines changed

‎hardware_interface/include/hardware_interface/actuator.hpp‎

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -64,11 +64,23 @@ class Actuator final
6464
HARDWARE_INTERFACE_PUBLIC
6565
const rclcpp_lifecycle::State & error();
6666

67+
[[deprecated(
68+
"Replaced by vector<std::shared_ptr<StateInterface>> on_export_state_interfaces() method. "
69+
"Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC
70+
std::vector<StateInterface>
71+
export_state_interfaces();
72+
6773
HARDWARE_INTERFACE_PUBLIC
68-
std::vector<StateInterface> export_state_interfaces();
74+
std::vector<std::shared_ptr<StateInterface>> on_export_state_interfaces();
75+
76+
[[deprecated(
77+
"Replaced by vector<std::shared_ptr<CommandInterface>> on_export_state_interfaces() method. "
78+
"Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC
79+
std::vector<CommandInterface>
80+
export_command_interfaces();
6981

7082
HARDWARE_INTERFACE_PUBLIC
71-
std::vector<CommandInterface> export_command_interfaces();
83+
std::vector<std::shared_ptr<CommandInterface>> on_export_command_interfaces();
7284

7385
HARDWARE_INTERFACE_PUBLIC
7486
return_type prepare_command_mode_switch(

‎hardware_interface/include/hardware_interface/actuator_interface.hpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -149,7 +149,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
149149
* \return vector of state interfaces
150150
*/
151151
[[deprecated(
152-
"Replaced by vector<StateInterface&> on_export_state_interfaces() method. Exporting is "
152+
"Replaced by vector<std::shared_ptr<StateInterface>> on_export_state_interfaces() method. "
153+
"Exporting is "
153154
"handled "
154155
"by the Framework.")]] virtual std::vector<StateInterface>
155156
export_state_interfaces()
@@ -186,7 +187,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
186187
* \return vector of command interfaces
187188
*/
188189
[[deprecated(
189-
"Replaced by vector<CommandInterface&> on_export_command_interfaces() method. Exporting is "
190+
"Replaced by vector<std::shared_ptr<CommandInterface>> on_export_command_interfaces() method. "
191+
"Exporting is "
190192
"handled "
191193
"by the Framework.")]] virtual std::vector<CommandInterface>
192194
export_command_interfaces()

‎hardware_interface/include/hardware_interface/sensor.hpp‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -65,8 +65,14 @@ class Sensor final
6565
HARDWARE_INTERFACE_PUBLIC
6666
const rclcpp_lifecycle::State & error();
6767

68+
[[deprecated(
69+
"Replaced by vector<std::shared_ptr<StateInterface>> on_export_state_interfaces() method. "
70+
"Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC
71+
std::vector<StateInterface>
72+
export_state_interfaces();
73+
6874
HARDWARE_INTERFACE_PUBLIC
69-
std::vector<StateInterface> export_state_interfaces();
75+
std::vector<std::shared_ptr<StateInterface>> on_export_state_interfaces();
7076

7177
HARDWARE_INTERFACE_PUBLIC
7278
std::string get_name() const;

‎hardware_interface/include/hardware_interface/sensor_interface.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -134,7 +134,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
134134
* \return vector of state interfaces
135135
*/
136136
[[deprecated(
137-
"Replaced by vector<StateInterface&> on_export_state_interfaces() method. Exporting is handled "
137+
"Replaced by vector<std::shared_ptr<StateInterface>> on_export_state_interfaces() method. "
138+
"Exporting is handled "
138139
"by the Framework.")]] virtual std::vector<StateInterface>
139140
export_state_interfaces()
140141
{

‎hardware_interface/include/hardware_interface/system.hpp‎

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -65,11 +65,23 @@ class System final
6565
HARDWARE_INTERFACE_PUBLIC
6666
const rclcpp_lifecycle::State & error();
6767

68+
[[deprecated(
69+
"Replaced by vector<std::shared_ptr<StateInterface>> on_export_state_interfaces() method. "
70+
"Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC
71+
std::vector<StateInterface>
72+
export_state_interfaces();
73+
6874
HARDWARE_INTERFACE_PUBLIC
69-
std::vector<StateInterface> export_state_interfaces();
75+
std::vector<std::shared_ptr<StateInterface>> on_export_state_interfaces();
76+
77+
[[deprecated(
78+
"Replaced by vector<std::shared_ptr<CommandInterface>> on_export_state_interfaces() method. "
79+
"Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC
80+
std::vector<CommandInterface>
81+
export_command_interfaces();
7082

7183
HARDWARE_INTERFACE_PUBLIC
72-
std::vector<CommandInterface> export_command_interfaces();
84+
std::vector<std::shared_ptr<CommandInterface>> on_export_command_interfaces();
7385

7486
HARDWARE_INTERFACE_PUBLIC
7587
return_type prepare_command_mode_switch(

‎hardware_interface/include/hardware_interface/system_interface.hpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -169,7 +169,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
169169
* \return vector of state interfaces
170170
*/
171171
[[deprecated(
172-
"Replaced by vector<StateInterface&> on_export_state_interfaces() method. Exporting is handled "
172+
"Replaced by vector<std::shared_ptr<StateInterface>> on_export_state_interfaces() method. "
173+
"Exporting is handled "
173174
"by the Framework.")]] virtual std::vector<StateInterface>
174175
export_state_interfaces()
175176
{
@@ -218,7 +219,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
218219
* \return vector of command interfaces
219220
*/
220221
[[deprecated(
221-
"Replaced by vector<CommandInterface&> on_export_command_interfaces() method. Exporting is "
222+
"Replaced by vector<std::shared_ptr<CommandInterface>> on_export_command_interfaces() method. "
223+
"Exporting is "
222224
"handled "
223225
"by the Framework.")]] virtual std::vector<CommandInterface>
224226
export_command_interfaces()

‎hardware_interface/src/actuator.cpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -192,12 +192,22 @@ std::vector<StateInterface> Actuator::export_state_interfaces()
192192
return impl_->export_state_interfaces();
193193
}
194194

195+
std::vector<std::shared_ptr<StateInterface>> Actuator::on_export_state_interfaces()
196+
{
197+
return impl_->on_export_state_interfaces();
198+
}
199+
195200
std::vector<CommandInterface> Actuator::export_command_interfaces()
196201
{
197202
// TODO(karsten1987): Might be worth to do some brief sanity check here
198203
return impl_->export_command_interfaces();
199204
}
200205

206+
std::vector<std::shared_ptr<CommandInterface>> Actuator::on_export_command_interfaces()
207+
{
208+
return impl_->on_export_command_interfaces();
209+
}
210+
201211
return_type Actuator::prepare_command_mode_switch(
202212
const std::vector<std::string> & start_interfaces,
203213
const std::vector<std::string> & stop_interfaces)

‎hardware_interface/src/resource_manager.cpp‎

Lines changed: 131 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -430,28 +430,126 @@ class ResourceStorage
430430
return result;
431431
}
432432

433+
void insert_state_interface(std::shared_ptr<StateInterface> state_interface)
434+
{
435+
const auto [it, success] =
436+
state_interface_map_.insert(std::make_pair(state_interface->get_name(), state_interface));
437+
if (!success)
438+
{
439+
std::string msg(
440+
"ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" +
441+
state_interface->get_name() + "]");
442+
throw std::runtime_error(msg);
443+
}
444+
}
445+
446+
// TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_state_interfaces()
447+
// method is removed
448+
void insert_state_interface(const StateInterface & state_interface)
449+
{
450+
const auto [it, success] = state_interface_map_.emplace(std::make_pair(
451+
state_interface.get_name(), std::make_shared<StateInterface>(state_interface)));
452+
if (!success)
453+
{
454+
std::string msg(
455+
"ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" +
456+
state_interface.get_name() + "]");
457+
throw std::runtime_error(msg);
458+
}
459+
}
460+
// TODO(Manuel) END: for backward compatibility
461+
433462
template <class HardwareT>
434463
void import_state_interfaces(HardwareT & hardware)
435464
{
436-
auto interfaces = hardware.export_state_interfaces();
437465
std::vector<std::string> interface_names;
438-
interface_names.reserve(interfaces.size());
439-
for (auto & interface : interfaces)
466+
std::vector<StateInterface> interfaces = hardware.export_state_interfaces();
467+
// If no StateInterfaces has been exported, this could mean:
468+
// a) there is nothing to export -> on_export_state_interfaces() does return nothing as well
469+
// b) default implementation for export_state_interfaces() is used -> new functionality ->
470+
// Framework exports and creates everything
471+
if (interfaces.size() == 0)
440472
{
441-
auto key = interface.get_name();
442-
state_interface_map_.emplace(std::make_pair(key, std::move(interface)));
443-
interface_names.push_back(key);
473+
std::vector<std::shared_ptr<StateInterface>> interface_ptrs =
474+
hardware.on_export_state_interfaces();
475+
interface_names.reserve(interface_ptrs.size());
476+
for (auto const & interface : interface_ptrs)
477+
{
478+
insert_state_interface(interface);
479+
interface_names.push_back(interface->get_name());
480+
}
444481
}
482+
// TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_state_interfaces()
483+
// method is removed
484+
else
485+
{
486+
interface_names.reserve(interfaces.size());
487+
for (auto const & interface : interfaces)
488+
{
489+
insert_state_interface(interface);
490+
interface_names.push_back(interface.get_name());
491+
}
492+
}
493+
// TODO(Manuel) END: for backward compatibility
494+
445495
hardware_info_map_[hardware.get_name()].state_interfaces = interface_names;
446496
available_state_interfaces_.reserve(
447497
available_state_interfaces_.capacity() + interface_names.size());
448498
}
449499

500+
void insert_command_interface(std::shared_ptr<CommandInterface> command_interface)
501+
{
502+
const auto [it, success] = command_interface_map_.insert(
503+
std::make_pair(command_interface->get_name(), command_interface));
504+
if (!success)
505+
{
506+
std::string msg(
507+
"ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" +
508+
command_interface->get_name() + "]");
509+
throw std::runtime_error(msg);
510+
}
511+
}
512+
513+
// TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_command_interfaces()
514+
// method is removed
515+
void insert_command_interface(CommandInterface && command_interface)
516+
{
517+
std::string key = command_interface.get_name();
518+
const auto [it, success] = command_interface_map_.emplace(
519+
std::make_pair(key, std::make_shared<CommandInterface>(std::move(command_interface))));
520+
if (!success)
521+
{
522+
std::string msg(
523+
"ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" +
524+
key + "]");
525+
throw std::runtime_error(msg);
526+
}
527+
}
528+
// TODO(Manuel) END: for backward compatibility
529+
450530
template <class HardwareT>
451531
void import_command_interfaces(HardwareT & hardware)
452532
{
453533
auto interfaces = hardware.export_command_interfaces();
454-
hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces);
534+
// If no CommandInterface has been exported, this could mean:
535+
// a) there is nothing to export -> on_export_command_interfaces() does return nothing as well
536+
// b) default implementation for export_command_interfaces() is used -> new functionality ->
537+
// Framework exports and creates everything
538+
if (interfaces.size() == 0)
539+
{
540+
std::vector<std::shared_ptr<CommandInterface>> interface_ptrs =
541+
hardware.on_export_command_interfaces();
542+
hardware_info_map_[hardware.get_name()].command_interfaces =
543+
add_command_interfaces(interface_ptrs);
544+
}
545+
// TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_command_interfaces()
546+
// method is removed
547+
else
548+
{
549+
hardware_info_map_[hardware.get_name()].command_interfaces =
550+
add_command_interfaces(interfaces);
551+
}
552+
// TODO(Manuel) END: for backward compatibility
455553
}
456554

457555
/// Adds exported command interfaces into internal storage.
@@ -465,14 +563,35 @@ class ResourceStorage
465563
* \returns list of interface names that are added into internal storage. The output is used to
466564
* avoid additional iterations to cache interface names, e.g., for initializing info structures.
467565
*/
566+
// TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_command_interfaces()
567+
// method is removed
468568
std::vector<std::string> add_command_interfaces(std::vector<CommandInterface> & interfaces)
469569
{
470570
std::vector<std::string> interface_names;
471571
interface_names.reserve(interfaces.size());
472572
for (auto & interface : interfaces)
473573
{
474574
auto key = interface.get_name();
475-
command_interface_map_.emplace(std::make_pair(key, std::move(interface)));
575+
insert_command_interface(std::move(interface));
576+
claimed_command_interface_map_.emplace(std::make_pair(key, false));
577+
interface_names.push_back(key);
578+
}
579+
available_command_interfaces_.reserve(
580+
available_command_interfaces_.capacity() + interface_names.size());
581+
582+
return interface_names;
583+
}
584+
// TODO(Manuel) END: for backward compatibility
585+
586+
std::vector<std::string> add_command_interfaces(
587+
std::vector<std::shared_ptr<CommandInterface>> & interfaces)
588+
{
589+
std::vector<std::string> interface_names;
590+
interface_names.reserve(interfaces.size());
591+
for (auto & interface : interfaces)
592+
{
593+
auto key = interface->get_name();
594+
insert_command_interface(interface);
476595
claimed_command_interface_map_.emplace(std::make_pair(key, false));
477596
interface_names.push_back(key);
478597
}
@@ -709,9 +828,9 @@ class ResourceStorage
709828
std::unordered_map<std::string, std::vector<std::string>> controllers_reference_interfaces_map_;
710829

711830
/// Storage of all available state interfaces
712-
std::map<std::string, StateInterface> state_interface_map_;
831+
std::map<std::string, std::shared_ptr<StateInterface>> state_interface_map_;
713832
/// Storage of all available command interfaces
714-
std::map<std::string, CommandInterface> command_interface_map_;
833+
std::map<std::string, std::shared_ptr<CommandInterface>> command_interface_map_;
715834

716835
/// Vectors with interfaces available to controllers (depending on hardware component state)
717836
std::vector<std::string> available_state_interfaces_;
@@ -810,7 +929,7 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string &
810929
}
811930

812931
std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
813-
return LoanedStateInterface(resource_storage_->state_interface_map_.at(key));
932+
return LoanedStateInterface(*(resource_storage_->state_interface_map_.at(key)));
814933
}
815934

816935
// CM API: Called in "callback/slow"-thread
@@ -981,7 +1100,7 @@ LoanedCommandInterface ResourceManager::claim_command_interface(const std::strin
9811100
resource_storage_->claimed_command_interface_map_[key] = true;
9821101
std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
9831102
return LoanedCommandInterface(
984-
resource_storage_->command_interface_map_.at(key),
1103+
*(resource_storage_->command_interface_map_.at(key)),
9851104
std::bind(&ResourceManager::release_command_interface, this, key));
9861105
}
9871106

‎hardware_interface/src/sensor.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -189,6 +189,11 @@ std::vector<StateInterface> Sensor::export_state_interfaces()
189189
return impl_->export_state_interfaces();
190190
}
191191

192+
std::vector<std::shared_ptr<StateInterface>> Sensor::on_export_state_interfaces()
193+
{
194+
return impl_->on_export_state_interfaces();
195+
}
196+
192197
std::string Sensor::get_name() const { return impl_->get_name(); }
193198

194199
const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); }

‎hardware_interface/src/system.cpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -189,11 +189,21 @@ std::vector<StateInterface> System::export_state_interfaces()
189189
return impl_->export_state_interfaces();
190190
}
191191

192+
std::vector<std::shared_ptr<StateInterface>> System::on_export_state_interfaces()
193+
{
194+
return impl_->on_export_state_interfaces();
195+
}
196+
192197
std::vector<CommandInterface> System::export_command_interfaces()
193198
{
194199
return impl_->export_command_interfaces();
195200
}
196201

202+
std::vector<std::shared_ptr<CommandInterface>> System::on_export_command_interfaces()
203+
{
204+
return impl_->on_export_command_interfaces();
205+
}
206+
197207
return_type System::prepare_command_mode_switch(
198208
const std::vector<std::string> & start_interfaces,
199209
const std::vector<std::string> & stop_interfaces)

0 commit comments

Comments
 (0)