diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index bd2d30f543e..aaa2667a062 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- +* add a broadcaster for range sensor (`#1091 `_) +* Contributors: flochre + 3.17.0 (2023-08-07) ------------------- diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp new file mode 100644 index 00000000000..baa9022c759 --- /dev/null +++ b/controller_interface/include/semantic_components/range_sensor.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 flochre +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ +#define SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ + +#include +#include +#include + +#include "semantic_components/semantic_component_interface.hpp" +#include "sensor_msgs/msg/range.hpp" + +namespace semantic_components +{ +class RangeSensor : public SemanticComponentInterface +{ +public: + explicit RangeSensor(const std::string & name) : SemanticComponentInterface(name, 1) + { + interface_names_.emplace_back(name_ + "/" + "range"); + } + + virtual ~RangeSensor() = default; + + /** + * Return Range reported by a sensor + * + * \return value of the range in meters + */ + float get_range() { return state_interfaces_[0].get().get_value(); } + + /// Return Range message with range in meters + /** + * Constructs and return a Range message from the current values. + * \return Range message from values; + */ + bool get_values_as_message(sensor_msgs::msg::Range & message) + { + // update the message values + message.range = get_range(); + + return true; + } +}; + +} // namespace semantic_components + +#endif // SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ diff --git a/controller_interface/package.xml b/controller_interface/package.xml index c0d8c271bd9..d960bdf28a1 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.17.0 + 3.18.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 3deafc5565a..2a9e348b8e2 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- +* Controller sorting and proper execution in a chain (Fixes `#853 `_) (`#1063 `_) +* Contributors: Sai Kishor Kothakota, Christoph Fröhlich, Dr Denis, Bence Magyar + 3.17.0 (2023-08-07) ------------------- * [CM] Fixes the issue with individual controller's update rate (`#1082 `_) diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst index 437cb705098..d79e730d3f7 100644 --- a/controller_manager/doc/controller_chaining.rst +++ b/controller_manager/doc/controller_chaining.rst @@ -63,48 +63,14 @@ After configuring a chainable controller, controller manager calls ``input_inter This is the same process as done by ``ResourceManager`` and hardware interfaces. Controller manager maintains "claimed" status of interface in a vector (the same as done in ``ResourceManager``). + Activation and Deactivation Chained Controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Controller Manager has an additional parameter that describes how controllers are chained. -In the first version, the parameter-structure would have some semantic meaning embedded into it, as follows: - - -.. code-block:: yaml - - controller_manager: - ros__parameters: - chained_controllers: - - - parallel_group_1: - - controller1_1 - - controller1_2 - - - parallel_group_2: - - controller2_1 - - - parallel_group_3: - - controller3_1 - - controller3_2 - - controller3_3 - - ... - - - parallel_group_N: - - controllerN_1 - - ... - - controllerN_M - - - -This structure is motivated by ``filter_chain`` structure from `ros/filters repository `__, see `this file for implementation `__. - -This structure is stored internally by controller manager into an ordered map (``std::map>``) with group name as key. -When a controller should be deactivated, the controller manager deactivates all the controllers in the preceding groups first. -All other controllers from the group stay active, as well as all controllers in the following groups. -NOTE: In the future this could be done more intelligently, i.e., deactivate only controllers in the preceding groups that actually precede the controller that should be deactivated. - -On the other hand, the controller should be manually activated in the reverse order, i.e., from the those closer to the hardware toward those preceding them. +Chained controllers must be activated and deactivated together or in the proper order. +This means you must first activate all following controllers to have the preceding one activated. +For the deactivation there is the inverse rule - all preceding controllers have to be deactivated before the following controller is deactivated. +One can also think of it as an actual chain, you can not add a chain link or break the chain in the middle. Debugging outputs diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 1d9b7821aa1..adb6769d9ca 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.17.0 + 3.18.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 57bb1595ffe..8072652c55b 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + 3.17.0 (2023-08-07) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index f55673871ca..f7b9eb5642a 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.17.0 + 3.18.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 6ca31d134aa..0428cf4f8e5 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + 3.17.0 (2023-08-07) ------------------- * Add checks if hardware is initialized. (`#1054 `_) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index fbe85e48e87..e3a2f4f71e0 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.17.0 + 3.18.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 313a4d83ed2..46acee3f8dd 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + 3.17.0 (2023-08-07) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 52e4bb82d1c..f1c0365cfdd 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.17.0 + 3.18.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index b400547e271..272c2d3fe46 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + 3.17.0 (2023-08-07) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 74b1bc46cc6..15a379933a0 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.17.0 + 3.18.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index fc362d9f3aa..d633c415d18 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + 3.17.0 (2023-08-07) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index a2ce000ccda..c77b8282760 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.17.0 + 3.18.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 1a8b32b1d38..a5a153faf1e 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + 3.17.0 (2023-08-07) ------------------- * Add info where the pdf is saved to view_controller_chains (`#1094 `_) diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 1af234dcf33..92eba51b626 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.17.0 + 3.18.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 49d96a5eb66..aa2b19b830f 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.17.0", + version="3.18.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 1b79cea368e..9996ff5a85f 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + 3.17.0 (2023-08-07) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 8ccf0bc2c13..4888459d49e 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.17.0 + 3.18.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index d46d1afeb7b..318cf556d2f 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.17.0", + version="3.18.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index e3e1d776f0b..c45b040c1b5 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + 3.17.0 (2023-08-07) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 8b0687f6d70..9e2b2fded26 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.17.0 + 3.18.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl