Fail on invalid and unknown ROS specific arguments (#415)

* Fail on invalid and unknown ROS specific arguments.

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

* Address peer review comments.

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

rclpy/src/rclpy/_rclpy.c

@@ -39,6 +39,9 @@
 #include "rclpy_common/common.h"
 #include "./_rclpy_qos_event.c"
 
+static PyObject * RCLInvalidROSArgsError;
+static PyObject * UnknownROSArgsError;
+
 void
 _rclpy_context_capsule_destructor(PyObject * capsule)
 {
@@ -286,6 +289,48 @@ _rclpy_pyargs_to_list(PyObject * pyargs, int * num_args, char *** arg_values)
   return RCL_RET_OK;
 }
 
+/// Raise an UnknownROSArgsError exception
+/* \param[in] pyargs a sequence of string args
+ * \param[in] unknown_ros_args_count the number of unknown ROS args
+ * \param[in] unknown_ros_args_indices the indices to unknown ROS args
+ */
+void _rclpy_raise_unknown_ros_args(
+  PyObject * pyargs,
+  const int * unknown_ros_args_indices,
+  int unknown_ros_args_count)
+{
+  PyObject * unknown_ros_pyargs = NULL;
+
+  pyargs = PySequence_List(pyargs);
+  if (NULL == pyargs) {
+    goto cleanup;
+  }
+
+  unknown_ros_pyargs = PyList_New(0);
+  if (NULL == unknown_ros_pyargs) {
+    goto cleanup;
+  }
+
+  for (int i = 0; i < unknown_ros_args_count; ++i) {
+    PyObject * ros_pyarg = PyList_GetItem(
+      pyargs, (Py_ssize_t)unknown_ros_args_indices[i]);
+    if (NULL == ros_pyarg) {
+      goto cleanup;
+    }
+    if (PyList_Append(unknown_ros_pyargs, ros_pyarg) != 0) {
+      goto cleanup;
+    }
+  }
+
+  PyErr_Format(
+    UnknownROSArgsError,
+    "Found unknown ROS arguments: %R",
+    unknown_ros_pyargs);
+cleanup:
+  Py_XDECREF(unknown_ros_pyargs);
+  Py_XDECREF(pyargs);
+}
+
 /// Parse a sequence of strings into rcl_arguments_t struct
 /* Raises TypeError of pyargs is not a sequence
  * Raises OverflowError if len(pyargs) > INT_MAX
@@ -297,7 +342,6 @@ _rclpy_parse_args(PyObject * pyargs, rcl_arguments_t * parsed_args)
 {
   rcl_ret_t ret;
 
-  rcl_allocator_t allocator = rcl_get_default_allocator();
   int num_args = 0;
   char ** arg_values = NULL;
   // Py_None is a singleton so comparing pointer value works
@@ -312,14 +356,45 @@ _rclpy_parse_args(PyObject * pyargs, rcl_arguments_t * parsed_args)
   // Otherwise the remapping functions will error if the user passes no arguments to a node and sets
   // use_global_arguments to False.
 
+  rcl_allocator_t allocator = rcl_get_default_allocator();
   // Adding const via cast to eliminate warning about incompatible pointer type
   const char ** const_arg_values = (const char **)arg_values;
   ret = rcl_parse_arguments(num_args, const_arg_values, allocator, parsed_args);
 
   if (ret != RCL_RET_OK) {
-    PyErr_Format(PyExc_RuntimeError, "Failed to init: %s", rcl_get_error_string().str);
+    if (ret == RCL_RET_INVALID_ROS_ARGS) {
+      PyErr_Format(
+        RCLInvalidROSArgsError,
+        "Failed to parse ROS arguments: %s",
+        rcl_get_error_string().str);
+    } else {
+      PyErr_Format(
+        PyExc_RuntimeError,
+        "Failed to init: %s",
+        rcl_get_error_string().str);
+    }
     rcl_reset_error();
+    goto cleanup;
+  }
+
+  int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(parsed_args);
+  if (unparsed_ros_args_count > 0) {
+    int * unparsed_ros_args_indices = NULL;
+    ret = rcl_arguments_get_unparsed_ros(
+      parsed_args, allocator, &unparsed_ros_args_indices);
+    if (RCL_RET_OK != ret) {
+      PyErr_Format(
+        PyExc_RuntimeError,
+        "Failed to get unparsed ROS arguments: %s",
+        rcl_get_error_string().str);
+      rcl_reset_error();
+      goto cleanup;
+    }
+    _rclpy_raise_unknown_ros_args(pyargs, unparsed_ros_args_indices, unparsed_ros_args_count);
+    allocator.deallocate(unparsed_ros_args_indices, allocator.state);
+    ret = RCL_RET_ERROR;
   }
+cleanup:
   _rclpy_arg_list_fini(num_args, arg_values);
   return ret;
 }
@@ -4975,5 +5050,40 @@ static struct PyModuleDef _rclpymodule = {
 /// Init function of this module
 PyMODINIT_FUNC PyInit__rclpy(void)
 {
-  return PyModule_Create(&_rclpymodule);
+  PyObject * m = PyModule_Create(&_rclpymodule);
+  if (NULL == m) {
+    return NULL;
+  }
+
+  RCLInvalidROSArgsError = PyErr_NewExceptionWithDoc(
+    "_rclpy.RCLInvalidROSArgsError",
+    "Thrown when invalid ROS arguments are found by rcl.",
+    PyExc_RuntimeError, NULL);
+  if (NULL == RCLInvalidROSArgsError) {
+    Py_DECREF(m);
+    return NULL;
+  }
+  if(PyModule_AddObject(m, "RCLInvalidROSArgsError", RCLInvalidROSArgsError) != 0) {
+    Py_DECREF(m);
+    return NULL;
+  }
+
+  UnknownROSArgsError = PyErr_NewExceptionWithDoc(
+    "_rclpy.UnknownROSArgsError",
+    "Thrown when unknown ROS arguments are found.",
+    PyExc_RuntimeError, NULL);
+  if (NULL == UnknownROSArgsError) {
+    Py_DECREF(m);
+    return NULL;
+  }
+  if (PyModule_AddObject(m, "UnknownROSArgsError", UnknownROSArgsError) != 0) {
+    Py_DECREF(m);
+    return NULL;
+  }
+
+  if (PyErr_Occurred()) {
+    Py_DECREF(m);
+    return NULL;
+  }
+  return m;
 }

rclpy/test/test_node.py

@@ -1560,6 +1560,30 @@ def test_node_arguments(self):
         finally:
             rclpy.shutdown(context=context)
 
+    def test_bad_node_arguments(self):
+        context = rclpy.context.Context()
+        rclpy.init(context=context)
+
+        from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+
+        invalid_ros_args_error_pattern = r'Failed to parse ROS arguments:.*not-a-remap.*'
+        with self.assertRaisesRegex(_rclpy.RCLInvalidROSArgsError, invalid_ros_args_error_pattern):
+            rclpy.create_node(
+                'my_node',
+                namespace='/my_ns',
+                cli_args=['--ros-args', '-r', 'not-a-remap'],
+                context=context)
+
+        unknown_ros_args_error_pattern = r'Found unknown ROS arguments:.*\[\'--my-custom-flag\'\]'
+        with self.assertRaisesRegex(_rclpy.UnknownROSArgsError, unknown_ros_args_error_pattern):
+            rclpy.create_node(
+                'my_node',
+                namespace='/my_ns',
+                cli_args=['--ros-args', '--my-custom-flag'],
+                context=context)
+
+        rclpy.shutdown(context=context)
+
 
 if __name__ == '__main__':
     unittest.main()