feat: add simple rclpy based function calling (#11)

* feat(tools): rclpy based ros2 publisher and single msg reader

* refactor

* fix: remove misleading docstrings

* add typehints

* fix: rclpy ros tools docs

* don't add frame_id to msg header by default

* Update src/rai/tools/ros/utils.py

Co-authored-by: Maciej Majek <[email protected]>

* fix: ros2 pub tool

* refactor: rename `rclpy` scripts to `native`

- discussion: #11 (comment)

* ros native tools refactor to use global node

* feat(`tool_runner`): allow empty args_schema + logging

chore: pre-commit

* revert('ScenarioRunner`): convert to node

* refactor: change logging to logger

* logger.error -> logger.warning for tool call error

* fix: husarion_poc_example_ros_native

* fix logger

* Update src/rai/scenario_engine/tool_runner.py

Co-authored-by: Maciej Majek <[email protected]>

---------

Co-authored-by: Maciej Majek <[email protected]>

Author: 2 people

examples/husarion_poc_example_ros_native.py

@@ -0,0 +1,62 @@
+import logging
+import os
+from typing import List
+
+import rclpy
+from langchain_core.messages import HumanMessage, SystemMessage
+from langchain_openai import ChatOpenAI
+from rclpy.node import Node
+
+from rai.scenario_engine.messages import AgentLoop
+from rai.scenario_engine.scenario_engine import ScenarioPartType, ScenarioRunner
+from rai.tools.ros.cat_demo_tools import FinishTool
+from rai.tools.ros.native import (
+    Ros2GetOneMsgFromTopicTool,
+    Ros2GetTopicsNamesAndTypesTool,
+    Ros2PubMessageTool,
+)
+
+
+def main():
+    scenario: List[ScenarioPartType] = [
+        SystemMessage(
+            content="You are an autonomous agent. Your main goal is to fulfill the user's requests. "
+            "Do not make assumptions about the environment you are currently in. "
+            "Use the tooling provided to gather information about the environment."
+        ),
+        HumanMessage(content="The robot is moving. Send robot to the random location"),
+        AgentLoop(stop_action=FinishTool().__class__.__name__, stop_iters=50),
+    ]
+
+    log_usage = all((os.getenv("LANGFUSE_PK"), os.getenv("LANGFUSE_SK")))
+    llm = ChatOpenAI(model="gpt-4o")
+
+    rclpy.init()
+
+    rai_node = Node("rai")  # type: ignore
+
+    tools = [
+        Ros2GetTopicsNamesAndTypesTool(),
+        Ros2GetOneMsgFromTopicTool(node=rai_node),
+        Ros2PubMessageTool(node=rai_node),
+        FinishTool(),
+    ]
+
+    runner = ScenarioRunner(
+        scenario,
+        llm,
+        tools=tools,
+        llm_type="openai",
+        scenario_name="Husarion example",
+        log_usage=log_usage,
+        logging_level=logging.INFO,
+    )
+
+    runner.run()
+    rai_node.destroy_node()
+
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

poetry.lock

@@ -31,6 +31,7 @@ piper-tts = "^1.2.0"
 tabulate = "^0.9.0"
 lark = "^1.1.9"
 langfuse = "^2.36.1"
+netifaces = "^0.11.0"
 
 [tool.poetry.group.dev.dependencies]
 ipykernel = "^6.29.4"

pyproject.toml

@@ -1,3 +1,4 @@
+import logging
 from typing import Any, Dict, List, Literal, Sequence
 
 from langchain.tools import BaseTool
@@ -22,21 +23,32 @@ def images_to_vendor_format(images: List[str], vendor: str) -> List[Dict[str, An
 
 
 def run_tool_call(
-    tool_call: ToolCall, tools: Sequence[BaseTool]
+    tool_call: ToolCall,
+    tools: Sequence[BaseTool],
 ) -> Dict[str, Any] | Any:
+    logger = logging.getLogger(__name__)
     selected_tool = {k.name: k for k in tools}[tool_call["name"]]
+
     try:
-        args = selected_tool.args_schema(**tool_call["args"])  # type: ignore
+        if selected_tool.args_schema is not None:
+            args = selected_tool.args_schema(**tool_call["args"]).dict()
+        else:
+            args = dict()
     except Exception as e:
-        return f"Error in preparing arguments for {selected_tool.name}: {e}"
+        err_msg = f"Error in preparing arguments for {selected_tool.name}: {e}"
+        logger.error(err_msg)
+        return err_msg
 
-    print(f"Running tool: {selected_tool.name} with args: {args.dict()}")
+    logger.info(f"Running tool: {selected_tool.name} with args: {args}")
 
     try:
-        tool_output = selected_tool.run(args.dict())
+        tool_output = selected_tool.run(args)
     except Exception as e:
-        return f"Error running tool {selected_tool.name}: {e}"
+        err_msg = f"Error in running tool {selected_tool.name}: {e}"
+        logger.warning(err_msg)
+        return err_msg
 
+    logger.info(f"Successfully ran tool: {selected_tool.name}. Output: {tool_output}")
     return tool_output
 
 

src/rai/scenario_engine/tool_runner.py

@@ -0,0 +1,144 @@
+from typing import Any, Dict, Tuple, Type
+
+import rclpy
+from langchain.tools import BaseTool
+from langchain_core.pydantic_v1 import BaseModel, Field
+from rclpy.impl.rcutils_logger import RcutilsLogger
+from rclpy.node import Node
+from ros2cli.node.strategy import NodeStrategy
+from rosidl_runtime_py.set_message import set_message_fields
+
+from rai.communication.ros_communication import wait_for_message
+
+from .utils import import_message_from_str
+
+
+class Ros2BaseInput(BaseModel):
+    """Empty input for ros2 tool"""
+
+
+class Ros2BaseTool(BaseTool):
+    node: Node = Field(..., exclude=True, include=False, required=True)
+
+    args_schema: Type[Ros2BaseInput] = Ros2BaseInput
+
+    @property
+    def logger(self) -> RcutilsLogger:
+        return self.node.get_logger()
+
+
+class Ros2GetTopicsNamesAndTypesTool(BaseTool):
+    name: str = "Ros2GetTopicsNamesAndTypes"
+    description: str = "A tool for getting all ros2 topics names and types"
+
+    def _run(self):
+        with NodeStrategy(dict()) as node:
+            return [
+                (topic_name, topic_type)
+                for topic_name, topic_type in node.get_topic_names_and_types()
+                if len(topic_name.split("/")) <= 2
+            ]
+
+
+class Ros2GetOneMsgFromTopicInput(BaseModel):
+    """Input for the get_current_position tool."""
+
+    topic: str = Field(..., description="Ros2 topic")
+    msg_type: str = Field(
+        ..., description="Type of ros2 message in typical ros2 format."
+    )
+    timeout_sec: int = Field(
+        10, description="The time in seconds to wait for a message to be received."
+    )
+
+
+class Ros2GetOneMsgFromTopicTool(Ros2BaseTool):
+    """Get one message from a specific ros2 topic"""
+
+    name: str = "Ros2GetOneMsgFromTopic"
+    description: str = "A tool for getting one message from a ros2 topic"
+
+    args_schema: Type[Ros2GetOneMsgFromTopicInput] = Ros2GetOneMsgFromTopicInput
+
+    def _run(self, topic: str, msg_type: str, timeout_sec: int):
+        """Gets the current position from the specified topic."""
+        msg_cls: Type = import_message_from_str(msg_type)
+
+        qos_profile = (
+            rclpy.qos.qos_profile_sensor_data
+        )  # TODO(@boczekbartek): infer QoS from topic
+
+        success, msg = wait_for_message(
+            msg_cls,
+            self.node,
+            topic,
+            qos_profile=qos_profile,
+            time_to_wait=timeout_sec,
+        )
+        msg = msg.get_data()
+
+        if success:
+            self.logger.info(f"Received message of type {msg_type} from topic {topic}")
+        else:
+            self.logger.error(
+                f"Failed to receive message of type {msg_type} from topic {topic}"
+            )
+
+        if msg is None:
+            return {"content": "No message received."}
+
+        return {
+            "content": str(msg),
+        }
+
+
+class PubRos2MessageToolInput(BaseModel):
+    topic_name: str = Field(..., description="Ros2 topic to publish the message")
+    msg_type: str = Field(
+        ..., description="Type of ros2 message in typical ros2 format."
+    )
+    msg_args: Dict[str, Any] = Field(
+        ..., description="The arguments of the service call."
+    )
+
+
+class Ros2PubMessageTool(Ros2BaseTool):
+    name: str = "PubRos2MessageTool"
+    description: str = """A tool for publishing a message to a ros2 topic
+    Example usage:
+
+    ```python
+    tool = Ros2PubMessageTool()
+    tool.run(
+        {
+            "topic_name": "/some_topic",
+            "msg_type": "geometry_msgs/Point",
+            "msg_args": {"x": 0.0, "y": 0.0, "z": 0.0},
+        }
+    )
+
+    ```
+    """
+
+    args_schema: Type[PubRos2MessageToolInput] = PubRos2MessageToolInput
+
+    def _build_msg(
+        self, msg_type: str, msg_args: Dict[str, Any]
+    ) -> Tuple[object, Type]:
+        msg_cls: Type = import_message_from_str(msg_type)
+        msg = msg_cls()
+        set_message_fields(msg, msg_args)
+        return msg, msg_cls
+
+    def _run(self, topic_name: str, msg_type: str, msg_args: Dict[str, Any]):
+        """Publishes a message to the specified topic."""
+        if "/msg/" not in msg_type:
+            raise ValueError("msg_name must contain 'msg' in the name.")
+        msg, msg_cls = self._build_msg(msg_type, msg_args)
+
+        publisher = self.node.create_publisher(
+            msg_cls, topic_name, 10
+        )  # TODO(boczekbartek): infer qos profile from topic info
+
+        msg.header.stamp = self.node.get_clock().now().to_msg()
+        publisher.publish(msg)

src/rai/tools/ros/native.py

@@ -0,0 +1,10 @@
+from typing import Type
+
+from rosidl_parser.definition import NamespacedType
+from rosidl_runtime_py.import_message import import_message_from_namespaced_type
+from rosidl_runtime_py.utilities import get_namespaced_type
+
+
+def import_message_from_str(msg_type: str) -> Type[object]:
+    msg_namespaced_type: NamespacedType = get_namespaced_type(msg_type)
+    return import_message_from_namespaced_type(msg_namespaced_type)