diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..efa407c --- /dev/null +++ b/.gitignore @@ -0,0 +1,162 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/latest/usage/project/#working-with-version-control +.pdm.toml +.pdm-python +.pdm-build/ + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ \ No newline at end of file diff --git a/project/launch/brain.launch.py b/project/launch/brain.launch.py new file mode 100644 index 0000000..712f358 --- /dev/null +++ b/project/launch/brain.launch.py @@ -0,0 +1,35 @@ +import os +import xacro + +from ament_index_python.packages import get_package_share_directory as pkgdir + +from launch import LaunchDescription +from launch.actions import Shutdown +from launch_ros.actions import Node + + +def generate_launch_description(): + # node_usbcam = Node( + # name = 'usb_cam', + # package = 'usb_cam', + # executable = 'usb_cam_node_exe', + # namespace = 'usb_cam', + # output = 'screen', + # parameters = [{'camera_name': 'logitech'}, + # {'video_device': '/dev/video0'}, + # {'pixel_format': 'yuyv2rgb'}, + # {'image_width': 640}, + # {'image_height': 480}, + # {'framerate': 15.0}]) + + node_mapping = Node( + name = 'brain', + package = 'project', + executable = 'brain', + output = 'screen', + remappings = [('/image_raw', '/usb_cam/image_raw')]) + + return LaunchDescription([ + # node_usbcam, + node_mapping, + ]) \ No newline at end of file diff --git a/project/launch/detector.launch.py b/project/launch/detector.launch.py new file mode 100644 index 0000000..18771de --- /dev/null +++ b/project/launch/detector.launch.py @@ -0,0 +1,62 @@ +"""Launch the USB camera node and ball detector. + +This launch file is intended show how the pieces come together. +Please copy the relevant pieces. + +""" + +import os +import xacro + +from ament_index_python.packages import get_package_share_directory as pkgdir + +from launch import LaunchDescription +from launch.actions import Shutdown +from launch_ros.actions import Node + + +# +# Generate the Launch Description +# +def generate_launch_description(): + + ###################################################################### + # PREPARE THE LAUNCH ELEMENTS + + # Configure the USB camera node + node_usbcam = Node( + name = 'usb_cam', + package = 'usb_cam', + executable = 'usb_cam_node_exe', + namespace = 'usb_cam', + output = 'screen', + parameters = [{'camera_name': 'logitech'}, + {'video_device': '/dev/video0'}, + {'pixel_format': 'yuyv2rgb'}, + {'image_width': 640}, + {'image_height': 480}, + {'framerate': 15.0}, + {'auto_white_balance': False}, + {'white_balance': 3100}, + {'auto_exposure': False}, + {'exposure': 100}]) + + # Configure the ball detector node + node_detector = Node( + name = 'detector', + package = 'project', + executable = 'detector', + output = 'screen', + remappings = [('/image_raw', '/usb_cam/image_raw')]) + + + ###################################################################### + # COMBINE THE ELEMENTS INTO ONE LIST + + # Return the description, built as a python list. + return LaunchDescription([ + + # Start the nodes. + node_usbcam, + node_detector, + ]) diff --git a/project/launch/testurdf.launch.py b/project/launch/testurdf.launch.py new file mode 100644 index 0000000..5a9ceee --- /dev/null +++ b/project/launch/testurdf.launch.py @@ -0,0 +1,79 @@ +"""Show/Test the URDF of the robot (no actual hardware required) + + This simply drives the URDF from the GUI, bypassing any hardware. + + This should start + 1) RVIZ, ready to view the robot + 2) The robot_state_publisher (listening to /joint_states) + 3) The GUI to drive the URDF (sending to /joint_states) + +""" + +import os +import xacro + +from ament_index_python.packages import get_package_share_directory as pkgdir + +from launch import LaunchDescription +from launch.actions import Shutdown +from launch_ros.actions import Node + + +# +# Generate the Launch Description +# +def generate_launch_description(): + + ###################################################################### + # LOCATE FILES + + # Locate the RVIZ configuration file. + rvizcfg = os.path.join(pkgdir('project'), 'rviz/viewurdf.rviz') + + # Locate/load the robot's URDF file (XML). + # urdf = os.path.join(pkgdir('threedof'), 'urdf/threedof.urdf') + urdf = os.path.join(pkgdir('project'), 'urdf/threedof.urdf') + with open(urdf, 'r') as file: + robot_description = file.read() + + + ###################################################################### + # PREPARE THE LAUNCH ELEMENTS + + # Configure a node for RVIZ. + node_rviz = Node( + name = 'rviz', + package = 'rviz2', + executable = 'rviz2', + output = 'screen', + arguments = ['-d', rvizcfg], + on_exit = Shutdown()) + + # Configure a node for the robot_state_publisher. + node_robot_state_publisher = Node( + name = 'robot_state_publisher', + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + output = 'screen', + parameters = [{'robot_description': robot_description}]) + + # Configure a node for the GUI to substitute for the robot. + node_gui = Node( + name = 'gui', + package = 'joint_state_publisher_gui', + executable = 'joint_state_publisher_gui', + output = 'screen', + on_exit = Shutdown()) + + + ###################################################################### + # COMBINE THE ELEMENTS INTO ONE LIST + + # Return the description, built as a python list. + return LaunchDescription([ + + # Use RVIZ to view the URDF commanded by the GUI. + node_rviz, + node_robot_state_publisher, + node_gui, + ]) diff --git a/project/launch/toucharb.launch.py b/project/launch/toucharb.launch.py new file mode 100644 index 0000000..2ec9cae --- /dev/null +++ b/project/launch/toucharb.launch.py @@ -0,0 +1,91 @@ +"""Run the touchtable code + + This launches all necessary pieces. + + This should start + 1) RVIZ, ready to view the robot + 2) The robot_state_publisher (likely listening to default /joint_states) + 3) The HEBI node to communicate with the motors + 4) The trajectory code (sending /joint_commands at 100Hz) + +""" + +import os +import xacro + +from ament_index_python.packages import get_package_share_directory as pkgdir + +from launch import LaunchDescription +from launch.actions import Shutdown +from launch_ros.actions import Node + + +# +# Generate the Launch Description +# +def generate_launch_description(): + + ###################################################################### + # LOCATE FILES + + # Locate the RVIZ configuration file. + rvizcfg = os.path.join(pkgdir('project'), 'rviz/viewurdf.rviz') + + # Locate/load the robot's URDF file (XML). + # urdf = os.path.join(pkgdir('threedof'), 'urdf/threedof.urdf') + urdf = os.path.join(pkgdir('project'), 'urdf/threedof.urdf') + with open(urdf, 'r') as file: + robot_description = file.read() + + + ###################################################################### + # PREPARE THE LAUNCH ELEMENTS + + # Configure a node for RVIZ. + node_rviz = Node( + name = 'rviz', + package = 'rviz2', + executable = 'rviz2', + output = 'screen', + arguments = ['-d', rvizcfg], + on_exit = Shutdown()) + + # Configure a node for the robot_state_publisher. + node_robot_state_publisher = Node( + name = 'robot_state_publisher', + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + output = 'screen', + parameters = [{'robot_description': robot_description}]) + + # Configure a node for the hebi interface. + node_hebi = Node( + name = 'hebi', + package = 'hebiros', + executable = 'hebinode', + output = 'screen', + parameters = [{'family': 'robotlab'}, + {'motors': ['2.5', '2.4', '2.3']}, + {'joints': ['base', 'shoulder', 'elbow']}], + on_exit = Shutdown()) + + # Configure a trajectory node. PLACEHOLDER FOR YOUR CODE!! + node_trajectory = Node( + name = 'toucharb', + package = 'project', + executable = 'toucharb', + output = 'screen') + + + ###################################################################### + # COMBINE THE ELEMENTS INTO ONE LIST + + # Return the description, built as a python list. + return LaunchDescription([ + + # Drive the robot from the touchtable code. + node_rviz, + node_robot_state_publisher, + node_hebi, + node_trajectory, + ]) \ No newline at end of file diff --git a/project/launch/trajectory.launch.py b/project/launch/trajectory.launch.py new file mode 100644 index 0000000..b9cc08a --- /dev/null +++ b/project/launch/trajectory.launch.py @@ -0,0 +1,56 @@ +import os +import xacro + +from ament_index_python.packages import get_package_share_directory as pkgdir + +from launch import LaunchDescription +from launch.actions import Shutdown +from launch_ros.actions import Node + + +def generate_launch_description(): + rvizcfg = os.path.join(pkgdir('project'), 'rviz/viewurdf.rviz') + + urdf = os.path.join(pkgdir('project'), 'urdf/threedof.urdf') + with open(urdf, 'r') as file: + robot_description = file.read() + + + node_rviz = Node( + name = 'rviz', + package = 'rviz2', + executable = 'rviz2', + output = 'screen', + arguments = ['-d', rvizcfg], + on_exit = Shutdown()) + + node_robot_state_publisher = Node( + name = 'robot_state_publisher', + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + output = 'screen', + parameters = [{'robot_description': robot_description}]) + + node_hebi = Node( + name = 'hebi', + package = 'hebiros', + executable = 'hebinode', + output = 'screen', + parameters = [{'family': 'robotlab'}, + {'motors': ['2.5', '2.4', '2.3']}, + {'joints': ['base', 'shoulder', 'elbow']}], + on_exit = Shutdown()) + + node_trajectory = Node( + name = 'trajectory', + package = 'project', + executable = 'trajectory', + output = 'screen') + + + return LaunchDescription([ + node_rviz, + node_robot_state_publisher, + node_hebi, + node_trajectory, + ]) \ No newline at end of file diff --git a/project/launch/wave.launch.py b/project/launch/wave.launch.py new file mode 100644 index 0000000..dc88800 --- /dev/null +++ b/project/launch/wave.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + node_hebi = Node( + name = 'hebi', + package = 'hebiros', + executable = 'hebinode', + output = 'screen', + parameters = [{'family': 'robotlab'}, + {'motors': ['2.5', '2.4', '2.3']}, + {'joints': ['one', 'two', 'three']}]) + + node_demo = Node( + name = 'wave', + package = 'project', + executable = 'wave', + output = 'screen') + + + return LaunchDescription([ + node_hebi, + node_demo, + ]) \ No newline at end of file diff --git a/project/meshes/Table.STL b/project/meshes/Table.STL new file mode 100644 index 0000000..05aaade Binary files /dev/null and b/project/meshes/Table.STL differ diff --git a/project/meshes/X5.stl b/project/meshes/X5.stl new file mode 100644 index 0000000..1da75af Binary files /dev/null and b/project/meshes/X5.stl differ diff --git a/project/meshes/X8.stl b/project/meshes/X8.stl new file mode 100644 index 0000000..03c6900 Binary files /dev/null and b/project/meshes/X8.stl differ diff --git a/project/meshes/basiclink.dae b/project/meshes/basiclink.dae new file mode 100644 index 0000000..455445e --- /dev/null +++ b/project/meshes/basiclink.dae @@ -0,0 +1,97 @@ + + + + + 2023-01-17T22:53:09 + 2023-01-17T22:53:09 + + Z_UP + + + + + + + + + + + + + + + 0.823529 0.623529 0.0352941 1 + + + 1 1 1 1 + + + 1 + + + + + + 1 + + + + + + + + + + + 0.5 0.035 0 0 0.035 0.00635 0.5 0.035 0.00635 0 0.035 0 -0.0348805 -0.00289028 0 -0.0348805 -0.00289028 0.00635 -0.0348805 0.00289028 0.00635 -0.033929 -0.00859199 0 -0.033929 -0.00859199 0.00635 -0.0320521 -0.0140593 0.00635 -0.0320521 -0.0140593 0 -0.0293008 -0.0191432 0 -0.0293008 -0.0191432 0.00635 -0.0257503 -0.0237049 0 -0.0257503 -0.0237049 0.00635 -0.0214974 -0.0276199 0.00635 -0.0214974 -0.0276199 0 -0.0166582 -0.0307816 0 -0.0166582 -0.0307816 0.00635 -0.0113645 -0.0331036 0 -0.0113645 -0.0331036 0.00635 -0.00576081 -0.0345226 0.00635 -0.00576081 -0.0345226 0 4.03433e-18 -0.035 0 4.03433e-18 -0.035 0.00635 -0.0348805 0.00289028 0 -0.033929 0.00859199 0.00635 -0.033929 0.00859199 0 -0.0320521 0.0140593 0.00635 -0.0320521 0.0140593 0 -0.0293008 0.0191432 0 -0.0293008 0.0191432 0.00635 -0.0257503 0.0237049 0.00635 -0.0257503 0.0237049 0 -0.0214974 0.0276199 0.00635 -0.0214974 0.0276199 0 -0.0166582 0.0307816 0 -0.0166582 0.0307816 0.00635 -0.0113645 0.0331036 0.00635 -0.0113645 0.0331036 0 -0.00576081 0.0345226 0.00635 -0.00576081 0.0345226 0 0 0.035 0.00635 0 0.035 0 0.5 -0.035 0 0.5 -0.035 0.00635 4.03433e-18 -0.035 0.00635 4.03433e-18 -0.035 0 0.53488 0.00289028 0 0.53488 0.00289028 0.00635 0.53488 -0.00289028 0.00635 0.533929 0.00859199 0 0.533929 0.00859199 0.00635 0.532052 0.0140593 0.00635 0.532052 0.0140593 0 0.529301 0.0191432 0 0.529301 0.0191432 0.00635 0.52575 0.0237049 0 0.52575 0.0237049 0.00635 0.521497 0.0276199 0.00635 0.521497 0.0276199 0 0.516658 0.0307816 0 0.516658 0.0307816 0.00635 0.511364 0.0331036 0 0.511364 0.0331036 0.00635 0.505761 0.0345226 0.00635 0.505761 0.0345226 0 0.5 0.035 0 0.5 0.035 0.00635 0.53488 -0.00289028 0 0.533929 -0.00859199 0.00635 0.533929 -0.00859199 0 0.532052 -0.0140593 0.00635 0.532052 -0.0140593 0 0.529301 -0.0191432 0 0.529301 -0.0191432 0.00635 0.52575 -0.0237049 0.00635 0.52575 -0.0237049 0 0.521497 -0.0276199 0.00635 0.521497 -0.0276199 0 0.516658 -0.0307816 0 0.516658 -0.0307816 0.00635 0.511364 -0.0331036 0.00635 0.511364 -0.0331036 0 0.505761 -0.0345226 0.00635 0.505761 -0.0345226 0 0.5 -0.035 0.00635 0.5 -0.035 0 0 0.035 0.00635 -0.00576081 0.0345226 0.00635 -0.0113645 0.0331036 0.00635 -0.0320521 -0.0140593 0.00635 -0.0293008 -0.0191432 0.00635 0.529301 -0.0191432 0.00635 -0.0166582 0.0307816 0.00635 -0.0214974 0.0276199 0.00635 0.529301 0.0191432 0.00635 0.52575 0.0237049 0.00635 -0.0257503 0.0237049 0.00635 -0.033929 0.00859199 0.00635 0.533929 0.00859199 0.00635 -0.0320521 0.0140593 0.00635 0.532052 0.0140593 0.00635 -0.0293008 0.0191432 0.00635 0.533929 -0.00859199 0.00635 -0.0348805 -0.00289028 0.00635 -0.033929 -0.00859199 0.00635 0.53488 0.00289028 0.00635 -0.0348805 0.00289028 0.00635 0.53488 -0.00289028 0.00635 -0.0257503 -0.0237049 0.00635 0.5 -0.035 0.00635 0.52575 -0.0237049 0.00635 0.516658 -0.0307816 0.00635 0.521497 -0.0276199 0.00635 0.511364 -0.0331036 0.00635 4.03433e-18 -0.035 0.00635 -0.0166582 -0.0307816 0.00635 -0.0113645 -0.0331036 0.00635 -0.0214974 -0.0276199 0.00635 -0.00576081 -0.0345226 0.00635 0.505761 -0.0345226 0.00635 0.532052 -0.0140593 0.00635 0.5 0.035 0.00635 0.516658 0.0307816 0.00635 0.521497 0.0276199 0.00635 0.505761 0.0345226 0.00635 0.511364 0.0331036 0.00635 0 0.035 0 -0.0113645 0.0331036 0 -0.00576081 0.0345226 0 -0.0320521 -0.0140593 0 -0.033929 -0.00859199 0 0.532052 -0.0140593 0 -0.0166582 0.0307816 0 -0.0257503 0.0237049 0 -0.0214974 0.0276199 0 0.529301 -0.0191432 0 -0.0293008 -0.0191432 0 0.53488 -0.00289028 0 -0.0348805 -0.00289028 0 -0.0348805 0.00289028 0 0.5 -0.035 0 0.511364 -0.0331036 0 0.505761 -0.0345226 0 0.52575 -0.0237049 0 0.521497 -0.0276199 0 -0.0257503 -0.0237049 0 -0.0113645 -0.0331036 0 4.03433e-18 -0.035 0 -0.00576081 -0.0345226 0 -0.0166582 -0.0307816 0 -0.0214974 -0.0276199 0 0.516658 -0.0307816 0 0.533929 -0.00859199 0 0.53488 0.00289028 0 -0.033929 0.00859199 0 -0.0320521 0.0140593 0 0.533929 0.00859199 0 -0.0293008 0.0191432 0 0.532052 0.0140593 0 0.529301 0.0191432 0 0.52575 0.0237049 0 0.5 0.035 0 0.521497 0.0276199 0 0.511364 0.0331036 0 0.516658 0.0307816 0 0.505761 0.0345226 0 + + + + + + + + + + 0 1 0 0 1 0 0 1 0 0 1 0 -0.996584 -0.0825793 -0 -0.996584 -0.0825793 -0 -0.996584 0.0825795 0 -0.9694 -0.245486 -0 -0.9694 -0.245486 -0 -0.915773 -0.401695 -0 -0.915773 -0.401695 -0 -0.837166 -0.546948 -0 -0.837166 -0.546948 -0 -0.735724 -0.677282 -0 -0.735724 -0.677282 -0 -0.614213 -0.789141 -0 -0.614213 -0.789141 -0 -0.475947 -0.879474 -0 -0.475947 -0.879474 -0 -0.3247 -0.945817 -0 -0.3247 -0.945817 -0 -0.164595 -0.986361 -0 -0.164595 -0.986361 -0 -4.37114e-08 -1 -0 -4.37114e-08 -1 -0 -0.996584 0.0825795 0 -0.9694 0.245486 0 -0.9694 0.245486 0 -0.915773 0.401695 0 -0.915773 0.401695 0 -0.837166 0.546948 0 -0.837166 0.546948 0 -0.735724 0.677281 0 -0.735724 0.677281 0 -0.614213 0.789141 0 -0.614213 0.789141 0 -0.475947 0.879474 0 -0.475947 0.879474 0 -0.3247 0.945817 0 -0.3247 0.945817 0 -0.164595 0.986361 0 -0.164595 0.986361 0 1.19249e-08 1 0 1.19249e-08 1 0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 0.996584 0.0825793 0 0.996584 0.0825793 0 0.996584 -0.0825795 0 0.9694 0.245486 0 0.9694 0.245486 0 0.915773 0.401695 0 0.915773 0.401695 0 0.837166 0.546948 0 0.837166 0.546948 0 0.735724 0.677282 0 0.735724 0.677282 0 0.614213 0.789141 0 0.614213 0.789141 0 0.475947 0.879474 0 0.475947 0.879474 0 0.3247 0.945817 0 0.3247 0.945817 0 0.164595 0.986361 0 0.164595 0.986361 0 4.37114e-08 1 0 4.37114e-08 1 0 0.996584 -0.0825795 0 0.9694 -0.245486 0 0.9694 -0.245486 0 0.915773 -0.401695 0 0.915773 -0.401695 0 0.837166 -0.546948 0 0.837166 -0.546948 0 0.735724 -0.677281 0 0.735724 -0.677281 0 0.614213 -0.789141 0 0.614213 -0.789141 0 0.475947 -0.879474 0 0.475947 -0.879474 0 0.3247 -0.945817 0 0.3247 -0.945817 0 0.164595 -0.986361 0 0.164595 -0.986361 0 -1.19249e-08 -1 0 -1.19249e-08 -1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 + + + + + + + + + + + + + + +

