Fix service calls

Author: nbinford-formant

formant_ros2_adapter/scripts/components/formant_control/formant_control.py

@@ -68,6 +68,8 @@ def _handle_teleop(self, msg):
     def _handle_command(self, msg):
         self._logger.info("Command received %s" % str(msg))
         formant_stream = msg.command
+
+        # To do: this shouldn't always happen
         service_call_result = self._service_coordinator.call_service(
             formant_stream, msg.text
         )
@@ -85,6 +87,8 @@ def _handle_command(self, msg):
                     timestamp=msg_timestamp,
                 ),
             )
+
+        # To do: this shouldn't always happen
         self._publisher_coodinator.generic_publisher.publish_command(
             formant_stream, msg.text
         )

formant_ros2_adapter/scripts/components/publisher/generic_publisher.py

@@ -84,19 +84,23 @@ def __init__(
 
     def publish_command(self, formant_stream, msg: str):
         with self._config_lock:
-            for publisher in self._publishers[formant_stream]:
-                ros2_msg_type = publisher.msg_type.__name__
-                if ros2_msg_type == "String":
-                    ros2_msg = String()
-                    ros2_msg.data = msg
-                    publisher.publish(ros2_msg)
-                elif (ros2_msg_type in ROS2_NUMERIC_TYPES) and msg.isnumeric():
-                    self._publish_ros2_numeric(publisher, ros2_msg_type, msg)
-                else:
-                    self._logger.warn(
-                        "Unsupported ROS2 message type for command: %s" % formant_stream
-                    )
-                    continue
+            # To do: is this the right place to check for this?
+            if formant_stream in self._publishers:
+                for publisher in self._publishers[formant_stream]:
+                    ros2_msg_type = publisher.msg_type.__name__
+                    if ros2_msg_type == "String":
+                        ros2_msg = String()
+                        ros2_msg.data = msg
+                        publisher.publish(ros2_msg)
+                    elif (ros2_msg_type in ROS2_NUMERIC_TYPES) and msg.isnumeric():
+                        self._publish_ros2_numeric(publisher, ros2_msg_type, msg)
+                    else:
+                        self._logger.warn(
+                            "Unsupported ROS2 message type for command: %s" % formant_stream
+                        )
+                        continue
+            else:
+                self._logger.info(f"No publisher for formant stream {formant_stream}, skipping")
 
     def publish(self, formant_stream, msg):
         for publisher in self._publishers[formant_stream]:

formant_ros2_adapter/scripts/components/services/prepare_service_request.py

@@ -62,6 +62,8 @@ def prepare_serivce_request(service_client: Client, parameter: str) -> SrvTypeRe
         request_value,
     )
 
+    return service_request
+
 
 def get_bool_value(parameter):
     if parameter == "":

formant_ros2_adapter/scripts/components/services/service_coordinator.py

@@ -59,17 +59,19 @@ def _setup_services_for_config(self, service_config: ServiceClientConfig):
         service_name = service_config.service
         formant_stream = service_config.formant_stream
 
-        service_type = service_config.service_type
-        if service_type is None:
-            service_type = self._topic_type_provider.get_service_type_for_name(
+        service_type_string = service_config.service_type
+        if service_type_string is None:
+            service_type_string = self._topic_type_provider.get_service_type_for_name(
                 service_name
             )
-        if service_type is None:
+        if service_type_string is None:
             raise ValueError("No Service type found for %s" % service_name)
 
+        service_type = get_ros2_type_from_string(service_type_string)
+
         self._logger.debug(
             "Setting up service %s, %s, %s"
-            % (service_name, formant_stream, service_type)
+            % (service_name, formant_stream, service_type_string)
         )
         try:
             new_service_client = self._node.create_client(
@@ -82,8 +84,13 @@ def _setup_services_for_config(self, service_config: ServiceClientConfig):
                 self._service_clients[formant_stream] = []
 
             self._service_clients[formant_stream].append(new_service_client)
+
+            self._logger.debug("Set up service: %s" %service_name)
         except Exception as e:
-            self._logger.warn("Failed to set up service client for %s" % service_name)
+            self._logger.warn(
+                "Failed to set up service client for %s\n%s"
+                % (service_name, e)
+            )
 
     def call_service(self, formant_stream, parameter: str):
         with self._config_lock: