agenticros 0.1.0 → 0.1.1

package/runtime/ros2_ws/src/agenticros_sim/config/agenticros-sim.config.json

@@ -0,0 +1,28 @@
+{
+  "$comment": "AgenticROS config template tuned for sim-amr. Drop this in ~/.agenticros/config.json (or run `agenticros init` and answer 'simulation') to point the MCP server + OpenClaw plugin at the sim's topics. All values match agenticros_sim/config/amr_bridge.yaml. NOTE: robot.namespace is intentionally empty - the sim bridge publishes /cmd_vel, /scan, /imu/data, etc. at the graph root (no robot prefix), so AgenticROS must NOT namespace topics. For multi-robot sim, prefix the topics in amr_bridge.yaml and set namespace here to match.",
+  "transport": {
+    "mode": "local"
+  },
+  "robot": {
+    "namespace": "",
+    "name": "Sim AMR",
+    "cameraTopic": "/camera/camera/color/image_raw"
+  },
+  "safety": {
+    "maxLinearVelocity": 0.5,
+    "maxAngularVelocity": 1.0
+  },
+  "teleop": {
+    "cmdVelTopic": "/cmd_vel",
+    "speedDefault": 0.2,
+    "cameraTopics": [
+      { "topic": "/camera/camera/color/image_raw", "label": "Sim RGB" },
+      { "topic": "/camera/camera/depth/image_rect_raw", "label": "Sim depth" }
+    ]
+  },
+  "skills": {
+    "followme": {
+      "depthTopic": "/camera/camera/depth/image_rect_raw"
+    }
+  }
+}

package/runtime/ros2_ws/src/agenticros_sim/config/amr_bridge.yaml

@@ -0,0 +1,111 @@
+# amr_bridge.yaml
+#
+# Maps Gazebo Harmonic topics produced by the AMR model + world plugins to
+# ROS 2 topics on the names the real-robot AgenticROS plugin already expects.
+# Used by ros_gz_bridge's parameter_bridge node via the `config_file` arg.
+#
+# Key invariant: the camera topic names below match the RealSense ROS driver
+# defaults (`/camera/camera/color/image_raw`, `/camera/camera/depth/image_rect_raw`)
+# so plugin code that targets real hardware works against sim without any
+# config change. Same for /cmd_vel, /odom, /scan, /imu/data, /tf.
+#
+# Direction notation: GZ_TO_ROS  — gazebo publishes, ros subscribes (sensors)
+#                     ROS_TO_GZ  — ros publishes, gazebo subscribes (cmd_vel)
+#                     BIDIRECTIONAL — rare, /tf for example
+#
+# Field reference: https://github.com/gazebosim/ros_gz/blob/humble/ros_gz_bridge/README.md
+
+# ---------------- cmd_vel (ROS -> GZ) ----------------
+- ros_topic_name: "/cmd_vel"
+  gz_topic_name: "/model/agenticros_amr/cmd_vel"
+  ros_type_name: "geometry_msgs/msg/Twist"
+  gz_type_name: "gz.msgs.Twist"
+  direction: ROS_TO_GZ
+
+# ---------------- odometry (GZ -> ROS) ----------------
+# Note: the gz-sim diff-drive system publishes <odom_topic> as a bare topic
+# (no /model/<name>/ prefix), unlike <topic> which DOES get prefixed.
+- ros_topic_name: "/odom"
+  gz_topic_name: "/odometry"
+  ros_type_name: "nav_msgs/msg/Odometry"
+  gz_type_name: "gz.msgs.Odometry"
+  direction: GZ_TO_ROS
+
+# ---------------- /tf  (GZ -> ROS) ----------------
+- ros_topic_name: "/tf"
+  gz_topic_name: "/tf"
+  ros_type_name: "tf2_msgs/msg/TFMessage"
+  gz_type_name: "gz.msgs.Pose_V"
+  direction: GZ_TO_ROS
+
+# ---------------- pose info (GZ -> ROS) ----------------
+- ros_topic_name: "/tf_static"
+  gz_topic_name: "/model/agenticros_amr/pose_static"
+  ros_type_name: "tf2_msgs/msg/TFMessage"
+  gz_type_name: "gz.msgs.Pose_V"
+  direction: GZ_TO_ROS
+
+# ---------------- joint states (GZ -> ROS) ----------------
+- ros_topic_name: "/joint_states"
+  gz_topic_name: "/joint_states"
+  ros_type_name: "sensor_msgs/msg/JointState"
+  gz_type_name: "gz.msgs.Model"
+  direction: GZ_TO_ROS
+
+# ---------------- Depth camera (GZ -> ROS) ----------------
+# The rgbd_camera plugin publishes under <topic>camera</topic>, which becomes:
+#   /camera/image          (RGB)
+#   /camera/depth_image    (depth, 32FC1)
+#   /camera/points         (PointCloud2)
+#   /camera/camera_info    (CameraInfo)
+# We remap each to the RealSense layout the real-robot plugin expects.
+- ros_topic_name: "/camera/camera/color/image_raw"
+  gz_topic_name: "/camera/image"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/camera/camera/color/camera_info"
+  gz_topic_name: "/camera/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/camera/camera/depth/image_rect_raw"
+  gz_topic_name: "/camera/depth_image"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/camera/camera/depth/camera_info"
+  gz_topic_name: "/camera/depth_image/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/camera/camera/depth/points"
+  gz_topic_name: "/camera/points"
+  ros_type_name: "sensor_msgs/msg/PointCloud2"
+  gz_type_name: "gz.msgs.PointCloudPacked"
+  direction: GZ_TO_ROS
+
+# ---------------- 2D LIDAR (GZ -> ROS) ----------------
+- ros_topic_name: "/scan"
+  gz_topic_name: "/scan"
+  ros_type_name: "sensor_msgs/msg/LaserScan"
+  gz_type_name: "gz.msgs.LaserScan"
+  direction: GZ_TO_ROS
+
+# ---------------- IMU (GZ -> ROS) ----------------
+- ros_topic_name: "/imu/data"
+  gz_topic_name: "/imu"
+  ros_type_name: "sensor_msgs/msg/Imu"
+  gz_type_name: "gz.msgs.IMU"
+  direction: GZ_TO_ROS
+
+# ---------------- Clock (GZ -> ROS) ----------------
+- ros_topic_name: "/clock"
+  gz_topic_name: "/clock"
+  ros_type_name: "rosgraph_msgs/msg/Clock"
+  gz_type_name: "gz.msgs.Clock"
+  direction: GZ_TO_ROS

package/runtime/ros2_ws/src/agenticros_sim/config/amr_view.rviz

@@ -0,0 +1,172 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1
+      Splitter Ratio: 0.5
+    Tree Height: 600
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+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: 20
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 0.5
+      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; 255; 255
+      Color Transformer: Intensity
+      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 (Pixels): 3
+      Size (m): 0.05
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Image Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera/camera/color/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: RGB
+      Normalize Range: true
+      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/PointCloud2
+      Color: 255; 255; 255
+      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: DepthCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.02
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /camera/camera/depth/points
+      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
+      Line color: 128; 128; 0
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 7
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        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.785
+    Saved: ~

package/runtime/ros2_ws/src/agenticros_sim/env-hooks/gz_resource_path.dsv.in

@@ -0,0 +1,3 @@
+prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/@PROJECT_NAME@
+prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/@PROJECT_NAME@/models
+prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/@PROJECT_NAME@/worlds

package/runtime/ros2_ws/src/agenticros_sim/env-hooks/gz_resource_path.sh.in

@@ -0,0 +1,7 @@
+# Generated env hook — prepends the agenticros_sim share/ tree onto the
+# Gazebo Harmonic resource search path so `gz sim agenticros_indoor.sdf` and
+# `<uri>model://agenticros_amr</uri>` resolve without absolute paths.
+
+ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@"
+ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/models"
+ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/worlds"

package/runtime/ros2_ws/src/agenticros_sim/launch/sim_amr.launch.py

@@ -0,0 +1,159 @@
+"""sim_amr.launch.py
+
+Bring up the AgenticROS 2-wheel AMR in Gazebo Harmonic with full ROS-side
+plumbing:
+  * gz sim agenticros_indoor.sdf                  (the world + everything in it)
+  * ros_gz_sim create                             (spawn the AMR if it isn't
+                                                  already baked into the world)
+  * ros_gz_bridge parameter_bridge config_file    (gz <-> ROS topic bridge)
+  * Optional RViz (--rviz launch arg)
+
+Launch args:
+  world       (default 'agenticros_indoor.sdf')    Override the world file.
+  use_rviz    (default 'false')                    Show RViz with the AMR config.
+  use_sim_time (default 'true')                    /clock is bridged; downstream
+                                                  nodes should honour this.
+  x, y, z, yaw                                    Spawn pose for the AMR.
+  bridge_config_file                              Override the bridge YAML.
+  gui         (default 'true')                    Headless if 'false' (CI / docker).
+
+Examples:
+  ros2 launch agenticros_sim sim_amr.launch.py
+  ros2 launch agenticros_sim sim_amr.launch.py use_rviz:=true
+  ros2 launch agenticros_sim sim_amr.launch.py gui:=false       # headless
+"""
+
+from __future__ import annotations
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchContext, LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+PKG_NAME = "agenticros_sim"
+
+
+def generate_launch_description() -> LaunchDescription:
+    pkg_share = get_package_share_directory(PKG_NAME)
+    default_world = os.path.join(pkg_share, "worlds", "agenticros_indoor.sdf")
+    default_bridge = os.path.join(pkg_share, "config", "amr_bridge.yaml")
+    default_rviz = os.path.join(pkg_share, "config", "amr_view.rviz")
+    model_sdf = os.path.join(pkg_share, "models", "agenticros_amr", "model.sdf")
+
+    # --- Launch arguments ---
+    world_arg = DeclareLaunchArgument(
+        "world", default_value=default_world,
+        description="SDF world file to load (absolute path).",
+    )
+    use_rviz_arg = DeclareLaunchArgument(
+        "use_rviz", default_value="false",
+        description="Open RViz with the AMR view config.",
+    )
+    use_sim_time_arg = DeclareLaunchArgument(
+        "use_sim_time", default_value="true",
+        description="Tell ROS nodes to use /clock published by gz.",
+    )
+    x_arg = DeclareLaunchArgument("x", default_value="0.0", description="AMR spawn X (m)")
+    y_arg = DeclareLaunchArgument("y", default_value="0.0", description="AMR spawn Y (m)")
+    z_arg = DeclareLaunchArgument("z", default_value="0.1", description="AMR spawn Z (m)")
+    yaw_arg = DeclareLaunchArgument("yaw", default_value="0.0", description="AMR yaw (rad)")
+    bridge_arg = DeclareLaunchArgument(
+        "bridge_config_file", default_value=default_bridge,
+        description="ros_gz_bridge parameter_bridge config YAML.",
+    )
+    gui_arg = DeclareLaunchArgument(
+        "gui", default_value="true",
+        description="If 'false', run gz-sim headless (-s -r).",
+    )
+    rviz_config_arg = DeclareLaunchArgument(
+        "rviz_config", default_value=default_rviz,
+        description="RViz config path (used when use_rviz is true).",
+    )
+
+    # ---------- Spawn the AMR ----------
+    spawn_amr = Node(
+        package="ros_gz_sim",
+        executable="create",
+        name="spawn_agenticros_amr",
+        output="screen",
+        arguments=[
+            "-name", "agenticros_amr",
+            "-file", model_sdf,
+            "-x", LaunchConfiguration("x"),
+            "-y", LaunchConfiguration("y"),
+            "-z", LaunchConfiguration("z"),
+            "-Y", LaunchConfiguration("yaw"),
+        ],
+    )
+
+    # ---------- gz <-> ROS bridge ----------
+    bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name="agenticros_amr_bridge",
+        output="screen",
+        parameters=[{
+            "config_file": LaunchConfiguration("bridge_config_file"),
+            "qos_overrides./tf_static.publisher.durability": "transient_local",
+            "use_sim_time": LaunchConfiguration("use_sim_time"),
+        }],
+    )
+
+    # ---------- Optional RViz ----------
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="screen",
+        condition=IfCondition(LaunchConfiguration("use_rviz")),
+        arguments=["-d", LaunchConfiguration("rviz_config")],
+        parameters=[{"use_sim_time": LaunchConfiguration("use_sim_time")}],
+    )
+
+    # The gz sim launcher needs a single space-separated `gz_args` string. We
+    # build it inside an OpaqueFunction so the conditional headless flag is
+    # evaluated at launch-time (substitutions can't do string concatenation).
+    return LaunchDescription([
+        world_arg,
+        use_rviz_arg,
+        use_sim_time_arg,
+        x_arg, y_arg, z_arg, yaw_arg,
+        bridge_arg,
+        gui_arg,
+        rviz_config_arg,
+        OpaqueFunction(function=_launch_gz_sim),
+        spawn_amr,
+        bridge,
+        rviz,
+    ])
+
+
+def _launch_gz_sim(context: LaunchContext, *_, **__):
+    """Resolve launch args and emit the gz_sim IncludeLaunchDescription action."""
+    gui = context.launch_configurations.get("gui", "true").lower() == "true"
+    world = context.launch_configurations.get(
+        "world",
+        os.path.join(get_package_share_directory(PKG_NAME), "worlds", "agenticros_indoor.sdf"),
+    )
+    gz_args = f"-r {world}" if gui else f"-r -s --headless-rendering {world}"
+
+    gz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("ros_gz_sim"), "launch", "gz_sim.launch.py"]
+            )
+        ),
+        launch_arguments=[("gz_args", gz_args)],
+    )
+    return [gz_launch]

package/runtime/ros2_ws/src/agenticros_sim/models/agenticros_amr/model.config

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<model>
+  <name>AgenticROS AMR</name>
+  <version>0.1</version>
+  <sdf version="1.10">model.sdf</sdf>
+  <author>
+    <name>PlaiPin</name>
+    <email>hello@plaipin.com</email>
+  </author>
+  <description>
+    Two-wheel differential-drive AMR with a forward-facing RealSense-D435-style
+    depth camera, a 2D GPU lidar, and an IMU. Topic layout matches the real
+    AgenticROS robot (camera/camera/color/image_raw, camera/camera/depth/
+    image_rect_raw, scan, imu/data, odom, cmd_vel) so plugin code is identical
+    in real and sim.
+  </description>
+</model>