First pass of Modulr Agent

Author: modulrmike

.gitignore

@@ -0,0 +1,2 @@
+/target
+Cargo.lock

Cargo.toml

@@ -0,0 +1,40 @@
+[package]
+name = "modulr_agent"
+version = "0.1.0"
+edition = "2024"
+
+[dependencies]
+webrtc = "0.10"
+tokio = { version = "1", features = ["full"] }
+tokio-tungstenite = { version = "0.20", features = ["native-tls"] }
+tungstenite = "0.20"
+serde = { version = "1", features = ["derive"] }
+serde_json = "1"
+anyhow = "1"
+futures-util = "0.3.31"
+gstreamer = "0.24.2"
+gstreamer-app = "0.24.2"
+bytes = "1.10.1"
+thiserror = "2.0.17"
+
+# ROS 1 dependencies
+rosrust = { version = "0.9.12", optional = true }
+rosrust_msg = { version = "0.1.8", optional = true }
+
+# ROS 2 dependencies
+rclrs = { version = "*", optional = true }
+geometry_msgs = { version = "*", optional = true }
+sensor_msgs = { version = "*", optional = true }
+std_msgs = { version = "*", optional = true }
+env_logger = "0.11.8"
+log = "0.4.28"
+
+[features]
+default = ["ros2"]
+ros1 = ["dep:rosrust", "dep:rosrust_msg"]
+ros2 = [
+    "dep:rclrs",
+    "dep:geometry_msgs",
+    "dep:sensor_msgs",
+    "dep:std_msgs",
+]

README.md

@@ -0,0 +1,84 @@
+# Modulr Robot Agent
+
+This package contains the agent to be installed on any robot connecting to the Modulr infrastructure.
+
+## Building the Package
+
+This package has only been tested on Ubuntu systems. To build the package, you will need to install Rust: https://rustup.rs/
+
+You will also need to install GStreamer:
+
+```bash
+sudo apt install -y  libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev \
+      gstreamer1.0-plugins-base gstreamer1.0-plugins-good \
+      gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly \
+      gstreamer1.0-libav libgstrtspserver-1.0-dev libges-1.0-dev
+```
+
+Finally, ROS is required. ROS 1 and ROS 2 are both supported.
+
+## ROS 1
+
+Follow the installation instructions here: https://wiki.ros.org/noetic/Installation/Ubuntu
+
+Then, build the package by running:
+
+```bash
+cargo build --features ros1 --no-default-features
+```
+
+You can run the package with logging enabled using:
+
+```bash
+RUST_LOG=debug cargo run --features ros1 --no-default-features
+```
+
+*Alternatively, edit the Cargo.toml file such that ros1 is the default feature, then run without any of the --features or --no-default-features flags.*
+
+## ROS 2
+
+Follow the installation instructions here: https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html
+
+Note that any distro of ROS 2 should be compatible, but only Jazzy has been tested so far.
+
+Some extra installation is required for Rust-related build tooling:
+
+```bash
+sudo apt install -y git libclang-dev python3-pip python3-vcstool
+pip install git+https://github.com/colcon/colcon-cargo.git --break-system-packages
+pip install git+https://github.com/colcon/colcon-ros-cargo.git --break-system-packages
+```
+
+Create a ROS 2 workspace for the package:
+
+```bash
+mkdir -p ~/modulr_ws/src
+cd ~/modulr_ws
+git clone <this repo> src/modulr_agent
+vcs import src < src/modulr_agent/ros2rust.repos
+# Change the following line depending on your installed version of ROS 2
+vcs import src < src/ros2_rust/ros2_rust_jazzy.repos
+```
+
+Build the package (first time command):
+
+```bash
+cd ~/modulr_ws
+colcon build
+source install/setup.bash
+```
+
+Build the package (subsequent calls):
+
+```bash
+cd ~/modulr_ws
+colcon build --packages-select modulr_agent
+```
+
+Run the package with debug logging:
+
+```bash
+RUST_LOG=debug ./install/modulr_agent/bin/modulr_agent
+```
+
+*Note that cargo commands should be enabled, including cargo build and cargo run, but these did not work on the first system. It is a TODO to fix this build issue.*

package.xml

@@ -0,0 +1,16 @@
+<package format="3">
+  <name>modulr_agent</name>
+  <version>0.1.0</version>
+  <description></description>
+  <maintainer email="michael@modulr.cloud">Michael Hart</maintainer>
+  <license>MIT-0</license>
+
+  <depend>rclrs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <export>
+    <build_type>ament_cargo</build_type>
+  </export>
+</package>

