creating modularity within perception module for several packages - centralized launch

Parasmai Conjeevaram · Parasmai Conjeevaram · commit 74fbba6d6d23 · 2025-05-28T16:28:06.000Z
diff --git a/autonomy/samples/python/aggregator/package.xml b/autonomy/samples/python/aggregator/package.xml
@@ -3,9 +3,9 @@
 <package format="3">
   <name>aggregator</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Sample aggregator node for data collection</description>
   <maintainer email="e23zhou@watonomous.ca">eddyzhou</maintainer>
-  <license>Apache2.0: License declaration</license>
+  <license>Apache2.0</license>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
diff --git a/autonomy/samples/python/aggregator/setup.py b/autonomy/samples/python/aggregator/setup.py
@@ -1,5 +1,11 @@
 import os
-from glob import glob
+from glob    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='eddyzhou, aryanafrouzi',
+    maintainer_email='e23zhou@watonomous.ca, aafrouzi@watonomous.ca',
+    description='Sample aggregator node for data collection',
+    license='Apache2.0',
+    tests_require=['pytest'], glob
 from setuptools import setup
 
 package_name = 'aggregator'
@@ -21,8 +27,8 @@
     zip_safe=True,
     maintainer='eddyzhou, aryanafrouzi',
     maintainer_email='e23zhou@watonomous.ca, aryanafrouzi@swaprobotics.com',
-    description='TODO: Package description',
-    license='TODO: License declaration',
+    description='Sample aggregator node for data collection',
+    license='Apache2.0',
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
diff --git a/autonomy/samples/python/producer/package.xml b/autonomy/samples/python/producer/package.xml
@@ -3,9 +3,9 @@
 <package format="3">
   <name>producer</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Sample producer node for data generation</description>
   <maintainer email="e23zhou@watonomous.ca">eddyzhou</maintainer>
-  <license>Apache2.0: License declaration</license>
+  <license>Apache2.0</license>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
diff --git a/autonomy/samples/python/producer/setup.py b/autonomy/samples/python/producer/setup.py
@@ -23,8 +23,8 @@
     zip_safe=True,
     maintainer='eddyzhou, aryanafrouzi',
     maintainer_email='e23zhou@watonomous.ca, aafrouzi@watonomous.ca',
-    description='TODO: Package description',
-    license='TODO: License declaration',
+    description='Sample producer node for data generation',
+    license='Apache2.0',
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
diff --git a/autonomy/samples/python/transformer/package.xml b/autonomy/samples/python/transformer/package.xml
@@ -3,9 +3,9 @@
 <package format="3">
   <name>transformer</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Sample transformer node for data processing</description>
   <maintainer email="e23zhou@watonomous.ca">eddyzhou</maintainer>
-  <license>Apache2.0: License declaration</license>
+  <license>Apache2.0</license>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
diff --git a/autonomy/samples/python/transformer/setup.py b/autonomy/samples/python/transformer/setup.py
@@ -23,8 +23,8 @@
     zip_safe=True,
     maintainer='eddyzhou, aryanafrouzi',
     maintainer_email='e23zhou@watonomous.ca, aafrouzi@watonomous.ca',
-    description='TODO: Package description',
-    license='TODO: License declaration',
+    description='Sample transformer node for data processing',
+    license='Apache2.0',
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
diff --git a/autonomy/samples/sample_msgs/package.xml b/autonomy/samples/sample_msgs/package.xml
@@ -4,7 +4,7 @@
   <version>0.0.0</version>
   <description>Sample ROS messages</description>
 
-  <maintainer email="watouser@todo.todo">watouser</maintainer>
+  <maintainer email="pconjeevaram@watonomous.ca">conjeevaram</maintainer>
   <license>Apache2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
diff --git a/autonomy/wato_msgs/sample_msgs/package.xml b/autonomy/wato_msgs/sample_msgs/package.xml
@@ -4,7 +4,7 @@
   <version>0.0.0</version>
   <description>Sample ROS messages</description>
 
-  <maintainer email="watouser@todo.todo">watouser</maintainer>
+  <maintainer email="pconjeevaram@watonomous.ca">conjeevaram</maintainer>
   <license>Apache2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
diff --git a/docker/perception/perception.Dockerfile b/docker/perception/perception.Dockerfile
@@ -7,6 +7,8 @@ WORKDIR ${AMENT_WS}/src
 
 # Copy in source code 
 COPY src/perception/depth_estimation depth_estimation 
+COPY src/perception/pose_estimation pose_estimation
+COPY src/perception/perception_launch perception_launch
 COPY src/wato_msgs/sample_msgs sample_msgs
 
 # Scan for rosdeps
