Merge pull request #121 from livanov93/port-master-to-humble

Port master to humble

Author: ahcorde

.github/dependabot.yml

@@ -0,0 +1,13 @@
+# To get started with Dependabot version updates, you'll need to specify which
+# package ecosystems to update and where the package manifests are located.
+# Please see the documentation for all configuration options:
+# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates
+
+version: 2
+updates:
+  - package-ecosystem: "github-actions"
+    # Workflow files stored in the
+    # default location of `.github/workflows`
+    directory: "/"
+    schedule:
+      interval: "weekly"

.github/workflows/ci-format.yaml

@@ -0,0 +1,23 @@
+# This is a format job. Pre-commit has a first-party GitHub action, so we use
+# that: https://github.com/pre-commit/action
+
+name: Format
+
+on:
+  workflow_dispatch:
+  pull_request:
+
+jobs:
+  pre-commit:
+    name: Format
+    runs-on: ubuntu-22.04
+    steps:
+    - uses: actions/checkout@v3
+    - uses: actions/setup-python@v4
+      with:
+        python-version: 3.10.6
+    - name: Install system hooks
+      run: sudo apt install -qq cppcheck ament-cmake-uncrustify ament-cmake-pep257
+    - uses: pre-commit/[email protected]
+      with:
+        extra_args: --all-files --hook-stage manual

.github/workflows/triage.yml

@@ -16,4 +16,3 @@ jobs:
           COLUMN: Inbox
           GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }}
           CHECK_ORG_PROJECT: true
-

.pre-commit-config.yaml

@@ -0,0 +1,136 @@
+# 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: v4.1.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
+        exclude_types: [rst]
+      - id: fix-byte-order-marker
+
+
+  # Python hooks
+  - repo: https://github.com/asottile/pyupgrade
+    rev: v2.31.1
+    hooks:
+    -   id: pyupgrade
+        args: [--py36-plus]
+
+  # # PEP 257
+  - repo: local
+    hooks:
+    - id: ament_pep257
+      name: ament_pep257
+      description: Format files with pep257.
+      entry: ament_pep257
+      language: system
+      args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
+
+  - repo: https://github.com/pycqa/flake8
+    rev: 4.0.1
+    hooks:
+    - id: flake8
+      args: ["--extend-ignore=E501"]
+
+  # CPP hooks
+  - repo: local
+    hooks:
+      - id: ament_uncrustify
+        name: ament_uncrustify
+        description: Format files with uncrustify.
+        entry: ament_uncrustify
+        language: system
+        files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
+        args: ["--reformat"]
+
+  - 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=100", "--filter=-whitespace/newline"]
+
+  # 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.10.1
+    hooks:
+      - id: doc8
+        args: ['--max-line-length=100', '--ignore=D001']
+        exclude: CHANGELOG\.rst$
+
+  - repo: https://github.com/pre-commit/pygrep-hooks
+    rev: v1.9.0
+    hooks:
+      - id: rst-backticks
+        exclude: CHANGELOG\.rst$
+      - 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.1.0
+    hooks:
+      - id: codespell
+        args: ['--write-changes']
+        exclude: CHANGELOG\.rst|\.(svg|pyc)$

Dockerfile/Dockerfile

@@ -1,7 +1,8 @@
-FROM ubuntu:20.04
+FROM ubuntu:22.04
 
 ENV DEBIAN_FRONTEND noninteractive
 ENV IGNITION_VERSION fortress
+ENV ROS_DISTRO humble
 
 # Make sure everything is up to date before building from source
 RUN apt-get update \
@@ -30,10 +31,10 @@ RUN mkdir -p /home/ros2_ws/src \
     && cd /home/ros2_ws/src \
     && git clone https://github.com/ros-controls/gz_ros2_control/ \
     && rosdep init && rosdep update \
-    && rosdep install --from-paths ./ -i -y --rosdistro rolling
+    && rosdep install --from-paths ./ -i -y --rosdistro ${ROS_DISTRO}
 
 RUN cd /home/ros2_ws/ \
-  && . /opt/ros/rolling/setup.sh \
+  && . /opt/ros/"${ROS_DISTRO}"/setup.sh \
   && colcon build --merge-install
 
 COPY entrypoint.sh /entrypoint.sh

README.md

@@ -1,19 +1,27 @@
 # ign_ros2_control
 
