Skip to content

Commit 017f0da

Browse files
authored
[RM] Rename load_urdf method to load_and_initialize_components and add error handling there to avoid stack crashing when error happens. (#1354)
1 parent fbb893b commit 017f0da

File tree

12 files changed

+731
-192
lines changed

12 files changed

+731
-192
lines changed

controller_manager/include/controller_manager/controller_manager.hpp

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -193,7 +193,24 @@ class ControllerManager : public rclcpp::Node
193193
// the executor (see issue #260).
194194
// rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_;
195195

196-
// Per controller update rate support
196+
/// Interface for external components to check if Resource Manager is initialized.
197+
/**
198+
* Checks if components in Resource Manager are loaded and initialized.
199+
* \returns true if they are initialized, false otherwise.
200+
*/
201+
CONTROLLER_MANAGER_PUBLIC
202+
bool is_resource_manager_initialized() const
203+
{
204+
return resource_manager_ && resource_manager_->are_components_initialized();
205+
}
206+
207+
/// Update rate of the main control loop in the controller manager.
208+
/**
209+
* Update rate of the main control loop in the controller manager.
210+
* The method is used for per-controller update rate support.
211+
*
212+
* \returns update rate of the controller manager.
213+
*/
197214
CONTROLLER_MANAGER_PUBLIC
198215
unsigned int get_update_rate() const;
199216

controller_manager/src/controller_manager.cpp

Lines changed: 28 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -187,8 +187,8 @@ ControllerManager::ControllerManager(
187187
std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
188188
const std::string & node_namespace, const rclcpp::NodeOptions & options)
189189
: rclcpp::Node(manager_node_name, node_namespace, options),
190-
resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
191-
update_rate_, this->get_node_clock_interface())),
190+
resource_manager_(
191+
std::make_unique<hardware_interface::ResourceManager>(this->get_node_clock_interface())),
192192
diagnostics_updater_(this),
193193
executor_(executor),
194194
loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
@@ -216,7 +216,13 @@ ControllerManager::ControllerManager(
216216
"[Deprecated] Passing the robot description parameter directly to the control_manager node "
217217
"is deprecated. Use the 'robot_description' topic from 'robot_state_publisher' instead.");
218218
init_resource_manager(robot_description_);
219-
init_services();
219+
if (is_resource_manager_initialized())
220+
{
221+
RCLCPP_WARN(
222+
get_logger(),
223+
"You have to restart the framework when using robot description from parameter!");
224+
init_services();
225+
}
220226
}
221227

222228
diagnostics_updater_.setHardwareID("ros2_control");
@@ -243,7 +249,7 @@ ControllerManager::ControllerManager(
243249
RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
244250
}
245251

246-
if (resource_manager_->is_urdf_already_loaded())
252+
if (is_resource_manager_initialized())
247253
{
248254
init_services();
249255
}
@@ -271,36 +277,32 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
271277
RCLCPP_INFO(get_logger(), "Received robot description from topic.");
272278
RCLCPP_DEBUG(
273279
get_logger(), "'Content of robot description file: %s", robot_description.data.c_str());
274-
// TODO(mamueluth): errors should probably be caught since we don't want controller_manager node
275-
// to die if a non valid urdf is passed. However, should maybe be fine tuned.
276-
try
280+
robot_description_ = robot_description.data;
281+
if (is_resource_manager_initialized())
277282
{
278-
robot_description_ = robot_description.data;
279-
if (resource_manager_->is_urdf_already_loaded())
280-
{
281-
RCLCPP_WARN(
282-
get_logger(),
283-
"ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
284-
"description file.");
285-
return;
286-
}
287-
init_resource_manager(robot_description_);
288-
init_services();
283+
RCLCPP_WARN(
284+
get_logger(),
285+
"ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
286+
"description file.");
287+
return;
289288
}
290-
catch (std::runtime_error & e)
289+
init_resource_manager(robot_description_);
290+
if (is_resource_manager_initialized())
291291
{
292-
RCLCPP_ERROR_STREAM(
293-
get_logger(),
294-
"The published robot description file (urdf) seems not to be genuine. The following error "
295-
"was caught:"
296-
<< e.what());
292+
init_services();
297293
}
298294
}
299295

300296
void ControllerManager::init_resource_manager(const std::string & robot_description)
301297
{
302-
// TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong...
303-
resource_manager_->load_urdf(robot_description);
298+
if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_))
299+
{
300+
RCLCPP_WARN(
301+
get_logger(),
302+
"Could not load and initialize hardware. Please check previous output for more details. "
303+
"After you have corrected your URDF, try to publish robot description again.");
304+
return;
305+
}
304306

305307
// Get all components and if they are not defined in parameters activate them automatically
306308
auto components_to_activate = resource_manager_->get_components_status();

controller_manager/test/test_controller_manager_urdf_passing.cpp

Lines changed: 24 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -29,11 +29,13 @@ class TestableControllerManager : public controller_manager::ControllerManager
2929
{
3030
friend TestControllerManagerWithTestableCM;
3131

32-
FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called);
33-
FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
34-
FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed);
35-
FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
36-
FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
32+
FRIEND_TEST(
33+
TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called);
34+
FRIEND_TEST(
35+
TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback);
36+
FRIEND_TEST(
37+
TestControllerManagerWithTestableCM,
38+
expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description);
3739

3840
public:
3941
TestableControllerManager(
@@ -59,29 +61,36 @@ class TestControllerManagerWithTestableCM
5961
}
6062
};
6163

62-
TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called)
64+
TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called)
6365
{
64-
ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
66+
ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
6567
}
6668

