feat: agriculture demo (#285)

docs/demos/agriculture.md

@@ -0,0 +1,76 @@
+# Autonomous Tractor Demo
+
+This demo showcases autonomous tractors operating in an agricultural field using ROS 2. The tractor is controlled using a conventional navigation stack. Sometimes, due to the ever-changing environment, the tractor may encounter unexpected situations. The conventional stack is not designed to handle these situations and usually ends up replanning the path. The tractor can handle this optimally by calling the RAI agent to decide what to do.
+
+![Tractor in field meets an obstacle](../imgs/agriculture_demo.gif)
+
+## Quick Start
+
+1. **Download the Latest Release**
+
+   Download the latest binary release for your ROS 2 distribution.
+
+   - [ros2-humble-agriculture-demo](https://robotec-ml-roscon2024-demos.s3.eu-central-1.amazonaws.com/RAIAgricultureDemo_1.0.0_jammyhumble.zip)
+   - [ros2-jazzy-agriculture-demo](https://robotec-ml-roscon2024-demos.s3.eu-central-1.amazonaws.com/RAIAgricultureDemo_1.0.0_noblejazzy.zip)
+
+2. **Install Required Packages**
+
+   ```bash
+   sudo apt install ros-${ROS_DISTRO}-ackermann-msgs ros-${ROS_DISTRO}-gazebo-msgs ros-${ROS_DISTRO}-control-toolbox ros-${ROS_DISTRO}-nav2-bringup
+   ```
+
+3. **Unpack the Binary and Run the Simulation**
+   Unpack the binary
+
+   - For Jazzy:
+
+   ```bash
+   unzip RAIAgricultureDemo_1.0.0_noblejazzy.zip
+   ```
+
+   - For Humble:
+
+   ```bash
+   unzip RAIAgricultureDemo_1.0.0_jammyhumble.zip
+   ```
+
+   ```bash
+   . /opt/ros/${ROS_DISTRO}/setup.bash
+   ./RAIAgricultureDemoGamePackage/RAIAgricultureDemo.GameLauncher -bg_ConnectToAssetProcessor=0
+   ```
+
+4. **Start the Tractor Node**
+
+   ```bash
+   python examples/agriculture-demo.py --tractor_number 1
+   ```
+
+You are now ready to run the demo and see the tractor in action!
+
+## Running the Demo
+
+The demo simulates a scenario where the tractor stops due to an unexpected situation. The RAI Agent decides the next action based on the current state.
+
+### RAI Agent decisions
+
+RAI Agent's mission is to decide the next action based on the current state of the anomaly. There are three exposed services to control the tractor:
+
+- continue
+
+Used when the anomaly is flagged as a false positive.
+
+- flash
+
+Used to flash the lights on the tractor to e.g. get the attention of the animals
+
+- replan
+
+Used to replan the path/skip the alley.
+
+### What Happens in the Demo?
+
+- The node listens for the tractor's state and calls the RaiNode using ROS 2 action when an anomaly is detected.
+- The RaiNode decides the next action based on the current state.
+- The tractor performs the action and the demo continues.
+
+For more details on configuring RAI for specific robots, refer to the [RAI documentation](../create_robots_whoami.md).

examples/agriculture-demo.py

@@ -0,0 +1,179 @@
+# Copyright (C) 2024 Robotec.AI
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#         http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language goveself.rning permissions and
+# limitations under the License.
+
+import argparse
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from std_srvs.srv import Trigger
+
+from rai.node import RaiStateBasedLlmNode, describe_ros_image
+from rai.tools.ros.native import (
+    GetCameraImage,
+    GetMsgFromTopic,
+    Ros2GenericServiceCaller,
+    Ros2GetRobotInterfaces,
+    Ros2ShowMsgInterfaceTool,
+)
+from rai.tools.time import WaitForSecondsTool
+from rai_interfaces.action import Task
+
+
+class MockBehaviorTreeNode(Node):
+    def __init__(self, tractor_number: int):
+        super().__init__(f"mock_behavior_tree_node_{tractor_number}")
+        self.tractor_number = tractor_number
+
+        # Create a callback group for concurrent execution
+        self.callback_group = ReentrantCallbackGroup()
+
+        # Create service client
+        self.current_state_client = self.create_client(
+            Trigger,
+            f"/tractor{tractor_number}/current_state",
+            callback_group=self.callback_group,
+        )
+
+        # Create action client
+        self.perform_task_client = ActionClient(
+            self, Task, "/perform_task", callback_group=self.callback_group
+        )
+
+        # Create timer for periodic checks
+        self.create_timer(
+            5.0, self.check_tractor_state, callback_group=self.callback_group
+        )
+
+        self.get_logger().info(
+            f"Mock Behavior Tree Node for Tractor {tractor_number} initialized"
+        )
+
+    async def check_tractor_state(self):
+        # Call the current_state service
+        response = await self.current_state_client.call_async(Trigger.Request())
+
+        self.get_logger().info(f"Current state: {response.message}")
+
+        if "STOPPED" in response.message:
+            self.get_logger().info(
+                "The tractor has stopped. Calling RaiNode to decide what to do."
+            )
+
+            # Send goal to perform_task action server
+            goal_msg = Task.Goal()
+            goal_msg.priority = 10
+            goal_msg.description = ""
+            goal_msg.task = "Anomaly detected. Please decide what to do."
+
+            self.perform_task_client.wait_for_server()
+
+            future = self.perform_task_client.send_goal_async(goal_msg)
+            await future
+
+            goal_handle = future.result()
+            if goal_handle.accepted:
+                self.get_logger().info("Goal accepted by perform_task action server")
+                result = await goal_handle.get_result_async()
+                self.get_logger().info(f"Result: {result.result}")
+            else:
+                self.get_logger().warn("Goal rejected by perform_task action server")
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Autonomous Tractor Demo")
+    parser.add_argument(
+        "--tractor_number",
+        type=int,
+        choices=[1, 2],
+        help="Tractor number (1 or 2)",
+        default=1,
+    )
+    args = parser.parse_args()
+
+    tractor_number = args.tractor_number
+    tractor_prefix = f"/tractor{tractor_number}"
+
+    rclpy.init()
+
+    observe_topics = [
+        f"{tractor_prefix}/camera_image_color",
+    ]
+
+    observe_postprocessors = {
+        f"{tractor_prefix}/camera_image_color": describe_ros_image
+    }
+
+    topics_whitelist = [
+        "/rosout",
+        f"{tractor_prefix}/camera_image_color",
+        # Services
+        f"{tractor_prefix}/continue",
+        f"{tractor_prefix}/current_state",
+        f"{tractor_prefix}/flash",
+        f"{tractor_prefix}/replan",
+    ]
+
+    actions_whitelist = []
+
+    SYSTEM_PROMPT = f"""
+    You are autonomous tractor {tractor_number} operating in an agricultural field. You are activated whenever the tractor stops due to an unexpected situation. Your task is to call a service based on your assessment of the situation.
+
+    The system is not perfect, so it may stop you unnecessarily at times.
+
+    You are to call one of the following services based on the situation:
+    1. continue - If you believe the stop was unnecessary.
+    2. current_state - If you want to check the current state of the tractor.
+    3. flash - If you want to flash the lights to signal possible animals to move out of the way.
+    4. replan - If you want to replan the path due to an obstacle in front of you.
+
+    Important: You must call only one service. The tractor can only handle one service call.
+    """
+
+    rai_node = RaiStateBasedLlmNode(
+        observe_topics=observe_topics,
+        observe_postprocessors=observe_postprocessors,
+        whitelist=topics_whitelist + actions_whitelist,
+        system_prompt=SYSTEM_PROMPT,
+        tools=[
+            Ros2ShowMsgInterfaceTool,
+            WaitForSecondsTool,
+            GetMsgFromTopic,
+            GetCameraImage,
+            Ros2GetRobotInterfaces,
+            Ros2GenericServiceCaller,
+        ],
+    )
+
+    mock_node = MockBehaviorTreeNode(tractor_number)
+
+    # Use a MultiThreadedExecutor to allow for concurrent execution
+    executor = MultiThreadedExecutor()
+    executor.add_node(rai_node)
+    executor.add_node(mock_node)
+
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rai_node.destroy_node()
+        mock_node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/rai/rai/tools/ros/native.py

@@ -13,6 +13,7 @@
 # limitations under the License.
 #
 
+import importlib
 import json
 import time
 from typing import Any, Dict, OrderedDict, Tuple, Type
@@ -30,6 +31,7 @@
 from langchain.tools import BaseTool
 from pydantic import BaseModel, Field
 from rclpy.impl.rcutils_logger import RcutilsLogger
+from rosidl_runtime_py.utilities import get_namespaced_type
 
 from .utils import convert_ros_img_to_base64, import_message_from_str
 
@@ -223,6 +225,57 @@ def _run(self, topic_name: str):
             return str(msg), {}
 
 
+class Ros2GenericServiceCallerInput(BaseModel):
+    service_name: str = Field(..., description="Name of the ROS2 service to call")
+    service_type: str = Field(
+        ..., description="Type of the ROS2 service in typical ros2 format"
+    )
+    request_args: Dict[str, Any] = Field(
+        ..., description="Arguments for the service request"
+    )
+
+
+class Ros2GenericServiceCaller(Ros2BaseTool):
+    name: str = "Ros2GenericServiceCaller"
+    description: str = "A tool for calling any ROS2 service dynamically."
+
+    args_schema: Type[Ros2GenericServiceCallerInput] = Ros2GenericServiceCallerInput
+
+    def _build_request(self, service_type: str, request_args: Dict[str, Any]) -> Any:
+        srv_module, _, srv_name = service_type.split("/")
+        srv_class = getattr(importlib.import_module(f"{srv_module}.srv"), srv_name)
+        request = srv_class.Request()
+        rosidl_runtime_py.set_message.set_message_fields(request, request_args)
+        return request
+
+    def _run(self, service_name: str, service_type: str, request_args: Dict[str, Any]):
+        if not service_name.startswith("/"):
+            service_name = f"/{service_name}"
+
+        try:
+            request = self._build_request(service_type, request_args)
+        except Exception as e:
+            return f"Failed to build service request: {e}"
+        namespaced_type = get_namespaced_type(service_type)
+        client = self.node.create_client(
+            rosidl_runtime_py.import_message.import_message_from_namespaced_type(
+                namespaced_type
+            ),
+            service_name,
+        )
+
+        if not client.wait_for_service(timeout_sec=1.0):
+            return f"Service '{service_name}' is not available"
+
+        future = client.call_async(request)
+        rclpy.spin_until_future_complete(self.node, future)
+
+        if future.result() is not None:
+            return str(future.result())
+        else:
+            return f"Service call to '{service_name}' failed"
+
+
 class GetCameraImage(Ros2BaseTool):
     name: str = "get_camera_image"
     description: str = "get image from robots camera"