Skip to content

Commit 9b750b1

Browse files
saikishorsaikishor
authored
Merge branch 'master' into integrate/pal_statistics
2 parents a89b26e + 3755e03 commit 9b750b1

File tree

30 files changed

+162
-17
lines changed

30 files changed

+162
-17
lines changed

‎controller_interface/CHANGELOG.rst

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,12 @@
22
Changelog for package controller_interface
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
4.25.0 (2025-01-29)
6+
-------------------
7+
* Use `target_compile_definitions` instead of installing test files (`#2009 <https://github.com/ros-controls/ros2_control/issues/2009>`_)
8+
* Add GPS semantic component (`#2000 <https://github.com/ros-controls/ros2_control/issues/2000>`_)
9+
* Contributors: Sai Kishor Kothakota, Wiktor Bajor
10+
511
4.24.0 (2025-01-13)
612
-------------------
713
* Trigger shutdown transition in destructor (`#1979 <https://github.com/ros-controls/ros2_control/issues/1979>`_)

‎controller_interface/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="2">
44
<name>controller_interface</name>
5-
<version>4.24.0</version>
5+
<version>4.25.0</version>
66
<description>Description of controller_interface</description>
77
<maintainer email="[email protected]">Bence Magyar</maintainer>
88
<maintainer email="[email protected]">Denis Štogl</maintainer>

‎controller_manager/CHANGELOG.rst

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,16 @@
22
Changelog for package controller_manager
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
4.25.0 (2025-01-29)
6+
-------------------
7+
* Handle SIGINT properly in the controller manager (`#2014 <https://github.com/ros-controls/ros2_control/issues/2014>`_)
8+
* Fix the initial wrong periodicity reported by controller_manager (`#2018 <https://github.com/ros-controls/ros2_control/issues/2018>`_)
9+
* Use `target_compile_definitions` instead of installing test files (`#2009 <https://github.com/ros-controls/ros2_control/issues/2009>`_)
10+
* Fix a heading level (`#2007 <https://github.com/ros-controls/ros2_control/issues/2007>`_)
11+
* Update path of GPL (`#1994 <https://github.com/ros-controls/ros2_control/issues/1994>`_)
12+
* Fix: on_shutdown callback of controllers never get executed (`#1995 <https://github.com/ros-controls/ros2_control/issues/1995>`_)
13+
* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Wiktor Bajor
14+
515
4.24.0 (2025-01-13)
616
-------------------
717
* [CM] Remove obsolete ControllerMock from the tests (`#1990 <https://github.com/ros-controls/ros2_control/issues/1990>`_)

‎controller_manager/include/controller_manager/controller_manager.hpp

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,12 @@ class ControllerManager : public rclcpp::Node
8282

8383
virtual ~ControllerManager();
8484

85+
/// Shutdown all controllers in the controller manager.
86+
/**
87+
* \return true if all controllers are successfully shutdown, false otherwise.
88+
*/
89+
bool shutdown_controllers();
90+
8591
void robot_description_callback(const std_msgs::msg::String & msg);
8692

8793
void init_resource_manager(const std::string & robot_description);
@@ -531,6 +537,7 @@ class ControllerManager : public rclcpp::Node
531537
int used_by_realtime_controllers_index_ = -1;
532538
};
533539

540+
std::unique_ptr<rclcpp::PreShutdownCallbackHandle> preshutdown_cb_handle_{nullptr};
534541
RTControllerListWrapper rt_controllers_wrapper_;
535542
std::unordered_map<std::string, ControllerChainSpec> controller_chain_spec_;
536543
std::vector<std::string> ordered_controllers_names_;

‎controller_manager/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="2">
44
<name>controller_manager</name>
5-
<version>4.24.0</version>
5+
<version>4.25.0</version>
66
<description>Description of controller_manager</description>
77
<maintainer email="[email protected]">Bence Magyar</maintainer>
88
<maintainer email="[email protected]">Denis Štogl</maintainer>

‎controller_manager/src/controller_manager.cpp

Lines changed: 64 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -292,7 +292,46 @@ ControllerManager::ControllerManager(
292292
init_controller_manager();
293293
}
294294

295-
ControllerManager::~ControllerManager() { CLEAR_ALL_ROS2_CONTROL_INTROSPECTION_REGISTRIES(); }
295+
ControllerManager::~ControllerManager()
296+
{
297+
CLEAR_ALL_ROS2_CONTROL_INTROSPECTION_REGISTRIES();
298+
if (preshutdown_cb_handle_)
299+
{
300+
rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
301+
context->remove_pre_shutdown_callback(*(preshutdown_cb_handle_.get()));
302+
preshutdown_cb_handle_.reset();
303+
}
304+
}
305+
306+
bool ControllerManager::shutdown_controllers()
307+
{
308+
RCLCPP_INFO(get_logger(), "Shutting down all controllers in the controller manager.");
309+
// Shutdown all controllers
310+
std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
311+
std::vector<ControllerSpec> controllers_list = rt_controllers_wrapper_.get_updated_list(guard);
312+
bool ctrls_shutdown_status = true;
313+
for (auto & controller : controllers_list)
314+
{
315+
if (is_controller_active(controller.c))
316+
{
317+
RCLCPP_INFO(
318+
get_logger(), "Deactivating controller '%s'", controller.c->get_node()->get_name());
319+
controller.c->get_node()->deactivate();
320+
controller.c->release_interfaces();
321+
}
322+
if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c))
323+
{
324+
RCLCPP_INFO(
325+
get_logger(), "Shutting down controller '%s'", controller.c->get_node()->get_name());
326+
shutdown_controller(controller);
327+
}
328+
ctrls_shutdown_status &=
329+
(controller.c->get_node()->get_current_state().id() ==
330+
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
331+
executor_->remove_node(controller.c->get_node()->get_node_base_interface());
332+
}
333+
return ctrls_shutdown_status;
334+
}
296335

297336
void ControllerManager::init_controller_manager()
298337
{
@@ -332,10 +371,34 @@ void ControllerManager::init_controller_manager()
332371
diagnostics_updater_.add(
333372
"Controller Manager Activity", this,
334373
&ControllerManager::controller_manager_diagnostic_callback);
374+
335375
INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(
336376
this, hardware_interface::DEFAULT_INTROSPECTION_TOPIC,
337377
hardware_interface::DEFAULT_REGISTRY_KEY);
338378
START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY);
379+
380+
// Add on_shutdown callback to stop the controller manager
381+
rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
382+
preshutdown_cb_handle_ =
383+
std::make_unique<rclcpp::PreShutdownCallbackHandle>(context->add_pre_shutdown_callback(
384+
[this]()
385+
{
386+
RCLCPP_INFO(get_logger(), "Shutdown request received....");
387+
if (this->get_node_base_interface()->get_associated_with_executor_atomic().load())
388+
{
389+
executor_->remove_node(this->get_node_base_interface());
390+
}
391+
executor_->cancel();
392+
if (!this->shutdown_controllers())
393+
{
394+
RCLCPP_ERROR(get_logger(), "Failed shutting down the controllers.");
395+
}
396+
if (!resource_manager_->shutdown_components())
397+
{
398+
RCLCPP_ERROR(get_logger(), "Failed shutting down hardware components.");
399+
}
400+
RCLCPP_INFO(get_logger(), "Shutting down the controller manager.");
401+
}));
339402
}
340403

341404
void ControllerManager::initialize_parameters()

‎controller_manager/src/ros2_control_node.cpp

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -93,6 +93,10 @@ int main(int argc, char ** argv)
9393
}
9494
}
9595

96+
// wait for the clock to be available
97+
cm->get_clock()->wait_until_started();
98+
cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate()));
99+
96100
RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
97101
const int thread_priority = cm->get_parameter_or<int>("thread_priority", kSchedPriority);
98102
RCLCPP_INFO(
@@ -122,7 +126,7 @@ int main(int argc, char ** argv)
122126
auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());
123127
auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds());
124128
std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds>
125-
next_iteration_time{cm_now};
129+
next_iteration_time{cm_now - period};
126130

127131
// for calculating the measured period of the loop
128132
rclcpp::Time previous_time = cm->now() - period;

‎controller_manager_msgs/CHANGELOG.rst

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,9 @@
22
Changelog for package controller_manager_msgs
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
4.25.0 (2025-01-29)
6+
-------------------
7+
58
4.24.0 (2025-01-13)
69
-------------------
710

‎controller_manager_msgs/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>controller_manager_msgs</name>
5-
<version>4.24.0</version>
5+
<version>4.25.0</version>
66
<description>Messages and services for the controller manager.</description>
77
<maintainer email="[email protected]">Bence Magyar</maintainer>
88
<maintainer email="[email protected]">Denis Štogl</maintainer>

‎hardware_interface/CHANGELOG.rst

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,11 @@
22
Changelog for package hardware_interface
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
4.25.0 (2025-01-29)
6+
-------------------
7+
* Handle SIGINT properly in the controller manager (`#2014 <https://github.com/ros-controls/ros2_control/issues/2014>`_)
8+
* Contributors: Sai Kishor Kothakota
9+
510
4.24.0 (2025-01-13)
611
-------------------
712
* Add missing link of mock_components to hardware_interface (`#1992 <https://github.com/ros-controls/ros2_control/issues/1992>`_)

0 commit comments

Comments
 (0)