diff --git a/docker/perception/sensor_fusion/sensor_fusion.Dockerfile b/docker/perception/sensor_fusion/sensor_fusion.Dockerfile new file mode 100644 index 00000000..f6210f7c --- /dev/null +++ b/docker/perception/sensor_fusion/sensor_fusion.Dockerfile @@ -0,0 +1,55 @@ +ARG BASE_IMAGE=ghcr.io/watonomous/wato_monorepo/base:humble-ubuntu22.04 + +################################ Source ################################ +FROM ${BASE_IMAGE} as source + +WORKDIR ${AMENT_WS}/src + +# Copy in source code +COPY src/perception/sensor_fusion sensor_fusion +COPY src/wato_msgs/sample_msgs sample_msgs + +# Scan for rosdeps +RUN apt-get -qq update && rosdep update && \ + rosdep install --from-paths . --ignore-src -r -s \ + | grep 'apt-get install' \ + | awk '{print $3}' \ + | sort > /tmp/colcon_install_list + +################################# Dependencies ################################ +FROM ${BASE_IMAGE} as dependencies + +# Install Rosdep requirements +COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list +RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) + +# Copy in source code from source stage +WORKDIR ${AMENT_WS} +COPY --from=source ${AMENT_WS}/src src + +# Dependency Cleanup +WORKDIR / +RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \ + rm -rf /root/* /root/.ros /tmp/* /var/lib/apt/lists/* /usr/share/doc/* + +################################ Build ################################ +FROM dependencies as build + +# Build ROS2 packages +WORKDIR ${AMENT_WS} +RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ + colcon build \ + --cmake-args -DCMAKE_BUILD_TYPE=Release + +# Entrypoint will run before any CMD on launch. Sources ~/opt//setup.bash and ~/ament_ws/install/setup.bash +COPY docker/wato_ros_entrypoint.sh ${AMENT_WS}/wato_ros_entrypoint.sh +ENTRYPOINT ["./wato_ros_entrypoint.sh"] + +################################ Prod ################################ +FROM build as deploy + +# Source Cleanup and Security Setup +RUN chown -R $USER:$USER ${AMENT_WS} +RUN rm -rf src/* + +USER ${USER} diff --git a/modules/docker-compose.perception.yaml b/modules/docker-compose.perception.yaml index d30474f9..6b27b651 100644 --- a/modules/docker-compose.perception.yaml +++ b/modules/docker-compose.perception.yaml @@ -89,3 +89,14 @@ services: target: deploy image: "${PERCEPTION_DEPTH_ESTIMATION_IMAGE}:${TAG}" command: /bin/bash -c "ros2 launch depth_estimation eve.launch.py" + + sensor_fusion: + build: + context: .. + dockerfile: docker/perception/sensor_fusion/sensor_fusion.Dockerfile + cache_from: + - "${PERCEPTION_SENSOR_FUSION_IMAGE}:build_${TAG}" + - "${PERCEPTION_SENSOR_FUSION_IMAGE}:build_main" + target: deploy + image: "${PERCEPTION_SENSOR_FUSION_IMAGE}:${TAG}" + command: /bin/bash -c "ros2 launch sensor_fusion eve.launch.py" diff --git a/src/perception/camera_object_detection/config/eve_2cam_config.yaml b/src/perception/camera_object_detection/config/eve_2cam_config.yaml new file mode 100755 index 00000000..208ccbe6 --- /dev/null +++ b/src/perception/camera_object_detection/config/eve_2cam_config.yaml @@ -0,0 +1,23 @@ +left_camera_object_detection_node: + ros__parameters: + camera_topic: /camera/left/image_color + publish_vis_topic: /camera/left/camera_detections_viz + publish_detection_topic: /camera/left/camera_detections + model_path: /perception_models/yolov8m.pt + image_size: 1024 + +center_camera_object_detection_node: + ros__parameters: + camera_topic: /camera/center/image_color + publish_vis_topic: /camera/center/camera_detections_viz + publish_detection_topic: /camera/center/camera_detections + model_path: /perception_models/yolov8m.pt + image_size: 1024 + +right_camera_object_detection_node: + ros__parameters: + camera_topic: /camera/right/image_color + publish_vis_topic: /camera/right/camera_detections_viz + publish_detection_topic: /camera/right/camera_detections + model_path: /perception_models/yolov8m.pt + image_size: 1024 diff --git a/src/perception/sensor_fusion/package.xml b/src/perception/sensor_fusion/package.xml new file mode 100644 index 00000000..fc39e222 --- /dev/null +++ b/src/perception/sensor_fusion/package.xml @@ -0,0 +1,23 @@ + + + + sensor_fusion + 0.0.0 + Sensor fusion node that subscribes to multiple sensor topics and synchronizes their output as a unified output message. + bolty + TODO: License declaration + + + sensor_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + + diff --git a/src/perception/sensor_fusion/sensor_fusion/__init__.py b/src/perception/sensor_fusion/sensor_fusion/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/perception/sensor_fusion/sensor_fusion/sensor_fusion_node.py b/src/perception/sensor_fusion/sensor_fusion/sensor_fusion_node.py new file mode 100644 index 00000000..df1701fb --- /dev/null +++ b/src/perception/sensor_fusion/sensor_fusion/sensor_fusion_node.py @@ -0,0 +1,45 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_msgs.msg import String +from message_filters import Subscriber, TimeSynchronizer +from cv_bridge import CvBridge +import json + +class SensorFusionNode(Node): + def __init__(self): + super().__init__('sensor_fusion_node') + + self.bridge = CvBridge() + + # Subscribe to camera topics + self.subscribers = [ + Subscriber(self, Image, f'/camera{i}/image_raw') for i in range(1, 9) + ] + + # TimeSynchronizer to synchronize messages + self.ts = TimeSynchronizer(self.subscribers, 10) + self.ts.registerCallback(self.image_callback) + + # Publisher for synchronized images + self.publisher = self.create_publisher(String, 'synchronized_images', 10) + + def image_callback(self, *images): + # Convert ROS images to OpenCV format and encode them + encoded_images = [self.bridge.cv2_to_imgmsg(cv2.imencode('.jpg', self.bridge.imgmsg_to_cv2(img, 'bgr8'))[1], encoding='bgr8') for img in images] + + # Create a JSON object to hold the images + images_dict = {f'camera{i+1}': img.data.tobytes().hex() for i, img in enumerate(encoded_images)} + + # Publish the synchronized images as an array of images + self.publisher.publish(json.dumps(images_dict)) + +def main(args=None): + rclpy.init(args=args) + node = SensorFusionNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/perception/sensor_fusion/setup.py b/src/perception/sensor_fusion/setup.py new file mode 100644 index 00000000..1d0c2f96 --- /dev/null +++ b/src/perception/sensor_fusion/setup.py @@ -0,0 +1,31 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'sensor_fusion' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'launch', 'include'), glob('launch/include/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Justin', + maintainer_email='j89leung@watonomous.ca', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'sensor_fusion_node = sensor_fusion.sensor_fusion_node:main' + ], + }, +) diff --git a/watod-config.sh b/watod-config.sh index 3255dd40..d4d00cc6 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -15,7 +15,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -# ACTIVE_MODULES="" +ACTIVE_MODULES="perception" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. @@ -31,7 +31,7 @@ ## Tag to use. Images are formatted as : with forward slashes replaced with dashes. ## DEFAULT = "" -# TAG="" +TAG="jusleung" # Docker Registry to pull/push images. DEFAULT = "ghcr.io/watonomous/wato_monorepo" # REGISTRY_URL="" diff --git a/watod_scripts/watod-setup-env.sh b/watod_scripts/watod-setup-env.sh index a165c028..96c46cb4 100755 --- a/watod_scripts/watod-setup-env.sh +++ b/watod_scripts/watod-setup-env.sh @@ -73,6 +73,7 @@ PERCEPTION_LANE_DETECTION_IMAGE=${PERCEPTION_LANE_DETECTION_IMAGE:-"$REGISTRY_UR PERCEPTION_SEMANTIC_SEGMENTATION_IMAGE=${PERCEPTION_SEMANTIC_SEGMENTATION_IMAGE:-"$REGISTRY_URL/perception/semantic_segmentation"} PERCEPTION_TRACKING_IMAGE=${PERCEPTION_TRACKING_IMAGE:-"$REGISTRY_URL/perception/tracking"} PERCEPTION_DEPTH_ESTIMATION_IMAGE=${PERCEPTION_DEPTH_ESTIMATION_IMAGE:-"$REGISTRY_URL/perception/depth_estimation"} +PERCEPTION_SENSOR_FUSION_IMAGE=${PERCEPTION_SENSOR_FUSION_IMAGE:-"$REGISTRY_URL/perception/sensor_fusion"} # World Modeling WORLD_MODELING_HD_MAP_IMAGE=${WORLD_MODELING_HD_MAP_IMAGE:-"$REGISTRY_URL/world_modeling/hd_map"} @@ -168,6 +169,7 @@ echo "PERCEPTION_LANE_DETECTION_IMAGE=$PERCEPTION_LANE_DETECTION_IMAGE" >> "$MOD echo "PERCEPTION_SEMANTIC_SEGMENTATION_IMAGE=$PERCEPTION_SEMANTIC_SEGMENTATION_IMAGE" >> "$MODULES_DIR/.env" echo "PERCEPTION_TRACKING_IMAGE=$PERCEPTION_TRACKING_IMAGE" >> "$MODULES_DIR/.env" echo "PERCEPTION_DEPTH_ESTIMATION_IMAGE=$PERCEPTION_DEPTH_ESTIMATION_IMAGE" >> "$MODULES_DIR/.env" +echo "PERCEPTION_SENSOR_FUSION_IMAGE=$PERCEPTION_SENSOR_FUSION_IMAGE" >> "$MODULES_DIR/.env" # World Modeling echo "WORLD_MODELING_HD_MAP_IMAGE=$WORLD_MODELING_HD_MAP_IMAGE" >> "$MODULES_DIR/.env"