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

Fix the namespaced controller_manager spawner + added tests #1640

Merged
merged 13 commits into from
Jul 29, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions controller_manager/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,7 @@ if(BUILD_TESTING)

ament_add_gmock(test_spawner_unspawner
test/test_spawner_unspawner.cpp
TIMEOUT 120
)
target_link_libraries(test_spawner_unspawner
controller_manager
Expand Down
83 changes: 51 additions & 32 deletions controller_manager/controller_manager/spawner.py
Original file line number Diff line number Diff line change
Expand Up @@ -136,17 +136,20 @@ def is_controller_loaded(node, controller_manager, controller_name):
return any(c.name == controller_name for c in controllers)


def get_parameter_from_param_file(controller_name, parameter_file, parameter_name):
def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name):
with open(parameter_file) as f:
namespaced_controller = (
controller_name if namespace == "/" else f"{namespace}/{controller_name}"
)
parameters = yaml.safe_load(f)
if controller_name in parameters:
value = parameters[controller_name]
if namespaced_controller in parameters:
value = parameters[namespaced_controller]
if not isinstance(value, dict) or "ros__parameters" not in value:
raise RuntimeError(
f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {controller_name}"
f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}"
)
if parameter_name in parameters[controller_name]["ros__parameters"]:
return parameters[controller_name]["ros__parameters"][parameter_name]
if parameter_name in parameters[namespaced_controller]["ros__parameters"]:
return parameters[namespaced_controller]["ros__parameters"][parameter_name]
else:
return None

Expand All @@ -170,7 +173,11 @@ def main(args=None):
required=False,
)
parser.add_argument(
"-n", "--namespace", help="Namespace for the controller", default="", required=False
"-n",
"--namespace",
help="DEPRECATED Namespace for the controller_manager and the controller(s)",
default=None,
required=False,
)
parser.add_argument(
"--load-only",
Expand Down Expand Up @@ -217,7 +224,6 @@ def main(args=None):
args = parser.parse_args(command_line_args)
controller_names = args.controller_names
controller_manager_name = args.controller_manager
controller_namespace = args.namespace
param_file = args.param_file
controller_manager_timeout = args.controller_manager_timeout

Expand All @@ -226,9 +232,27 @@ def main(args=None):

node = Node("spawner_" + controller_names[0])

if node.get_namespace() != "/" and args.namespace:
raise RuntimeError(
f"Setting namespace through both '--namespace {args.namespace}' arg and the ROS 2 standard way "
f"'--ros-args -r __ns:={node.get_namespace()}' is not allowed!"
)

if args.namespace:
warnings.filterwarnings("always")
warnings.warn(
"The '--namespace' argument is deprecated and will be removed in future releases."
" Use the ROS 2 standard way of setting the node namespacing using --ros-args -r __ns:=<namespace>",
DeprecationWarning,
)

spawner_namespace = args.namespace if args.namespace else node.get_namespace()

if not spawner_namespace.startswith("/"):
spawner_namespace = f"/{spawner_namespace}"

if not controller_manager_name.startswith("/"):
spawner_namespace = node.get_namespace()
if spawner_namespace != "/":
if spawner_namespace and spawner_namespace != "/":
controller_manager_name = f"{spawner_namespace}/{controller_manager_name}"
else:
controller_manager_name = f"/{controller_manager_name}"
Expand All @@ -244,11 +268,8 @@ def main(args=None):

for controller_name in controller_names:
fallback_controllers = args.fallback_controllers
prefixed_controller_name = controller_name
if controller_namespace:
prefixed_controller_name = controller_namespace + "/" + controller_name

if is_controller_loaded(node, controller_manager_name, prefixed_controller_name):
if is_controller_loaded(node, controller_manager_name, controller_name):
node.get_logger().warn(
bcolors.WARNING
+ "Controller already loaded, skipping load_controller"
Expand All @@ -258,11 +279,13 @@ def main(args=None):
controller_type = (
None
if param_file is None
else get_parameter_from_param_file(controller_name, param_file, "type")
else get_parameter_from_param_file(
controller_name, spawner_namespace, param_file, "type"
)
)
if controller_type:
parameter = Parameter()
parameter.name = prefixed_controller_name + ".type"
parameter.name = controller_name + ".type"
parameter.value = get_parameter_value(string_value=controller_type)

response = call_set_parameters(
Expand All @@ -277,7 +300,7 @@ def main(args=None):
+ controller_type
+ '" for '
+ bcolors.BOLD
+ prefixed_controller_name
+ controller_name
+ bcolors.ENDC
)
else:
Expand All @@ -287,14 +310,14 @@ def main(args=None):
+ controller_type
+ '" for '
+ bcolors.BOLD
+ prefixed_controller_name
+ controller_name
+ bcolors.ENDC
)
return 1

if param_file:
parameter = Parameter()
parameter.name = prefixed_controller_name + ".params_file"
parameter.name = controller_name + ".params_file"
parameter.value = get_parameter_value(string_value=param_file)

response = call_set_parameters(
Expand All @@ -309,7 +332,7 @@ def main(args=None):
+ param_file
+ '" for '
+ bcolors.BOLD
+ prefixed_controller_name
+ controller_name
+ bcolors.ENDC
)
else:
Expand All @@ -319,19 +342,19 @@ def main(args=None):
+ param_file
+ '" for '
+ bcolors.BOLD
+ prefixed_controller_name
+ controller_name
+ bcolors.ENDC
)
return 1

