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 new file mode 100644 index 0000000..0265102 --- /dev/null +++ b/docker/perception/perception.Dockerfile @@ -0,0 +1,51 @@ +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/perception/pose_estimation pose_estimation +COPY src/perception/perception_launch perception_launch +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..cadda49 --- /dev/null +++ b/modules/docker-compose.perception.yaml @@ -0,0 +1,13 @@ +services: + perception: # PYTHON + 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 perception_launch perception.launch.py" + volumes: + - ${MONO_DIR}/src/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..3f7f1ae --- /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/ +depth_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/modules_future/docker-compose.perception.yaml b/src/perception/depth_estimation/depth_estimation/__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/depth_estimation/__init__.py diff --git a/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py b/src/perception/depth_estimation/depth_estimation/depth_estimation_core.py new file mode 100644 index 0000000..4b7bdcc --- /dev/null +++ b/src/perception/depth_estimation/depth_estimation/depth_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 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 new file mode 100755 index 0000000..7878c58 --- /dev/null +++ b/src/perception/depth_estimation/depth_estimation/depth_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 depth_estimation.depth_estimation_core import DepthEstimationCore + + +class DepthEstimationNode(Node): + + def __init__(self): + pass + + + +def main(args=None): + rclpy.init(args=args) + return + +if __name__ == '__main__': + main() diff --git a/src/perception/depth_estimation/package.xml b/src/perception/depth_estimation/package.xml new file mode 100644 index 0000000..137cb74 --- /dev/null +++ b/src/perception/depth_estimation/package.xml @@ -0,0 +1,21 @@ + + + + depth_estimation + 0.0.0 + Depth estimation module for humanoid robot perception + eddyzhou + Apache2.0 + + rclpy + sample_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception/depth_estimation/resource/depth_estimation b/src/perception/depth_estimation/resource/depth_estimation 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..64c2910 --- /dev/null +++ b/src/perception/depth_estimation/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/depth_estimation +[install] +install-scripts=$base/lib/depth_estimation diff --git a/src/perception/depth_estimation/setup.py b/src/perception/depth_estimation/setup.py new file mode 100755 index 0000000..3a911e4 --- /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 = 'depth_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='eddyzhou, aryanafrouzi', + maintainer_email='e23zhou@watonomous.ca, aafrouzi@watonomous.ca', + description='Depth estimation module for humanoid robot perception', + license='Apache2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'depth_estimation_node = depth_estimation.depth_estimation_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..7c56ecb --- /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 depth_estimation.depth_estimation_core import DepthEstimationCore + + +def test_update_position(): + depth_estimation_core = DepthEstimationCore(1, 1, 1, 1) + depth_estimation_core.update_position() + + assert depth_estimation_core.serialize_data() == \ + "x:1.5773502691896257;y:1.5773502691896257;z:1.5773502691896257;" diff --git a/src/perception/perception_launch/launch/perception.launch.py b/src/perception/perception_launch/launch/perception.launch.py new file mode 100644 index 0000000..c98dd12 --- /dev/null +++ b/src/perception/perception_launch/launch/perception.launch.py @@ -0,0 +1,50 @@ +# 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(): + # 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=[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