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="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
88
<maintainer email="denis@stoglrobotics.de">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="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
88
<maintainer email="denis@stoglrobotics.de">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="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
88
<maintainer email="denis@stoglrobotics.de">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>`_)

‎hardware_interface/include/hardware_interface/resource_manager.hpp

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,13 @@ class ResourceManager
7676

7777
virtual ~ResourceManager();
7878

79+
/// Shutdown all hardware components, irrespective of their state.
80+
/**
81+
* The method is called when the controller manager is being shutdown.
82+
* @return true if all hardware components are successfully shutdown, false otherwise.
83+
*/
84+
bool shutdown_components();
85+
7986
/// Load resources from on a given URDF.
8087
/**
8188
* The resource manager can be post-initialized with a given URDF.

‎hardware_interface/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="2">
33
<name>hardware_interface</name>
4-
<version>4.24.0</version>
4+
<version>4.25.0</version>
55
<description>ros2_control hardware interface</description>
66
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
77
<maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

‎hardware_interface/src/resource_manager.cpp

Lines changed: 17 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -573,7 +573,7 @@ class ResourceStorage
573573
result = shutdown_hardware(hardware);
574574
break;
575575
case State::PRIMARY_STATE_ACTIVE:
576-
result = shutdown_hardware(hardware);
576+
result = deactivate_hardware(hardware) && shutdown_hardware(hardware);
577577
break;
578578
case State::PRIMARY_STATE_FINALIZED:
579579
result = true;
@@ -1044,6 +1044,22 @@ ResourceManager::ResourceManager(
10441044
}
10451045
}
10461046

1047+
bool ResourceManager::shutdown_components()
1048+
{
1049+
std::unique_lock<std::recursive_mutex> guard(resource_interfaces_lock_);
1050+
bool shutdown_status = true;
1051+
for (auto const & hw_info : resource_storage_->hardware_info_map_)
1052+
{
1053+
rclcpp_lifecycle::State finalized_state(
1054+
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED);
1055+
if (set_component_state(hw_info.first, finalized_state) != return_type::OK)
1056+
{
1057+
shutdown_status = false;
1058+
}
1059+
}
1060+
return shutdown_status;
1061+
}
1062+
10471063
// CM API: Called in "callback/slow"-thread
10481064
bool ResourceManager::load_and_initialize_components(
10491065
const std::string & urdf, const unsigned int update_rate)

‎hardware_interface_testing/CHANGELOG.rst

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

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

‎hardware_interface_testing/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="2">
33
<name>hardware_interface_testing</name>
4-
<version>4.24.0</version>
4+
<version>4.25.0</version>
55
<description>ros2_control hardware interface testing</description>
66
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
77
<maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

‎joint_limits/CHANGELOG.rst

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

5+
4.25.0 (2025-01-29)
6+
-------------------
7+
* Define _USE_MATH_DEFINES in joint_soft_limiter.cpp to ensure that M_PI is defined (`#2001 <https://github.com/ros-controls/ros2_control/issues/2001>`_)
8+
* Use actual position when limiting desired position (`#1988 <https://github.com/ros-controls/ros2_control/issues/1988>`_)
9+
* Contributors: Sai Kishor Kothakota, Silvio Traversaro
10+
511
4.24.0 (2025-01-13)
612
-------------------
713
* Return strong type for joint_limits helpers (`#1981 <https://github.com/ros-controls/ros2_control/issues/1981>`_)

‎joint_limits/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
<package format="3">
22
<name>joint_limits</name>
3-
<version>4.24.0</version>
3+
<version>4.25.0</version>
44
<description>Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces.</description>
55

66
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>

‎ros2_control.jazzy.repos

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@ repositories:
22
ros-controls/realtime_tools:
33
type: git
44
url: https://github.com/ros-controls/realtime_tools.git
5-
version: master
5+
version: jazzy
66
ros-controls/control_msgs:
77
type: git
88
url: https://github.com/ros-controls/control_msgs.git

‎ros2_control/CHANGELOG.rst

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

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

‎ros2_control/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="3">
33
<name>ros2_control</name>
4-
<version>4.24.0</version>
4+
<version>4.25.0</version>
55
<description>Metapackage for ROS2 control related packages</description>
66
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
77
<maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

‎ros2_control_test_assets/CHANGELOG.rst

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

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

‎ros2_control_test_assets/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>ros2_control_test_assets</name>
5-
<version>4.24.0</version>
5+
<version>4.25.0</version>
66
<description>The package provides shared test resources for ros2_control stack
77
</description>
88
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>

‎ros2controlcli/CHANGELOG.rst

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

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

‎ros2controlcli/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>ros2controlcli</name>
5-
<version>4.24.0</version>
5+
<version>4.25.0</version>
66
<description>
77
The ROS 2 command line tools for ROS2 Control.
88
</description>

‎ros2controlcli/setup.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919

2020
setup(
2121
name=package_name,
22-
version="4.24.0",
22+
version="4.25.0",
2323
packages=find_packages(exclude=["test"]),
2424
data_files=[
2525
("share/" + package_name, ["package.xml"]),

‎rqt_controller_manager/CHANGELOG.rst

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

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

‎rqt_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_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>rqt_controller_manager</name>
5-
<version>4.24.0</version>
5+
<version>4.25.0</version>
66
<description>Graphical frontend for interacting with the controller manager.</description>
77
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
88
<maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

0 commit comments

Comments
 (0)