@@ -44,53 +44,134 @@ return_type ChainableControllerInterface::update(
4444 return ret;
4545}
4646
47- std::vector<hardware_interface::StateInterface>
47+ std::vector<hardware_interface::StateInterface::SharedPtr >
4848ChainableControllerInterface::export_state_interfaces ()
4949{
5050 auto state_interfaces = on_export_state_interfaces ();
51+ std::vector<hardware_interface::StateInterface::SharedPtr> 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 (state_interface);
6692 }
6793
68- return state_interfaces ;
94+ return state_interfaces_ptrs_vec ;
6995}
7096
71- std::vector<hardware_interface::CommandInterface>
97+ std::vector<hardware_interface::CommandInterface::SharedPtr >
7298ChainableControllerInterface::export_reference_interfaces ()
7399{
74100 auto reference_interfaces = on_export_reference_interfaces ();
101+ std::vector<hardware_interface::CommandInterface::SharedPtr> reference_interfaces_ptrs_vec;
102+ reference_interfaces_ptrs_vec.reserve (reference_interfaces.size ());
103+ exported_reference_interface_names_.reserve (reference_interfaces.size ());
104+ ordered_reference_interfaces_.reserve (reference_interfaces.size ());
105+
106+ // BEGIN (Handle export change): for backward compatibility
107+ // check if the "reference_interfaces_" variable is resized to number of interfaces
108+ if (reference_interfaces_.size () != reference_interfaces.size ())
109+ {
110+ std::string error_msg =
111+ " The internal storage for reference values 'reference_interfaces_' variable has size '"
112+ std::to_string (reference_interfaces_.size ()) + " ', but it is expected to have the size '"
113+ std::to_string (reference_interfaces.size ()) +
114+ " ' equal to the number of exported reference interfaces. Please correct and recompile the "
115+ " controller with name '"
116+ get_node ()->get_name () + " ' and try again."
117+ throw std::runtime_error (error_msg);
118+ }
119+ // END
75120
76121 // check if the names of the reference interfaces begin with the controller's name
77- for (const auto & interface : reference_interfaces)
122+ const auto ref_interface_size = reference_interfaces.size ();
123+ for (auto & interface : reference_interfaces)
78124 {
79125 if (interface.get_prefix_name () != get_node ()->get_name ())
80126 {
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 ;
127+ std::string error_msg = " The name of the interface " get_name () +
128+ " does not begin with the controller's name. This is mandatory for "
129+ " reference interfaces. Please "
130+ " correct and recompile the controller with name "
131+ get_node ()->get_name () + " and try again."
132+ throw std::runtime_error (error_msg);
90133 }
134+
135+ hardware_interface::CommandInterface::SharedPtr reference_interface =
136+ std::make_shared<hardware_interface::CommandInterface>(std::move (interface));
137+ const auto inteface_name = reference_interface->get_name ();
138+ // check the exported interface name is unique
139+ auto [it, succ] = reference_interfaces_ptrs_.insert ({inteface_name, reference_interface});
140+ // either we have name duplicate which we want to avoid under all circumstances since interfaces
141+ // need to be uniquely identify able or something else really went wrong. In any case abort and
142+ // inform cm by throwing exception
143+ if (!succ)
144+ {
145+ std::string error_msg =
146+ " Could not insert Reference interface<"
147+ " > into reference_interfaces_ map. Check if you export duplicates. The "
148+ " map returned iterator with interface_name<"
149+ it->second ->get_name () +
150+ " >. If its a duplicate adjust exportation of InterfacesDescription so that all the "
151+ " interface names are unique."
152+ reference_interfaces_.clear ();
153+ exported_reference_interface_names_.clear ();
154+ reference_interfaces_ptrs_vec.clear ();
155+ throw std::runtime_error (error_msg);
156+ }
157+ ordered_reference_interfaces_.push_back (reference_interface);
158+ exported_reference_interface_names_.push_back (inteface_name);
159+ reference_interfaces_ptrs_vec.push_back (reference_interface);
91160 }
92161
93- return reference_interfaces;
162+ if (reference_interfaces_ptrs_.size () != ref_interface_size)
163+ {
164+ std::string error_msg =
165+ " The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '"
166+ std::to_string (reference_interfaces_ptrs_.size ()) +
167+ " ', but it is expected to have the size '" std::to_string (ref_interface_size) +
168+ " ' equal to the number of exported reference interfaces. Please correct and recompile the "
169+ " controller with name '"
170+ get_node ()->get_name () + " ' and try again."
171+ throw std::runtime_error (error_msg);
172+ }
173+
174+ return reference_interfaces_ptrs_vec;
94175}
95176
96177bool ChainableControllerInterface::set_chained_mode (bool chained_mode)
@@ -130,8 +211,8 @@ ChainableControllerInterface::on_export_state_interfaces()
130211 std::vector<hardware_interface::StateInterface> state_interfaces;
131212 for (size_t i = 0 ; i < exported_state_interface_names_.size (); ++i)
132213 {
133- state_interfaces.emplace_back (hardware_interface::StateInterface (
134- get_node ()->get_name (), exported_state_interface_names_[i], &state_interfaces_values_[i])) ;
214+ state_interfaces.emplace_back (
215+ get_node ()->get_name (), exported_state_interface_names_[i], &state_interfaces_values_[i]);
135216 }
136217 return state_interfaces;
137218}
0 commit comments