Feature/multirobot changes ros (#19)

Adds messages for sending vlc_db information around and utilities for converting back and forth between the message form and the vlc_db form. 

* add msgs for ros comm

* add conversion functions

* add conversion tests

* test ROS tooling

* manual install ROS for ci

* fix ros dependencies

* forgot missing apt install line

* debug ros install

* fix cmakelists

* clean up ros ci action

* fix pytest error

* manually install pytest

* install ouroboros

* upgrade pip before ouroboros install

* source ws before running pytest

* specify scipy version

* ROS CI based on Noetic Container

---------

Co-authored-by: Aaron Ray <[email protected]>

.github/workflows/ci-action-ros.yaml

@@ -0,0 +1,48 @@
+name: Ouroboros-ROS-CI
+run-name: Ouroboros-ROS-CI
+on:
+    push:
+        branches: main
+    pull_request:
+        branches:
+            - main
+            - develop
+jobs:
+  Ouroboros-ROS-CI:
+    runs-on: ubuntu-latest
+    container: ros:noetic-ros-base-focal
+    steps:
+      - name: Update git
+        run: sudo apt update && sudo apt install -y git
+      - name: Check out repository code
+        uses: actions/checkout@v4
+        with:
+            path: src/ouroboros_repo
+            submodules: recursive
+      - name: Dependencies
+        run: |
+            sudo apt install -y libeigen3-dev pkg-config ros-noetic-cv-bridge python3-pip
+            sudo pip install --upgrade pip
+            sudo python3 -m pip install catkin_tools empy catkin_pkg
+      - name: Install Ouroboros
+        run: pwd && ls && cd src/ouroboros_repo && pwd && pip install .
+
+      - name: Install ROS packages with rosdep
+        shell: bash
+        run: |
+          source /opt/ros/noetic/setup.bash
+          rosdep update
+          rosdep install --from-paths src --ignore-src -r -s  # do a dry-run first
+          rosdep install --from-paths src --ignore-src -r -y
+      - name: catkin build
+        shell: bash
+        run: |
+          source /opt/ros/noetic/setup.bash
+          catkin build -s
+      - name: Run test script
+        shell: bash
+        run: |
+          source devel/setup.bash
+          cd src/ouroboros_repo
+          pytest extra/ouroboros_ros/tests
+      - run: echo "🍏 This job's status is ${{ job.status }}."

.github/workflows/ci-action.yml

@@ -31,5 +31,5 @@ jobs:
       - name: Install Ouroboros
         run: cd ${{ github.workspace }}/ouroboros_repo && pwd && pip install .
       - name: Run test script
-        run: cd ${{ github.workspace }}/ouroboros_repo && pytest --ignore=third_party
+        run: cd ${{ github.workspace }}/ouroboros_repo && pytest --ignore=third_party --ignore=extra/ouroboros_ros
       - run: echo "🍏 This job's status is ${{ job.status }}."

extra/ouroboros_msgs/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.10)
+project(ouroboros_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+    message_generation
+    std_msgs
+    geometry_msgs
+    sensor_msgs
+)
+
+add_message_files(
+  DIRECTORY
+    msg
+  FILES
+    SparkImageMsg.msg
+    VlcImageMetadataMsg.msg
+    VlcImageMsg.msg
+)
+
+add_service_files(
+  DIRECTORY
+    srv
+  FILES
+    VlcKeypointQuery.srv
+)
+
+generate_messages(DEPENDENCIES std_msgs geometry_msgs sensor_msgs)
+
+catkin_package(
+  CATKIN_DEPENDS
+    message_runtime
+    std_msgs
+    geometry_msgs
+    sensor_msgs
+    DEPENDS
+    INCLUDE_DIRS
+    LIBRARIES
+)

extra/ouroboros_msgs/msg/SparkImageMsg.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+sensor_msgs/Image rgb
+sensor_msgs/Image depth

extra/ouroboros_msgs/msg/VlcImageMetadataMsg.msg

@@ -0,0 +1,4 @@
+string image_uuid
+string session_id
+uint64 epoch_ns
+

extra/ouroboros_msgs/msg/VlcImageMsg.msg

@@ -0,0 +1,11 @@
+std_msgs/Header header
+ouroboros_msgs/VlcImageMetadataMsg metadata
+ouroboros_msgs/SparkImageMsg image
+float32[] embedding
+sensor_msgs/Image keypoints
+float32[] keypoint_depths
+sensor_msgs/Image descriptors
+bool has_pose_hint
+geometry_msgs/PoseStamped pose_hint
+
+

extra/ouroboros_msgs/package.xml

@@ -0,0 +1,15 @@
+<package format="2">
+  <name>ouroboros_msgs</name>
+  <version>0.0.0</version>
+  <description>ROS msgs for Ouroboros VLC Server</description>
+  <maintainer email="[email protected]">Aaron Ray</maintainer>
+
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>  
+</package>

extra/ouroboros_msgs/srv/VlcKeypointQuery.srv

@@ -0,0 +1,3 @@
+string image_uuid
+---
+ouroboros_msgs/VlcImageMsg vlc_image

extra/ouroboros_ros/CMakeLists.txt

@@ -3,6 +3,7 @@ project(ouroboros_ros)
 
 find_package(catkin REQUIRED COMPONENTS
     rospy
+    ouroboros_msgs
     dynamic_reconfigure
 )
 
@@ -15,4 +16,5 @@ generate_dynamic_reconfigure_options(
 catkin_package(
   CATKIN_DEPENDS
     rospy
+    ouroboros_msgs
 )

extra/ouroboros_ros/package.xml

@@ -9,6 +9,7 @@
   <!-- Dependencies which this package needs to build itself. -->
   <buildtool_depend>catkin</buildtool_depend>
   <depend>rospy</depend>
+  <depend>ouroboros_msgs</depend>
   <depend>python3-matplotlib</depend>
   <depend>python3-scipy</depend>
 </package>

extra/ouroboros_ros/src/ouroboros_ros/conversions.py

@@ -0,0 +1,145 @@
+import numpy as np
+import rospy
+from cv_bridge import CvBridge
+from geometry_msgs.msg import PoseStamped
+from ouroboros_msgs.msg import SparkImageMsg, VlcImageMetadataMsg, VlcImageMsg
+
+from ouroboros import SparkImage, VlcImage, VlcImageMetadata, VlcPose
+
+
+def vlc_image_metadata_to_msg(metadata: VlcImageMetadata) -> VlcImageMetadataMsg:
+    metadata_msg = VlcImageMetadataMsg()
+    metadata_msg.image_uuid = metadata.image_uuid
+    metadata_msg.session_id = metadata.session_id
+    metadata_msg.epoch_ns = metadata.epoch_ns
+    return metadata_msg
+
+
+def spark_image_to_msg(image: SparkImage) -> SparkImageMsg:
+    bridge = CvBridge()
+    image_msg = SparkImageMsg()
+    image_msg.rgb = bridge.cv2_to_imgmsg(image.rgb, encoding="passthrough")
+    image_msg.depth = bridge.cv2_to_imgmsg(image.depth, encoding="passthrough")
+    return image_msg
+
+
+def vlc_pose_to_msg(pose: VlcPose) -> PoseStamped:
+    pose_msg = PoseStamped()
+    pose_msg.header.stamp = rospy.Time(nsecs=pose.time_ns)
+    pose_msg.pose.position.x = pose.position[0]
+    pose_msg.pose.position.y = pose.position[1]
+    pose_msg.pose.position.z = pose.position[2]
+    pose_msg.pose.orientation.x = pose.rotation[0]
+    pose_msg.pose.orientation.y = pose.rotation[1]
+    pose_msg.pose.orientation.z = pose.rotation[2]
+    pose_msg.pose.orientation.w = pose.rotation[3]
+    return pose_msg
+
+
+def vlc_image_to_msg(
+    vlc: VlcImage,
+    *,
+    convert_image=True,
+    convert_embedding=True,
+    convert_keypoints=True,
+    convert_descriptors=True,
+) -> VlcImageMsg:
+    bridge = CvBridge()
+    vlc_msg = VlcImageMsg()
+    if vlc.image is not None and convert_image:
+        vlc_msg.image = spark_image_to_msg(vlc.image)
+    vlc_msg.header = vlc_msg.image.header
+    vlc_msg.metadata = vlc_image_metadata_to_msg(vlc.metadata)
+    if vlc.embedding is not None and convert_embedding:
+        vlc_msg.embedding = vlc.embedding.tolist()
+    if vlc.keypoints is not None and convert_keypoints:
+        vlc_msg.keypoints = bridge.cv2_to_imgmsg(vlc.keypoints, encoding="passthrough")
+        if vlc.keypoint_depths is not None:
+            vlc_msg.keypoint_depths = vlc.keypoint_depths.tolist()
+    if vlc.descriptors is not None and convert_descriptors:
+        vlc_msg.descriptors = bridge.cv2_to_imgmsg(
+            vlc.descriptors, encoding="passthrough"
+        )
+    if vlc.pose_hint is not None:
+        vlc_msg.has_pose_hint = True
+        vlc_msg.pose_hint = vlc_pose_to_msg(vlc.pose_hint)
+    return vlc_msg
+
+
+def vlc_image_metadata_from_msg(metadata_msg: VlcImageMetadataMsg) -> VlcImageMetadata:
+    return VlcImageMetadata(
+        image_uuid=metadata_msg.image_uuid,
+        session_id=metadata_msg.session_id,
+        epoch_ns=metadata_msg.epoch_ns,
+    )
+
+
+def spark_image_from_msg(image_msg: SparkImageMsg) -> SparkImage:
+    bridge = CvBridge()
+
+    if image_msg.rgb.encoding == "":
+        rgb = None
+    else:
+        rgb = bridge.imgmsg_to_cv2(image_msg.rgb, desired_encoding="passthrough")
+
+    if image_msg.depth.encoding == "":
+        depth = None
+    else:
+        depth = bridge.imgmsg_to_cv2(image_msg.depth, desired_encoding="passthrough")
+
+    return SparkImage(
+        rgb=rgb,
+        depth=depth,
+    )
+
+
+def vlc_pose_from_msg(pose_msg: PoseStamped) -> VlcPose:
+    pos = pose_msg.pose.position
+    quat = pose_msg.pose.orientation
+    return VlcPose(
+        time_ns=pose_msg.header.stamp.to_nsec(),
+        position=np.array([pos.x, pos.y, pos.z]),
+        rotation=np.array([quat.x, quat.y, quat.z, quat.w]),
+    )
+
+
+def vlc_image_from_msg(vlc_msg: VlcImageMsg) -> VlcImage:
+    bridge = CvBridge()
+    pose_hint = None
+    if vlc_msg.has_pose_hint:
+        pose_hint = vlc_pose_from_msg(vlc_msg.pose_hint)
+
+    if len(vlc_msg.embedding) == 0:
+        embedding = None
+    else:
+        embedding = np.array(vlc_msg.embedding)
+
+    if vlc_msg.keypoints.encoding == "":
+        keypoints = None
+    else:
+        keypoints = bridge.imgmsg_to_cv2(
+            vlc_msg.keypoints, desired_encoding="passthrough"
+        )
+
+    if len(vlc_msg.keypoint_depths) == 0:
+        keypoint_depths = None
+    else:
+        keypoint_depths = np.array(vlc_msg.keypoint_depths)
+
+    if vlc_msg.descriptors.encoding == "":
+        descriptors = None
+    else:
+        descriptors = bridge.imgmsg_to_cv2(
+            vlc_msg.descriptors, desired_encoding="passthrough"
+        )
+    vlc_image = VlcImage(
+        metadata=vlc_image_metadata_from_msg(vlc_msg.metadata),
+        image=spark_image_from_msg(vlc_msg.image),
+        embedding=embedding,
+        keypoints=keypoints,
+        keypoint_depths=keypoint_depths,
+        descriptors=descriptors,
+        pose_hint=pose_hint,
+    )
+
+    return vlc_image

extra/ouroboros_ros/tests/test_conversions.py

@@ -0,0 +1,86 @@
+"""Test pose utilities."""
+
+from datetime import datetime
+
+import numpy as np
+import numpy.testing as npt
+from ouroboros_ros.conversions import (
+    spark_image_from_msg,
+    spark_image_to_msg,
+    vlc_image_from_msg,
+    vlc_image_metadata_from_msg,
+    vlc_image_metadata_to_msg,
+    vlc_image_to_msg,
+    vlc_pose_from_msg,
+    vlc_pose_to_msg,
+)
+
+import ouroboros as ob
+
+
+def test_vlc_metadata_conversion():
+    vlc_db = ob.VlcDb(3)
+    session_id = vlc_db.add_session(0)
+    img_stamp = datetime.now()
+    img_uuid = vlc_db.add_image(session_id, img_stamp, ob.SparkImage())
+
+    metadata = vlc_db.get_image(img_uuid).metadata
+
+    metadata_msg = vlc_image_metadata_to_msg(metadata)
+    metadata_converted = vlc_image_metadata_from_msg(metadata_msg)
+
+    assert metadata_converted.image_uuid == metadata.image_uuid
+    assert metadata_converted.session_id == metadata.session_id
+    assert metadata_converted.epoch_ns == metadata.epoch_ns
+
+
+def test_spark_image_conversion():
+    height = 10
+    width = 10
+    rgb_image = np.random.randint(0, 256, (height, width, 3), dtype=np.uint8)
+    depth_image = np.random.uniform(0, 10, (height, width)).astype(np.float32)
+
+    image = ob.SparkImage(rgb=rgb_image, depth=depth_image)
+    image_msg = spark_image_to_msg(image)
+    image_converted = spark_image_from_msg(image_msg)
+
+    npt.assert_array_equal(image_converted.rgb, image.rgb)
+    npt.assert_array_equal(image_converted.depth, image.depth)
+
+
+def test_vlc_pose_conversion():
+    pose = ob.VlcPose(
+        time_ns=100, position=np.array([1, 2, 3]), rotation=np.array([1, 0, 0, 0])
+    )
+
+    geom_msg = vlc_pose_to_msg(pose)
+    pose_converted = vlc_pose_from_msg(geom_msg)
+
+    assert pose_converted.time_ns == pose.time_ns
+    npt.assert_array_equal(pose_converted.position, pose.position)
+    npt.assert_array_equal(pose_converted.rotation, pose.rotation)
+
+
+def test_vlc_image_conversion():
+    height = 10
+    width = 10
+    rgb_image = np.random.randint(0, 256, (height, width, 3), dtype=np.uint8)
+    depth_image = np.random.uniform(0, 10, (height, width)).astype(np.float32)
+    embedding = np.random.uniform(0, 1, (100,)).astype(np.float32)
+    keypoints = np.random.uniform(0, 10, (10, 3)).astype(np.float32)
+    descriptors = np.random.uniform(0, 1, (10, 256)).astype(np.float32)
+
+    vlc_db = ob.VlcDb(100)
+    session_id = vlc_db.add_session(0)
+    img_stamp = datetime.now()
+    img_uuid = vlc_db.add_image(
+        session_id, img_stamp, ob.SparkImage(rgb=rgb_image, depth=depth_image)
+    )
+    vlc_db.update_embedding(img_uuid, embedding)
+    vlc_db.update_keypoints(img_uuid, keypoints, descriptors)
+
+    vlc_img = vlc_db.get_image(img_uuid)
+
+    vlc_img_msg = vlc_image_to_msg(vlc_img)
+    vlc_img_converted = vlc_image_from_msg(vlc_img_msg)
+    assert vlc_img_converted.metadata == vlc_img.metadata

setup.py

@@ -110,7 +110,7 @@ def _get_eigen_flags():
         "opencv-python",
         "pyyaml",
         "pytest",
-        "scipy",
+        "scipy>=1.4.0",
     ],
     ext_modules=ext_modules,
     cmdclass={"build_ext": build_ext},