From ee0ea1e6f2e8ba3634c1c8c81f1dbb548a81e50d Mon Sep 17 00:00:00 2001 From: Miekale Date: Wed, 5 Mar 2025 02:08:50 +0000 Subject: [PATCH 1/5] init perception --- docker/perception/perception.Dockerfile | 49 ++++++++++++ modules/docker-compose.perception.yaml | 13 +++ .../depth_estimation/config/params.yaml | 9 +++ .../launch/producer.launch.py | 39 +++++++++ src/perception/depth_estimation/package.xml | 18 +++++ .../depth_estimation/producer/__init__.py | 0 .../producer/producer_core.py | 35 ++++++++ .../producer/producer_node.py | 79 +++++++++++++++++++ .../depth_estimation/resource/producer | 0 src/perception/depth_estimation/setup.cfg | 4 + src/perception/depth_estimation/setup.py | 34 ++++++++ .../depth_estimation/test/test_copyright.py | 23 ++++++ .../depth_estimation/test/test_flake8.py | 25 ++++++ .../depth_estimation/test/test_pep257.py | 23 ++++++ .../depth_estimation/test/test_producer.py | 23 ++++++ 15 files changed, 374 insertions(+) create mode 100644 docker/perception/perception.Dockerfile create mode 100644 modules/docker-compose.perception.yaml create mode 100644 src/perception/depth_estimation/config/params.yaml create mode 100755 src/perception/depth_estimation/launch/producer.launch.py create mode 100644 src/perception/depth_estimation/package.xml rename modules_future/docker-compose.perception.yaml => src/perception/depth_estimation/producer/__init__.py (100%) mode change 100644 => 100755 create mode 100644 src/perception/depth_estimation/producer/producer_core.py create mode 100755 src/perception/depth_estimation/producer/producer_node.py create mode 100644 src/perception/depth_estimation/resource/producer create mode 100644 src/perception/depth_estimation/setup.cfg create mode 100755 src/perception/depth_estimation/setup.py create mode 100755 src/perception/depth_estimation/test/test_copyright.py create mode 100755 src/perception/depth_estimation/test/test_flake8.py create mode 100755 src/perception/depth_estimation/test/test_pep257.py create mode 100644 src/perception/depth_estimation/test/test_producer.py diff --git a/docker/perception/perception.Dockerfile b/docker/perception/perception.Dockerfile new file mode 100644 index 0000000..176527a --- /dev/null +++ b/docker/perception/perception.Dockerfile @@ -0,0 +1,49 @@ +ARG BASE_IMAGE=ghcr.io/watonomous/robot_base/base:humble-ubuntu22.04 + +################################ Source ################################ +FROM ${BASE_IMAGE} AS source + +WORKDIR ${AMENT_WS}/src + +# Copy in source code +COPY src/perception/depth_estimation depth_estimation +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 --install-base ${WATONOMOUS_INSTALL} + +# Source and Build Artifact Cleanup +RUN rm -rf src/* build/* devel/* install/* log/* + +# 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"] diff --git a/modules/docker-compose.perception.yaml b/modules/docker-compose.perception.yaml new file mode 100644 index 0000000..cba574f --- /dev/null +++ b/modules/docker-compose.perception.yaml @@ -0,0 +1,13 @@ +services: + perception: # PYTHON + build: &producer_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 perception perception.launch.py" + volumes: + - ${MONO_DIR}/src/samples/python/perception:/root/ament_ws/src/perception \ No newline at end of file diff --git a/src/perception/depth_estimation/config/params.yaml b/src/perception/depth_estimation/config/params.yaml new file mode 100644 index 0000000..6895cc4 --- /dev/null +++ b/src/perception/depth_estimation/config/params.yaml @@ -0,0 +1,9 @@ +# more info on YAML configs for ROS2 Params: +# https://roboticsbackend.com/ros2-yaml-params/ +producer_node: + ros__parameters: + pos_x: 1.0 + pos_y: 1.0 + pos_z: 1.0 + + velocity: 1.0 \ No newline at end of file diff --git a/src/perception/depth_estimation/launch/producer.launch.py b/src/perception/depth_estimation/launch/producer.launch.py new file mode 100755 index 0000000..38ab651 --- /dev/null +++ b/src/perception/depth_estimation/launch/producer.launch.py @@ -0,0 +1,39 @@ +# 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 os +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + + +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( + get_package_share_directory('producer'), + 'config', + 'params.yaml' + ) + + return LaunchDescription([ + Node( + package='producer', + name='producer_node', + executable='producer_node', + parameters=[param_file_path] + ) + ]) diff --git a/src/perception/depth_estimation/package.xml b/src/perception/depth_estimation/package.xml new file mode 100644 index 0000000..c2b4136 --- /dev/null +++ b/src/perception/depth_estimation/package.xml @@ -0,0 +1,18 @@ + + + + producer + 0.0.0 + TODO: Package description + eddyzhou + Apache2.0: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/modules_future/docker-compose.perception.yaml b/src/perception/depth_estimation/producer/__init__.py old mode 100644 new mode 100755 similarity index 100% rename from modules_future/docker-compose.perception.yaml rename to src/perception/depth_estimation/producer/__init__.py diff --git a/src/perception/depth_estimation/producer/producer_core.py b/src/perception/depth_estimation/producer/producer_core.py new file mode 100644 index 0000000..3297652 --- /dev/null +++ b/src/perception/depth_estimation/producer/producer_core.py @@ -0,0 +1,35 @@ +# 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 ProducerCore(): + + 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/producer/producer_node.py b/src/perception/depth_estimation/producer/producer_node.py new file mode 100755 index 0000000..782265d --- /dev/null +++ b/src/perception/depth_estimation/producer/producer_node.py @@ -0,0 +1,79 @@ +# 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 producer.producer_core import ProducerCore + + +class ProducerNode(Node): + + def __init__(self): + super().__init__('python_producer') + # Declare and get the parameters + self.declare_parameter('pos_x', 0.0) + self.declare_parameter('pos_y', 0.0) + self.declare_parameter('pos_z', 0.0) + self.declare_parameter('velocity', 0.0) + + # For parameters, we need to explicitely declare its type for Python to know + # what to do with it + pos_x = self.get_parameter('pos_x').get_parameter_value().double_value + pos_y = self.get_parameter('pos_y').get_parameter_value().double_value + pos_z = self.get_parameter('pos_z').get_parameter_value().double_value + velocity = self.get_parameter('velocity').get_parameter_value().double_value + + # Initialize producer core logic for serialization + self.__producer = ProducerCore(pos_x, pos_y, pos_z, velocity) + + # Initialize ROS2 constructs + queue_size = 10 + self.publisher_ = self.create_publisher(Unfiltered, '/unfiltered_topic', queue_size) + + timer_period = 0.5 + self.timer = self.create_timer(timer_period, self.__publish_position) + + def __publish_position(self): + self.__producer.update_position() + msg = Unfiltered() + + msg.data = self.__producer.serialize_data() + msg.valid = True + msg.timestamp = int(time.time() * 1000) + + self.get_logger().info(f'Publishing: {msg.data}') + + self.publisher_.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + python_producer = ProducerNode() + + rclpy.spin(python_producer) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + python_producer.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/perception/depth_estimation/resource/producer b/src/perception/depth_estimation/resource/producer new file mode 100644 index 0000000..e69de29 diff --git a/src/perception/depth_estimation/setup.cfg b/src/perception/depth_estimation/setup.cfg new file mode 100644 index 0000000..fed6362 --- /dev/null +++ b/src/perception/depth_estimation/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/producer +[install] +install-scripts=$base/lib/producer diff --git a/src/perception/depth_estimation/setup.py b/src/perception/depth_estimation/setup.py new file mode 100755 index 0000000..0d9eedb --- /dev/null +++ b/src/perception/depth_estimation/setup.py @@ -0,0 +1,34 @@ +import os +from glob import glob +from setuptools import setup + +package_name = 'producer' + +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'))), + # Include config files for parameters + (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.yaml'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='eddyzhou, aryanafrouzi', + maintainer_email='e23zhou@watonomous.ca, aafrouzi@watonomous.ca', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'producer_node = producer.producer_node:main' + ], + }, +) diff --git a/src/perception/depth_estimation/test/test_copyright.py b/src/perception/depth_estimation/test/test_copyright.py new file mode 100755 index 0000000..cc8ff03 --- /dev/null +++ b/src/perception/depth_estimation/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception/depth_estimation/test/test_flake8.py b/src/perception/depth_estimation/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception/depth_estimation/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception/depth_estimation/test/test_pep257.py b/src/perception/depth_estimation/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception/depth_estimation/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception/depth_estimation/test/test_producer.py b/src/perception/depth_estimation/test/test_producer.py new file mode 100644 index 0000000..ac4d6f6 --- /dev/null +++ b/src/perception/depth_estimation/test/test_producer.py @@ -0,0 +1,23 @@ +# 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. + +from producer.producer_core import ProducerCore + + +def test_update_position(): + producer_core = ProducerCore(1, 1, 1, 1) + producer_core.update_position() + + assert producer_core.serialize_data() == \ + "x:1.5773502691896257;y:1.5773502691896257;z:1.5773502691896257;" From 3e05c1bd4f76ee809a7e2bdcfe08ecc479657bc2 Mon Sep 17 00:00:00 2001 From: rayhant2 Date: Wed, 12 Mar 2025 21:01:13 -0400 Subject: [PATCH 2/5] Refactored Depth Estimation Package --- .../depth_estimation/config/params.yaml | 2 +- .../{producer => depth_estimation}/__init__.py | 0 .../depth_estimation_core.py} | 2 +- .../depth_estimation_node.py} | 16 ++++++++-------- ...ucer.launch.py => depth_estimation.launch.py} | 8 ++++---- src/perception/depth_estimation/package.xml | 2 +- .../resource/{producer => depth_estimation} | 0 src/perception/depth_estimation/setup.cfg | 4 ++-- src/perception/depth_estimation/setup.py | 4 ++-- .../depth_estimation/test/test_producer.py | 8 ++++---- 10 files changed, 23 insertions(+), 23 deletions(-) rename src/perception/depth_estimation/{producer => depth_estimation}/__init__.py (100%) rename src/perception/depth_estimation/{producer/producer_core.py => depth_estimation/depth_estimation_core.py} (97%) rename src/perception/depth_estimation/{producer/producer_node.py => depth_estimation/depth_estimation_node.py} (83%) rename src/perception/depth_estimation/launch/{producer.launch.py => depth_estimation.launch.py} (85%) rename src/perception/depth_estimation/resource/{producer => depth_estimation} (100%) diff --git a/src/perception/depth_estimation/config/params.yaml b/src/perception/depth_estimation/config/params.yaml index 6895cc4..3f7f1ae 100644 --- a/src/perception/depth_estimation/config/params.yaml +++ b/src/perception/depth_estimation/config/params.yaml @@ -1,6 +1,6 @@ # more info on YAML configs for ROS2 Params: # https://roboticsbackend.com/ros2-yaml-params/ -producer_node: +depth_estimation_node: ros__parameters: pos_x: 1.0 pos_y: 1.0 diff --git a/src/perception/depth_estimation/producer/__init__.py b/src/perception/depth_estimation/depth_estimation/__init__.py similarity index 100% rename from src/perception/depth_estimation/producer/__init__.py rename to src/perception/depth_estimation/depth_estimation/__init__.py diff --git a/src/perception/depth_estimation/producer/producer_core.py b/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py similarity index 97% rename from src/perception/depth_estimation/producer/producer_core.py rename to src/perception/depth_estimation/depth_estimation/depth_estimation_core.py index 3297652..3cd6873 100644 --- a/src/perception/depth_estimation/producer/producer_core.py +++ b/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py @@ -15,7 +15,7 @@ import math -class ProducerCore(): +class DepthEstimationCore(): def __init__(self, pos_x, pos_y, pos_z, vel): # Init member variables for serialization diff --git a/src/perception/depth_estimation/producer/producer_node.py b/src/perception/depth_estimation/depth_estimation/depth_estimation_node.py similarity index 83% rename from src/perception/depth_estimation/producer/producer_node.py rename to src/perception/depth_estimation/depth_estimation/depth_estimation_node.py index 782265d..71b653e 100755 --- a/src/perception/depth_estimation/producer/producer_node.py +++ b/src/perception/depth_estimation/depth_estimation/depth_estimation_node.py @@ -18,10 +18,10 @@ from rclpy.node import Node from sample_msgs.msg import Unfiltered -from producer.producer_core import ProducerCore +from depth_estimation.depth_estimation_core import DepthEstimationCore -class ProducerNode(Node): +class DepthEstimationNode(Node): def __init__(self): super().__init__('python_producer') @@ -39,7 +39,7 @@ def __init__(self): velocity = self.get_parameter('velocity').get_parameter_value().double_value # Initialize producer core logic for serialization - self.__producer = ProducerCore(pos_x, pos_y, pos_z, velocity) + self.__producer = DepthEstimationCore(pos_x, pos_y, pos_z, velocity) # Initialize ROS2 constructs queue_size = 10 @@ -49,10 +49,10 @@ def __init__(self): self.timer = self.create_timer(timer_period, self.__publish_position) def __publish_position(self): - self.__producer.update_position() + self.__depth_estimation.update_position() msg = Unfiltered() - msg.data = self.__producer.serialize_data() + msg.data = self.__depth_estimation.serialize_data() msg.valid = True msg.timestamp = int(time.time() * 1000) @@ -64,14 +64,14 @@ def __publish_position(self): def main(args=None): rclpy.init(args=args) - python_producer = ProducerNode() + python_depth_estimation = DepthEstimationNode() - rclpy.spin(python_producer) + rclpy.spin(python_depth_estimation) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) - python_producer.destroy_node() + python_depth_estimation.destroy_node() rclpy.shutdown() diff --git a/src/perception/depth_estimation/launch/producer.launch.py b/src/perception/depth_estimation/launch/depth_estimation.launch.py similarity index 85% rename from src/perception/depth_estimation/launch/producer.launch.py rename to src/perception/depth_estimation/launch/depth_estimation.launch.py index 38ab651..4cb8416 100755 --- a/src/perception/depth_estimation/launch/producer.launch.py +++ b/src/perception/depth_estimation/launch/depth_estimation.launch.py @@ -24,16 +24,16 @@ def generate_launch_description(): # path is the share directory. Check setup.py for how # the param file got there param_file_path = os.path.join( - get_package_share_directory('producer'), + get_package_share_directory('depth_estimation'), 'config', 'params.yaml' ) return LaunchDescription([ Node( - package='producer', - name='producer_node', - executable='producer_node', + package='depth_estimation', + name='depth_estimation_node', + executable='depth_estimation_node', parameters=[param_file_path] ) ]) diff --git a/src/perception/depth_estimation/package.xml b/src/perception/depth_estimation/package.xml index c2b4136..7933863 100644 --- a/src/perception/depth_estimation/package.xml +++ b/src/perception/depth_estimation/package.xml @@ -1,7 +1,7 @@ - producer + depth_estimation 0.0.0 TODO: Package description eddyzhou diff --git a/src/perception/depth_estimation/resource/producer b/src/perception/depth_estimation/resource/depth_estimation similarity index 100% rename from src/perception/depth_estimation/resource/producer rename to src/perception/depth_estimation/resource/depth_estimation diff --git a/src/perception/depth_estimation/setup.cfg b/src/perception/depth_estimation/setup.cfg index fed6362..64c2910 100644 --- a/src/perception/depth_estimation/setup.cfg +++ b/src/perception/depth_estimation/setup.cfg @@ -1,4 +1,4 @@ [develop] -script_dir=$base/lib/producer +script_dir=$base/lib/depth_estimation [install] -install-scripts=$base/lib/producer +install-scripts=$base/lib/depth_estimation diff --git a/src/perception/depth_estimation/setup.py b/src/perception/depth_estimation/setup.py index 0d9eedb..4476846 100755 --- a/src/perception/depth_estimation/setup.py +++ b/src/perception/depth_estimation/setup.py @@ -2,7 +2,7 @@ from glob import glob from setuptools import setup -package_name = 'producer' +package_name = 'depth_estimation' setup( name=package_name, @@ -28,7 +28,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'producer_node = producer.producer_node:main' + 'depth_estimation_node = depth_estimation.depth_estimation_node:main' ], }, ) diff --git a/src/perception/depth_estimation/test/test_producer.py b/src/perception/depth_estimation/test/test_producer.py index ac4d6f6..7c56ecb 100644 --- a/src/perception/depth_estimation/test/test_producer.py +++ b/src/perception/depth_estimation/test/test_producer.py @@ -12,12 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -from producer.producer_core import ProducerCore +from depth_estimation.depth_estimation_core import DepthEstimationCore def test_update_position(): - producer_core = ProducerCore(1, 1, 1, 1) - producer_core.update_position() + depth_estimation_core = DepthEstimationCore(1, 1, 1, 1) + depth_estimation_core.update_position() - assert producer_core.serialize_data() == \ + assert depth_estimation_core.serialize_data() == \ "x:1.5773502691896257;y:1.5773502691896257;z:1.5773502691896257;" From ada1a3c1bd1b810698cca3dbeacc714f99cbf945 Mon Sep 17 00:00:00 2001 From: rayhant2 Date: Wed, 12 Mar 2025 21:01:39 -0400 Subject: [PATCH 3/5] Refactored Depth Estimation Package --- docker/perception/perception.Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/perception/perception.Dockerfile b/docker/perception/perception.Dockerfile index 176527a..e7eecc8 100644 --- a/docker/perception/perception.Dockerfile +++ b/docker/perception/perception.Dockerfile @@ -6,7 +6,7 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/perception/depth_estimation depth_estimation +COPY src/perception/depth_estimation depth_estimation COPY src/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps From 3d74dea77caa51815c76a32d315b29f5869f0da1 Mon Sep 17 00:00:00 2001 From: Miekale Date: Fri, 21 Mar 2025 01:14:29 +0000 Subject: [PATCH 4/5] fixing docker file --- modules/docker-compose.perception.yaml | 2 +- .../depth_estimation/depth_estimation_core.py | 14 +----- .../depth_estimation/depth_estimation_node.py | 47 +------------------ 3 files changed, 4 insertions(+), 59 deletions(-) diff --git a/modules/docker-compose.perception.yaml b/modules/docker-compose.perception.yaml index cba574f..b8c6c67 100644 --- a/modules/docker-compose.perception.yaml +++ b/modules/docker-compose.perception.yaml @@ -8,6 +8,6 @@ services: - "${SAMPLES_PRODUCER_IMAGE:?}:main" image: "${SAMPLES_PRODUCER_IMAGE:?}:${TAG}" profiles: [deploy] - command: /bin/bash -c "ros2 launch perception perception.launch.py" + command: /bin/bash -c "ros2 launch depth_estimation depth_estimation.launch.py" volumes: - ${MONO_DIR}/src/samples/python/perception:/root/ament_ws/src/perception \ No newline at end of file diff --git a/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py b/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py index 3cd6873..8afe084 100644 --- a/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py +++ b/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py @@ -18,18 +18,6 @@ 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/depth_estimation/depth_estimation_node.py b/src/perception/depth_estimation/depth_estimation/depth_estimation_node.py index 71b653e..7878c58 100755 --- a/src/perception/depth_estimation/depth_estimation/depth_estimation_node.py +++ b/src/perception/depth_estimation/depth_estimation/depth_estimation_node.py @@ -24,56 +24,13 @@ class DepthEstimationNode(Node): def __init__(self): - super().__init__('python_producer') - # Declare and get the parameters - self.declare_parameter('pos_x', 0.0) - self.declare_parameter('pos_y', 0.0) - self.declare_parameter('pos_z', 0.0) - self.declare_parameter('velocity', 0.0) + pass - # For parameters, we need to explicitely declare its type for Python to know - # what to do with it - pos_x = self.get_parameter('pos_x').get_parameter_value().double_value - pos_y = self.get_parameter('pos_y').get_parameter_value().double_value - pos_z = self.get_parameter('pos_z').get_parameter_value().double_value - velocity = self.get_parameter('velocity').get_parameter_value().double_value - - # Initialize producer core logic for serialization - self.__producer = DepthEstimationCore(pos_x, pos_y, pos_z, velocity) - - # Initialize ROS2 constructs - queue_size = 10 - self.publisher_ = self.create_publisher(Unfiltered, '/unfiltered_topic', queue_size) - - timer_period = 0.5 - self.timer = self.create_timer(timer_period, self.__publish_position) - - def __publish_position(self): - self.__depth_estimation.update_position() - msg = Unfiltered() - - msg.data = self.__depth_estimation.serialize_data() - msg.valid = True - msg.timestamp = int(time.time() * 1000) - - self.get_logger().info(f'Publishing: {msg.data}') - - self.publisher_.publish(msg) def main(args=None): rclpy.init(args=args) - - python_depth_estimation = DepthEstimationNode() - - rclpy.spin(python_depth_estimation) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - python_depth_estimation.destroy_node() - rclpy.shutdown() - + return if __name__ == '__main__': main() From 74fbba6d6d23ec9b472c7f50f241ae9f11f6d8e7 Mon Sep 17 00:00:00 2001 From: Parasmai Conjeevaram Date: Wed, 28 May 2025 02:47:00 +0000 Subject: [PATCH 5/5] creating modularity within perception module for several packages - centralized launch --- .../samples/python/aggregator/package.xml | 4 +- autonomy/samples/python/aggregator/setup.py | 12 ++++-- autonomy/samples/python/producer/package.xml | 4 +- autonomy/samples/python/producer/setup.py | 4 +- .../samples/python/transformer/package.xml | 4 +- autonomy/samples/python/transformer/setup.py | 4 +- autonomy/samples/sample_msgs/package.xml | 2 +- autonomy/wato_msgs/sample_msgs/package.xml | 2 +- docker/perception/perception.Dockerfile | 2 + modules/docker-compose.perception.yaml | 6 +-- .../depth_estimation/depth_estimation_core.py | 15 ++++++++ src/perception/depth_estimation/package.xml | 7 +++- src/perception/depth_estimation/setup.py | 4 +- .../launch/perception.launch.py} | 21 +++++++--- src/perception/perception_launch/package.xml | 22 +++++++++++ .../perception_launch/__init__.py | 13 +++++++ .../resource/perception_launch | 1 + src/perception/perception_launch/setup.cfg | 4 ++ src/perception/perception_launch/setup.py | 31 +++++++++++++++ .../pose_estimation/config/params.yaml | 9 +++++ src/perception/pose_estimation/package.xml | 21 ++++++++++ .../pose_estimation/__init__.py | 0 .../pose_estimation/pose_estimation_core.py | 38 +++++++++++++++++++ .../pose_estimation/pose_estimation_node.py | 36 ++++++++++++++++++ .../pose_estimation/resource/pose_estimation | 0 src/perception/pose_estimation/setup.cfg | 4 ++ src/perception/pose_estimation/setup.py | 34 +++++++++++++++++ .../pose_estimation/test/test_copyright.py | 23 +++++++++++ .../pose_estimation/test/test_flake8.py | 25 ++++++++++++ .../pose_estimation/test/test_pep257.py | 23 +++++++++++ .../pose_estimation/test/test_producer.py | 23 +++++++++++ watod_scripts/watod-setup-dev-env.sh | 0 32 files changed, 371 insertions(+), 27 deletions(-) rename src/perception/{depth_estimation/launch/depth_estimation.launch.py => perception_launch/launch/perception.launch.py} (67%) mode change 100755 => 100644 create mode 100644 src/perception/perception_launch/package.xml create mode 100644 src/perception/perception_launch/perception_launch/__init__.py create mode 100644 src/perception/perception_launch/resource/perception_launch create mode 100644 src/perception/perception_launch/setup.cfg create mode 100644 src/perception/perception_launch/setup.py create mode 100644 src/perception/pose_estimation/config/params.yaml create mode 100644 src/perception/pose_estimation/package.xml create mode 100755 src/perception/pose_estimation/pose_estimation/__init__.py create mode 100644 src/perception/pose_estimation/pose_estimation/pose_estimation_core.py create mode 100755 src/perception/pose_estimation/pose_estimation/pose_estimation_node.py create mode 100644 src/perception/pose_estimation/resource/pose_estimation create mode 100644 src/perception/pose_estimation/setup.cfg create mode 100755 src/perception/pose_estimation/setup.py create mode 100755 src/perception/pose_estimation/test/test_copyright.py create mode 100755 src/perception/pose_estimation/test/test_flake8.py create mode 100755 src/perception/pose_estimation/test/test_pep257.py create mode 100644 src/perception/pose_estimation/test/test_producer.py mode change 100644 => 100755 watod_scripts/watod-setup-dev-env.sh diff --git a/autonomy/samples/python/aggregator/package.xml b/autonomy/samples/python/aggregator/package.xml index 50daeed..d45bb34 100644 --- a/autonomy/samples/python/aggregator/package.xml +++ b/autonomy/samples/python/aggregator/package.xml @@ -3,9 +3,9 @@ aggregator 0.0.0 - TODO: Package description + Sample aggregator node for data collection eddyzhou - Apache2.0: License declaration + Apache2.0 ament_copyright ament_flake8 diff --git a/autonomy/samples/python/aggregator/setup.py b/autonomy/samples/python/aggregator/setup.py index f77c180..e0bb020 100755 --- 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 index c2b4136..f60515e 100644 --- a/autonomy/samples/python/producer/package.xml +++ b/autonomy/samples/python/producer/package.xml @@ -3,9 +3,9 @@ producer 0.0.0 - TODO: Package description + Sample producer node for data generation eddyzhou - Apache2.0: License declaration + Apache2.0 ament_copyright ament_flake8 diff --git a/autonomy/samples/python/producer/setup.py b/autonomy/samples/python/producer/setup.py index 0d9eedb..bfadccf 100755 --- 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 index abca609..10ee285 100644 --- a/autonomy/samples/python/transformer/package.xml +++ b/autonomy/samples/python/transformer/package.xml @@ -3,9 +3,9 @@ transformer 0.0.0 - TODO: Package description + Sample transformer node for data processing eddyzhou - Apache2.0: License declaration + Apache2.0 ament_copyright ament_flake8 diff --git a/autonomy/samples/python/transformer/setup.py b/autonomy/samples/python/transformer/setup.py index 5cd86e5..4e4b579 100755 --- 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 index 2cf3341..90ad1fe 100644 --- a/autonomy/samples/sample_msgs/package.xml +++ b/autonomy/samples/sample_msgs/package.xml @@ -4,7 +4,7 @@ 0.0.0 Sample ROS messages - watouser + conjeevaram Apache2.0 ament_cmake diff --git a/autonomy/wato_msgs/sample_msgs/package.xml b/autonomy/wato_msgs/sample_msgs/package.xml index 2cf3341..90ad1fe 100644 --- a/autonomy/wato_msgs/sample_msgs/package.xml +++ b/autonomy/wato_msgs/sample_msgs/package.xml @@ -4,7 +4,7 @@ 0.0.0 Sample ROS messages - watouser + conjeevaram Apache2.0 ament_cmake diff --git a/docker/perception/perception.Dockerfile b/docker/perception/perception.Dockerfile index e7eecc8..0265102 100644 --- 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 index b8c6c67..cadda49 100644 --- a/modules/docker-compose.perception.yaml +++ b/modules/docker-compose.perception.yaml @@ -1,6 +1,6 @@ services: perception: # PYTHON - build: &producer_build + build: &perception_build context: .. dockerfile: docker/perception/perception.Dockerfile cache_from: @@ -8,6 +8,6 @@ services: - "${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 \ No newline at end of file + - ${MONO_DIR}/src/perception:/root/ament_ws/src/perception \ No newline at end of file diff --git a/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py b/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py index 8afe084..4b7bdcc 100644 --- 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 index 7933863..137cb74 100644 --- a/src/perception/depth_estimation/package.xml +++ b/src/perception/depth_estimation/package.xml @@ -3,9 +3,12 @@ depth_estimation 0.0.0 - TODO: Package description + Depth estimation module for humanoid robot perception eddyzhou - Apache2.0: License declaration + Apache2.0 + + rclpy + sample_msgs ament_copyright ament_flake8 diff --git a/src/perception/depth_estimation/setup.py b/src/perception/depth_estimation/setup.py index 4476846..3a911e4 100755 --- 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/depth_estimation/launch/depth_estimation.launch.py b/src/perception/perception_launch/launch/perception.launch.py old mode 100755 new mode 100644 similarity index 67% rename from src/perception/depth_estimation/launch/depth_estimation.launch.py rename to src/perception/perception_launch/launch/perception.launch.py index 4cb8416..c98dd12 --- a/src/perception/depth_estimation/launch/depth_estimation.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 new file mode 100644 index 0000000..413649b --- /dev/null +++ b/src/perception/perception_launch/package.xml @@ -0,0 +1,22 @@ + + + + perception_launch + 0.0.0 + Launch files for the perception module + conjeevaram + Apache2.0 + + launch + launch_ros + depth_estimation + pose_estimation + + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + diff --git a/src/perception/perception_launch/perception_launch/__init__.py b/src/perception/perception_launch/perception_launch/__init__.py new file mode 100644 index 0000000..41f2430 --- /dev/null +++ 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 new file mode 100644 index 0000000..03fed6f --- /dev/null +++ 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 new file mode 100644 index 0000000..f7e4f1b --- /dev/null +++ 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 new file mode 100644 index 0000000..2c2ad67 --- /dev/null +++ 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 new file mode 100644 index 0000000..d8ff2ab --- /dev/null +++ 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 \ No newline at end of file diff --git a/src/perception/pose_estimation/package.xml b/src/perception/pose_estimation/package.xml new file mode 100644 index 0000000..6249b92 --- /dev/null +++ b/src/perception/pose_estimation/package.xml @@ -0,0 +1,21 @@ + + + + pose_estimation + 0.0.0 + Pose estimation module for humanoid robot perception + conjeevaram + Apache2.0 + + rclpy + sample_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception/pose_estimation/pose_estimation/__init__.py b/src/perception/pose_estimation/pose_estimation/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/perception/pose_estimation/pose_estimation/pose_estimation_core.py b/src/perception/pose_estimation/pose_estimation/pose_estimation_core.py new file mode 100644 index 0000000..6dd3a3a --- /dev/null +++ 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 new file mode 100755 index 0000000..4122bdd --- /dev/null +++ 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 new file mode 100644 index 0000000..e69de29 diff --git a/src/perception/pose_estimation/setup.cfg b/src/perception/pose_estimation/setup.cfg new file mode 100644 index 0000000..289d06f --- /dev/null +++ b/src/perception/pose_estimation/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pose_estimation +[install] +install-scripts=$base/lib/pose_estimation diff --git a/src/perception/pose_estimation/setup.py b/src/perception/pose_estimation/setup.py new file mode 100755 index 0000000..8ac72a8 --- /dev/null +++ b/src/perception/pose_estimation/setup.py @@ -0,0 +1,34 @@ +import os +from glob import glob +from setuptools import setup + +package_name = 'pose_estimation' + +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'))), + # Include config files for parameters + (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.yaml'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='parasmaiconjeevaram, miekalesmith', + maintainer_email='pconjeevaram@watonomous.ca', + description='Pose estimation module for humanoid robot perception', + license='Apache2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'pose_estimation_node = pose_estimation.pose_estimation_node:main' + ], + }, +) diff --git a/src/perception/pose_estimation/test/test_copyright.py b/src/perception/pose_estimation/test/test_copyright.py new file mode 100755 index 0000000..cc8ff03 --- /dev/null +++ b/src/perception/pose_estimation/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/perception/pose_estimation/test/test_flake8.py b/src/perception/pose_estimation/test/test_flake8.py new file mode 100755 index 0000000..27ee107 --- /dev/null +++ b/src/perception/pose_estimation/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/perception/pose_estimation/test/test_pep257.py b/src/perception/pose_estimation/test/test_pep257.py new file mode 100755 index 0000000..b234a38 --- /dev/null +++ b/src/perception/pose_estimation/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception/pose_estimation/test/test_producer.py b/src/perception/pose_estimation/test/test_producer.py new file mode 100644 index 0000000..c1bde35 --- /dev/null +++ b/src/perception/pose_estimation/test/test_producer.py @@ -0,0 +1,23 @@ +# 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. + +from pose_estimation.pose_estimation_core import PoseEstimationCore + + +def test_update_position(): + pose_estimation_core = PoseEstimationCore(1, 1, 1, 1) + pose_estimation_core.update_position() + + assert pose_estimation_core.serialize_data() == \ + "x:1.5773502691896257;y:1.5773502691896257;z:1.5773502691896257;" diff --git a/watod_scripts/watod-setup-dev-env.sh b/watod_scripts/watod-setup-dev-env.sh old mode 100644 new mode 100755