Merge pull request #88 from KCL-BMEIS/dev-galactic-demos

Dev galactic demos

README.md

@@ -21,7 +21,8 @@ Next, launch the simulation via
 source install/setup.bash
 ros2 launch lbr_bringup lbr_bringup.launch.py model:=med7 sim:=true # model:=[iiwa7/iiwa14/med7/med14]
 ```
-Now, run the [demos](lbr_demos/README.md). To get started with the real robot, checkout the [Documentation](#documentation) above.
+
+Now, run the [demos](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/galactic/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Documentation](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/galactic/index.html) above.
 
 ## Acknowledgements
 <img src="https://www.kcl.ac.uk/newimages/Wellcome-EPSRC-Centre-medical-engineering-logo.xa827df3f.JPG?f=webp" alt="wellcome" height="45" width="65" align="left">

lbr_demos/doc/lbr_demos.rst

@@ -0,0 +1,34 @@
+LBR Demos
+=========
+Demos for controlling the LBR through the Fast Robot Interface (FRI) from ROS 2.
+
+.. warning::
+    On the real robot, do always execute in ``T1`` mode first.
+
+LBR FRI ROS 2 Demos
+-------------------
+.. note::
+    These demos are compatible with and closely follow KUKA's FRI example applications. They send commands to ``/lbr_command`` and read states from ``/lbr_state``.
+
+.. note::
+    A real robot is required to run these demos. Make sure you followed :ref:`Robot Setup` first.
+
+.. toctree::
+    :titlesonly:
+
+    LBR FRI ROS 2 C++ Demos <../lbr_fri_ros2_cpp_demos/doc/lbr_fri_ros2_cpp_demos.rst>
+    LBR FRI ROS 2 Python Demos <../lbr_fri_ros2_python_demos/doc/lbr_fri_ros2_python_demos.rst>
+    LBR FRI ROS 2 Advanced Python Demos <../lbr_fri_ros2_advanced_python_demos/doc/lbr_fri_ros2_advanced_python_demos.rst>
+
+LBR ROS 2 Control Demos
+-----------------------
+These demos demonstrate the LBR integration into the ROS 2 ecosystem through ``ros2_control``.
+
+.. note::
+    These demos run in simulation **and** on the real robot. For the real robot, make sure you followed :ref:`Robot Setup` first.
+
+.. toctree::
+    :titlesonly:
+
+    LBR ROS 2 Control C++ Demos <../lbr_ros2_control_cpp_demos/doc/lbr_ros2_control_cpp_demos.rst>
+    LBR ROS 2 Control Python Demos <../lbr_ros2_control_python_demos/doc/lbr_ros2_control_python_demos.rst>

lbr_demos/lbr_fri_ros2_advanced_python_demos/LICENSE

@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.

lbr_demos/lbr_fri_ros2_advanced_python_demos/doc/lbr_fri_ros2_advanced_python_demos.rst

@@ -0,0 +1,29 @@
+LBR FRI ROS 2 Advanced Python Demos
+===================================
+Collection of advanced usage examples for the ``lbr_fri_ros2`` package through Python.
+
+.. warning::
+    Do always execute in ``T1`` mode first.
+
+Admittance Controller
+---------------------
+.. warning::
+    Not well behaved around singularities, put the robot in a well-behaved configuration first, e.g. ``A1 = 0°``, ``A2 = -30°``, ``A3 = 0°``, ``A4 = 60°``, ``A5 = 0°``, ``A6 = -90°``, ``A7 = 0°``. This can be done using the ``smartPAD`` in ``T1`` mode.
+
+1. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
+
+    .. thumbnail:: ../../doc/img/applications_lbr_server.png
+
+2. Launch the robot driver:
+
+.. code-block:: bash
+
+    ros2 launch lbr_fri_ros2 lbr_app.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
+
+3. Run the `admittance_control_node <https://github.com/KCL-BMEIS/lbr_fri_ros2_stack/blob/galactic/lbr_demos/lbr_fri_ros2_advanced_python_demos/lbr_fri_ros2_advanced_python_demos/admittance_control_node.py>`_:
+
+.. code-block:: bash
+
+    ros2 run lbr_fri_ros2_advanced_python_demos admittance_control_node
+
+4. Now gently move the robot at the end-effector.

lbr_demos/scripts/admittance_control_node.py → lbr_demos/lbr_fri_ros2_advanced_python_demos/lbr_fri_ros2_advanced_python_demos/admittance_control_node.py

@@ -1,12 +1,11 @@
-#!/usr/bin/python3
 import os
 
 import kinpy
 import numpy as np
 import rclpy
 import xacro
 from ament_index_python import get_package_share_directory
-from rclpy import qos
+from rclpy.qos import qos_profile_sensor_data
 from rclpy.node import Node
 
 from lbr_fri_msgs.msg import LBRCommand, LBRState
@@ -92,10 +91,10 @@ def __init__(self, node_name="admittance_control_node") -> None:
 
         # publishers and subscribers
         self.lbr_state_sub_ = self.create_subscription(
-            LBRState, "/lbr_state", self.lbr_state_cb_, qos.qos_profile_system_default
+            LBRState, "/lbr_state", self.lbr_state_cb_, qos_profile_sensor_data
         )
         self.lbr_command_pub_ = self.create_publisher(
-            LBRCommand, "/lbr_command", qos.qos_profile_system_default
+            LBRCommand, "/lbr_command", qos_profile_sensor_data
         )
         self.lbr_command_timer_ = self.create_timer(self.dt_, self.timer_cb_)
 
@@ -145,7 +144,3 @@ def main(args=None):
     admittance_control_node = AdmittanceControlNode()
     rclpy.spin(admittance_control_node)
     rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

lbr_demos/lbr_fri_ros2_advanced_python_demos/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lbr_fri_ros2_advanced_python_demos</name>
+  <version>1.0.0</version>
+  <description>Advanced Python demos for the lbr_fri_ros2.</description>
+  <maintainer email="[email protected]">mhubii</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>lbr_fri_msgs</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

lbr_demos/lbr_fri_ros2_advanced_python_demos/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/lbr_fri_ros2_advanced_python_demos
+[install]
+install_scripts=$base/lib/lbr_fri_ros2_advanced_python_demos

lbr_demos/lbr_fri_ros2_advanced_python_demos/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+
+package_name = "lbr_fri_ros2_advanced_python_demos"
+
+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"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="mhubii",
+    maintainer_email="[email protected]",
+    description="Advanced Python demos for the lbr_fri_ros2.",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "admittance_control_node = lbr_fri_ros2_advanced_python_demos.admittance_control_node:main",
+        ],
+    },
+)

lbr_demos/lbr_fri_ros2_advanced_python_demos/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'

lbr_demos/lbr_fri_ros2_advanced_python_demos/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)

lbr_demos/lbr_fri_ros2_advanced_python_demos/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'

lbr_demos/lbr_fri_ros2_cpp_demos/CMakeLists.txt

@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.8)
+project(lbr_fri_ros2_cpp_demos)
+
+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)
+find_package(fri REQUIRED)
+find_package(lbr_fri_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+
+# joint sine overlay
+add_executable(joint_sine_overlay_node
+  src/joint_sine_overlay_node.cpp
+)
+
+ament_target_dependencies(joint_sine_overlay_node
+  fri  
+  lbr_fri_msgs
+  rclcpp
+)
+
+# torque sine overlay
+add_executable(torque_sine_overlay_node
+  src/torque_sine_overlay_node.cpp
+)
+
+ament_target_dependencies(torque_sine_overlay_node
+  fri  
+  lbr_fri_msgs
+  rclcpp
+)
+
+# wrench sine overlay
+add_executable(wrench_sine_overlay_node
+  src/wrench_sine_overlay_node.cpp
+)
+
+ament_target_dependencies(wrench_sine_overlay_node
+  fri  
+  lbr_fri_msgs
+  rclcpp
+)
+
+install(TARGETS
+  joint_sine_overlay_node
+  torque_sine_overlay_node
+  wrench_sine_overlay_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()