Skip to content

Commit 72f5b76

Browse files
destoglmamueluth
destogl
authored and
mamueluth
committed
Make forward controller chainable.
Enabling chanable mode for forward command controllers. Fix all bugs in chained-controllers mode. Remove debug output from forwarding controller. Updated example ffwd chainable controller for new structure.
1 parent 51c982a commit 72f5b76

22 files changed

+763
-179
lines changed

‎forward_command_controller/CMakeLists.txt

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,8 @@ find_package(std_msgs REQUIRED)
2222

2323
add_library(forward_command_controller
2424
SHARED
25+
src/chainable_forward_controller.cpp
26+
src/forward_controller.cpp
2527
src/forward_controllers_base.cpp
2628
src/forward_command_controller.cpp
2729
src/multi_interface_forward_command_controller.cpp
@@ -101,6 +103,31 @@ if(BUILD_TESTING)
101103
target_link_libraries(test_multi_interface_forward_command_controller
102104
forward_command_controller
103105
)
106+
107+
ament_add_gmock(
108+
test_load_chained_forward_command_controller
109+
test/test_load_chained_forward_command_controller.cpp
110+
)
111+
target_include_directories(test_load_chained_forward_command_controller PRIVATE include)
112+
ament_target_dependencies(
113+
test_load_chained_forward_command_controller
114+
controller_manager
115+
hardware_interface
116+
ros2_control_test_assets
117+
)
118+
119+
ament_add_gmock(
120+
test_load_chained_multi_interface_forward_command_controller
121+
test/test_load_chained_multi_interface_forward_command_controller.cpp
122+
)
123+
target_include_directories(test_load_chained_multi_interface_forward_command_controller PRIVATE include)
124+
ament_target_dependencies(
125+
test_load_chained_multi_interface_forward_command_controller
126+
controller_manager
127+
hardware_interface
128+
ros2_control_test_assets
129+
)
130+
104131
endif()
105132

106133
ament_export_dependencies(

‎forward_command_controller/forward_command_plugin.xml

Lines changed: 52 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,61 @@
1+
<!--<library path="forward_command_controller">
2+
<class name="forward_command_controller/ForwardCommandController"
3+
type="forward_command_controller::ForwardCommandController"
4+
base_class_type="controller_interface::ControllerInterface">
5+
<description>
6+
The forward command controller commands a group of joints in a given interface
7+
</description>
8+
</class>
9+
<class name="forward_command_controller/MultiInterfaceForwardCommandController"
10+
type="forward_command_controller::MultiInterfaceForwardCommandController"
11+
base_class_type="controller_interface::ControllerInterface">
12+
<description>
13+
MultiInterfaceForwardController ros2_control controller.
14+
</description>
15+
</class>
16+
17+
<class name="forward_command_controller/ChainableForwardCommandController"
18+
type="forward_command_controller::ChainableForwardCommandController"
19+
base_class_type="controller_interface::ControllerInterface">
20+
<description>
21+
The forward command controller commands a group of joints in a given interface
22+
</description>
23+
</class>
24+
<class name="forward_command_controller/ChainableMultiInterfaceForwardCommandController"
25+
type="forward_command_controller::ChainableMultiInterfaceForwardCommandController"
26+
base_class_type="controller_interface::ControllerInterface">
27+
<description>
28+
MultiInterfaceForwardController ros2_control controller.
29+
</description>
30+
</class>
31+
</library>-->
32+
133
<library path="forward_command_controller">
2-
<class name="forward_command_controller/ForwardCommandController" type="forward_command_controller::ForwardCommandController" base_class_type="controller_interface::ControllerInterface">
34+
<class name="forward_command_controller/ForwardCommandController"
35+
type="forward_command_controller::ForwardCommandController"
36+
base_class_type="controller_interface::ControllerInterface">
337
<description>
438
The forward command controller commands a group of joints in a given interface
539
</description>
640
</class>
741
<class name="forward_command_controller/MultiInterfaceForwardCommandController"
8-
type="forward_command_controller::MultiInterfaceForwardCommandController" base_class_type="controller_interface::ControllerInterface">
42+
type="forward_command_controller::MultiInterfaceForwardCommandController"
43+
base_class_type="controller_interface::ControllerInterface">
44+
<description>
45+
MultiInterfaceForwardController ros2_control controller.
46+
</description>
47+
</class>
48+
49+
<class name="forward_command_controller/ChainableForwardCommandController"
50+
type="forward_command_controller::ChainableForwardCommandController"
51+
base_class_type="controller_interface::ChainableControllerInterface">
52+
<description>
53+
The forward command controller commands a group of joints in a given interface
54+
</description>
55+
</class>
56+
<class name="forward_command_controller/ChainableMultiInterfaceForwardCommandController"
57+
type="forward_command_controller::ChainableMultiInterfaceForwardCommandController"
58+
base_class_type="controller_interface::ChainableControllerInterface">
959
<description>
1060
MultiInterfaceForwardController ros2_control controller.
1161
</description>

‎forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_
16+
#define FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_
17+
18+
#include <memory>
19+
#include <string>
20+
#include <vector>
21+
22+
#include "controller_interface/chainable_controller_interface.hpp"
23+
#include "forward_command_controller/forward_controllers_base.hpp"
24+
#include "forward_command_controller/visibility_control.h"
25+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
26+
27+
namespace forward_command_controller
28+
{
29+
using CmdType = std_msgs::msg::Float64MultiArray;
30+
31+
/**
32+
* \brief Forward command controller for a set of joints and interfaces.
33+
*
34+
* This class forwards the command signal down to a set of joints or interfaces.
35+
*
36+
* Subscribes to:
37+
* - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply.
38+
*/
39+
class ChainableForwardController : public ForwardControllersBase,
40+
public controller_interface::ChainableControllerInterface
41+
{
42+
public:
43+
FORWARD_COMMAND_CONTROLLER_PUBLIC
44+
ChainableForwardController();
45+
46+
FORWARD_COMMAND_CONTROLLER_PUBLIC
47+
~ChainableForwardController() = default;
48+
49+
FORWARD_COMMAND_CONTROLLER_PUBLIC
50+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
51+
52+
FORWARD_COMMAND_CONTROLLER_PUBLIC
53+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
54+
55+
FORWARD_COMMAND_CONTROLLER_PUBLIC
56+
controller_interface::CallbackReturn on_init() override;
57+
58+
FORWARD_COMMAND_CONTROLLER_PUBLIC
59+
controller_interface::CallbackReturn on_configure(
60+
const rclcpp_lifecycle::State & previous_state) override;
61+
62+
FORWARD_COMMAND_CONTROLLER_PUBLIC
63+
controller_interface::CallbackReturn on_activate(
64+
const rclcpp_lifecycle::State & previous_state) override;
65+
66+
FORWARD_COMMAND_CONTROLLER_PUBLIC
67+
controller_interface::CallbackReturn on_deactivate(
68+
const rclcpp_lifecycle::State & previous_state) override;
69+
70+
protected:
71+
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
72+
73+
bool on_set_chained_mode(bool chained_mode) override;
74+
75+
controller_interface::return_type update_reference_from_subscribers() override;
76+
77+
controller_interface::return_type update_and_write_commands(
78+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
79+
80+
std::vector<std::string> reference_interface_names_;
81+
};
82+
83+
} // namespace forward_command_controller
84+
85+
#endif // FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_

‎forward_command_controller/include/forward_command_controller/forward_command_controller.hpp

Lines changed: 66 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -18,8 +18,11 @@
1818
#include <string>
1919
#include <vector>
2020

21+
#include "forward_command_controller/chainable_forward_controller.hpp"
22+
#include "forward_command_controller/forward_controller.hpp"
2123
#include "forward_command_controller/forward_controllers_base.hpp"
2224
#include "forward_command_controller/visibility_control.h"
25+
#include "rclcpp_lifecycle/lifecycle_node.hpp"
2326

2427
namespace forward_command_controller
2528
{
@@ -34,20 +37,79 @@ namespace forward_command_controller
3437
* Subscribes to:
3538
* - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply.
3639
*/
37-
class ForwardCommandController : public ForwardControllersBase
40+
template <
41+
typename T,
42+
typename std::enable_if<
43+
std::is_convertible<T *, forward_command_controller::ForwardControllersBase *>::value,
44+
T>::type * = nullptr,
45+
typename std::enable_if<
46+
std::is_convertible<T *, controller_interface::ControllerInterfaceBase *>::value, T>::type * =
47+
nullptr>
48+
class BaseForwardCommandController : public T
3849
{
3950
public:
4051
FORWARD_COMMAND_CONTROLLER_PUBLIC
41-
ForwardCommandController();
52+
BaseForwardCommandController() : T() {}
4253

4354
protected:
44-
void declare_parameters() override;
45-
controller_interface::CallbackReturn read_parameters() override;
55+
void declare_parameters() override
56+
{
57+
controller_interface::ControllerInterfaceBase::auto_declare<std::vector<std::string>>(
58+
"joints", std::vector<std::string>());
59+
controller_interface::ControllerInterfaceBase::auto_declare<std::string>("interface_name", "");
60+
};
61+
62+
controller_interface::CallbackReturn read_parameters() override
63+
{
64+
joint_names_ = T::get_node()->get_parameter("joints").as_string_array();
65+
66+
if (joint_names_.empty())
67+
{
68+
RCLCPP_ERROR(T::get_node()->get_logger(), "'joints' parameter was empty");
69+
return controller_interface::CallbackReturn::ERROR;
70+
}
71+
72+
// Specialized, child controllers set interfaces before calling configure function.
73+
if (interface_name_.empty())
74+
{
75+
interface_name_ = T::get_node()->get_parameter("interface_name").as_string();
76+
}
77+
78+
if (interface_name_.empty())
79+
{
80+
RCLCPP_ERROR(T::get_node()->get_logger(), "'interface_name' parameter was empty");
81+
return controller_interface::CallbackReturn::ERROR;
82+
}
83+
84+
for (const auto & joint : joint_names_)
85+
{
86+
T::command_interface_names_.push_back(joint + "/" + interface_name_);
87+
}
88+
89+
return controller_interface::CallbackReturn::SUCCESS;
90+
};
4691

4792
std::vector<std::string> joint_names_;
4893
std::string interface_name_;
4994
};
5095

96+
class ForwardCommandController : public BaseForwardCommandController<ForwardController>
97+
{
98+
public:
99+
FORWARD_COMMAND_CONTROLLER_PUBLIC
100+
ForwardCommandController() : BaseForwardCommandController<ForwardController>() {}
101+
};
102+
103+
class ChainableForwardCommandController
104+
: public BaseForwardCommandController<ChainableForwardController>
105+
{
106+
public:
107+
FORWARD_COMMAND_CONTROLLER_PUBLIC
108+
ChainableForwardCommandController() : BaseForwardCommandController<ChainableForwardController>()
109+
{
110+
}
111+
};
112+
51113
} // namespace forward_command_controller
52114

53115
#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_

‎forward_command_controller/include/forward_command_controller/forward_controller.hpp

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_
16+
#define FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_
17+
18+
#include "controller_interface/controller_interface.hpp"
19+
#include "forward_command_controller/forward_controllers_base.hpp"
20+
#include "forward_command_controller/visibility_control.h"
21+
22+
namespace forward_command_controller
23+
{
24+
using CmdType = std_msgs::msg::Float64MultiArray;
25+
26+
/**
27+
* \brief Forward command controller for a set of joints and interfaces.
28+
*/
29+
class ForwardController : public ForwardControllersBase,
30+
public controller_interface::ControllerInterface
31+
{
32+
public:
33+
FORWARD_COMMAND_CONTROLLER_PUBLIC
34+
ForwardController();
35+
36+
FORWARD_COMMAND_CONTROLLER_PUBLIC
37+
~ForwardController() = default;
38+
39+
FORWARD_COMMAND_CONTROLLER_PUBLIC
40+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
41+
42+
FORWARD_COMMAND_CONTROLLER_PUBLIC
43+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
44+
45+
FORWARD_COMMAND_CONTROLLER_PUBLIC
46+
controller_interface::CallbackReturn on_init() override;
47+
48+
FORWARD_COMMAND_CONTROLLER_PUBLIC
49+
controller_interface::CallbackReturn on_configure(
50+
const rclcpp_lifecycle::State & previous_state) override;
51+
52+
FORWARD_COMMAND_CONTROLLER_PUBLIC
53+
controller_interface::CallbackReturn on_activate(
54+
const rclcpp_lifecycle::State & previous_state) override;
55+
56+
FORWARD_COMMAND_CONTROLLER_PUBLIC
57+
controller_interface::CallbackReturn on_deactivate(
58+
const rclcpp_lifecycle::State & previous_state) override;
59+
60+
FORWARD_COMMAND_CONTROLLER_PUBLIC
61+
controller_interface::return_type update(
62+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
63+
};
64+
65+
} // namespace forward_command_controller
66+
67+
#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_

0 commit comments

Comments
 (0)