agenticros 0.0.1 → 0.1.1

package/runtime/ros2_ws/src/agenticros_bringup/launch/turtlebot3_gazebo_rviz.launch.py

@@ -0,0 +1,42 @@
+"""TurtleBot3 Gazebo world + RViz (use_sim_time:=true). For machines with a display."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    bringup = FindPackageShare("agenticros_bringup")
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([bringup, "launch", "gazebo_turtlebot3.launch.py"])
+        ),
+        launch_arguments={
+            "robot_namespace": LaunchConfiguration("robot_namespace"),
+            "turtlebot3_model": LaunchConfiguration("turtlebot3_model"),
+        }.items(),
+    )
+    rviz = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([bringup, "launch", "rviz.launch.py"])
+        ),
+        launch_arguments={"use_sim_time": "true"}.items(),
+    )
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "turtlebot3_model",
+                default_value="burger",
+                description="TurtleBot3 model: burger | waffle | waffle_pi",
+            ),
+            DeclareLaunchArgument(
+                "robot_namespace",
+                default_value="",
+                description="Same as AgenticROS robot.namespace; enables cmd_vel relay",
+            ),
+            gazebo,
+            rviz,
+        ]
+    )

package/runtime/ros2_ws/src/agenticros_bringup/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agenticros_bringup</name>
+  <version>0.0.1</version>
+  <description>Launch files and RViz configs for TurtleBot3 Gazebo simulation and visualization aligned with AgenticROS defaults (cmd_vel, /scan, rosbridge).</description>
+  <maintainer email="team@plaipin.com">PlaiPin</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>rosbridge_server</exec_depend>
+  <exec_depend>turtlebot3_gazebo</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

package/runtime/ros2_ws/src/agenticros_bringup/rviz/turtlebot3_agenticros.rviz

@@ -0,0 +1,174 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /TF1
+        - /LaserScan1
+      Splitter Ratio: 0.5
+    Tree Height: 464
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Marker Scale: 0.15
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/LaserScan
+      Color: 255; 85; 0
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (m): 0.04
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 8
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.5
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.8
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000013c0000025ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000410000025f000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000025ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000410000025f000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000028900fffffffb0000000800540069006d00650100000000000004500000000000000000000001eb0000025f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 60
+  Y: 60

package/runtime/ros2_ws/src/agenticros_bringup/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/agenticros_bringup
+[install]
+install_scripts=$base/lib/agenticros_bringup

package/runtime/ros2_ws/src/agenticros_bringup/setup.py

@@ -0,0 +1,28 @@
+from glob import glob
+
+from setuptools import find_packages, setup
+
+package_name = "agenticros_bringup"
+
+setup(
+    name=package_name,
+    version="0.0.1",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name + "/launch", glob("launch/*.py")),
+        ("share/" + package_name + "/rviz", glob("rviz/*.rviz")),
+    ],
+    install_requires=["setuptools"],
+    entry_points={
+        "console_scripts": [
+            "cmd_vel_relay = agenticros_bringup.cmd_vel_relay:main",
+        ],
+    },
+    zip_safe=True,
+    maintainer="PlaiPin",
+    maintainer_email="team@plaipin.com",
+    description="AgenticROS-friendly simulation and RViz bringup.",
+    license="Apache-2.0",
+)

package/runtime/ros2_ws/src/agenticros_discovery/agenticros_discovery/discovery_node.py

@@ -0,0 +1,172 @@
+"""
+AgenticROS Discovery Node
+
+Introspects the running ROS2 system and publishes a capability manifest
+describing available topics, services, and actions. This manifest is consumed
+by the AgenticROS plugin to inform the AI agent about what the robot can do.
+
+Published topic:
+  /agenticros/capabilities  (agenticros_msgs/msg/CapabilityManifest)
+
+Service:
+  /agenticros/get_capabilities  (agenticros_msgs/srv/GetCapabilities)
+
+Parameters:
+  robot_name       — Name of the robot (default: "Robot")
+  robot_namespace   — Namespace filter; empty = discover all (default: "")
+  publish_interval  — Seconds between manifest publications (default: 5.0)
+"""
+
+from __future__ import annotations
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, DurabilityPolicy
+
+from agenticros_msgs.msg import CapabilityManifest
+from agenticros_msgs.srv import GetCapabilities
+
+
+# Internal ROS2 topics/services that clutter the manifest
+_INTERNAL_PREFIXES = (
+    "/rosout",
+    "/parameter_events",
+    "/agenticros/",
+)
+
+
+class DiscoveryNode(Node):
+    """Periodically discovers ROS2 capabilities and publishes a manifest."""
+
+    def __init__(self) -> None:
+        super().__init__("agenticros_discovery")
+
+        # Parameters
+        self.declare_parameter("robot_name", "Robot")
+        self.declare_parameter("robot_namespace", "")
+        self.declare_parameter("publish_interval", 5.0)
+
+        self.robot_name: str = self.get_parameter("robot_name").value
+        self.robot_namespace: str = self.get_parameter("robot_namespace").value
+        self.publish_interval: float = self.get_parameter("publish_interval").value
+
+        # Publisher — transient local so late subscribers get the last manifest
+        qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
+        self.manifest_pub = self.create_publisher(
+            CapabilityManifest, "/agenticros/capabilities", qos
+        )
+
+        # Service — on-demand query
+        self.get_caps_srv = self.create_service(
+            GetCapabilities, "/agenticros/get_capabilities", self._handle_get_capabilities
+        )
+
+        # Timer — periodic discovery
+        self.timer = self.create_timer(self.publish_interval, self._on_timer)
+
+        self.get_logger().info(
+            f"Discovery node started: robot={self.robot_name}, "
+            f"namespace='{self.robot_namespace}', interval={self.publish_interval}s"
+        )
+
+    def _on_timer(self) -> None:
+        """Discover capabilities and publish the manifest."""
+        manifest = self._build_manifest()
+        self.manifest_pub.publish(manifest)
+        self.get_logger().debug(
+            f"Published manifest: {len(manifest.topic_names)} topics, "
+            f"{len(manifest.service_names)} services, "
+            f"{len(manifest.action_names)} actions"
+        )
+
+    def _handle_get_capabilities(
+        self,
+        request: GetCapabilities.Request,
+        response: GetCapabilities.Response,
+    ) -> GetCapabilities.Response:
+        """Handle on-demand capability query."""
+        # Allow overriding namespace per-request
+        saved_ns = self.robot_namespace
+        if request.robot_namespace:
+            self.robot_namespace = request.robot_namespace
+
+        try:
+            response.manifest = self._build_manifest()
+            response.success = True
+            response.error_message = ""
+        except Exception as e:
+            response.success = False
+            response.error_message = str(e)
+        finally:
+            self.robot_namespace = saved_ns
+
+        return response
+
+    def _build_manifest(self) -> CapabilityManifest:
+        """Query the ROS2 graph and build a CapabilityManifest message."""
+        manifest = CapabilityManifest()
+        manifest.robot_name = self.robot_name
+        manifest.robot_namespace = self.robot_namespace
+        manifest.stamp = self.get_clock().now().to_msg()
+
+        ns_prefix = self.robot_namespace if self.robot_namespace else ""
+
+        # Discover topics
+        for name, types in self.get_topic_names_and_types():
+            if not self._should_include(name, ns_prefix):
+                continue
+            manifest.topic_names.append(name)
+            manifest.topic_types.append(types[0] if types else "")
+
+        # Discover services
+        for name, types in self.get_service_names_and_types():
+            if not self._should_include(name, ns_prefix):
+                continue
+            manifest.service_names.append(name)
+            manifest.service_types.append(types[0] if types else "")
+
+        # Discover actions — heuristic: look for */_action/feedback topics
+        feedback_suffix = "/_action/feedback"
+        for name, types in self.get_topic_names_and_types():
+            if name.endswith(feedback_suffix):
+                action_name = name[: -len(feedback_suffix)]
+                if not self._should_include(action_name, ns_prefix):
+                    continue
+                action_type = types[0] if types else ""
+                # Convert feedback type to action type
+                if action_type.endswith("_FeedbackMessage"):
+                    action_type = action_type[: -len("_FeedbackMessage")]
+                manifest.action_names.append(action_name)
+                manifest.action_types.append(action_type)
+
+        return manifest
+
+    def _should_include(self, name: str, ns_prefix: str) -> bool:
+        """Check if a topic/service/action should be included in the manifest."""
+        # Filter by namespace if set
+        if ns_prefix and not name.startswith(ns_prefix):
+            return False
+
+        # Exclude ROS2 internal topics
+        for prefix in _INTERNAL_PREFIXES:
+            if name.startswith(prefix):
+                return False
+
+        return True
+
+
+def main() -> None:
+    """Entry point for the discovery node."""
+    rclpy.init()
+    node = DiscoveryNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

package/runtime/ros2_ws/src/agenticros_discovery/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agenticros_discovery</name>
+  <version>0.0.1</version>
+  <description>ROS2 node that introspects available topics, services, and actions and publishes a capability manifest</description>
+  <maintainer email="team@plaipin.com">PlaiPin</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>agenticros_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

package/runtime/ros2_ws/src/agenticros_discovery/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/agenticros_discovery
+
+[install]
+install_scripts=$base/lib/agenticros_discovery

package/runtime/ros2_ws/src/agenticros_discovery/setup.py

@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = "agenticros_discovery"
+
+setup(
+    name=package_name,
+    version="0.0.1",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="PlaiPin",
+    maintainer_email="team@plaipin.com",
+    description="ROS2 capability discovery node for AgenticROS",
+    license="Apache-2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "discovery_node = agenticros_discovery.discovery_node:main",
+        ],
+    },
+)

package/runtime/ros2_ws/src/agenticros_follow_me/README.md

@@ -0,0 +1,66 @@
+# agenticros_follow_me
+
+ROS2 node for the **Follow Me** mission: person tracking and follower control. Publishes `geometry_msgs/Twist` to `cmd_vel` and advertises ROS2 services used by the AgenticROS plugin. All Follow Me behavior is in-repo; AgenticROS does not depend on external projects for this mission.
+
+## Features
+
+- Person detection via **RealSense** depth + **MediaPipe** Pose (or simple depth fallback / mock when unavailable).
+- Follower controller maintains a configurable distance and publishes velocity commands to `cmd_vel`.
+- ROS2 services: `follow_me/start`, `follow_me/stop`, `follow_me/set_distance`, `follow_me/get_status`, `follow_me/set_target`.
+
+## Dependencies
+
+- **ROS2** (Jazzy or compatible).
+- **agenticros_msgs** (from this workspace).
+- Optional (for real camera + detection):
+  - `pyrealsense2` — RealSense SDK.
+  - `mediapipe` — pose detection.
+  - `opencv-python` — image handling.
+  - `numpy`
+
+Without them, the node runs in mock mode (simulated person).
+
+## Build
+
+```bash
+cd ros2_ws
+colcon build --packages-select agenticros_msgs agenticros_follow_me
+source install/setup.bash
+```
+
+## Run
+
+```bash
+ros2 run agenticros_follow_me follow_me_node
+```
+
+**If you get "No executable found"** (e.g. on some boards), run the node as a module instead (from the same shell where you sourced the workspace):
+
+```bash
+source install/setup.bash
+python3 -m agenticros_follow_me
+```
+
+Parameters:
+
+- `use_camera` (bool, default: true) — use RealSense when available; otherwise mock.
+- `target_distance` (float, default: 1.0) — follow distance in meters.
+- `cmd_vel_topic` (string, default: `cmd_vel`) — topic for Twist commands.
+
+Example with custom topic (e.g. namespaced):
+
+```bash
+ros2 run agenticros_follow_me follow_me_node --ros-args -p cmd_vel_topic:=robot_ns/cmd_vel
+```
+
+## Services
+
+| Service | Type | Description |
+|--------|------|-------------|
+| `follow_me/start` | FollowMeStart | Start following (optional target description). |
+| `follow_me/stop` | FollowMeStop | Stop following. |
+| `follow_me/set_distance` | FollowMeSetDistance | Set target distance (m). |
+| `follow_me/get_status` | FollowMeGetStatus | Get enabled, tracking, distances, twist. |
+| `follow_me/set_target` | FollowMeSetTarget | Lock onto a person by description (closest match without VLM). |
+
+The AgenticROS plugin uses these via the **follow_robot** tool when the user says "follow me", "stop following", etc.

package/runtime/ros2_ws/src/agenticros_follow_me/agenticros_follow_me/__init__.py

@@ -0,0 +1 @@
+# AgenticROS Follow Me mission

package/runtime/ros2_ws/src/agenticros_follow_me/agenticros_follow_me/__main__.py

@@ -0,0 +1,5 @@
+"""Allow running as: python3 -m agenticros_follow_me (after sourcing install/setup.bash)."""
+from .follow_me_node import main
+
+if __name__ == "__main__":
+    main()