if not fallback_controllers and param_file:
fallback_controllers = get_parameter_from_param_file(
controller_name, param_file, "fallback_controllers"
controller_name, spawner_namespace, param_file, "fallback_controllers"
)

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

response = call_set_parameters(
Expand All @@ -346,7 +369,7 @@ def main(args=None):
+ ",".join(fallback_controllers)
+ '"] for '
+ bcolors.BOLD
+ prefixed_controller_name
+ controller_name
+ bcolors.ENDC
)
else:
Expand All @@ -356,7 +379,7 @@ def main(args=None):
+ ",".join(fallback_controllers)
+ '"] for '
+ bcolors.BOLD
+ prefixed_controller_name
+ controller_name
+ bcolors.ENDC
)
return 1
Expand All @@ -367,16 +390,12 @@ def main(args=None):
bcolors.FAIL
+ "Failed loading controller "
+ bcolors.BOLD
+ prefixed_controller_name
+ controller_name
+ bcolors.ENDC
)
return 1
node.get_logger().info(
bcolors.OKBLUE
+ "Loaded "
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
bcolors.OKBLUE + "Loaded " + bcolors.BOLD + controller_name + bcolors.ENDC
)

if not args.load_only:
Expand All @@ -401,7 +420,7 @@ def main(args=None):
bcolors.OKGREEN
+ "Configured and activated "
+ bcolors.BOLD
+ prefixed_controller_name
+ controller_name
+ bcolors.ENDC
)

Expand Down
10 changes: 6 additions & 4 deletions controller_manager/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ There are two scripts to interact with controller manager from launch files:
controller_name

positional arguments:
controller_name Name of the controller
controller_names List of controllers

options:
-h, --help show this help message and exit
Expand All @@ -158,14 +158,16 @@ There are two scripts to interact with controller manager from launch files:
-p PARAM_FILE, --param-file PARAM_FILE
Controller param file to be loaded into controller node before configure
-n NAMESPACE, --namespace NAMESPACE
Namespace for the controller
DEPRECATED Namespace for the controller_manager and the controller(s)
--load-only Only load the controller and leave unconfigured.
--inactive Load and configure the controller, however do not activate them
-t CONTROLLER_TYPE, --controller-type CONTROLLER_TYPE
If not provided it should exist in the controller manager namespace
-u, --unload-on-kill Wait until this application is interrupted and unload controller
--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT
Time to wait for the controller manager
--activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether
--fallback_controllers FALLBACK_CONTROLLERS [FALLBACK_CONTROLLERS ...]
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


``unspawner``
Expand Down
5 changes: 3 additions & 2 deletions controller_manager/test/controller_manager_test_common.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,14 +61,15 @@ class ControllerManagerFixture : public ::testing::Test
{
public:
explicit ControllerManagerFixture(
const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf)
const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf,
const std::string & cm_namespace = "")
: robot_description_(robot_description)
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
cm_ = std::make_shared<CtrlMgr>(
std::make_unique<hardware_interface::ResourceManager>(
rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
executor_, TEST_CM_NAME);
executor_, TEST_CM_NAME, cm_namespace);
// We want to be able to not pass robot description immediately
if (!robot_description_.empty())
{
Expand Down
14 changes: 14 additions & 0 deletions controller_manager/test/test_controller_spawner_with_type.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -11,3 +11,17 @@ chainable_ctrl_with_parameters_and_type:
ctrl_with_parameters_and_no_type:
ros__parameters:
joint_names: ["joint2"]

/foo_namespace/ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_controller"
joint_names: ["joint1"]

/foo_namespace/chainable_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_chainable_controller"
joint_names: ["joint1"]

/foo_namespace/ctrl_with_parameters_and_no_type:
ros__parameters:
joint_names: ["joint2"]
Loading
Loading