diff --git a/modules/docker-compose.perception.yaml b/modules/docker-compose.perception.yaml
@@ -1,13 +1,13 @@
 services:
   perception: # PYTHON
-    build: &producer_build
+    build: &perception_build
       context: ..
       dockerfile: docker/perception/perception.Dockerfile
       cache_from:
         - "${SAMPLES_PRODUCER_IMAGE:?}:${TAG}"
         - "${SAMPLES_PRODUCER_IMAGE:?}:main"
     image: "${SAMPLES_PRODUCER_IMAGE:?}:${TAG}"
     profiles: [deploy]
-    command: /bin/bash -c "ros2 launch depth_estimation depth_estimation.launch.py"
+    command: /bin/bash -c "ros2 launch perception_launch perception.launch.py"
     volumes:
-      - ${MONO_DIR}/src/samples/python/perception:/root/ament_ws/src/perception
+      - ${MONO_DIR}/src/perception:/root/ament_ws/src/perception
diff --git a/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py b/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py
@@ -18,6 +18,21 @@
 class DepthEstimationCore():
 
     def __init__(self, pos_x, pos_y, pos_z, vel):
+        # Init member variables for serialization
+        self.__pos_x = pos_x
+        self.__pos_y = pos_y
+        self.__pos_z = pos_z
+        self.__velocity = vel
+
+    def update_position(self):
+        # velocity in 3D delta_x = delta_y = delta_z
+        self.__pos_x += self.__velocity / math.sqrt(3)
+        self.__pos_y += self.__velocity / math.sqrt(3)
+        self.__pos_z += self.__velocity / math.sqrt(3)
+
+    def serialize_data(self):
+        return "x:" + str(self.__pos_x) + ";y:" + \
+            str(self.__pos_y) + ";z:" + str(self.__pos_z) + ";"
 
 
 
diff --git a/src/perception/depth_estimation/package.xml b/src/perception/depth_estimation/package.xml
@@ -3,9 +3,12 @@
 <package format="3">
   <name>depth_estimation</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Depth estimation module for humanoid robot perception</description>
   <maintainer email="e23zhou@watonomous.ca">eddyzhou</maintainer>
-  <license>Apache2.0: License declaration</license>
+  <license>Apache2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>sample_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
diff --git a/src/perception/depth_estimation/setup.py b/src/perception/depth_estimation/setup.py
@@ -23,8 +23,8 @@
     zip_safe=True,
     maintainer='eddyzhou, aryanafrouzi',
     maintainer_email='e23zhou@watonomous.ca, aafrouzi@watonomous.ca',
-    description='TODO: Package description',
-    license='TODO: License declaration',
+    description='Depth estimation module for humanoid robot perception',
+    license='Apache2.0',
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
diff --git a/src/perception/perception_launch/launch/perception.launch.py b/src/perception/perception_launch/launch/perception.launch.py
@@ -20,20 +20,31 @@
 
 
 def generate_launch_description():
-    # To load the yaml file, we are searching for its
-    # path is the share directory. Check setup.py for how
-    # the param file got there
-    param_file_path = os.path.join(
+    # Load depth estimation parameters
+    depth_param_file_path = os.path.join(
         get_package_share_directory('depth_estimation'),
         'config',
         'params.yaml'
     )
+    
+    # Load pose estimation parameters
+    pose_param_file_path = os.path.join(
+        get_package_share_directory('pose_estimation'),
+        'config',
+        'params.yaml'
+    )
 
     return LaunchDescription([
         Node(
             package='depth_estimation',
             name='depth_estimation_node',
             executable='depth_estimation_node',
-            parameters=[param_file_path]
+            parameters=[depth_param_file_path]
+        ),
+        Node(
+            package='pose_estimation',
+            name='pose_estimation_node',
+            executable='pose_estimation_node',
+            parameters=[pose_param_file_path]
         )
     ])
diff --git a/src/perception/perception_launch/package.xml b/src/perception/perception_launch/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>perception_launch</name>
+  <version>0.0.0</version>
+  <description>Launch files for the perception module</description>
+  <maintainer email="pconjeevaram@watonomous.ca">conjeevaram</maintainer>
+  <license>Apache2.0</license>
+
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>depth_estimation</exec_depend>
+  <exec_depend>pose_estimation</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/src/perception/perception_launch/perception_launch/__init__.py b/src/perception/perception_launch/perception_launch/__init__.py
@@ -0,0 +1,13 @@
+# Copyright 2023 WATonomous
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
diff --git a/src/perception/perception_launch/resource/perception_launch b/src/perception/perception_launch/resource/perception_launch
@@ -0,0 +1 @@
+perception_launch
diff --git a/src/perception/perception_launch/setup.cfg b/src/perception/perception_launch/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/perception_launch
+[install]
+install-scripts=$base/lib/perception_launch
diff --git a/src/perception/perception_launch/setup.py b/src/perception/perception_launch/setup.py
@@ -0,0 +1,31 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = 'perception_launch'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        # Install marker file in the package index
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        # Include our package.xml file
+        (os.path.join('share', package_name), ['package.xml']),
+        # Include all launch files
+        (os.path.join('share', package_name, 'launch'),
+         glob(os.path.join('launch', '*.launch.py'))),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='conjeevaram',
+    maintainer_email='pconjeevaram@watonomous.ca',
+    description='Launch files for the perception module',
+    license='Apache2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)
diff --git a/src/perception/pose_estimation/config/params.yaml b/src/perception/pose_estimation/config/params.yaml
@@ -0,0 +1,9 @@
+# more info on YAML configs for ROS2 Params:
+# https://roboticsbackend.com/ros2-yaml-params/
+pose_estimation_node: 
+  ros__parameters:
+    pos_x: 1.0
+    pos_y: 1.0
+    pos_z: 1.0
+    
+    velocity: 1.0
diff --git a/src/perception/pose_estimation/package.xml b/src/perception/pose_estimation/package.xml
@@ -0,0 +1,21 @@
+<?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>pose_estimation</name>
+  <version>0.0.0</version>
+  <description>Pose estimation module for humanoid robot perception</description>
+  <maintainer email="pconjeevaram@watonomous.ca">conjeevaram</maintainer>
+  <license>Apache2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>sample_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>
diff --git a/src/perception/pose_estimation/pose_estimation/__init__.py b/src/perception/pose_estimation/pose_estimation/__init__.py
diff --git a/src/perception/pose_estimation/pose_estimation/pose_estimation_core.py b/src/perception/pose_estimation/pose_estimation/pose_estimation_core.py
@@ -0,0 +1,38 @@
+# Copyright 2023 WATonomous
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import math
+
+
+class PoseEstimationCore():
+
+    def __init__(self, pos_x, pos_y, pos_z, vel):
+        # Init member variables for serialization
+        self.__pos_x = pos_x
+        self.__pos_y = pos_y
+        self.__pos_z = pos_z
+        self.__velocity = vel
+
+    def update_position(self):
+        # velocity in 3D delta_x = delta_y = delta_z
+        self.__pos_x += self.__velocity / math.sqrt(3)
+        self.__pos_y += self.__velocity / math.sqrt(3)
+        self.__pos_z += self.__velocity / math.sqrt(3)
+
+    def serialize_data(self):
+        return "x:" + str(self.__pos_x) + ";y:" + \
+            str(self.__pos_y) + ";z:" + str(self.__pos_z) + ";"
+
+
+
diff --git a/src/perception/pose_estimation/pose_estimation/pose_estimation_node.py b/src/perception/pose_estimation/pose_estimation/pose_estimation_node.py
@@ -0,0 +1,36 @@
+# Copyright 2023 WATonomous
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import time
+
+import rclpy
+from rclpy.node import Node
+
+from sample_msgs.msg import Unfiltered
+from pose_estimation.pose_estimation_core import PoseEstimationCore
+
+
+class PoseEstimationNode(Node):
+
+    def __init__(self):
+        pass
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    return
+
+if __name__ == '__main__':
+    main()
diff --git a/src/perception/pose_estimation/resource/pose_estimation b/src/perception/pose_estimation/resource/pose_estimation
diff --git a/src/perception/pose_estimation/setup.cfg b/src/perception/pose_estimation/setup.cfg
diff --git a/src/perception/pose_estimation/setup.py b/src/perception/pose_estimation/setup.py
diff --git a/src/perception/pose_estimation/test/test_copyright.py b/src/perception/pose_estimation/test/test_copyright.py
diff --git a/src/perception/pose_estimation/test/test_flake8.py b/src/perception/pose_estimation/test/test_flake8.py
diff --git a/src/perception/pose_estimation/test/test_pep257.py b/src/perception/pose_estimation/test/test_pep257.py
diff --git a/src/perception/pose_estimation/test/test_producer.py b/src/perception/pose_estimation/test/test_producer.py
diff --git a/watod_scripts/watod-setup-dev-env.sh b/watod_scripts/watod-setup-dev-env.sh