ros2rust.repos

@@ -0,0 +1,5 @@
+repositories:
+  ros2_rust:
+    type: git
+    url: https://github.com/ros2-rust/ros2_rust.git
+    version: main

src/main.rs

@@ -0,0 +1,121 @@
+mod ros_bridge;
+mod video_pipeline;
+mod webrtc_link;
+mod webrtc_message;
+
+use std::sync::Arc;
+
+use anyhow::Result;
+use bytes::Bytes;
+use log::{info, warn};
+use tokio::sync::Mutex;
+
+use crate::ros_bridge::RosBridge;
+use crate::video_pipeline::VideoPipeline;
+use crate::video_pipeline::VideoPipelineError;
+use crate::webrtc_link::WebRtcLink;
+use crate::webrtc_link::WebRtcLinkError;
+use crate::webrtc_message::WebRtcMessage;
+
+#[tokio::main]
+async fn main() -> Result<()> {
+    env_logger::init();
+
+    let robot_id = "robot1";
+    let signaling_url = "ws://192.168.132.19:8765";
+    let webrtc_link = Arc::new(Mutex::new(WebRtcLink::new(robot_id, signaling_url)));
+    let pipeline = Arc::new(Mutex::new(VideoPipeline::new()));
+    let bridge = Arc::new(Mutex::new(RosBridge::try_new()?));
+
+    info!("Creating system components and callbacks");
+
+    // Browser -> WebRTC -> ROS
+    let bridge_clone = Arc::clone(&bridge);
+    webrtc_link
+        .lock()
+        .await
+        .on_webrtc_message(Box::new(move |msg: &WebRtcMessage| {
+            let bridge_clone = Arc::clone(&bridge_clone);
+            let msg_clone = msg.clone();
+            Box::pin(async move {
+                match msg_clone {
+                    WebRtcMessage::MovementCommand(cmd) => {
+                        bridge_clone
+                            .lock()
+                            .await
+                            .post_movement_command(&cmd)
+                            .expect("Failed to post movement command!");
+                    }
+                }
+            })
+        }))
+        .await;
+
+    // TODO: only queue frames in pipeline when WebRTC session is established
+
+    // Pipeline -> WebRTC -> Browser
+    let webrtc_link_clone = Arc::clone(&webrtc_link);
+    pipeline
+        .lock()
+        .await
+        .on_frame_ready(Box::new(move |frame: &Bytes| {
+            let webrtc_link_clone = Arc::clone(&webrtc_link_clone);
+            let frame_clone = Bytes::copy_from_slice(frame);
+            Box::pin(async move {
+                let err = webrtc_link_clone
+                    .lock()
+                    .await
+                    .write_frame(frame_clone)
+                    .await;
+                match err {
+                    Err(WebRtcLinkError::IncorrectStateForOperation) => {
+                        warn!("Still waiting for WebRTC pipeline to connect.")
+                    }
+                    Err(_) => {
+                        panic!("Failed to write frame!")
+                    }
+                    _ => (),
+                }
+            })
+        }))
+        .await;
+
+    // Robot frame -> Pipeline
+    let pipeline_clone = Arc::clone(&pipeline);
+    bridge
+        .lock()
+        .await
+        .on_image_frame_received(Box::new(move |data: &Bytes| {
+            let data_clone = Bytes::copy_from_slice(data);
+            let pipeline_clone = Arc::clone(&pipeline_clone);
+            Box::pin(async move {
+                pipeline_clone
+                    .lock()
+                    .await
+                    .queue_frame(&data_clone)
+                    .await
+                    .expect("Failed to write camera frame to pipeline!");
+            })
+        }));
+
+    info!("Starting all tasks running");
+
+    tokio::spawn(async move {
+        let mut guard = webrtc_link.lock().await;
+        guard.try_connect().await?;
+        guard.try_register().await?;
+        Ok::<(), WebRtcLinkError>(())
+    });
+    let pipeline_clone = Arc::clone(&pipeline);
+    tokio::spawn(async move {
+        pipeline_clone.lock().await.launch().await?;
+        info!("Spawned the launch task successfully");
+        Ok::<(), VideoPipelineError>(())
+    });
+    bridge.lock().await.spin();
+
+    let _ = tokio::signal::ctrl_c().await;
+    info!("Exit requested, cleaning up...");
+    pipeline.lock().await.stop_pipeline().await?;
+    Ok(())
+}

src/ros_bridge/mod.rs

@@ -0,0 +1,21 @@
+mod ros_bridge_error;
+use std::pin::Pin;
+
+use bytes::Bytes;
+pub use ros_bridge_error::RosBridgeError;
+
+pub type OnCameraImageHdlrFn =
+    Box<dyn (FnMut(&Bytes) -> Pin<Box<dyn Future<Output = ()> + Send + 'static>>) + Send + Sync>;
+
+#[cfg(feature = "ros1")]
+mod ros1_bridge;
+#[cfg(feature = "ros1")]
+pub use ros1_bridge::RosBridge;
+
+#[cfg(feature = "ros2")]
+mod ros2_bridge;
+#[cfg(feature = "ros2")]
+pub use ros2_bridge::RosBridge;
+
+#[cfg(all(feature = "ros1", feature = "ros2"))]
+compile_error!("select only one ros feature for RosBridge");

src/ros_bridge/ros1_bridge.rs

@@ -0,0 +1,97 @@
+use std::sync::{Arc, Mutex};
+
+use bytes::Bytes;
+use log::{debug, error};
+use rosrust::{Publisher, Subscriber};
+use std::pin::Pin;
+use tokio::sync::mpsc;
+
+use crate::{
+    ros_bridge::{OnCameraImageHdlrFn, RosBridgeError},
+    webrtc_message::MovementCommand,
+};
+
+pub struct RosBridge {
+    _image_sub: Subscriber,
+    image_listeners: Arc<Mutex<Vec<RosBridgeOnFrameCallback>>>,
+    mvmt_pub: Publisher<rosrust_msg::geometry_msgs::Twist>,
+}
+
+impl RosBridge {
+    pub fn try_new() -> Result<Self, RosBridgeError> {
+        rosrust::init("modulr_agent");
+
+        let image_listeners: Arc<Mutex<Vec<OnCameraImageHdlrFn>>> = Arc::new(Mutex::new(vec![]));
+
+        let (tx, mut rx) = mpsc::channel::<Bytes>(10);
+
+        let listeners_clone = Arc::clone(image_listeners);
+        let image_sub = rosrust::subscribe(
+            "/camera/image_raw",
+            1,
+            move |v: rosrust_msg::sensor_msgs::Image| {
+                let buffer = Bytes::from(v.data);
+                if tx.try_send(buffer).is_err() {
+                    error!("Failed to send image buffer to channel!");
+                }
+            },
+        )
+        .map_err(|_| RosBridgeError::SubscriptionCreateFailure)?;
+
+        let listeners_clone = Arc::clone(&image_listeners);
+        tokio::spawn(async move {
+            while let Some(buffer) = rx.recv().await {
+                // debug!("Received frame from ROS camera subscription");
+                for listener in listeners_clone.lock().unwrap().iter_mut() {
+                    tokio::spawn(listener(&buffer));
+                }
+            }
+        });
+
+        let mvmt_pub =
+            rosrust::publish("/cmd_vel", 2).map_err(|_| RosBridgeError::PublisherCreateFailure)?;
+
+        Ok(Self {
+            _image_sub: image_sub,
+            image_listeners,
+            mvmt_pub,
+        })
+    }
+
+    // Register a callback for camera images
+    pub fn on_image_frame_received(&mut self, listener: OnCameraImageHdlrFn) {
+        if let Ok(mut listeners) = self.image_listeners.try_lock() {
+            listeners.push(listener);
+        }
+    }
+
+    // Post a new movement command
+    pub fn post_movement_command(
+        &mut self,
+        command: &MovementCommand,
+    ) -> Result<(), RosBridgeError> {
+        debug!("Received movement command: {:?}", command);
+        let msg = rosrust_msg::geometry_msgs::Twist {
+            linear: rosrust_msg::geometry_msgs::Vector3 {
+                x: command.forward,
+                y: 0f64,
+                z: 0f64,
+            },
+            angular: rosrust_msg::geometry_msgs::Vector3 {
+                x: 0f64,
+                y: 0f64,
+                z: command.turn,
+            },
+        };
+        self.mvmt_pub
+            .send(msg)
+            .map_err(|_| RosBridgeError::PublishFailed)?;
+        Ok(())
+    }
+
+    pub fn spin(&self) {
+        std::thread::spawn(|| {
+            rosrust::spin();
+        });
+    }
+}