enforce correct message type is passed to various API

rclpy/rclpy/action/client.py

@@ -349,7 +349,13 @@ def send_goal(self, goal, **kwargs):
         :type goal: action_type.Goal
         :return: The result response.
         :rtype: action_type.Result
+        :raises: TypeError if the type of the passed goal isn't an instance of
+          the Goal type of the provided action when the service was
+          constructed.
         """
+        if not isinstance(goal, self._action_type.Goal):
+            raise TypeError()
+
         event = threading.Event()
 
         def unblock(future):
@@ -386,7 +392,13 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None):
         :return: a Future instance to a goal handle that completes when the goal request
             has been accepted or rejected.
         :rtype: :class:`rclpy.task.Future` instance
+        :raises: TypeError if the type of the passed goal isn't an instance of
+          the Goal type of the provided action when the service was
+          constructed.
         """
+        if not isinstance(goal, self._action_type.Goal):
+            raise TypeError()
+
         request = self._action_type.Impl.SendGoalService.Request()
         request.goal_id = self._generate_random_uuid() if goal_uuid is None else goal_uuid
         request.goal = goal

rclpy/rclpy/action/server.py

@@ -134,6 +134,9 @@ def execute(self, execute_callback=None):
         self._action_server.notify_execute(self, execute_callback)
 
     def publish_feedback(self, feedback):
+        if not isinstance(feedback, self._action_server.action_type.Feedback):
+            raise TypeError()
+
         with self._lock:
             # Ignore for already destructed goal handles
             if self._handle is None:

rclpy/rclpy/client.py

@@ -79,7 +79,13 @@ def call(self, request: SrvTypeRequest) -> SrvTypeResponse:
 
         :param request: The service request.
         :return: The service response.
+        :raises: TypeError if the type of the passed request isn't an instance
+          of the Request type of the provided service when the client was
+          constructed.
         """
+        if not isinstance(request, self.srv_type.Request):
+            raise TypeError()
+
         event = threading.Event()
 
         def unblock(future):
@@ -116,7 +122,13 @@ def call_async(self, request: SrvTypeRequest) -> Future:
 
         :param request: The service request.
         :return: A future that completes when the request does.
+        :raises: TypeError if the type of the passed request isn't an instance
+          of the Request type of the provided service when the client was
+          constructed.
         """
+        if not isinstance(request, self.srv_type.Request):
+            raise TypeError()
+
         sequence_number = _rclpy.rclpy_send_request(self.client_handle, request)
         if sequence_number in self._pending_requests:
             raise RuntimeError('Sequence (%r) conflicts with pending request' % sequence_number)

rclpy/rclpy/publisher.py

@@ -56,7 +56,10 @@ def publish(self, msg: MsgType) -> None:
         """
         Send a message to the topic for the publisher.
 
-        :param msg: The ROS message to publish. The message must be the same type as the type
-            provided when the publisher was constructed.
+        :param msg: The ROS message to publish.
+        :raises: TypeError if the type of the passed message isn't an instance
+          of the provided type when the publisher was constructed.
         """
+        if not isinstance(msg, self.msg_type):
+            raise TypeError()
         _rclpy.rclpy_publish(self.publisher_handle, msg)

rclpy/rclpy/service.py

@@ -71,5 +71,10 @@ def send_response(self, response: SrvTypeResponse, header) -> None:
 
         :param response: The service response.
         :param header: Capsule pointing to the service header from the original request.
+        :raises: TypeError if the type of the passed response isn't an instance
+          of the Response type of the provided service when the service was
+          constructed.
         """
+        if not isinstance(response, self.srv_type.Response):
+            raise TypeError()
         _rclpy.rclpy_send_response(self.service_handle, response, header)

rclpy/test/action/test_client.py

@@ -327,6 +327,16 @@ def test_get_result_async(self):
         finally:
             ac.destroy()
 
+    def test_different_type_raises(self):
+        ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+        try:
+            with self.assertRaises(TypeError):
+                ac.send_goal('different goal type')
+            with self.assertRaises(TypeError):
+                ac.send_goal_async('different goal type')
+        finally:
+            ac.destroy()
+
 
 if __name__ == '__main__':
     unittest.main()

rclpy/test/action/test_server.py

@@ -618,6 +618,40 @@ def execute_with_feedback(goal_handle):
             self.mock_action_client.feedback_msg.feedback.sequence.tolist())
         action_server.destroy()
 
+    def test_different_feedback_type_raises(self):
+
+        def execute_with_feedback(goal_handle):
+            try:
+                goal_handle.publish_feedback('different feedback type')
+            except TypeError:
+                feedback = Fibonacci.Feedback()
+                feedback.sequence = [1, 1, 2, 3]
+                goal_handle.publish_feedback(feedback)
+            goal_handle.succeed()
+            return Fibonacci.Result()
+
+        action_server = ActionServer(
+            self.node,
+            Fibonacci,
+            'fibonacci',
+            execute_callback=execute_with_feedback,
+        )
+
+        try:
+            goal_msg = Fibonacci.Impl.SendGoalService.Request()
+            goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes))
+            goal_future = self.mock_action_client.send_goal(goal_msg)
+
+            rclpy.spin_until_future_complete(
+                self.node, goal_future, self.executor)
+
+            feedback_msg = self.mock_action_client.feedback_msg
+            self.assertIsNotNone(feedback_msg)
+            self.assertEqual(
+                [1, 1, 2, 3], feedback_msg.feedback.sequence.tolist())
+        finally:
+            action_server.destroy()
+
 
 if __name__ == '__main__':
     unittest.main()

rclpy/test/test_client.py

@@ -90,6 +90,25 @@ def test_concurrent_calls_to_service(self):
             self.node.destroy_client(cli)
             self.node.destroy_service(srv)
 
+    def test_different_type_raises(self):
+        cli = self.node.create_client(GetParameters, 'get/parameters')
+        srv = self.node.create_service(
+            GetParameters, 'get/parameters',
+            lambda request, response: 'different response type')
+        try:
+            with self.assertRaises(TypeError):
+                cli.call('different request type')
+            with self.assertRaises(TypeError):
+                cli.call_async('different request type')
+            self.assertTrue(cli.wait_for_service(timeout_sec=20))
+            future = cli.call_async(GetParameters.Request())
+            executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
+            with self.assertRaises(TypeError):
+                rclpy.spin_until_future_complete(self.node, future, executor=executor)
+        finally:
+            self.node.destroy_client(cli)
+            self.node.destroy_service(srv)
+
 
 if __name__ == '__main__':
     unittest.main()

rclpy/test/test_messages.py

@@ -45,3 +45,8 @@ def test_invalid_string_raises(self):
         pub = self.node.create_publisher(Primitives, 'chatter')
         with self.assertRaises(UnicodeEncodeError):
             pub.publish(msg)
+
+    def test_different_type_raises(self):
+        pub = self.node.create_publisher(Primitives, 'chatter')
+        with self.assertRaises(TypeError):
+            pub.publish('different message type')