diff --git a/.gitignore b/.gitignore index 87ba9fe..254a8fe 100644 --- a/.gitignore +++ b/.gitignore @@ -119,4 +119,10 @@ dependencies/gitman.log # Ignore venv .venv/ -venv/ \ No newline at end of file +venv/ + +detections +rosbags/* +src/detection/object_detection/models/* +frames_*.gv +frames_*.pdf diff --git a/gitman.yml b/gitman.yml index 052ca42..68aa846 100644 --- a/gitman.yml +++ b/gitman.yml @@ -59,7 +59,7 @@ sources: - source: . target: src/estimation/holistic_fusion - name: smb_bringup - repo: https://github.com/ETHZ-RobotX/smb_bringup.git + repo: https://github.com/acpoll/smb_bringup.git rev: main type: git links: @@ -72,6 +72,13 @@ sources: links: - source: . target: src/utils/smb_power_status + - name: ros2_utils + repo: https://github.com/acpoll/ros2_utils.git + rev: main + type: git + links: + - source: . + target: src/utils/ros2_utils - name: smb_low_level_controller_ros2 repo: https://github.com/ETHZ-RobotX/smb_lowlevel_controller_ros2.git rev: main @@ -89,7 +96,7 @@ sources: - source: . target: src/planning/far_planner - name: tare_planner - repo: https://github.com/ETHZ-RobotX/tare_planner.git + repo: https://github.com/acpoll/tare_planner.git rev: main type: git links: @@ -147,9 +154,16 @@ sources: - source: . target: src/visualization/joy_panel_cpp - name: smb_object_detection - repo: https://github.com/ETHZ-RobotX/smb_object_detection_ros2.git + repo: https://github.com/moskitoo/smb_object_detection_ros2.git + rev: docker_working + type: git + links: + - source: . + target: src/perception/smb_object_detection + - name: smb_autonomous + repo: https://github.com/acpoll/smb_autonomous.git rev: main type: git links: - source: . - target: src/perception/smb_object_detection \ No newline at end of file + target: src/smb_autonomous \ No newline at end of file diff --git a/src/planning/simple_planner/package.xml b/src/planning/simple_planner/package.xml new file mode 100644 index 0000000..84d221c --- /dev/null +++ b/src/planning/simple_planner/package.xml @@ -0,0 +1,18 @@ + + + + simple_planner + 0.0.0 + TODO: Package description + anton + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/planning/simple_planner/resource/simple_planner b/src/planning/simple_planner/resource/simple_planner new file mode 100644 index 0000000..e69de29 diff --git a/src/planning/simple_planner/setup.cfg b/src/planning/simple_planner/setup.cfg new file mode 100644 index 0000000..f018c89 --- /dev/null +++ b/src/planning/simple_planner/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/simple_planner +[install] +install_scripts=$base/lib/simple_planner diff --git a/src/planning/simple_planner/setup.py b/src/planning/simple_planner/setup.py new file mode 100644 index 0000000..cd3a0b6 --- /dev/null +++ b/src/planning/simple_planner/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'simple_planner' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='anton', + maintainer_email='antoncarlospollak@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'simple_planner = simple_planner.simple_planner:main' + ], + }, +) diff --git a/src/planning/simple_planner/simple_planner/__init__.py b/src/planning/simple_planner/simple_planner/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/planning/simple_planner/simple_planner/simple_planner.py b/src/planning/simple_planner/simple_planner/simple_planner.py new file mode 100644 index 0000000..729ce51 --- /dev/null +++ b/src/planning/simple_planner/simple_planner/simple_planner.py @@ -0,0 +1,45 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PointStamped + + +class SimplePlanner(Node): + def __init__(self): + super().__init__('simple_planner') + + # Create publisher for PointStamped messages + self.publisher_ = self.create_publisher(PointStamped, '/way_point', 10) + + # Create timer to publish at 1 Hz + # self.timer = self.create_timer(1.0, self.publish_waypoint) + self.get_logger().info('Simple planner node started, publishing to /way_point') + self.publish_waypoint() + + + def publish_waypoint(self): + msg = PointStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'map' # or 'base_link' depending on your needs + + # Hardcoded waypoint coordinates + msg.point.x = 2.0 + msg.point.y = 0.0 + msg.point.z = 0.0 + msg.point + self.publisher_.publish(msg) + self.publisher_.publish(msg) + +def main(args=None): + rclpy.init(args=args) + simple_planner = SimplePlanner() + + try: + rclpy.spin(simple_planner) + except KeyboardInterrupt: + pass + finally: + simple_planner.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + SimplePlanner.main() \ No newline at end of file diff --git a/src/planning/simple_planner/test/test_copyright.py b/src/planning/simple_planner/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/planning/simple_planner/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/planning/simple_planner/test/test_flake8.py b/src/planning/simple_planner/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/planning/simple_planner/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/planning/simple_planner/test/test_pep257.py b/src/planning/simple_planner/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/planning/simple_planner/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'