Skip to content

Commit a3ef839

Browse files
authored
Merge branch 'master' into feat/interface/remapping
2 parents bfa9281 + 8f7374f commit a3ef839

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

57 files changed

+926
-521
lines changed

controller_interface/CHANGELOG.rst

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

5+
4.16.1 (2024-08-24)
6+
-------------------
7+
8+
4.16.0 (2024-08-22)
9+
-------------------
10+
* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 <https://github.com/ros-controls/ros2_control/issues/1701>`_)
11+
* Avoid using the global arguments for internal controller nodes (`#1694 <https://github.com/ros-controls/ros2_control/issues/1694>`_)
12+
* Contributors: Sai Kishor Kothakota
13+
514
4.15.0 (2024-08-05)
615
-------------------
716

controller_interface/include/controller_interface/async_controller.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -82,7 +82,8 @@ class AsyncControllerThread
8282
TimePoint next_iteration_time =
8383
TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds()));
8484

85-
if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
85+
if (
86+
controller_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
8687
{
8788
auto const current_time = controller_->get_node()->now();
8889
auto const measured_period = current_time - previous_time;

controller_interface/include/controller_interface/controller_interface_base.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -148,7 +148,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
148148
std::shared_ptr<const rclcpp_lifecycle::LifecycleNode> get_node() const;
149149

150150
CONTROLLER_INTERFACE_PUBLIC
151-
const rclcpp_lifecycle::State & get_state() const;
151+
const rclcpp_lifecycle::State & get_lifecycle_state() const;
152152

153153
CONTROLLER_INTERFACE_PUBLIC
154154
unsigned int get_update_rate() const;

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.15.0</version>
5+
<version>4.16.1</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_interface/src/chainable_controller_interface.cpp

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -97,7 +97,7 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
9797
{
9898
bool result = false;
9999

100-
if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
100+
if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
101101
{
102102
result = on_set_chained_mode(chained_mode);
103103

@@ -112,7 +112,8 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
112112
get_node()->get_logger(),
113113
"Can not change controller's chained mode because it is no in '%s' state. "
114114
"Current state is '%s'.",
115-
hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str());
115+
hardware_interface::lifecycle_state_names::UNCONFIGURED,
116+
get_lifecycle_state().label().c_str());
116117
}
117118

118119
return result;

controller_interface/src/controller_interface_base.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -83,7 +83,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
8383
// Then we don't need to do state-machine related checks.
8484
//
8585
// Other solution is to add check into the LifecycleNode if a transition is valid to trigger
86-
if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
86+
if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
8787
{
8888
update_rate_ = static_cast<unsigned int>(get_node()->get_parameter("update_rate").as_int());
8989
is_async_ = get_node()->get_parameter("is_async").as_bool();
@@ -169,7 +169,7 @@ void ControllerInterfaceBase::release_interfaces()
169169
state_interfaces_.clear();
170170
}
171171

172-
const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state() const
172+
const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const
173173
{
174174
return node_->get_current_state();
175175
}

controller_manager/CHANGELOG.rst

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

5+
4.16.1 (2024-08-24)
6+
-------------------
7+
* propage a portion of global args to the controller nodes (`#1712 <https://github.com/ros-controls/ros2_control/issues/1712>`_)
8+
* Contributors: Sai Kishor Kothakota
9+
10+
4.16.0 (2024-08-22)
11+
-------------------
12+
* inform user what reason is for not setting rt policy, inform is policy (`#1705 <https://github.com/ros-controls/ros2_control/issues/1705>`_)
13+
* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 <https://github.com/ros-controls/ros2_control/issues/1701>`_)
14+
* Fix spawner tests timeout (`#1692 <https://github.com/ros-controls/ros2_control/issues/1692>`_)
15+
* Refactor spawner to be able to reuse code for ros2controlcli (`#1661 <https://github.com/ros-controls/ros2_control/issues/1661>`_)
16+
* Robustify controller spawner and add integration test with many controllers (`#1501 <https://github.com/ros-controls/ros2_control/issues/1501>`_)
17+
* Handle waiting in Spawner and align Hardware Spawner functionality (`#1562 <https://github.com/ros-controls/ros2_control/issues/1562>`_)
18+
* Make list controller and list hardware components immediately visualize the state. (`#1606 <https://github.com/ros-controls/ros2_control/issues/1606>`_)
19+
* [CI] Add coveragepy and remove ignore: test (`#1668 <https://github.com/ros-controls/ros2_control/issues/1668>`_)
20+
* Fix spawner unload on kill test (`#1675 <https://github.com/ros-controls/ros2_control/issues/1675>`_)
21+
* [CM] Add more logging for easier debugging (`#1645 <https://github.com/ros-controls/ros2_control/issues/1645>`_)
22+
* refactor SwitchParams to fix the incosistencies in the spawner tests (`#1638 <https://github.com/ros-controls/ros2_control/issues/1638>`_)
23+
* Contributors: Bence Magyar, Christoph Fröhlich, Dr. Denis, Felix Exner (fexner), Manuel Muth, Sai Kishor Kothakota
24+
525
4.15.0 (2024-08-05)
626
-------------------
727
* Add missing include for executors (`#1653 <https://github.com/ros-controls/ros2_control/issues/1653>`_)

controller_manager/controller_manager/spawner.py

Lines changed: 18 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -234,7 +234,7 @@ def main(args=None):
234234

235235
node.get_logger().info(
236236
bcolors.OKGREEN
237-
+ "Configured and activated all the parsed controllers list!"
237+
+ f"Configured and activated all the parsed controllers list : {controller_names}!"
238238
+ bcolors.ENDC
239239
)
240240

@@ -258,16 +258,25 @@ def main(args=None):
258258
)
259259
return 1
260260

261-
node.get_logger().info("Deactivated controller")
262-
263-
ret = unload_controller(node, controller_manager_name, controller_name)
264-
if not ret.ok:
265-
node.get_logger().error(
266-
bcolors.FAIL + "Failed to unload controller" + bcolors.ENDC
261+
node.get_logger().info(
262+
f"Successfully deactivated controllers : {controller_names}"
267263
)
268-
return 1
269264

270-
node.get_logger().info("Unloaded controller")
265+
unload_status = True
266+
for controller_name in controller_names:
267+
ret = unload_controller(node, controller_manager_name, controller_name)
268+
if not ret.ok:
269+
unload_status = False
270+
node.get_logger().error(
271+
bcolors.FAIL
272+
+ f"Failed to unload controller : {controller_name}"
273+
+ bcolors.ENDC
274+
)
275+
276+
if unload_status:
277+
node.get_logger().info(f"Successfully unloaded controllers : {controller_names}")
278+
else:
279+
return 1
271280
return 0
272281
except KeyboardInterrupt:
273282
pass

controller_manager/include/controller_manager/controller_manager.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -577,6 +577,7 @@ class ControllerManager : public rclcpp::Node
577577
std::map<std::string, std::vector<std::string>> controller_chained_reference_interfaces_cache_;
578578
std::map<std::string, std::vector<std::string>> controller_chained_state_interfaces_cache_;
579579

580+
rclcpp::NodeOptions cm_node_options_;
580581
std::string robot_description_;
581582
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
582583
rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;

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.15.0</version>
5+
<version>4.16.1</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>

0 commit comments

Comments
 (0)