Skip to content

Commit

Permalink
Merge branch 'master' into add-simple-joint-limiter
Browse files Browse the repository at this point in the history
  • Loading branch information
destogl authored Aug 30, 2023
2 parents 098a431 + c122455 commit c184da8
Show file tree
Hide file tree
Showing 31 changed files with 1,237 additions and 76 deletions.
8 changes: 8 additions & 0 deletions controller_interface/CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,14 @@
Changelog for package controller_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

3.18.0 (2023-08-17)
-------------------
* add a broadcaster for range sensor (`#1091 <https://github.com/ros-controls/ros2_control/issues/1091>`_)
* Contributors: flochre

3.17.0 (2023-08-07)
-------------------

3.16.0 (2023-07-09)
-------------------

Expand Down
60 changes: 60 additions & 0 deletions controller_interface/include/semantic_components/range_sensor.hpp
Original file line number Diff line number Diff line change
@@ -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 <limits>
#include <string>
#include <vector>

#include "semantic_components/semantic_component_interface.hpp"
#include "sensor_msgs/msg/range.hpp"

namespace semantic_components
{
class RangeSensor : public SemanticComponentInterface<sensor_msgs::msg::Range>
{
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_
2 changes: 1 addition & 1 deletion controller_interface/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>controller_interface</name>
<version>3.16.0</version>
<version>3.18.0</version>
<description>Description of controller_interface</description>
<maintainer email="[email protected]">Bence Magyar</maintainer>
<maintainer email="[email protected]">Denis Štogl</maintainer>
Expand Down
11 changes: 11 additions & 0 deletions controller_manager/CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,17 @@
Changelog for package controller_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

3.18.0 (2023-08-17)
-------------------
* Controller sorting and proper execution in a chain (Fixes `#853 <https://github.com/ros-controls/ros2_control/issues/853>`_) (`#1063 <https://github.com/ros-controls/ros2_control/issues/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 <https://github.com/ros-controls/ros2_control/issues/1082>`_)
* Fix deprecation warning (`#1093 <https://github.com/ros-controls/ros2_control/issues/1093>`_)
* Contributors: Christoph Fröhlich, Sai Kishor Kothakota

3.16.0 (2023-07-09)
-------------------
* added controller manager runner to have update cycles (`#1075 <https://github.com/ros-controls/ros2_control/issues/1075>`_)
Expand Down
44 changes: 5 additions & 39 deletions controller_manager/doc/controller_chaining.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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 <https://github.com/ros/filters/tree/ros2>`__, see `this file for implementation <https://github.com/ros/filters/blob/ros2/include/filters/filter_chain.hpp>`__.

This structure is stored internally by controller manager into an ordered map (``std::map<std::string, std::vector<std::string>>``) 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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -389,6 +389,29 @@ class ControllerManager : public rclcpp::Node
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it);

/// A method to be used in the std::sort method to sort the controllers to be able to
/// execute them in a proper order
/**
* Compares the controllers ctrl_a and ctrl_b and then returns which comes first in the sequence
*
* @note The following conditions needs to be handled while ordering the controller list
* 1. The controllers that do not use any state or command interfaces are updated first
* 2. The controllers that use only the state system interfaces only are updated next
* 3. The controllers that use any of an another controller's reference interface are updated
* before the preceding controller
* 4. The controllers that use the controller's estimated interfaces are updated after the
* preceding controller
* 5. The controllers that only use the hardware command interfaces are updated last
* 6. All inactive controllers go at the end of the list
*
* \param[in] controllers list of controllers to compare their names to interface's prefix.
*
* @return true, if ctrl_a needs to execute first, else false
*/
bool controller_sorting(
const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b,
const std::vector<controller_manager::ControllerSpec> & controllers);

void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
diagnostic_updater::Updater diagnostics_updater_;

Expand Down
2 changes: 1 addition & 1 deletion controller_manager/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>controller_manager</name>
<version>3.16.0</version>
<version>3.18.0</version>
<description>Description of controller_manager</description>
<maintainer email="[email protected]">Bence Magyar</maintainer>
<maintainer email="[email protected]">Denis Štogl</maintainer>
Expand Down
Loading

0 comments on commit c184da8

Please sign in to comment.