+ROS2 Distro | Build Status | Package build |
+:---------: | :----: | :----------: |
+[![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) |  [![Build Status](http://build.ros2.org/buildStatus/icon?job=Hdev__gz_ros2_control__ubuntu_focal_amd64)](http://build.ros2.org/job/Hdev__gz_ros2_control__ubuntu_focal_amd64) |  [![Build Status](http://build.ros2.org/buildStatus/icon?job=Hbin_uF64__gz_ros2_control__ubuntu_focal_amd64__binary)](http://build.ros2.org/job/Hbin_uF64__gz_ros2_control__ubuntu_focal_amd64__binary) |
+
 This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Ignition Gazebo](http://ignitionrobotics.org/) simulator.
 More information about `ros2_control` can be found here: https://control.ros.org/
 
 This package provides an Ignition Gazebo system plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model.
 
-Tested on:
+[![Build Status](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci.yaml/badge.svg?branch=galactic)](https://github.com/ros-controls/gz_ros2_control/actions/workflows/ci.yaml)
 
- - Debs:
-   - [Ignition Edifice](https://ignitionrobotics.org/docs/edifice) + [ROS 2 Galactic](https://docs.ros.org/en/galactic/Installation.html)
- - From source:
-   - [Ignition Citadel](https://ignitionrobotics.org/docs/citadel) + [ROS 2 Galactic](https://docs.ros.org/en/galactic/Installation.html)
-   - [Ignition Fortress](https://ignitionrobotics.org/docs/fortress) + [ROS 2 Galactic](https://docs.ros.org/en/galactic/Installation.html)
+ROS version | Gazebo version | Branch | Binaries hosted at
+-- | -- | -- | --
+Foxy | Citadel | [foxy](https://github.com/ros-controls/gz_ros2_control/tree/foxy) | https://packages.ros.org
+Foxy | Edifice | [foxy](https://github.com/ros-controls/gz_ros2_control/tree/foxy) | only from source
+Galactic | Edifice | [galactic](https://github.com/ros-controls/gz_ros2_control/tree/galactic) | https://packages.ros.org
+Galactic | Fortress | [galactic](https://github.com/ros-controls/gz_ros2_control/tree/galactic) | only from source
+Humble | Fortress | [ros2](https://github.com/ros-controls/gz_ros2_control/tree/master) | https://packages.ros.org
+Rolling | Edifice | [ros2](https://github.com/ros-controls/gz_ros2_control/tree/master) | only from source
+Rolling | Fortress | [ros2](https://github.com/ros-controls/gz_ros2_control/tree/master) | https://packages.ros.org
+Rolling | Garden (not released) | [ros2](https://github.com/ros-controls/gz_ros2_control/tree/master) | only from source
 
-If you want to run this with `ROS 2 Foxy` please check the branch `foxy`.
 
 # Compile from source
 
@@ -118,6 +126,38 @@ include:
 </ros2_control>
 ```
 
+
+### Using mimic joints in simulation
+
+To use `mimic` joints in `ign_ros2_control` you should define its parameters to your URDF.
+We should include:
+
+- `<mimic>` tag to the mimicked joint ([detailed manual(https://wiki.ros.org/urdf/XML/joint))
+- `mimic` and `multiplier` parameters to joint definition in `<ros2_control>` tag
+
+```xml
+<joint name="left_finger_joint" type="prismatic">
+  <mimic joint="right_finger_joint"/>
+  <axis xyz="0 1 0"/>
+  <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
+  <parent link="base"/>
+  <child link="finger_left"/>
+  <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+</joint>
+```
+
+```xml
+<joint name="left_finger_joint">
+  <param name="mimic">right_finger_joint</param>
+  <param name="multiplier">1</param>
+  <command_interface name="position"/>
+  <state_interface name="position"/>
+  <state_interface name="velocity"/>
+  <state_interface name="effort"/>
+</joint>
+```
+
+
 ## Add the ign_ros2_control plugin
 
 In addition to the `ros2_control` tags, a Gazebo plugin needs to be added to your URDF that
@@ -128,7 +168,7 @@ robot hardware interfaces between `ros2_control` and Gazebo.
 
 ```xml
 <gazebo>
-    <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control">
+    <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
       <robot_param>robot_description</robot_param>
       <robot_param_node>robot_state_publisher</robot_param_node>
       <parameters>$(find ign_ros2_control_demos)/config/cartpole_controller.yaml</parameters>
@@ -167,7 +207,7 @@ robot model is loaded. For example, the following XML will load the default plug
   ...
 <ros2_control>
 <gazebo>
-  <plugin name="ign_ros2_control" filename="libign_ros2_control-system.so">
+  <plugin name="ign_ros2_control::IgnitionROS2ControlPlugin" filename="libign_ros2_control-system.so">
     ...
   </plugin>
 </gazebo>
@@ -180,7 +220,7 @@ and use the tag `<controller_manager_prefix_node_name>` to set the controller ma
 
 ```xml
 <gazebo>
-  <plugin name="ign_ros2_control" filename="libign_ros2_control-system.so">
+  <plugin name="ign_ros2_control::IgnitionROS2ControlPlugin" filename="libign_ros2_control-system.so">
     <parameters>$(find ign_ros2_control_demos)/config/cartpole_controller.yaml</parameters>
     <controller_manager_prefix_node_name>controller_manager</controller_manager_prefix_node_name>
   </plugin>
@@ -233,3 +273,18 @@ ros2 run ign_ros2_control_demos example_effort
 ros2 run ign_ros2_control_demos example_diff_drive
 ros2 run ign_ros2_control_demos example_tricycle_drive
 ```
+
+The following example shows parallel gripper with mimic joint:
+
+![](img/gripper.gif)
+
+
+```bash
+ros2 launch ign_ros2_control_demos gripper_mimic_joint_example.launch.py
+```
+
+Send example commands:
+
+```bash
+ros2 run ign_ros2_control_demos example_gripper
+```

ign_ros2_control/CHANGELOG.rst

@@ -13,7 +13,7 @@ Changelog for package ign_ros2_control
 * Fix ignition version in package.xml - Rolling (`#41 <https://github.com/ros-controls/gz_ros2_control/issues/41>`_)
 * Fixed position control (`#29 <https://github.com/ros-controls/gz_ros2_control/issues/29>`_)
 * Add support for initial_values for hardware interfaces when starting simulation. (`#27 <https://github.com/ros-controls/gz_ros2_control/issues/27>`_)
-* Contributors: Alejandro Hernández Cordero, Bence Magyar, Denis Štogl, Guillaume Beuzeboc
+* Contributors: Alejandro Hernández Cordero, Denis Štogl, Guillaume Beuzeboc, Tianyu Li
 
 0.4.0 (2022-03-18)
 ------------------

ign_ros2_control/include/ign_ros2_control/ign_system.hpp

@@ -56,6 +56,11 @@ class IgnitionSystem : public IgnitionSystemInterface
   // Documentation Inherited
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
+  // Documentation Inherited
+  hardware_interface::return_type perform_command_mode_switch(
+    const std::vector<std::string> & start_interfaces,
+    const std::vector<std::string> & stop_interfaces) override;
+
   // Documentation Inherited
   hardware_interface::return_type read(
     const rclcpp::Time & time,

ign_ros2_control/src/ign_ros2_control_plugin.cpp

@@ -270,6 +270,18 @@ void IgnitionROS2ControlPlugin::Configure(
     return;
   }
 
+  std::vector<std::string> arguments = {"--ros-args"};
+
+  auto sdfPtr = const_cast<sdf::Element *>(_sdf.get());
+
+  sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters");
+  while (argument_sdf) {
+    std::string argument = argument_sdf->Get<std::string>();
+    arguments.push_back(RCL_PARAM_FILE_FLAG);
+    arguments.push_back(argument);
+    argument_sdf = argument_sdf->GetNextElement("parameters");
+  }
+
   // Get controller manager node name
   std::string controllerManagerNodeName{"controller_manager"};
 
@@ -279,9 +291,6 @@ void IgnitionROS2ControlPlugin::Configure(
     controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName;
   }
 
-  std::vector<std::string> arguments = {"--ros-args", "--params-file", paramFileName};
-  auto sdfPtr = const_cast<sdf::Element *>(_sdf.get());
-
   if (sdfPtr->HasElement("ros")) {
     sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros");
 
@@ -428,6 +437,10 @@ void IgnitionROS2ControlPlugin::Configure(
     std::chrono::duration_cast<std::chrono::nanoseconds>(
       std::chrono::duration<double>(1.0 / static_cast<double>(this->dataPtr->update_rate))));
 
+  // Force setting of use_sime_time parameter
+  this->dataPtr->controller_manager_->set_parameter(
+    rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true)));
+
   this->dataPtr->entity_ = _entity;
 }