@@ -44,53 +44,135 @@ return_type ChainableControllerInterface::update(
4444 return ret;
4545}
4646
47- std::vector<hardware_interface::StateInterface>
47+ std::vector<hardware_interface::StateInterface::ConstSharedPtr >
4848ChainableControllerInterface::export_state_interfaces ()
4949{
5050 auto state_interfaces = on_export_state_interfaces ();
51+ std::vector<hardware_interface::StateInterface::ConstSharedPtr> state_interfaces_ptrs_vec;
52+ state_interfaces_ptrs_vec.reserve (state_interfaces.size ());
53+ ordered_exported_state_interfaces_.reserve (state_interfaces.size ());
54+ exported_state_interface_names_.reserve (state_interfaces.size ());
5155
5256 // check if the names of the controller state interfaces begin with the controller's name
5357 for (const auto & interface : state_interfaces)
5458 {
5559 if (interface.get_prefix_name () != get_node ()->get_name ())
5660 {
57- RCLCPP_FATAL (
58- get_node ()->get_logger (),
59- " The name of the interface '%s' does not begin with the controller's name. This is "
60- " mandatory for state interfaces. No state interface will be exported. Please "
61- " correct and recompile the controller with name '%s' and try again."
62- interface.get_name ().c_str (), get_node ()->get_name ());
63- state_interfaces.clear ();
64- break ;
61+ std::string error_msg =
62+ " The prefix of the interface '" get_prefix_name () +
63+ " ' does not equal the controller's name '" get_node ()->get_name () +
64+ " '. This is mandatory for state interfaces. No state interface will be exported. Please "
65+ " correct and recompile the controller with name '"
66+ get_node ()->get_name () + " ' and try again."
67+ throw std::runtime_error (error_msg);
68+ }
69+ auto state_interface = std::make_shared<hardware_interface::StateInterface>(interface);
70+ const auto interface_name = state_interface->get_name ();
71+ auto [it, succ] = exported_state_interfaces_.insert ({interface_name, state_interface});
72+ // either we have name duplicate which we want to avoid under all circumstances since interfaces
73+ // need to be uniquely identify able or something else really went wrong. In any case abort and
74+ // inform cm by throwing exception
75+ if (!succ)
76+ {
77+ std::string error_msg =
78+ " Could not insert StateInterface<"
79+ " > into exported_state_interfaces_ map. Check if you export duplicates. The "
80+ " map returned iterator with interface_name<"
81+ it->second ->get_name () +
82+ " >. If its a duplicate adjust exportation of InterfacesDescription so that all the "
83+ " interface names are unique."
84+ exported_state_interfaces_.clear ();
85+ exported_state_interface_names_.clear ();
86+ state_interfaces_ptrs_vec.clear ();
87+ throw std::runtime_error (error_msg);
6588 }
89+ ordered_exported_state_interfaces_.push_back (state_interface);
90+ exported_state_interface_names_.push_back (interface_name);
91+ state_interfaces_ptrs_vec.push_back (
92+ std::const_pointer_cast<const hardware_interface::StateInterface>(state_interface));
6693 }
6794
68- return state_interfaces ;
95+ return state_interfaces_ptrs_vec ;
6996}
7097
71- std::vector<hardware_interface::CommandInterface>
98+ std::vector<hardware_interface::CommandInterface::SharedPtr >
7299ChainableControllerInterface::export_reference_interfaces ()
73100{
74101 auto reference_interfaces = on_export_reference_interfaces ();
102+ std::vector<hardware_interface::CommandInterface::SharedPtr> reference_interfaces_ptrs_vec;
103+ reference_interfaces_ptrs_vec.reserve (reference_interfaces.size ());
104+ exported_reference_interface_names_.reserve (reference_interfaces.size ());
105+ ordered_reference_interfaces_.reserve (reference_interfaces.size ());
106+
107+ // BEGIN (Handle export change): for backward compatibility
108+ // check if the "reference_interfaces_" variable is resized to number of interfaces
109+ if (reference_interfaces_.size () != reference_interfaces.size ())
110+ {
111+ std::string error_msg =
112+ " The internal storage for reference values 'reference_interfaces_' variable has size '"
113+ std::to_string (reference_interfaces_.size ()) + " ', but it is expected to have the size '"
114+ std::to_string (reference_interfaces.size ()) +
115+ " ' equal to the number of exported reference interfaces. Please correct and recompile the "
116+ " controller with name '"
117+ get_node ()->get_name () + " ' and try again."
118+ throw std::runtime_error (error_msg);
119+ }
120+ // END
75121
76122 // check if the names of the reference interfaces begin with the controller's name
77- for (const auto & interface : reference_interfaces)
123+ const auto ref_interface_size = reference_interfaces.size ();
124+ for (auto & interface : reference_interfaces)
78125 {
79126 if (interface.get_prefix_name () != get_node ()->get_name ())
80127 {
81- RCLCPP_FATAL (
82- get_node ()->get_logger (),
83- " The name of the interface '%s' does not begin with the controller's name. This is "
84- " mandatory "
85- " for reference interfaces. No reference interface will be exported. Please correct and "
86- " recompile the controller with name '%s' and try again."
87- interface.get_name ().c_str (), get_node ()->get_name ());
88- reference_interfaces.clear ();
89- break ;
128+ std::string error_msg = " The name of the interface " get_name () +
129+ " does not begin with the controller's name. This is mandatory for "
130+ " reference interfaces. Please "
131+ " correct and recompile the controller with name "
132+ get_node ()->get_name () + " and try again."
133+ throw std::runtime_error (error_msg);
90134 }
135+
136+ hardware_interface::CommandInterface::SharedPtr reference_interface =
137+ std::make_shared<hardware_interface::CommandInterface>(std::move (interface));
138+ const auto inteface_name = reference_interface->get_name ();
139+ // check the exported interface name is unique
140+ auto [it, succ] = reference_interfaces_ptrs_.insert ({inteface_name, reference_interface});
141+ // either we have name duplicate which we want to avoid under all circumstances since interfaces
142+ // need to be uniquely identify able or something else really went wrong. In any case abort and
143+ // inform cm by throwing exception
144+ if (!succ)
145+ {
146+ std::string error_msg =
147+ " Could not insert Reference interface<"
148+ " > into reference_interfaces_ map. Check if you export duplicates. The "
149+ " map returned iterator with interface_name<"
150+ it->second ->get_name () +
151+ " >. If its a duplicate adjust exportation of InterfacesDescription so that all the "
152+ " interface names are unique."
153+ reference_interfaces_.clear ();
154+ exported_reference_interface_names_.clear ();
155+ reference_interfaces_ptrs_vec.clear ();
156+ throw std::runtime_error (error_msg);
157+ }
158+ ordered_reference_interfaces_.push_back (reference_interface);
159+ exported_reference_interface_names_.push_back (inteface_name);
160+ reference_interfaces_ptrs_vec.push_back (reference_interface);
91161 }
92162
93- return reference_interfaces;
163+ if (reference_interfaces_ptrs_.size () != ref_interface_size)
164+ {
165+ std::string error_msg =
166+ " The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '"
167+ std::to_string (reference_interfaces_ptrs_.size ()) +
168+ " ', but it is expected to have the size '" std::to_string (ref_interface_size) +
169+ " ' equal to the number of exported reference interfaces. Please correct and recompile the "
170+ " controller with name '"
171+ get_node ()->get_name () + " ' and try again."
172+ throw std::runtime_error (error_msg);
173+ }
174+
175+ return reference_interfaces_ptrs_vec;
94176}
95177
96178bool ChainableControllerInterface::set_chained_mode (bool chained_mode)
@@ -130,8 +212,8 @@ ChainableControllerInterface::on_export_state_interfaces()
130212 std::vector<hardware_interface::StateInterface> state_interfaces;
131213 for (size_t i = 0 ; i < exported_state_interface_names_.size (); ++i)
132214 {
133- state_interfaces.emplace_back (hardware_interface::StateInterface (
134- get_node ()->get_name (), exported_state_interface_names_[i], &state_interfaces_values_[i])) ;
215+ state_interfaces.emplace_back (
216+ get_node ()->get_name (), exported_state_interface_names_[i], &state_interfaces_values_[i]);
135217 }
136218 return state_interfaces;
137219}
0 commit comments