Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add fallback controllers list to the ControllerInfo #1503

Merged
4 changes: 4 additions & 0 deletions controller_manager/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -189,6 +189,10 @@ if(BUILD_TESTING)
test_controller
ros2_control_test_assets::ros2_control_test_assets
)

install(FILES test/test_controller_spawner_with_fallback_controllers.yaml
DESTINATION test)

install(FILES test/test_controller_spawner_with_type.yaml
DESTINATION test)

Expand Down
46 changes: 46 additions & 0 deletions controller_manager/controller_manager/spawner.py
Original file line number Diff line number Diff line change
Expand Up @@ -210,6 +210,14 @@ def main(args=None):
action="store_true",
required=False,
)
parser.add_argument(
"--fallback_controllers",
help="Fallback controllers list are activated as a fallback strategy when the"
" spawned controllers fail. When the argument is provided, it takes precedence over"
" the fallback_controllers list in the param file",
default=None,
nargs="+",
)

command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
args = parser.parse_args(command_line_args)
Expand Down Expand Up @@ -249,6 +257,7 @@ def main(args=None):
return 1

for controller_name in controller_names:
fallback_controllers = args.fallback_controllers
controller_type = args.controller_type
prefixed_controller_name = controller_name
if controller_namespace:
Expand Down Expand Up @@ -329,6 +338,43 @@ def main(args=None):
)
return 1

if not fallback_controllers and param_file:
saikishor marked this conversation as resolved.
Show resolved Hide resolved
fallback_controllers = get_parameter_from_param_file(
controller_name, param_file, "fallback_controllers"
)

if fallback_controllers:
parameter = Parameter()
parameter.name = prefixed_controller_name + ".fallback_controllers"
parameter.value = get_parameter_value(string_value=str(fallback_controllers))

response = call_set_parameters(
node=node, node_name=controller_manager_name, parameters=[parameter]
)
assert len(response.results) == 1
result = response.results[0]
if result.successful:
node.get_logger().info(
bcolors.OKCYAN
+ 'Setting fallback_controllers to ["'
+ ",".join(fallback_controllers)
+ '"] for '
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
else:
node.get_logger().fatal(
bcolors.FAIL
+ 'Could not set fallback_controllers to ["'
+ ",".join(fallback_controllers)
+ '"] for '
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
return 1

ret = load_controller(node, controller_manager_name, controller_name)
if not ret.ok:
node.get_logger().fatal(
Expand Down
11 changes: 11 additions & 0 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -482,6 +482,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
controller_spec.info.parameters_file = parameters_file;
}

const std::string fallback_ctrl_param = controller_name + ".fallback_controllers";
std::vector<std::string> fallback_controllers;
if (!has_parameter(fallback_ctrl_param))
{
declare_parameter(fallback_ctrl_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY);
}
if (get_parameter(fallback_ctrl_param, fallback_controllers) && !fallback_controllers.empty())
{
controller_spec.info.fallback_controllers_names = fallback_controllers;
}

return add_controller_impl(controller_spec);
}

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
ctrl_1:
ros__parameters:
joint_names: ["joint1"]

ctrl_2:
ros__parameters:
joint_names: ["joint2"]
fallback_controllers: ["ctrl_6", "ctrl_7", "ctrl_8"]

ctrl_3:
ros__parameters:
joint_names: ["joint3"]
fallback_controllers: ["ctrl_9"]
55 changes: 55 additions & 0 deletions controller_manager/test/test_spawner_unspawner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -324,3 +324,58 @@ TEST_F(TestLoadController, unload_on_kill)

ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
}

TEST_F(TestLoadController, spawner_test_fallback_controllers)
{
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
"/test/test_controller_spawner_with_fallback_controllers.yaml";

cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME));

ControllerManagerRunner cm_runner(this);
EXPECT_EQ(
call_spawner(
"ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 "
"-p " +
test_file_path),
0);

ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
{
auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}

// Try to spawn now the controller with fallback controllers inside the yaml
EXPECT_EQ(
call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0);

ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
{
auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8"));
ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto ctrl_3 = cm_->get_loaded_controllers()[2];
ASSERT_EQ(ctrl_3.info.name, "ctrl_3");
ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9"));
ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,9 @@ struct ControllerInfo

/// List of claimed interfaces by the controller.
std::vector<std::string> claimed_interfaces;

/// List of fallback controller names to be activated if this controller fails.
std::vector<std::string> fallback_controllers_names;
};

} // namespace hardware_interface
Expand Down
Loading