67-
TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback)
69+
TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback)
6870
{
69-
ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
71+
ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
7072
// mimic callback
7173
auto msg = std_msgs::msg::String();
7274
msg.data = ros2_control_test_assets::minimal_robot_urdf;
7375
cm_->robot_description_callback(msg);
74-
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
76+
ASSERT_TRUE(cm_->resource_manager_->are_components_initialized());
7577
}
7678

77-
TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed)
79+
TEST_P(
80+
TestControllerManagerWithTestableCM,
81+
expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description)
7882
{
79-
ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
80-
// mimic callback
83+
ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
84+
// wrong urdf
8185
auto msg = std_msgs::msg::String();
82-
msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf;
86+
msg.data = ros2_control_test_assets::minimal_uninitializable_robot_urdf;
87+
cm_->robot_description_callback(msg);
88+
ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
89+
// correct urdf
90+
msg = std_msgs::msg::String();
91+
msg.data = ros2_control_test_assets::minimal_robot_urdf;
8392
cm_->robot_description_callback(msg);
84-
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
93+
ASSERT_TRUE(cm_->resource_manager_->are_components_initialized());
8594
}
8695

8796
INSTANTIATE_TEST_SUITE_P(

hardware_interface/include/hardware_interface/resource_manager.hpp

Lines changed: 25 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -49,8 +49,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
4949
{
5050
public:
5151
/// Default constructor for the Resource Manager.
52-
ResourceManager(
53-
unsigned int update_rate = 100,
52+
explicit ResourceManager(
5453
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
5554

5655
/// Constructor for the Resource Manager.
@@ -59,18 +58,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
5958
* hardware components listed within as well as populate their respective
6059
* state and command interfaces.
6160
*
62-
* If the interfaces ought to be validated, the constructor throws an exception
63-
* in case the URDF lists interfaces which are not available.
64-
*
6561
* \param[in] urdf string containing the URDF.
66-
* \param[in] validate_interfaces boolean argument indicating whether the exported
67-
* interfaces ought to be validated. Defaults to true.
6862
* \param[in] activate_all boolean argument indicating if all resources should be immediately
6963
* activated. Currently used only in tests.
64+
* \param[in] update_rate Update rate of the controller manager to calculate calling frequency
65+
* of async components.
66+
* \param[in] clock_interface reference to the clock interface of the CM node for getting time
67+
* used for triggering async components.
7068
*/
7169
explicit ResourceManager(
72-
const std::string & urdf, bool validate_interfaces = true, bool activate_all = false,
73-
unsigned int update_rate = 100,
70+
const std::string & urdf, bool activate_all = false, const unsigned int update_rate = 100,
7471
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
7572

7673
ResourceManager(const ResourceManager &) = delete;
@@ -79,29 +76,26 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
7976

8077
/// Load resources from on a given URDF.
8178
/**
82-
* The resource manager can be post initialized with a given URDF.
79+
* The resource manager can be post-initialized with a given URDF.
8380
* This is mainly used in conjunction with the default constructor
8481
* in which the URDF might not be present at first initialization.
8582
*
8683
* \param[in] urdf string containing the URDF.
87-
* \param[in] validate_interfaces boolean argument indicating whether the exported
88-
* interfaces ought to be validated. Defaults to true.
89-
* \param[in] load_and_initialize_components boolean argument indicating whether to load and
90-
* initialize the components present in the parsed URDF. Defaults to true.
84+
* \param[in] update_rate update rate of the main control loop, i.e., of the controller manager.
85+
* \returns false if URDF validation has failed.
9186
*/
92-
void load_urdf(
93-
const std::string & urdf, bool validate_interfaces = true,
94-
bool load_and_initialize_components = true);
87+
virtual bool load_and_initialize_components(
88+
const std::string & urdf, const unsigned int update_rate = 100);
9589

9690
/**
97-
* @brief if the resource manager load_urdf(...) function has been called this returns true.
98-
* We want to permit to load the urdf later on but we currently don't want to permit multiple
99-
* calls to load_urdf (reloading/loading different urdf).
91+
* @brief if the resource manager load_and_initialize_components(...) function has been called
92+
* this returns true. We want to permit to loading the urdf later on, but we currently don't want
93+
* to permit multiple calls to load_and_initialize_components (reloading/loading different urdf).
10094
*
101-
* @return true if resource manager's load_urdf() has been already called.
102-
* @return false if resource manager's load_urdf() has not been yet called.
95+
* @return true if the resource manager has successfully loaded and initialized the components
96+
* @return false if the resource manager doesn't have any components loaded and initialized.
10397
*/
104-
bool is_urdf_already_loaded() const;
98+
bool are_components_initialized() const;
10599

106100
/// Claim a state interface given its key.
107101
/**
@@ -413,23 +407,24 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
413407
*/
414408
bool state_interface_exists(const std::string & key) const;
415409

410+
protected:
411+
bool components_are_loaded_and_initialized_ = false;
412+
413+
mutable std::recursive_mutex resource_interfaces_lock_;
414+
mutable std::recursive_mutex claimed_command_interfaces_lock_;
415+
mutable std::recursive_mutex resources_lock_;
416+
416417
private:
417-
void validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
418+
bool validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
418419

419420
void release_command_interface(const std::string & key);
420421

421422
std::unordered_map<std::string, bool> claimed_command_interface_map_;
422423

423-
mutable std::recursive_mutex resource_interfaces_lock_;
424-
mutable std::recursive_mutex claimed_command_interfaces_lock_;
425-
mutable std::recursive_mutex resources_lock_;
426-
427424
std::unique_ptr<ResourceStorage> resource_storage_;
428425

429426
// Structure to store read and write status so it is not initialized in the real-time loop
430427
HardwareReadWriteStatus read_write_status;
431-
432-
bool is_urdf_loaded__ = false;
433428
};
434429

435430
} // namespace hardware_interface

0 commit comments

Comments
 (0)