Add test node package (#72)

destogl · bmagyar · web-flow · commit 9f09c40e78bb · 2021-04-17T22:19:31.000+02:00
* Added test-node package

Co-authored-by: Bence Magyar &lt;bence.magyar.robotics@gmail.com&gt;
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -41,6 +41,7 @@ jobs:
             ros2_control_demo_hardware 
             ros2_control_demo_robot
             ros2_control_demos
+            ros2_control_test_nodes
           vcs-repo-file-url: |
             https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_control_demos/ros2_control_demos.repos
           colcon-mixin-name: coverage-gcc
diff --git a/.github/workflows/linters.yaml b/.github/workflows/linters.yaml
@@ -21,3 +21,4 @@ jobs:
           ros2_control_demo_hardware 
           ros2_control_demo_robot
           ros2_control_demos
+          ros2_control_test_nodes
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -0,0 +1,142 @@
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v3.4.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-ast
+      - id: check-case-conflict
+      - id: check-docstring-first
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-xml
+      - id: check-yaml
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+      - id: check-byte-order-marker  # Forbid UTF-8 byte-order markers
+
+  # Python hooks
+  - repo: https://github.com/asottile/pyupgrade
+    rev: v2.12.0
+    hooks:
+    -   id: pyupgrade
+        args: [--py36-plus]
+
+  - repo: https://github.com/psf/black
+    rev: 20.8b1
+    hooks:
+      - id: black
+        args: ["--line-length=100"]
+
+  # PEP 257
+  - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257
+    rev: v0.3.3
+    hooks:
+    - id: pep257
+      args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
+
+  - repo: https://github.com/pycqa/flake8
+    rev: 3.9.0
+    hooks:
+    - id: flake8
+      args: ["--ignore=E501"]
+
+  # CPP hooks
+  # The same options as in ament_cppcheck are used, but its not working...
+  #- repo: https://github.com/pocc/pre-commit-hooks
+    #rev: v1.1.1
+    #hooks:
+      #- id: cppcheck
+        #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose']
+
+  - repo: local
+    hooks:
+      - id: ament_cppcheck
+        name: ament_cppcheck
+        description: Static code analysis of C/C++ files.
+        stages: [commit]
+        entry: ament_cppcheck
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+
+  # Maybe use https://github.com/cpplint/cpplint instead
+  - repo: local
+    hooks:
+      - id: ament_cpplint
+        name: ament_cpplint
+        description: Static code analysis of C/C++ files.
+        stages: [commit]
+        entry: ament_cpplint
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+        args: ["--linelength=120"]
+
+  - repo: local
+    hooks:
+      - id: clang-format
+        name: clang-format
+        description: Format files with ClangFormat.
+        entry: clang-format-10
+        language: system
+        files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
+        args: ['-fallback-style=none', '-i']
+
+  # Cmake hooks
+  - repo: local
+    hooks:
+      - id: ament_lint_cmake
+        name: ament_lint_cmake
+        description: Check format of CMakeLists.txt files.
+        stages: [commit]
+        entry: ament_lint_cmake
+        language: system
+        files: CMakeLists.txt$
+
+  # Copyright
+  - repo: local
+    hooks:
+      - id: ament_copyright
+        name: ament_copyright
+        description: Check if copyright notice is available in all files.
+        stages: [commit]
+        entry: ament_copyright
+        language: system
+
+  # Docs - RestructuredText hooks
+  - repo: https://github.com/PyCQA/doc8
+    rev: 0.9.0a1
+    hooks:
+      - id: doc8
+        args: ['--max-line-length=100', '--ignore=D001']
+
+  - repo: https://github.com/pre-commit/pygrep-hooks
+    rev: v1.8.0
+    hooks:
+      - id: rst-backticks
+      - id: rst-directive-colons
+      - id: rst-inline-touching-normal
+
+  # Spellcheck in comments and docs
+  # skipping of *.svg files is not working...
+  - repo: https://github.com/codespell-project/codespell
+    rev: v2.0.0
+    hooks:
+      - id: codespell
+        args: ['--write-changes']
+        exclude: \.(svg|pyc)$
diff --git a/README.md b/README.md
@@ -116,6 +116,7 @@ Direct joint commands are sent to this robot via the `ForwardCommandController`.
 The sections below describe their usage.
 Check the [Results](##result) section on how to ensure that things went well.
 
+
 ### JointStateController
 
 Open another terminal and load, configure and start `joint_state_controller`:
@@ -174,6 +175,10 @@ Now you should also see the *RRbot* represented correctly in `rviz2`.
    - 0.5
    - 0.5"
    ```
+   b. Or you can start demo node which sends two goals every 5 seconds in a loop:
+   ```
+   ros2 launch ros2_control_test_nodes rrbot_test_forward_position_controller.launch.py
+   ```
 
 ## Result
 
diff --git a/ros2_control_demo_robot/config/rrbot_forward_position_publisher.yaml b/ros2_control_demo_robot/config/rrbot_forward_position_publisher.yaml
@@ -0,0 +1,11 @@
+publisher_forward_position_controller:
+  ros__parameters:
+
+    controller_name: "forward_position_controller"
+    wait_sec_between_publish: 5
+
+    goal_names: ["pos1", "pos2", "pos3", "pos4"]
+    pos1: [0.785, 0.785]
+    pos2: [0, 0]
+    pos3: [-0.785, -0.785]
+    pos4: [0, 0]
diff --git a/ros2_control_demo_robot/launch/test_forward_position_controller.launch.py b/ros2_control_demo_robot/launch/test_forward_position_controller.launch.py
@@ -0,0 +1,42 @@
+# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt)
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    position_goals = os.path.join(
+        get_package_share_directory('ros2_control_test_nodes'),
+        'configs',
+        'rrbot_forward_position_publisher.yaml'
+        )
+
+    return LaunchDescription([
+      Node(
+        package='ros2_control_test_nodes',
+        executable='publisher_forward_position_controller',
+        parameters=[position_goals],
+        output={
+          'stdout': 'screen',
+          'stderr': 'screen',
+          },
+        )
+
+    ])
diff --git a/ros2_control_demo_robot/package.xml b/ros2_control_demo_robot/package.xml
@@ -14,6 +14,7 @@
   <exec_depend>joint_state_publisher_gui</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>ros2_control_demo_hardware</exec_depend>
+  <exec_depend>ros2_control_test_nodes</exec_depend>
   <exec_depend>rviz2</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml
@@ -13,6 +13,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <exec_depend>ros2_control_demo_hardware</exec_depend>
+  <exec_depend>ros2_control_test_nodes</exec_depend>
   <exec_depend>ros2_control_demo_robot</exec_depend>
 
   <export>
diff --git a/ros2_control_test_nodes/package.xml b/ros2_control_test_nodes/package.xml
@@ -0,0 +1,25 @@
+<?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>ros2_control_test_nodes</name>
+  <version>0.0.0</version>
+  <description>Demo nodes for showing and testing functionalities of the ros2_control framework.</description>
+
+  <maintainer email="denis@stogl.de">Denis Štogl</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>trajectory_msgs</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>
+
diff --git a/ros2_control_test_nodes/resource/ros2_control_test_nodes b/ros2_control_test_nodes/resource/ros2_control_test_nodes
@@ -0,0 +1 @@
+
diff --git a/ros2_control_test_nodes/ros2_control_test_nodes/__init__.py b/ros2_control_test_nodes/ros2_control_test_nodes/__init__.py
@@ -0,0 +1,13 @@
+# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt)
+#
+# 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/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py b/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py
@@ -0,0 +1,79 @@
+# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt)
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rclpy
+from rclpy.node import Node
+
+from std_msgs.msg import Float64MultiArray
+
+
+class PublisherForwardPosition(Node):
+
+    def __init__(self):
+        super().__init__('publisher_forward_position_controller')
+        # Declare all parameters
+        self.declare_parameter('controller_name', 'forward_position_controller')
+        self.declare_parameter('wait_sec_between_publish', 5)
+        self.declare_parameter('goal_names', ['pos1', 'pos2'])
+
+        # Read parameters
+        controller_name = self.get_parameter('controller_name').value
+        wait_sec_between_publish = self.get_parameter('wait_sec_between_publish').value
+        goal_names = self.get_parameter('goal_names').value
+
+        # Read all positions from parameters
+        self.goals = []
+        for name in goal_names:
+            self.declare_parameter(name)
+            goal = self.get_parameter(name).value
+            if goal is None or len(goal) == 0:
+                raise Exception(f'Values for goal "{name}" not set!')
+
+            float_goal = []
+            for value in goal:
+                float_goal.append(float(value))
+            self.goals.append(float_goal)
+
+        publish_topic = '/' + controller_name + '/' + 'commands'
+
+        self.get_logger().info(
+            f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\
+              every {wait_sec_between_publish} s'
+        )
+
+        self.publisher_ = self.create_publisher(Float64MultiArray, publish_topic, 1)
+        self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback)
+        self.i = 0
+
+    def timer_callback(self):
+        msg = Float64MultiArray()
+        msg.data = self.goals[self.i]
+        self.get_logger().info(f'Publishing: "{msg.data}"')
+        self.publisher_.publish(msg)
+        self.i += 1
+        self.i %= len(self.goals)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    publisher_forward_position = PublisherForwardPosition()
+
+    rclpy.spin(publisher_forward_position)
+    publisher_forward_position.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/ros2_control_test_nodes/setup.cfg b/ros2_control_test_nodes/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/ros2_control_test_nodes
+[install]
+install-scripts=$base/lib/ros2_control_test_nodes
diff --git a/ros2_control_test_nodes/setup.py b/ros2_control_test_nodes/setup.py