0 1 2 1 0 3 4 5 6 5 7 8 7 5 4 9 8 10 7 10 8 11 12 9 9 10 11 12 13 14 13 12 11 15 14 16 13 16 14 17 18 15 15 16 17 18 19 20 19 18 17 21 20 22 19 22 20 23 24 21 21 22 23 4 6 25 26 27 25 25 6 26 27 28 29 28 27 26 30 29 31 28 31 29 32 33 30 30 31 32 33 34 35 34 33 32 36 35 37 34 37 35 38 39 36 36 37 38 39 40 41 40 39 38 40 42 41 41 42 43 44 45 46 46 47 44 48 49 50 49 51 52 51 49 48 53 52 54 51 54 52 55 56 53 53 54 55 56 57 58 57 56 55 59 58 60 57 60 58 61 62 59 59 60 61 62 63 64 63 62 61 65 64 66 63 66 64 67 68 65 65 66 67 48 50 69 70 71 69 69 50 70 71 72 73 72 71 70 74 73 75 72 75 73 76 77 74 74 75 76 77 78 79 78 77 76 80 79 81 78 81 79 82 83 80 80 81 82 83 84 85 84 83 82 84 86 85 85 86 87 88 89 90 91 92 93 88 90 94 95 88 94 96 97 98 98 88 95 99 100 101 102 96 103 104 105 106 107 108 109 110 111 112 112 92 110 111 113 114 115 113 111 116 117 118 116 111 110 117 116 119 120 116 118 110 119 116 111 114 112 115 111 121 92 112 93 122 91 93 106 122 104 122 106 91 108 105 109 109 105 104 100 99 107 107 99 108 100 102 101 96 98 103 102 103 101 123 88 97 88 98 97 124 123 125 123 97 125 126 123 127 123 124 127 128 129 130 131 132 133 129 128 134 128 135 136 134 128 136 131 137 138 139 140 141 142 143 144 145 138 137 142 145 146 145 142 147 148 149 150 149 151 152 147 149 152 148 151 149 146 153 142 142 149 147 143 142 153 147 138 145 133 132 154 137 131 133 139 154 140 132 140 154 155 141 156 139 141 155 156 157 158 158 155 156 159 160 157 158 157 160 161 160 159 135 162 161 161 159 135 163 164 162 135 128 162 163 165 166 165 163 167 163 166 164 163 162 128

+ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/project/meshes/basiclink.stl b/project/meshes/basiclink.stl new file mode 100644 index 0000000..ab0e7df Binary files /dev/null and b/project/meshes/basiclink.stl differ diff --git a/project/meshes/pointer.stl b/project/meshes/pointer.stl new file mode 100644 index 0000000..78463c8 Binary files /dev/null and b/project/meshes/pointer.stl differ diff --git a/project/meshes/upperarm.stl b/project/meshes/upperarm.stl new file mode 100644 index 0000000..3ab71ee Binary files /dev/null and b/project/meshes/upperarm.stl differ diff --git a/project/package.xml b/project/package.xml new file mode 100644 index 0000000..6fb612d --- /dev/null +++ b/project/package.xml @@ -0,0 +1,24 @@ + + + + project + 0.0.0 + TODO: Package description + robot + TODO: License declaration + + rclpy + geometry_msgs + sensor_msgs + std_msgs + project_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/project/project/__init__.py b/project/project/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/project/project/brain.py b/project/project/brain.py new file mode 100644 index 0000000..f6f2919 --- /dev/null +++ b/project/project/brain.py @@ -0,0 +1,198 @@ +import cv2 +import numpy as np +from math import sin, cos, pi, dist + +import rclpy +import cv_bridge + +from rclpy.node import Node + +from geometry_msgs.msg import Point +from project_msgs.msg import Object, ObjectArray, PointArray, Segment, SegmentArray, State + +from hw6sols.KinematicChainSol import KinematicChain + + +CYCLE = 3 * pi / 2 +WAITING_POS = [0.0, 0.0, -pi / 2] + + +class DemoNode(Node): + def __init__(self, name): + super().__init__(name) + + self.chain = KinematicChain(self, 'world', 'tip', ['base', 'shoulder', 'elbow']) + + self.object_array = ObjectArray() + self.object_array.objects = [] + + self.point_array = [] + + self.segment_array = SegmentArray() + self.segment_array.segments = [] + + self.x_waiting = [] + self.actpos = [] + + self.pub_segs = self.create_publisher(SegmentArray, name + '/segment_array', 1) + + self.get_logger().info('Name: %s' % name) + + self.bridge = cv_bridge.CvBridge() + + self.sub_obj_array = self.create_subscription( + ObjectArray, '/detector/object_array', self.recv_obj_array, 1) + + self.sub_state = self.create_subscription( + State, '/trajectory/state', self.recv_state, 1) + + self.get_logger().info("Brain running...") + + + def shutdown(self): + self.destroy_node() + + + def newton_raphson(self, xgoal): + xdistance = [] + qstepsize = [] + q = self.actpos + N = 500 + + for i in range(N+1): + (x, _, Jv, _) = self.chain.fkin(q) + xdelta = (xgoal - x) + qdelta = np.linalg.inv(Jv) @ xdelta + q = q + qdelta * 0.5 + xdistance.append(np.linalg.norm(xdelta)) + qstepsize.append(np.linalg.norm(qdelta)) + + if np.linalg.norm(x-xgoal) < 1e-12: + self.get_logger().info("Completed in %d iterations" % i) + return q.tolist() + + return WAITING_POS + + + def recv_state(self, msg): + self.x_waiting = [msg.x_waiting_x, msg.x_waiting_y, msg.x_waiting_z] + self.actpos = [msg.actpos_x, msg.actpos_y, msg.actpos_z] + + + def recv_obj_array(self, msg): + self.object_array.objects = [] + + for obj in msg.objects: + self.object_array.objects.append(obj) + + for obj in self.object_array.objects: + if obj.type == Object.DISK: + disc_world_msg = Point() + disc_world_msg.x = obj.x + disc_world_msg.y = obj.y + disc_world_msg.z = 0.012 + self.point_array.append(disc_world_msg) + + elif obj.type == Object.STRIP: + strip_world_start_msg = Point() + strip_world_end_msg = Point() + + TAP_FACTOR = 0.04 + strip_world_start_msg.x = obj.x - TAP_FACTOR * np.cos(np.radians(obj.theta)) + strip_world_start_msg.y = obj.y + TAP_FACTOR * np.sin(np.radians(obj.theta)) + strip_world_end_msg.x = obj.x + TAP_FACTOR * np.cos(np.radians(obj.theta)) + strip_world_end_msg.y = obj.y - TAP_FACTOR * np.sin(np.radians(obj.theta)) + + self.point_array.append(strip_world_start_msg) + self.point_array.append(strip_world_end_msg) + + if len(self.point_array) > 0 and self.x_waiting != []: + # self.pub_points.publish(self.point_array) + # # self.get_logger().info('All points: %s' % self.point_array.points) + + # self.point_array.points = [] + + cart_points = [self.x_waiting] + for pt in self.point_array: + cart_points.append([pt.x, pt.y, pt.z]) + self.point_array = [] + + Tmove = CYCLE / 2 + + for i in range(len(cart_points) - 1): + p1 = cart_points[i] + p2 = cart_points[i + 1] + + transitional = [(p1[0] + p2[0]) / 2, (p1[1] + p2[1]) / 2, 0.07] + + qT = self.newton_raphson(transitional) + q2 = self.newton_raphson(p2) + + if i == 0: + a_seg = Segment() + a_seg.px = q2[0] + a_seg.py = q2[1] + a_seg.pz = q2[2] + a_seg.vx = 0.0 + a_seg.vy = 0.0 + a_seg.vz = 0.0 + a_seg.t = Tmove * 2 + self.segment_array.segments.append(a_seg) + continue + + dx = (transitional[0] - p1[0]) + dy = (transitional[1] - p1[1]) + v_cart = np.array([dx / Tmove, dy / Tmove, 0.0]) + + (_, _, Jv, _) = self.chain.fkin(qT) + qdotT = np.linalg.pinv(Jv) @ v_cart + qdotT = qdotT.flatten().tolist() + + seg1 = Segment() + seg1.px = qT[0] + seg1.py = qT[1] + seg1.pz = qT[2] + seg1.vx = qdotT[0] + seg1.vy = qdotT[1] + seg1.vz = qdotT[2] + seg1.t = Tmove + # Segment(pf=qT, vf=qdotT, Tmove=Tmove) + # seg2 = Segment(pf=q2, vf=[0.0, 0.0, 0.0], Tmove=Tmove) + seg2 = Segment() + seg2.px = q2[0] + seg2.py = q2[1] + seg2.pz = q2[2] + seg2.vx = 0.0 + seg2.vy = 0.0 + seg2.vz = 0.0 + seg2.t = Tmove + + self.segment_array.segments.append(seg1) + self.segment_array.segments.append(seg2) + + a_seg = Segment() + a_seg.px = WAITING_POS[0] + a_seg.py = WAITING_POS[1] + a_seg.pz = WAITING_POS[2] + a_seg.vx = 0.0 + a_seg.vy = 0.0 + a_seg.vz = 0.0 + a_seg.t = Tmove * 2 + self.segment_array.segments.append(a_seg) + + self.pub_segs.publish(self.segment_array) + self.get_logger().info('All segs: %s' % self.segment_array.segments) + + self.segment_array.segments = [] + + +def main(args=None): + rclpy.init(args=args) + node = DemoNode('brain') + rclpy.spin(node) + node.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/project/project/detector.py b/project/project/detector.py new file mode 100644 index 0000000..d6711a1 --- /dev/null +++ b/project/project/detector.py @@ -0,0 +1,200 @@ +import cv2 +import numpy as np + +import rclpy +import cv_bridge + +from rclpy.node import Node +from sensor_msgs.msg import Image + +from project_msgs.msg import Object, ObjectArray + + +TAP_FACTOR = 4.5 + + +class DetectorNode(Node): + red = (255, 0, 0) + green = ( 0, 255, 0) + blue = ( 0, 0, 255) + yellow = (255, 255, 0) + white = (255, 255, 255) + + def __init__(self, name): + super().__init__(name) + + self.hsvlimits = np.array([[10, 40], [60, 220], [125, 255]]) + + # Assume the center of marker sheet is at the world origin. + self.x0 = 0.664 + self.y0 = 0.455 + + self.pubrgb = self.create_publisher(Image, name +'/image_raw', 3) + self.pubbin = self.create_publisher(Image, name +'/binary', 3) + + self.pub_obj_array = self.create_publisher(ObjectArray, name + '/object_array', 1) + + self.get_logger().info("Name: %s" % name) + + self.bridge = cv_bridge.CvBridge() + + self.object_array = ObjectArray() + self.object_array.objects = [] + + self.sub = self.create_subscription( + Image, '/image_raw', self.process, 1) + + # Report. + self.get_logger().info("Ball detector running...") + + def shutdown(self): + self.destroy_node() + + + def pixelToWorld(self, image, u, v, x0, y0, annotateImage=True): + ''' + Convert the (u,v) pixel position into (x,y) world coordinates + Inputs: + image: The image as seen by the camera + u: The horizontal (column) pixel coordinate + v: The vertical (row) pixel coordinate + x0: The x world coordinate in the center of the marker paper + y0: The y world coordinate in the center of the marker paper + annotateImage: Annotate the image with the marker information + + Outputs: + point: The (x,y) world coordinates matching (u,v), or None + + Return None for the point if not all the Aruco markers are detected + ''' + + markerCorners, markerIds, _ = cv2.aruco.detectMarkers( + image, cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)) + if annotateImage: + cv2.aruco.drawDetectedMarkers(image, markerCorners, markerIds) + + if (markerIds is None or len(markerIds) != 4 or + set(markerIds.flatten()) != set([1,2,3,4])): + return None + + uvMarkers = np.zeros((4,2), dtype='float32') + for i in range(4): + uvMarkers[markerIds[i]-1,:] = np.mean(markerCorners[i], axis=1) + + DX = 0.1016 + DY = 0.06985 + xyMarkers = np.float32([[x0+dx, y0+dy] for (dx, dy) in + [(-DX, DY), (DX, DY), (-DX, -DY), (DX, -DY)]]) + + M = cv2.getPerspectiveTransform(uvMarkers, xyMarkers) + + uvObj = np.float32([u, v]) + xyObj = cv2.perspectiveTransform(uvObj.reshape(1,1,2), M).reshape(2) + + if annotateImage: + s = "(%7.4f, %7.4f)" % (xyObj[0], xyObj[1]) + cv2.putText(image, s, (u-80, v-8), cv2.FONT_HERSHEY_SIMPLEX, + 0.5, (255, 0, 0), 2, cv2.LINE_AA) + + return xyObj + + + def process(self, msg): + self.object_array.objects = [] + + assert(msg.encoding == "rgb8") + frame = self.bridge.imgmsg_to_cv2(msg, "passthrough") + + hsv = cv2.cvtColor(frame, cv2.COLOR_RGB2HSV) + + binary = cv2.inRange(hsv, self.hsvlimits[:,0], self.hsvlimits[:,1]) + + iter = 2 + binary = cv2.erode( binary, None, iterations=iter) + binary = cv2.dilate(binary, None, iterations=2*iter) + binary = cv2.erode( binary, None, iterations=iter) + + (contours, hierarchy) = cv2.findContours( + binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + cv2.drawContours(frame, contours, -1, self.blue, 2) + + if len(contours) > 0: + for contour in sorted(contours, key=cv2.contourArea, reverse=True): + ((ur, vr), radius) = cv2.minEnclosingCircle(contour) + ur = int(ur) + vr = int(vr) + radius = int(radius) + + try: + ellipse = cv2.fitEllipse(contour) + ((ue, ve), (we, he), angle) = ellipse + except Exception as e: + self.get_logger().info("Exception: %s" % str(e)) + ellipse = None + + if ellipse is not None: + if he > we * 2: + rotatedRect = cv2.minAreaRect(contour) + ((um, vm), (wm, hm), angle) = cv2.minAreaRect(contour) + + if wm < hm: + angle += 90 + (wm, hm) = (hm, wm) + + box = np.int0(cv2.boxPoints(rotatedRect)) + cv2.drawContours(frame, [box], 0, self.green, 2) + + cv2.line(frame, + (int(um - TAP_FACTOR * (hm / 2) * np.sin(np.radians(angle))), int(vm + TAP_FACTOR * (hm / 2) * np.cos(np.radians(angle)))), + (int(um + TAP_FACTOR * (hm / 2) * np.sin(np.radians(angle))), int(vm - TAP_FACTOR * (hm / 2) * np.cos(np.radians(angle)))), + self.yellow, 2) + cv2.circle(frame, (int(um), int(vm)), 5, self.red, -1) + + strip_world = self.pixelToWorld(frame, int(um), int(vm), self.x0, self.y0, annotateImage=False) + if strip_world is not None: + strip_world_center_x, strip_world_center_y = strip_world + obj_rect = Object() + obj_rect.type = Object.STRIP + obj_rect.x = float(strip_world_center_x) + obj_rect.y = float(strip_world_center_y) + obj_rect.z = 0.0 + obj_rect.theta = angle + + self.object_array.objects.append(obj_rect) + else: + self.get_logger().info("PANICCCC!!!! strip_world is None") + + else: + cv2.ellipse(frame, ellipse, self.green, 2) + cv2.circle(frame, (int(ue), int(ve)), 5, self.red, -1) + + disk_world = self.pixelToWorld(frame, int(ue), int(ve), self.x0, self.y0, annotateImage=False) + if disk_world is not None: + disk_world_x, disk_world_y = disk_world + obj_disk = Object() + obj_disk.type = Object.DISK + obj_disk.x = float(disk_world_x) + obj_disk.y = float(disk_world_y) + obj_disk.z = 0.0 + obj_disk.theta = 0.0 + + self.object_array.objects.append(obj_disk) + else: + self.get_logger().info("PANICCCC!!!! disk_world is None") + + self.pubrgb.publish(self.bridge.cv2_to_imgmsg(frame, "rgb8")) + self.pub_obj_array.publish(self.object_array) + self.pubbin.publish(self.bridge.cv2_to_imgmsg(binary)) + + +def main(args=None): + rclpy.init(args=args) + node = DetectorNode('detector') + rclpy.spin(node) + node.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/project/project/receivepoint.py b/project/project/receivepoint.py new file mode 100644 index 0000000..e3150da --- /dev/null +++ b/project/project/receivepoint.py @@ -0,0 +1,53 @@ +import rclpy + +from rclpy.node import Node +from geometry_msgs.msg import Point + + +class DemoNode(Node): + # Initialization. + def __init__(self, name): + # Initialize the node, naming it as specified + super().__init__(name) + + # Create a subscriber to receive point messages. + self.pointsub = self.create_subscription( + Point, '/point', self.recvpoint, 10) + + # Report. + self.get_logger().info("Running %s" % name) + + # Shutdown + def shutdown(self): + # No particular cleanup, just shut down the node. + self.destroy_node() + + + # Receive a point message - called by incoming messages. + def recvpoint(self, pointmsg): + # Extract the data. + x = pointmsg.x + y = pointmsg.y + z = pointmsg.z + + # Report. + self.get_logger().info("Running point %r, %r, %r" % (x,y,z)) + + +def main(args=None): + # Initialize ROS. + rclpy.init(args=args) + + # Instantiate the DEMO node. + node = DemoNode('receivepoint') + + # Spin the node until interrupted. + rclpy.spin(node) + + # Shutdown the node and ROS. + node.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/project/project/toucharb.py b/project/project/toucharb.py new file mode 100644 index 0000000..4324044 --- /dev/null +++ b/project/project/toucharb.py @@ -0,0 +1,225 @@ +import numpy as np +import rclpy +from enum import Enum +from math import sin, cos, pi + +from rclpy.node import Node +from sensor_msgs.msg import JointState +from geometry_msgs.msg import Point, Pose + +from hw5code.TrajectoryUtils import goto, spline, goto5, spline5 +from hw6sols.KinematicChainSol import KinematicChain + + +RATE = 100.0 # Hertz +CYCLE = 3 * pi / 2 +WAITING_POS = [0.0, 0.0, -pi / 2] +JOINT_NAMES = ['base', 'shoulder', 'elbow'] + + +class Mode(Enum): + WAITING = 0 + POINTING = 1 + RETURNING = 2 + START_UP = 3 + POINTING_RECT_START = 4 + POINTING_RECT_MID = 5 + POINTING_RECT_END = 6 + + +class DemoNode(Node): + def __init__(self, name): + super().__init__(name) + + self.chain = KinematicChain(self, 'world', 'tip', JOINT_NAMES) + self.mode = Mode.START_UP + + self.position0 = self.grabfbk() + # self.get_logger().info("Initial positions: %r" % self.position0) + + self.t = 0 + self.t_start = 0 + (ptip, _, _, _) = self.chain.fkin(WAITING_POS) + self.x_waiting = ptip + self.qD = WAITING_POS + self.xD = ptip + self.qddot = None + self.qgoal = None + self.lastpointcmd = self.x_waiting + self.pointcmd = self.x_waiting + self.actpos = self.position0.copy() + self.A = -1.85 + self.B = 0 + + self.cmdmsg = JointState() + self.cmdpub = self.create_publisher(JointState, '/joint_commands', 10) + + self.get_logger().info("Waiting for a /joint_commands subscriber...") + while(not self.count_subscribers('/joint_commands')): + pass + + self.fbksub = self.create_subscription( + JointState, '/joint_states', self.recvfbk, 10) + + self.pointsub = self.create_subscription( + Point, '/point', self.recvpoint, 10) + + rate = RATE + self.starttime = self.get_clock().now() + self.timer = self.create_timer(1/rate, self.update) + self.get_logger().info("Sending commands with dt of %f seconds (%fHz)" % + (self.timer.timer_period_ns * 1e-9, rate)) + + def shutdown(self): + self.destroy_node() + + + # Grab a single feedback - DO NOT CALL THIS REPEATEDLY! + def grabfbk(self): + def cb(fbkmsg): + self.grabpos = list(fbkmsg.position) + self.grabready = True + + sub = self.create_subscription(JointState, '/joint_states', cb, 1) + self.grabready = False + while not self.grabready: + rclpy.spin_once(self) + self.destroy_subscription(sub) + + return self.grabpos + + + def sendcmd(self, pos, vel, eff = []): + self.cmdmsg.header.stamp = self.get_clock().now().to_msg() + self.cmdmsg.name = JOINT_NAMES + self.cmdmsg.position = pos + self.cmdmsg.velocity = vel + self.cmdmsg.effort = eff + self.cmdpub.publish(self.cmdmsg) + + + # Receive feedback - called repeatedly by incoming messages. + def recvfbk(self, fbkmsg): + self.actpos = fbkmsg.position + pass + + + def recvpoint(self, pointmsg): + x = pointmsg.x + y = pointmsg.y + z = pointmsg.z + + if self.mode is Mode.WAITING: + if ((x - 0.7455) ** 2 + (y - 0.04) ** 2 + (z - 0.11) ** 2) ** (1 / 2) < 0.74 and z >= 0.0 and y > 0.0: + self.set_pointcmd([x, y, z]) + self.qgoal = self.newton_raphson(self.pointcmd) + self.get_logger().info("qgoal: %r" % self.qgoal) + self.set_mode(Mode.POINTING) + + self.get_logger().info("Running point %r, %r, %r" % (x,y,z)) + else: + self.get_logger().info('Not in the dome. Please try again dummy...') + else: + self.get_logger().info('Not waiting yet. Please wait dummy...') + + + def super_smart_goto(self, t, initial_pos, final_pos, cycle): + (q, qdot) = goto5(t, cycle, np.array(initial_pos).reshape(3, 1), np.array(final_pos).reshape(3, 1)) + return q.flatten().tolist(), qdot.flatten().tolist() + + + def set_mode(self, new_mode): + self.mode = new_mode + self.t_start = self.t + + + def set_pointcmd(self, new_pointcmd): + self.lastpointcmd = self.pointcmd + self.pointcmd = new_pointcmd + + + def gravity(self, pos): + theta_sh = pos[1] + tau_shoulder = self.A * sin(theta_sh) + self.B * cos(theta_sh) - 0.1 + return [0.0, tau_shoulder, 0.0] + + + def newton_raphson(self, xgoal): + xdistance = [] + qstepsize = [] + q = self.actpos + N = 1000 + for i in range(N+1): + (x, _, Jv, _) = self.chain.fkin(q) + xdelta = (xgoal - x) + qdelta = np.linalg.inv(Jv) @ xdelta + q = q + qdelta * 0.3 + xdistance.append(np.linalg.norm(xdelta)) + qstepsize.append(np.linalg.norm(qdelta)) + + if np.linalg.norm(x-xgoal) < 1e-12: + self.get_logger().info("Completed in %d iterations" % i) + return q.tolist() + return WAITING_POS + + + def update(self): + # Grab the current time. + now = self.get_clock().now() + self.t = (now - self.starttime).nanoseconds * 1e-9 + + if self.mode is Mode.START_UP: + if self.t < CYCLE: + qd, qddot = self.super_smart_goto(self.t, self.position0, [self.position0[0], WAITING_POS[1], self.position0[2]], CYCLE) + elif self.t < 2 * CYCLE: + qd, qddot = self.super_smart_goto(self.t - CYCLE, [self.position0[0], WAITING_POS[1], self.position0[2]], [WAITING_POS[0], WAITING_POS[1], self.position0[2]], CYCLE) + elif self.t < 3 * CYCLE: + qd, qddot = self.super_smart_goto(self.t - CYCLE * 2, [WAITING_POS[0], WAITING_POS[1], self.position0[2]], WAITING_POS, CYCLE) + else: + qd, qddot = WAITING_POS, [0.0, 0.0, 0.0] + + self.set_mode(Mode.WAITING) + + elif self.mode is Mode.POINTING: + if self.t - self.t_start < CYCLE: + qd, qddot = self.super_smart_goto(self.t - self.t_start, WAITING_POS, self.qgoal, CYCLE) + self.qD = qd + self.qddot = qddot + + else: + qd = self.qD + qddot = self.qddot + + self.set_mode(Mode.RETURNING) + + elif self.mode is Mode.RETURNING: + if self.t - self.t_start < CYCLE: + qd, qddot = self.super_smart_goto(self.t - self.t_start, self.qD, WAITING_POS, CYCLE) + else: + qd, qddot = WAITING_POS, [0.0, 0.0, 0.0] + self.pointcmd = self.x_waiting + self.set_mode(Mode.WAITING) + self.get_logger().info("HIT WAITING: %s" % (self.mode)) + + self.qD = WAITING_POS + self.xD = self.x_waiting + + else: # elif self.mode is Mode.WAITING: + qd, qddot = WAITING_POS, [0.0, 0.0, 0.0] + + tau = self.gravity(self.actpos) + self.sendcmd(qd, qddot, tau) + + +def main(args=None): + rclpy.init(args=args) + node = DemoNode('toucharb') + + rclpy.spin(node) + + node.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/project/project/trajectory.py b/project/project/trajectory.py new file mode 100644 index 0000000..8c9e8cb --- /dev/null +++ b/project/project/trajectory.py @@ -0,0 +1,274 @@ +import numpy as np +import rclpy +from enum import Enum +from math import sin, cos, pi, dist + +from rclpy.node import Node +from sensor_msgs.msg import JointState + +from project_msgs.msg import PointArray, Segment, SegmentArray, State + +from hw5code.TrajectoryUtils import goto, spline, goto5, spline5 +from hw6sols.KinematicChainSol import KinematicChain + + +RATE = 100.0 # Hertz +CYCLE = 3 * pi / 2 +WAITING_POS = [0.0, 0.0, -pi / 2] +JOINT_NAMES = ['base', 'shoulder', 'elbow'] + + +class Mode(Enum): + WAITING = 0 + POINTING = 1 + RETURNING = 2 + START_UP = 3 + POINTING_RECT_START = 4 + POINTING_RECT_MID = 5 + POINTING_RECT_END = 6 + + +class Spline(): + # Initialization connecting last command to next segment + def __init__(self, tcmd, pcmd, vcmd, segment): + # Save the initial time and duration. + self.t0 = tcmd + self.T = segment.t + + # Pre-compute the parameters. + p0 = np.array(pcmd) + v0 = np.array(vcmd) + pf = np.array([segment.px, segment.py, segment.pz]) + vf = np.array([segment.vx, segment.vy, segment.vz]) + T = self.T + + self.a = p0 + self.b = v0 + self.c = np.zeros_like(p0) + self.d = + 10 * (pf - p0) / T ** 3 - 6 * v0 / T ** 2 - 4 * vf / T ** 2 + self.e = - 15 * (pf - p0) / T ** 4 + 8 * v0 / T ** 3 + 7 * vf / T ** 3 + self.f = + 6 * (pf - p0) / T ** 5 - 3 * v0 / T ** 4 - 3 * vf / T ** 4 + + # Evaluation at any time (Shortening self to s). + def evaluate(s, t): + # Get the time relative to the start time. + t = t - s.t0 + + # Compute the current commands. + p = s.a + s.b*t + s.c*t**2 + s.d*t**3 + s.e*t**4 + s.f*t**5 + v = s.b + 2*s.c*t + 3*s.d*t**2 + 4*s.e*t**3 + 5*s.f*t**4 + + # Return as a list. + return (p.tolist(),v.tolist()) + + +class DemoNode(Node): + def __init__(self, name): + super().__init__(name) + + self.chain = KinematicChain(self, 'world', 'tip', JOINT_NAMES) + self.mode = Mode.START_UP + + self.position0 = self.grabfbk() + # self.get_logger().info("Initial positions: %r" % self.position0) + + self.t = 0 + self.t_start = 0 + (ptip, _, _, _) = self.chain.fkin(WAITING_POS) + self.x_waiting = ptip + self.qD = WAITING_POS + self.xD = ptip + self.qddot = [0.0, 0.0, 0.0] + self.qgoal = None + self.lastpointcmd = self.x_waiting + self.pointcmd = self.x_waiting + self.actpos = self.position0.copy() + self.A = -1.85 + self.B = 0 + + self.cmdmsg = JointState() + self.cmdpub = self.create_publisher(JointState, '/joint_commands', 10) + + self.state_pub = self.create_publisher(State, name + '/state', 10) + + self.get_logger().info("Waiting for a /joint_commands subscriber...") + while(not self.count_subscribers('/joint_commands')): + pass + + self.fbksub = self.create_subscription( + JointState, '/joint_states', self.recvfbk, 10) + + self.sub_seg_array = self.create_subscription( + SegmentArray, '/brain/segment_array', self.recv_segment_array, 1) + + self.segments = [] + self.spline = None + self.abort = False + self.tcmd = 0 + self.pcmd = WAITING_POS[:] + self.vcmd = [0.0, 0.0, 0.0] + + rate = RATE + self.starttime = self.get_clock().now() + self.timer = self.create_timer(1/rate, self.update) + self.get_logger().info("Sending commands with dt of %f seconds (%fHz)" % + (self.timer.timer_period_ns * 1e-9, rate)) + + + def shutdown(self): + self.destroy_node() + + + # Grab a single feedback - DO NOT CALL THIS REPEATEDLY! + def grabfbk(self): + def cb(fbkmsg): + self.grabpos = list(fbkmsg.position) + self.grabready = True + + sub = self.create_subscription(JointState, '/joint_states', cb, 1) + self.grabready = False + while not self.grabready: + rclpy.spin_once(self) + self.destroy_subscription(sub) + + return self.grabpos + + + def sendcmd(self, pos, vel, eff = []): + self.cmdmsg.header.stamp = self.get_clock().now().to_msg() + self.cmdmsg.name = JOINT_NAMES + self.cmdmsg.position = pos + self.cmdmsg.velocity = vel + self.cmdmsg.effort = eff + self.cmdpub.publish(self.cmdmsg) + + + # Receive feedback - called repeatedly by incoming messages. + def recvfbk(self, fbkmsg): + self.actpos = fbkmsg.position + + state = State() + state.x_waiting_x = self.x_waiting[0] + state.x_waiting_y = self.x_waiting[1] + state.x_waiting_z = self.x_waiting[2] + + state.actpos_x = self.actpos[0] + state.actpos_y = self.actpos[1] + state.actpos_z = self.actpos[2] + + self.state_pub.publish(state) + + + def recv_segment_array(self, msg): + self.get_logger().info("Received a list of segments: %r" % msg.segments) + if self.mode is Mode.WAITING: + self.segments = msg.segments + + self.tcmd = self.t + self.pcmd = self.actpos[:] + self.vcmd = [0.0, 0.0, 0.0] + + self.set_mode(Mode.POINTING) + + + def super_smart_goto(self, t, initial_pos, final_pos, cycle): + (q, qdot) = goto5(t, cycle, np.array(initial_pos).reshape(3, 1), np.array(final_pos).reshape(3, 1)) + return q.flatten().tolist(), qdot.flatten().tolist() + + + def set_mode(self, new_mode): + self.mode = new_mode + self.t_start = self.t + + + def set_pointcmd(self, new_pointcmd): + self.lastpointcmd = self.pointcmd + self.pointcmd = new_pointcmd + + + def gravity(self, pos): + theta_sh = pos[1] + tau_shoulder = self.A * sin(theta_sh) + self.B * cos(theta_sh) - 0.1 + return [0.0, tau_shoulder, 0.0] + + + def update(self): + now = self.get_clock().now() + self.t = (now - self.starttime).nanoseconds * 1e-9 + + if self.mode is Mode.START_UP: + if self.t < CYCLE: + qd, qddot = self.super_smart_goto(self.t, self.position0, [self.position0[0], WAITING_POS[1], self.position0[2]], CYCLE) + elif self.t < 2 * CYCLE: + qd, qddot = self.super_smart_goto(self.t - CYCLE, [self.position0[0], WAITING_POS[1], self.position0[2]], [WAITING_POS[0], WAITING_POS[1], self.position0[2]], CYCLE) + elif self.t < 3 * CYCLE: + qd, qddot = self.super_smart_goto(self.t - CYCLE * 2, [WAITING_POS[0], WAITING_POS[1], self.position0[2]], WAITING_POS, CYCLE) + else: + qd, qddot = WAITING_POS, [0.0, 0.0, 0.0] + + self.set_mode(Mode.WAITING) + + elif self.mode is Mode.POINTING: + if self.spline and ((self.t - self.spline.t0) > self.spline.T or self.abort): + self.spline = None + self.abort = False + self.tcmd = self.t + + if not self.spline and len(self.segments) > 0: + next_seg = self.segments.pop(0) + self.spline = Spline(self.tcmd, self.pcmd, self.vcmd, next_seg) + + if self.spline: + (self.pcmd, self.vcmd) = self.spline.evaluate(self.t) + qd = self.pcmd + qddot = self.vcmd + else: + qd, qddot = self.pcmd, [0.0, 0.0, 0.0] + + if self.spline is None and len(self.segments) == 0: + self.get_logger().info("Trajectory complete, switching to WAITING mode.") + self.set_mode(Mode.WAITING) + self.pointcmd = self.x_waiting + qd, qddot = WAITING_POS, [0.0, 0.0, 0.0] + + if abs(dist(self.actpos, qd)) > 0.05: + self.spline = None + + a_seg = Segment() + a_seg.px = WAITING_POS[0] + a_seg.py = WAITING_POS[1] + a_seg.pz = WAITING_POS[2] + a_seg.vx = 0.0 + a_seg.vy = 0.0 + a_seg.vz = 0.0 + a_seg.t = CYCLE + self.segments = [a_seg] + + self.tcmd = (now - self.starttime).nanoseconds * 1e-9 + self.pcmd = self.actpos[:] + self.vcmd = [0.0, 0.0, 0.0] + + qd = self.qD + qddot = self.qddot + + self.get_logger().info("HIT RETURNING: %s" % (self.mode)) + + else: + qd, qddot = WAITING_POS, [0.0, 0.0, 0.0] + + tau = self.gravity(self.actpos) + self.sendcmd(qd, qddot, tau) + + +def main(args=None): + rclpy.init(args=args) + node = DemoNode('trajectory') + + rclpy.spin(node) + + node.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/project/project/wave.py b/project/project/wave.py new file mode 100644 index 0000000..3687a74 --- /dev/null +++ b/project/project/wave.py @@ -0,0 +1,121 @@ +import numpy as np +import rclpy +from math import sin, cos, pi + +from rclpy.node import Node +from sensor_msgs.msg import JointState + + +RATE = 100.0 # Hertz + + +class DemoNode(Node): + def __init__(self, name): + super().__init__(name) + + self.position0 = self.grabfbk() + self.get_logger().info("Initial positions: %r" % self.position0) + + self.cmdmsg = JointState() + self.cmdpub = self.create_publisher(JointState, '/joint_commands', 10) + + self.get_logger().info("Waiting for a /joint_commands subscriber...") + while(not self.count_subscribers('/joint_commands')): + pass + + self.fbksub = self.create_subscription( + JointState, '/joint_states', self.recvfbk, 10) + + rate = RATE + self.starttime = self.get_clock().now() + self.timer = self.create_timer(1/rate, self.update) + self.get_logger().info("Sending commands with dt of %f seconds (%fHz)" % + (self.timer.timer_period_ns * 1e-9, rate)) + + def shutdown(self): + self.destroy_node() + + + # Grab a single feedback - DO NOT CALL THIS REPEATEDLY! + def grabfbk(self): + def cb(fbkmsg): + self.grabpos = list(fbkmsg.position) + self.grabready = True + + sub = self.create_subscription(JointState, '/joint_states', cb, 1) + self.grabready = False + while not self.grabready: + rclpy.spin_once(self) + self.destroy_subscription(sub) + + return self.grabpos + + + def sendcmd(self, pos, vel, eff = []): + self.cmdmsg.header.stamp = self.get_clock().now().to_msg() + self.cmdmsg.name = ['one', 'two', 'three'] + self.cmdmsg.position = pos + self.cmdmsg.velocity = vel + self.cmdmsg.effort = eff + self.cmdpub.publish(self.cmdmsg) + + + # Receive feedback - called repeatedly by incoming messages. + def recvfbk(self, fbkmsg): + # print(list(fbkmsg.position)) + pass + + + def update(self): + # Grab the current time. + now = self.get_clock().now() + t = (now - self.starttime).nanoseconds * 1e-9 + + # Time it takes to initialize position for start of wave motion + START_SHIFT = pi + + # Define range of motor 3 + JOINT_THREE_MAX = 0.5 + JOINT_THREE_MIN = -0.5 + joint_three_amp = JOINT_THREE_MAX - JOINT_THREE_MIN + + # Define range of motor 2 + JOINT_TWO_MAX = 0.3 + JOINT_TWO_MIN = -0.3 + JOINT_TWO_SHIFT = -pi / 2 + joint_two_amp = JOINT_TWO_MAX - JOINT_TWO_MIN + + # Define range of motor 1 + JOINT_ONE_MAX = 0.2 + JOINT_ONE_MIN = -0.2 + joint_one_amp = JOINT_ONE_MAX - JOINT_ONE_MIN + + # Initialize motors to position arm at starting position + if t < START_SHIFT: + qd = [(0 - self.position0[0]) * (cos(t + pi) - 1) / 2, + (self.position0[1] - JOINT_TWO_MIN * 2) / 2 * cos(t) + ((self.position0[1] + JOINT_TWO_MIN * 2) / 2), + (0 - self.position0[2]) * (cos(t + pi) - 1) / 2] + qddot = [0.0, 0.0, 0.0] + + # Wave continuously + else: + qd = [joint_one_amp * cos(t - START_SHIFT + pi / 2), + joint_two_amp * sin(t + JOINT_TWO_SHIFT - START_SHIFT), + joint_three_amp * sin(t - START_SHIFT)] + qddot = [0.0, 0.0, 0.0] + + self.sendcmd(qd, qddot) + + +def main(args=None): + rclpy.init(args=args) + node = DemoNode('wave') + + rclpy.spin(node) + + node.shutdown() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/project/resource/project b/project/resource/project new file mode 100644 index 0000000..e69de29 diff --git a/project/rviz/viewurdf.rviz b/project/rviz/viewurdf.rviz new file mode 100644 index 0000000..fae2387 --- /dev/null +++ b/project/rviz/viewurdf.rviz @@ -0,0 +1,251 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Grid1/Offset1 + - /RobotModel1/Description Topic1 + Splitter Ratio: 0.5676470398902893 + Tree Height: 546 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + basemotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + baseplate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + elbowmotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbracket: + Alpha: 1 + Show Axes: false + Show Trail: false + lowerarm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shouldermotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + table: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tip: + Alpha: 1 + Show Axes: false + Show Trail: false + upperarm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + basemotor: + Value: true + baseplate: + Value: true + elbowmotor: + Value: true + lbracket: + Value: true + lowerarm: + Value: true + shouldermotor: + Value: true + table: + Value: true + tip: + Value: true + upperarm: + Value: true + world: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + table: + baseplate: + basemotor: + lbracket: + shouldermotor: + upperarm: + elbowmotor: + lowerarm: + tip: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 10 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.5459771156311035 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.2703981399536133 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.21039463579654694 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002aefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002ae000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002ae000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002c600fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002ae00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 59 + Y: 60 diff --git a/project/setup.cfg b/project/setup.cfg new file mode 100644 index 0000000..d7cd669 --- /dev/null +++ b/project/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/project +[install] +install_scripts=$base/lib/project diff --git a/project/setup.py b/project/setup.py new file mode 100644 index 0000000..42d0242 --- /dev/null +++ b/project/setup.py @@ -0,0 +1,44 @@ +from setuptools import find_packages, setup +from glob import glob +from os.path import isdir + +package_name = 'project' + +folders = ['launch', 'rviz', 'urdf', 'meshes'] + +otherfiles = [] +for topfolder in folders: + for folder in [topfolder] + \ + [f for f in glob(topfolder+'/*/', recursive=True) if isdir(f)]: + # Grab the files in this folder and append to the mapping. + files = [f for f in glob(folder+'/*') if not isdir(f)] + otherfiles.append(('share/' + package_name + '/' + folder, files)) + + +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']), + ] + otherfiles, + install_requires=['setuptools'], + zip_safe=True, + maintainer='robot', + maintainer_email='robot@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'wave = project.wave:main', + 'toucharb = project.toucharb:main', + 'receivepoint = project.receivepoint:main', + 'detector = project.detector:main', + 'brain = project.brain:main', + 'trajectory = project.trajectory:main', + ], + }, +) \ No newline at end of file diff --git a/project/test/test_copyright.py b/project/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/project/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/project/test/test_flake8.py b/project/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/project/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/project/test/test_pep257.py b/project/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/project/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/project/urdf/threedof.urdf b/project/urdf/threedof.urdf new file mode 100644 index 0000000..e210c90 --- /dev/null +++ b/project/urdf/threedof.urdf @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/project_msgs/CMakeLists.txt b/project_msgs/CMakeLists.txt new file mode 100644 index 0000000..7c25f60 --- /dev/null +++ b/project_msgs/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.8) +project(project_msgs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Object.msg" + "msg/ObjectArray.msg" + "msg/PointArray.msg" + "msg/Segment.msg" + "msg/SegmentArray.msg" + "msg/State.msg" + DEPENDENCIES geometry_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/project_msgs/LICENSE b/project_msgs/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/project_msgs/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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/project_msgs/msg/Object.msg b/project_msgs/msg/Object.msg new file mode 100644 index 0000000..5cff796 --- /dev/null +++ b/project_msgs/msg/Object.msg @@ -0,0 +1,10 @@ +int32 DISK=0 +int32 STRIP=1 + +int32 id +int32 type + +float64 x +float64 y +float64 z +float64 theta \ No newline at end of file diff --git a/project_msgs/msg/ObjectArray.msg b/project_msgs/msg/ObjectArray.msg new file mode 100644 index 0000000..e3134ef --- /dev/null +++ b/project_msgs/msg/ObjectArray.msg @@ -0,0 +1 @@ +Object[] objects \ No newline at end of file diff --git a/project_msgs/msg/PointArray.msg b/project_msgs/msg/PointArray.msg new file mode 100644 index 0000000..7b8976c --- /dev/null +++ b/project_msgs/msg/PointArray.msg @@ -0,0 +1 @@ +geometry_msgs/Point[] points \ No newline at end of file diff --git a/project_msgs/msg/Segment.msg b/project_msgs/msg/Segment.msg new file mode 100644 index 0000000..2e28d57 --- /dev/null +++ b/project_msgs/msg/Segment.msg @@ -0,0 +1,9 @@ +float64 px +float64 py +float64 pz + +float64 vx +float64 vy +float64 vz + +float64 t \ No newline at end of file diff --git a/project_msgs/msg/SegmentArray.msg b/project_msgs/msg/SegmentArray.msg new file mode 100644 index 0000000..cf88308 --- /dev/null +++ b/project_msgs/msg/SegmentArray.msg @@ -0,0 +1 @@ +Segment[] segments \ No newline at end of file diff --git a/project_msgs/msg/State.msg b/project_msgs/msg/State.msg new file mode 100644 index 0000000..7faae43 --- /dev/null +++ b/project_msgs/msg/State.msg @@ -0,0 +1,7 @@ +float64 x_waiting_x +float64 x_waiting_y +float64 x_waiting_z + +float64 actpos_x +float64 actpos_y +float64 actpos_z \ No newline at end of file diff --git a/project_msgs/package.xml b/project_msgs/package.xml new file mode 100644 index 0000000..63d2f67 --- /dev/null +++ b/project_msgs/package.xml @@ -0,0 +1,23 @@ + + + + project_msgs + 0.0.0 + TODO: Package description + robot + Apache-2.0 + + geometry_msgs + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + +