commit 9eff9ec1456d9db21715daf02b4078ed4d992c11 Author: Thomas Muller Date: Mon Sep 6 20:49:44 2021 +0000 Initial Commit URDF and STLs stolen from https://github.com/ongdexter/ar3_core/tree/master/ar3_description Bare minumum state publisher from ROS2 tutorials diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..981ddb1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,171 @@ + +# Created by https://www.toptal.com/developers/gitignore/api/python,vim +# Edit at https://www.toptal.com/developers/gitignore?templates=python,vim + +### Python ### +# 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 + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__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/ + +### Vim ### +# Swap +[._]*.s[a-v][a-z] +!*.svg # comment out if you don't need vector files +[._]*.sw[a-p] +[._]s[a-rt-v][a-z] +[._]ss[a-gi-z] +[._]sw[a-p] + +# Session +Session.vim +Sessionx.vim + +# Temporary +.netrwhist +*~ +# Auto-generated tag files +tags +# Persistent undo +[._]*.un~ + +# End of https://www.toptal.com/developers/gitignore/api/python,vim + +build/ +install/ +log/ + diff --git a/src/ar3_description/ar3_description/__init__.py b/src/ar3_description/ar3_description/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/ar3_description/ar3_description/state_publisher.py b/src/ar3_description/ar3_description/state_publisher.py new file mode 100644 index 0000000..f628aa4 --- /dev/null +++ b/src/ar3_description/ar3_description/state_publisher.py @@ -0,0 +1,85 @@ +from math import sin, cos, pi +import rclpy, random +from rclpy.node import Node +from rclpy.qos import QoSProfile +from geometry_msgs.msg import Quaternion +from sensor_msgs.msg import JointState +from tf2_ros import TransformBroadcaster, TransformStamped + +class StatePublisher(Node): + + def __init__(self): + rclpy.init() + super().__init__('state_publisher') + + qos_profile = QoSProfile(depth=10) + self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile) + self.broadcaster = TransformBroadcaster(self, qos=qos_profile) + self.nodeName = self.get_name() + self.get_logger().info("{0} started".format(self.nodeName)) + + degree = pi / 180.0 + loop_rate = self.create_rate(30) + + # robot state + tilt = 0. + tinc = degree + swivel = 0. + angle = 0. + height = 0. + hinc = 0.005 + + # message declarations + odom_trans = TransformStamped() + odom_trans.header.frame_id = 'odom' + odom_trans.child_frame_id = 'base_link' + joint_state = JointState() + + try: + while rclpy.ok(): + rclpy.spin_once(self) + + # update joint_state + now = self.get_clock().now() + joint_state.header.stamp = now.to_msg() + joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] + # joint_state.position = [sin(angle) * pi / 1] * 6 + # joint_state.position = [random.random() * pi / 2] * 6 + # joint_state.position = [angle * 16, pi/2., -25 / 180 * pi, 0., 0., 0.] + # joint_state.position = [angle * 0.01 * random.random()] * 6 + joint_state.position = [0., 0., 0., 0., angle, 0.] + + # update transform + # (moving in a circle with radius=2) + odom_trans.header.stamp = now.to_msg() + odom_trans.transform.translation.x = 0.0# cos(angle) * 0.4 + odom_trans.transform.translation.y = 0.0# sin(angle) * 0.4 + odom_trans.transform.translation.z = 0.0 + odom_trans.transform.rotation = \ + euler_to_quaternion(0, 0, 0) # roll,pitch,yaw + + # send the joint state and transform + self.joint_pub.publish(joint_state) + self.broadcaster.sendTransform(odom_trans) + + # Create new robot state + angle += degree + + # This will adjust as needed per iteration + loop_rate.sleep() + + except KeyboardInterrupt: + pass + +def euler_to_quaternion(roll, pitch, yaw): + qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2) + qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2) + qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2) + qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2) + return Quaternion(x=qx, y=qy, z=qz, w=qw) + +def main(): + node = StatePublisher() + +if __name__ == '__main__': + main() diff --git a/src/ar3_description/launch/demo.launch.py b/src/ar3_description/launch/demo.launch.py new file mode 100644 index 0000000..67ec413 --- /dev/null +++ b/src/ar3_description/launch/demo.launch.py @@ -0,0 +1,36 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + urdf_file_name = 'ar3.urdf' + urdf = os.path.join( + get_package_share_directory('ar3_description'), + urdf_file_name) + with open(urdf, 'r') as infp: + robot_desc = infp.read() + + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true'), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}], + arguments=[urdf]), + Node( + package='ar3_description', + executable='state_publisher', + name='state_publisher', + output='screen'), + ]) diff --git a/src/ar3_description/meshes/base_link.STL b/src/ar3_description/meshes/base_link.STL new file mode 100644 index 0000000..3840d27 Binary files /dev/null and b/src/ar3_description/meshes/base_link.STL differ diff --git a/src/ar3_description/meshes/link_1.STL b/src/ar3_description/meshes/link_1.STL new file mode 100644 index 0000000..620d2ec Binary files /dev/null and b/src/ar3_description/meshes/link_1.STL differ diff --git a/src/ar3_description/meshes/link_2.STL b/src/ar3_description/meshes/link_2.STL new file mode 100644 index 0000000..382c183 Binary files /dev/null and b/src/ar3_description/meshes/link_2.STL differ diff --git a/src/ar3_description/meshes/link_3.STL b/src/ar3_description/meshes/link_3.STL new file mode 100644 index 0000000..ac232bc Binary files /dev/null and b/src/ar3_description/meshes/link_3.STL differ diff --git a/src/ar3_description/meshes/link_4.STL b/src/ar3_description/meshes/link_4.STL new file mode 100644 index 0000000..237c61e Binary files /dev/null and b/src/ar3_description/meshes/link_4.STL differ diff --git a/src/ar3_description/meshes/link_5.STL b/src/ar3_description/meshes/link_5.STL new file mode 100644 index 0000000..618df82 Binary files /dev/null and b/src/ar3_description/meshes/link_5.STL differ diff --git a/src/ar3_description/meshes/link_6.STL b/src/ar3_description/meshes/link_6.STL new file mode 100644 index 0000000..62d26b9 Binary files /dev/null and b/src/ar3_description/meshes/link_6.STL differ diff --git a/src/ar3_description/package.xml b/src/ar3_description/package.xml new file mode 100644 index 0000000..0f9778e --- /dev/null +++ b/src/ar3_description/package.xml @@ -0,0 +1,21 @@ + + + + ar3_description + 1.0.0 + Robot description and state publisher for the AR3 arm + Thomas Muller + John Farell + TODO: License declaration + + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/ar3_description/resource/ar3_description b/src/ar3_description/resource/ar3_description new file mode 100644 index 0000000..e69de29 diff --git a/src/ar3_description/setup.cfg b/src/ar3_description/setup.cfg new file mode 100644 index 0000000..f78c577 --- /dev/null +++ b/src/ar3_description/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/ar3_description +[install] +install-scripts=$base/lib/ar3_description diff --git a/src/ar3_description/setup.py b/src/ar3_description/setup.py new file mode 100644 index 0000000..29ac58a --- /dev/null +++ b/src/ar3_description/setup.py @@ -0,0 +1,32 @@ +import os +from glob import glob +from setuptools import setup +from setuptools import find_packages + +package_name = 'ar3_description' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, glob('launch/*.py')), + ('share/' + package_name, glob('urdf/*')), + ('share/' + package_name + '/meshes', glob('meshes/*')) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer=['Thomas Muller', 'John Farell'], + maintainer_email=['tmuller2017@my.fit.edu', 'farell2017@my.fit.edu'], + description='Robot description and state publisher for the AR3 arm', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'state_publisher = ar3_description.state_publisher:main' + ], + }, +) diff --git a/src/ar3_description/test/test_copyright.py b/src/ar3_description/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/src/ar3_description/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/ar3_description/test/test_flake8.py b/src/ar3_description/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/ar3_description/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/ar3_description/test/test_pep257.py b/src/ar3_description/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/ar3_description/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/ar3_description/urdf/ar3.urdf b/src/ar3_description/urdf/ar3.urdf new file mode 100644 index 0000000..e7e788e --- /dev/null +++ b/src/ar3_description/urdf/ar3.urdf @@ -0,0 +1,361 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/ar3_description/urdf/ar3.urdf.xacro b/src/ar3_description/urdf/ar3.urdf.xacro new file mode 100644 index 0000000..147280a --- /dev/null +++ b/src/ar3_description/urdf/ar3.urdf.xacro @@ -0,0 +1,204 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +