[ros2controlcli] add params file parsing to load_controller verb and …

…add namespacing support (ros-controls#1703)

controller_manager/controller_manager/controller_manager_services.py

@@ -88,15 +88,23 @@ def service_caller(
  @return The service response
 
  """
- cli = node.create_client(service_type, service_name)
+ namespace = "" if node.get_namespace() == "/" else node.get_namespace()
+ fully_qualified_service_name = (
+ f"{namespace}/{service_name}" if not service_name.startswith("/") else service_name
+ )
+ cli = node.create_client(service_type, fully_qualified_service_name)
 
  while not cli.service_is_ready():
- node.get_logger().info(f"waiting for service {service_name} to become available...")
+ node.get_logger().info(
+ f"waiting for service {fully_qualified_service_name} to become available..."
+ )
  if service_timeout:
  if not cli.wait_for_service(service_timeout):
- raise ServiceNotFoundError(f"Could not contact service {service_name}")
+ raise ServiceNotFoundError(
+ f"Could not contact service {fully_qualified_service_name}"
+ )
  elif not cli.wait_for_service(10.0):
- node.get_logger().warn(f"Could not contact service {service_name}")
+ node.get_logger().warn(f"Could not contact service {fully_qualified_service_name}")
 
  node.get_logger().debug(f"requester: making request: {request}\n")
  future = None
@@ -105,13 +113,13 @@ def service_caller(
  rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout)
  if future.result() is None:
  node.get_logger().warning(
- f"Failed getting a result from calling {service_name} in "
+ f"Failed getting a result from calling {fully_qualified_service_name} in "
  f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)"
  )
  else:
  return future.result()
  raise RuntimeError(
- f"Could not successfully call service {service_name} after {max_attempts} attempts."
+ f"Could not successfully call service {fully_qualified_service_name} after {max_attempts} attempts."
  )
 
 

doc/release_notes.rst

@@ -118,3 +118,15 @@ ros2controlcli
  .. code-block:: bash
 
  ros2 control set_hardware_component_state <hardware_component_name> <state>
+
+* The ``load_controller`` now supports parsing of the params file (`#1703 <https://github.com/ros-controls/ros2_control/pull/1703>`_).
+
+ .. code-block:: bash
+
+ ros2 control load_controller <controller_name> <realtive_or_absolute_file_path>
+
+* All the ros2controlcli verbs now support the namespacing through the ROS 2 standard way (`#1703 <https://github.com/ros-controls/ros2_control/pull/1703>`_).
+
+ .. code-block:: bash
+
+ ros2 control <verb> <arguments> --ros-args -r __ns:=<namespace>

ros2controlcli/ros2controlcli/api/__init__.py

@@ -15,40 +15,13 @@
 
 from controller_manager import list_controllers, list_hardware_components
 
-import rclpy
-
 from ros2cli.node.direct import DirectNode
 
 from ros2node.api import NodeNameCompleter
 
 from ros2param.api import call_list_parameters
 
-
-def service_caller(service_name, service_type, request):
- try:
- rclpy.init()
-
- node = rclpy.create_node(f"ros2controlcli_{service_name.replace('/', '')}_requester")
-
- cli = node.create_client(service_type, service_name)
-
- if not cli.service_is_ready():
- node.get_logger().debug(f"waiting for service {service_name} to become available...")
-
- if not cli.wait_for_service(2.0):
- raise RuntimeError(f"Could not contact service {service_name}")
-
- node.get_logger().debug(f"requester: making request: {repr(request)}\n")
- future = cli.call_async(request)
- rclpy.spin_until_future_complete(node, future)
- if future.result() is not None:
- return future.result()
- else:
- future_exception = future.exception()
- raise RuntimeError(f"Exception while calling service: {repr(future_exception)}")
- finally:
- node.destroy_node()
- rclpy.shutdown()
+import argparse
 
 
 class ControllerNameCompleter:
@@ -89,16 +62,28 @@ def __call__(self, prefix, parsed_args, **kwargs):
  return [c.name for c in hardware_components if c.state.label in self.valid_states]
 
 
+class ParserROSArgs(argparse.Action):
+ def __call__(self, parser, namespace, values, option_string=None):
+ values = [option_string] + values
+ setattr(namespace, "argv", values)
+
+
 def add_controller_mgr_parsers(parser):
- """Parser arguments to get controller manager node name, defaults to /controller_manager."""
+ """Parser arguments to get controller manager node name, defaults to controller_manager."""
  arg = parser.add_argument(
  "-c",
  "--controller-manager",
- help="Name of the controller manager ROS node",
- default="/controller_manager",
+ help="Name of the controller manager ROS node (default: controller_manager)",
+ default="controller_manager",
  required=False,
  )
  arg.completer = NodeNameCompleter(include_hidden_nodes_key="include_hidden_nodes")
  parser.add_argument(
  "--include-hidden-nodes", action="store_true", help="Consider hidden nodes as well"
  )
+ parser.add_argument(
+ "--ros-args",
+ nargs=argparse.REMAINDER,
+ help="Pass arbitrary arguments to the executable",
+ action=ParserROSArgs,
+ )

ros2controlcli/ros2controlcli/verb/list_controller_types.py

@@ -29,7 +29,7 @@ def add_arguments(self, parser, cli_name):
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
+ with NodeStrategy(args).direct_node as node:
  response = list_controller_types(node, args.controller_manager)
  types_and_classes = zip(response.types, response.base_classes)
  for c in types_and_classes:

ros2controlcli/ros2controlcli/verb/list_controllers.py

@@ -95,7 +95,7 @@ def add_arguments(self, parser, cli_name):
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
+ with NodeStrategy(args).direct_node as node:
  response = list_controllers(node, args.controller_manager)
 
  if not response.controller:

ros2controlcli/ros2controlcli/verb/list_hardware_components.py

@@ -36,7 +36,7 @@ def add_arguments(self, parser, cli_name):
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
+ with NodeStrategy(args).direct_node as node:
  hardware_components = list_hardware_components(node, args.controller_manager)
 
  for idx, component in enumerate(hardware_components.component):

ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py

@@ -28,7 +28,7 @@ def add_arguments(self, parser, cli_name):
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
+ with NodeStrategy(args).direct_node as node:
  hardware_interfaces = list_hardware_interfaces(node, args.controller_manager)
  command_interfaces = sorted(
  hardware_interfaces.command_interfaces, key=lambda hwi: hwi.name

ros2controlcli/ros2controlcli/verb/load_controller.py

@@ -12,13 +12,22 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from controller_manager import configure_controller, load_controller, switch_controllers
+from controller_manager import (
+ configure_controller,
+ load_controller,
+ list_controllers,
+ switch_controllers,
+ set_controller_parameters_from_param_file,
+ bcolors,
+)
 
 from ros2cli.node.direct import add_arguments
 from ros2cli.node.strategy import NodeStrategy
 from ros2cli.verb import VerbExtension
 
 from ros2controlcli.api import add_controller_mgr_parsers, ControllerNameCompleter
+import os
+from argparse import OPTIONAL
 
 
 class LoadControllerVerb(VerbExtension):
@@ -28,37 +37,79 @@ def add_arguments(self, parser, cli_name):
  add_arguments(parser)
  arg = parser.add_argument("controller_name", help="Name of the controller")
  arg.completer = ControllerNameCompleter()
+ arg = parser.add_argument(
+ "param_file",
+ help="The YAML file with the controller parameters",
+ nargs=OPTIONAL,
+ default=None,
+ )
  arg = parser.add_argument(
  "--set-state",
  choices=["inactive", "active"],
  help="Set the state of the loaded controller",
+ default=None,
  )
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
- response = load_controller(node, args.controller_manager, args.controller_name)
- if not response.ok:
- return "Error loading controller, check controller_manager logs"
+ with NodeStrategy(args).direct_node as node:
+ controllers = list_controllers(node, args.controller_manager, 20.0).controller
+ if any(c.name == args.controller_name for c in controllers):
+ print(
+ f"{bcolors.WARNING}Controller : {args.controller_name} already loaded, skipping load_controller!{bcolors.ENDC}"
+ )
+ else:
+ if args.param_file:
+ if not os.path.exists(args.param_file):
+ print(
+ f"{bcolors.FAIL}Controller parameter file : {args.param_file} does not exist, Aborting!{bcolors.ENDC}"
+ )
+ return 1
+ if not os.path.isabs(args.param_file):
+ args.param_file = os.path.join(os.getcwd(), args.param_file)
 
- if not args.set_state:
- print(f"Successfully loaded controller {args.controller_name}")
- return 0
+ if not set_controller_parameters_from_param_file(
+ node,
+ args.controller_manager,
+ args.controller_name,
+ args.param_file,
+ node.get_namespace(),
+ ):
+ return 1
 
- # we in any case configure the controller
- response = configure_controller(node, args.controller_manager, args.controller_name)
- if not response.ok:
- return "Error configuring controller"
+ ret = load_controller(node, args.controller_manager, args.controller_name)
+ if not ret.ok:
+ print(
+ f"{bcolors.FAIL}Failed loading controller {args.controller_name} check controller_manager logs{bcolors.ENDC}"
+ )
+ return 1
+ print(
+ f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name}{bcolors.ENDC}"
+ )
 
- if args.set_state == "active":
- response = switch_controllers(
- node, args.controller_manager, [], [args.controller_name], True, True, 5.0
+ if args.set_state:
+
+ # we in any case configure the controller
+ response = configure_controller(
+ node, args.controller_manager, args.controller_name
  )
  if not response.ok:
- return "Error activating controller, check controller_manager logs"
+ print(
+ f"{bcolors.FAIL}Error configuring controller : {args.controller_name}{bcolors.ENDC}"
+ )
+ return 1
+
+ if args.set_state == "active":
+ response = switch_controllers(
+ node, args.controller_manager, [], [args.controller_name], True, True, 5.0
+ )
+ if not response.ok:
+ print(
+ f"{bcolors.FAIL}Error activating controller : {args.controller_name}, check controller_manager logs{bcolors.ENDC}"
+ )
+ return 1
 
- print(
- f"Successfully loaded controller {args.controller_name} into "
- f'state {"inactive" if args.set_state == "inactive" else "active"}'
- )
- return 0
+ print(
+ f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name} into state {args.set_state}{bcolors.ENDC}"
+ )
+ return 0

ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py

@@ -32,7 +32,7 @@ def add_arguments(self, parser, cli_name):
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
+ with NodeStrategy(args).direct_node as node:
  response = reload_controller_libraries(
  node, args.controller_manager, force_kill=args.force_kill
  )

ros2controlcli/ros2controlcli/verb/set_controller_state.py

@@ -37,7 +37,7 @@ def add_arguments(self, parser, cli_name):
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
+ with NodeStrategy(args).direct_node as node:
  controllers = list_controllers(node, args.controller_manager).controller
 
  try:

ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py

@@ -39,7 +39,7 @@ def add_arguments(self, parser, cli_name):
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
+ with NodeStrategy(args).direct_node as node:
 
  if args.state == "unconfigured":
 

ros2controlcli/ros2controlcli/verb/switch_controllers.py

@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from controller_manager import switch_controllers
+from controller_manager import switch_controllers, bcolors
 
 from ros2cli.node.direct import add_arguments
 from ros2cli.node.strategy import NodeStrategy
@@ -52,7 +52,7 @@ def add_arguments(self, parser, cli_name):
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
+ with NodeStrategy(args).direct_node as node:
  response = switch_controllers(
  node,
  args.controller_manager,
@@ -63,7 +63,12 @@ def main(self, *, args):
  args.switch_timeout,
  )
  if not response.ok:
- return "Error switching controllers, check controller_manager logs"
+ print(
+ bcolors.FAIL
+ + "Error switching controllers, check controller_manager logs"
+ + bcolors.ENDC
+ )
+ return 1
 
- print("Successfully switched controllers")
+ print(bcolors.OKBLUE + "Successfully switched controllers" + bcolors.ENDC)
  return 0

ros2controlcli/ros2controlcli/verb/unload_controller.py

@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from controller_manager import unload_controller
+from controller_manager import unload_controller, bcolors
 
 from ros2cli.node.direct import add_arguments
 from ros2cli.node.strategy import NodeStrategy
@@ -31,10 +31,15 @@ def add_arguments(self, parser, cli_name):
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
+ with NodeStrategy(args).direct_node as node:
  response = unload_controller(node, args.controller_manager, args.controller_name)
  if not response.ok:
- return "Error unloading controllers, check controller_manager logs"
-
- print(f"Successfully unloaded controller {args.controller_name}")
+ print(
+ f"{bcolors.FAIL}Error unloading controller {args.controller_name}, check controller_manager logs{bcolors.ENDC}"
+ )
+ return 1
+
+ print(
+ f"{bcolors.OKBLUE}Successfully unloaded controller {args.controller_name}{bcolors.ENDC}"
+ )
  return 0

ros2controlcli/ros2controlcli/verb/view_controller_chains.py

@@ -202,7 +202,7 @@ def add_arguments(self, parser, cli_name):
  add_controller_mgr_parsers(parser)
 
  def main(self, *, args):
- with NodeStrategy(args) as node:
+ with NodeStrategy(args).direct_node as node:
  list_controllers_response = list_controllers(node, args.controller_manager)
  list_hardware_response = list_hardware_interfaces(node, args.controller_manager)
  parse_response(list_controllers_response, list_hardware_response)