diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 069bf6ad..97dc9f8f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -1,4 +1,4 @@ -name: Ignition ros2 control CI +name: Gazebo-Sim ros2 control CI on: pull_request: @@ -8,45 +8,77 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - version: [fortress] + include: + - docker-image: "ubuntu:22.04" + gz-version: "fortress" + ros-distro: "humble" + - docker-image: "ubuntu:22.04" + gz-version: "fortress" + ros-distro: "rolling" + - docker-image: "ubuntu:22.04" + gz-version: "garden" + ros-distro: "humble" + - docker-image: "ubuntu:22.04" + gz-version: "garden" + ros-distro: "rolling" env: - IGNITION_VERSION: ${{ matrix.version }} + DOCKER_IMAGE: ${{ matrix.docker-image }} + GZ_VERSION: ${{ matrix.gz-version }} + ROS_DISTRO: ${{ matrix.ros-distro }} container: - image: ubuntu:22.04 + image: ${{ matrix.docker-image }} steps: - uses: actions/checkout@v2 - name: Setup colcon workspace id: configure + shell: bash run: | export DEBIAN_FRONTEND=noninteractive apt update -qq - apt install -qq -y lsb-release wget curl gnupg2 + apt install -qq -y lsb-release wget curl gnupg2 git cd .. mkdir -p /home/ros2_ws/src + if [ "$ROS_DISTRO" == "rolling" ]; then + git clone https://github.com/gazebosim/ros_gz/ + fi + if [ "$ROS_DISTRO" == "humble" ]; then + git clone https://github.com/gazebosim/ros_gz/ -b humble + fi cp -r gz_ros2_control /home/ros2_ws/src/ - sh -c 'echo "deb http://packages.ros.org/ros2-testing/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-testing.list' - curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null + wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null + if [ "$GZ_VERSION" == "garden" ]; then + export GZ_DEPS="libgz-sim7-dev libgz-plugin2-dev" + fi + apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -qq -y \ dirmngr \ python3-colcon-ros \ python3-colcon-common-extensions \ python3-rosdep \ - build-essential + build-essential \ + ${GZ_DEPS} + + if [ "$GZ_VERSION" == "garden" ]; then + export ROSDEP_ARGS="--skip-keys ros_gz_sim --skip-keys gz-plugin2 --skip-keys gz-sim7 --skip-keys gz-transport12 --skip-keys gz-math7 --skip-keys gz-msgs9" + fi cd /home/ros2_ws/src/ rosdep init rosdep update - rosdep install --from-paths ./ -i -y --rosdistro humble --ignore-src + rosdep install --from-paths ./ -i -y --rosdistro ${ROS_DISTRO} --ignore-src ${ROSDEP_ARGS} - name: Build project id: build run: | cd /home/ros2_ws/ - . /opt/ros/humble/local_setup.sh - colcon build --packages-up-to ign_ros2_control_demos + . /opt/ros/${ROS_DISTRO}/local_setup.sh + colcon build --packages-up-to gz_ros2_control_demos - name: Run tests id: test run: | cd /home/ros2_ws/ - . /opt/ros/humble/local_setup.sh - colcon test --event-handlers console_direct+ --packages-select ign_ros2_control ign_ros2_control_demos + . /opt/ros/${ROS_DISTRO}/local_setup.sh + colcon test --event-handlers console_direct+ --packages-select gz_ros2_control gz_ros2_control_demos colcon test-result diff --git a/Dockerfile/Dockerfile b/Dockerfile/Dockerfile index 349d0840..6b14d33f 100644 --- a/Dockerfile/Dockerfile +++ b/Dockerfile/Dockerfile @@ -1,7 +1,12 @@ FROM ubuntu:20.04 ENV DEBIAN_FRONTEND noninteractive +<<<<<<< HEAD ENV IGNITION_VERSION fortress +======= +ENV GZ_VERSION fortress +ENV ROS_DISTRO rolling +>>>>>>> ab810e7 (Renamed ign to gz (#67)) # Make sure everything is up to date before building from source RUN apt-get update \ @@ -39,4 +44,4 @@ RUN cd /home/ros2_ws/ \ COPY entrypoint.sh /entrypoint.sh ENTRYPOINT ["/entrypoint.sh"] -CMD ros2 launch ign_ros2_control_demos cart_example_position.launch.py +CMD ros2 launch gz_ros2_control_demos cart_example_position.launch.py diff --git a/README.md b/README.md index 28f5b990..b6009f89 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,16 @@ -# ign_ros2_control +# gz_ros2_control +<<<<<<< HEAD +======= +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__ign_ros2_control__ubuntu_focal_amd64)](http://build.ros2.org/job/Hdev__ign_ros2_control__ubuntu_focal_amd64) | [![Build Status](http://build.ros2.org/buildStatus/icon?job=Hbin_uF64__ign_ros2_control__ubuntu_focal_amd64__binary)](http://build.ros2.org/job/Hbin_uF64__ign_ros2_control__ubuntu_focal_amd64__binary) | + +>>>>>>> ab810e7 (Renamed ign to gz (#67)) 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. +This package provides a Gazebo-Sim system plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. Tested on: @@ -17,12 +24,11 @@ If you want to run this with `ROS 2 Foxy` please check the branch `foxy`. # Compile from source -If you want compile this from source, you should choose the Ignition version. The default one is `citadel`: +If you want compile this from source, you should choose the Gazebo version. The default one is `garden`: ```bash -export IGNITION_VERSION=citadel -export IGNITION_VERSION=edifice -export IGNITION_VERSION=fortress +export GZ_VERSION=fortress +export GZ_VERSION=garden ``` Then create a workspace, clone the repo and compile it: @@ -40,7 +46,7 @@ colcon build ## Video + Pictures -![](img/ign_ros2_control.gif) +![](img/gz_ros2_control.gif) ![](img/diff_drive.gif) @@ -51,7 +57,7 @@ colcon build ```bash cd Dockerfile -docker build -t ign_ros2_control . +docker build -t gz_ros2_control . ``` ### To run the demo @@ -61,7 +67,7 @@ docker build -t ign_ros2_control . Docker allows us to run the demo without the GUI if configured properly. The following command runs the demo without the GUI: ```bash -docker run -it --rm --name ignition_ros2_control_demo --net host ign_ros2_control ros2 launch ign_ros2_control_demos cart_example_position.launch.py gui:=false +docker run -it --rm --name gz_ros2_control_demo --net host gz_ros2_control ros2 launch gz_ros2_control_demos cart_example_position.launch.py gui:=false ``` Then on your local machine, you can run the Gazebo client: @@ -76,18 +82,18 @@ To run the demo with the GUI, we are going to use [rocker](https://github.com/os images with customized local support injected for things like nvidia support. Rocker also supports user id specific files for cleaner mounting file permissions. You can install this tool with the following [instructions](https://github.com/osrf/rocker/#installation) (make sure you meet all of the [prerequisites](https://github.com/osrf/rocker/#prerequisites)). -The following command will launch Ignition: +The following command will launch Gazebo: ```bash -rocker --x11 --nvidia --name ignition_ros2_control_demo ign_ros2_control:latest +rocker --x11 --nvidia --name gz_ros2_control_demo gz_ros2_control:latest ``` The following commands allow the cart to be moved along the rail: ```bash -docker exec -it ignition_ros2_control_demo bash +docker exec -it gz_ros2_control_demo bash source /home/ros2_ws/install/setup.bash -ros2 run ign_ros2_control_demos example_position +ros2 run gz_ros2_control_demos example_position ``` ## Add ros2_control tag to a URDF @@ -100,9 +106,9 @@ include: - `` tag including the robot controllers: commands and states. ```xml - + - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem @@ -118,31 +124,71 @@ include: ``` +<<<<<<< HEAD ## Add the ign_ros2_control plugin +======= + +### Using mimic joints in simulation + +To use `mimic` joints in `gz_ros2_control` you should define its parameters to your URDF. +We should include: + +- `` tag to the mimicked joint ([detailed manual(https://wiki.ros.org/urdf/XML/joint)) +- `mimic` and `multiplier` parameters to joint definition in `` tag + +```xml + + + + + + + + +``` + +```xml + + right_finger_joint + 1 + + + + + +``` + + +## Add the gz_ros2_control plugin +>>>>>>> ab810e7 (Renamed ign to gz (#67)) In addition to the `ros2_control` tags, a Gazebo plugin needs to be added to your URDF that actually parses the `ros2_control` tags and loads the appropriate hardware interfaces and -controller manager. By default the `ign_ros2_control` plugin is very simple, though it is also +controller manager. By default the `gz_ros2_control` plugin is very simple, though it is also extensible via an additional plugin architecture to allow power users to create their own custom robot hardware interfaces between `ros2_control` and Gazebo. ```xml +<<<<<<< HEAD +======= + +>>>>>>> ab810e7 (Renamed ign to gz (#67)) robot_description robot_state_publisher - $(find ign_ros2_control_demos)/config/cartpole_controller.yaml + $(find gz_ros2_control_demos)/config/cartpole_controller.yaml ``` -The `ign_ros2_control` `` tag also has the following optional child elements: +The `gz_ros2_control` `` tag also has the following optional child elements: - ``: YAML file with the configuration of the controllers -#### Default ign_ros2_control Behavior +#### Default gz_ros2_control Behavior -By default, without a `` tag, `ign_ros2_control` will attempt to get all of the information it needs to interface with a ros2_control-based controller out of the URDF. This is sufficient for most cases, and good for at least getting started. +By default, without a `` tag, `gz_ros2_control` will attempt to get all of the information it needs to interface with a ros2_control-based controller out of the URDF. This is sufficient for most cases, and good for at least getting started. The default behavior provides the following ros2_control interfaces: @@ -150,24 +196,28 @@ The default behavior provides the following ros2_control interfaces: - hardware_interface::EffortJointInterface - hardware_interface::VelocityJointInterface -#### Advanced: custom ign_ros2_control Simulation Plugins +#### Advanced: custom gz_ros2_control Simulation Plugins -The `ign_ros2_control` Gazebo plugin also provides a pluginlib-based interface to implement custom interfaces between Gazebo and `ros2_control` for simulating more complex mechanisms (nonlinear springs, linkages, etc). +The `gz_ros2_control` Gazebo plugin also provides a pluginlib-based interface to implement custom interfaces between Gazebo and `ros2_control` for simulating more complex mechanisms (nonlinear springs, linkages, etc). -These plugins must inherit the `ign_ros2_control::IgnitionSystemInterface`, which implements a simulated `ros2_control` +These plugins must inherit the `gz_ros2_control::GazeboSimSystemInterface`, which implements a simulated `ros2_control` `hardware_interface::SystemInterface`. SystemInterface provides API-level access to read and command joint properties. -The respective IgnitionSystemInterface sub-class is specified in a URDF model and is loaded when the +The respective GazeboSimSystemInterface sub-class is specified in a URDF model and is loaded when the robot model is loaded. For example, the following XML will load the default plugin: ```xml - + - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem ... +<<<<<<< HEAD +======= + +>>>>>>> ab810e7 (Renamed ign to gz (#67)) ... @@ -180,8 +230,13 @@ and use the tag `` to set the controller ma ```xml +<<<<<<< HEAD $(find ign_ros2_control_demos)/config/cartpole_controller.yaml +======= + + $(find gz_ros2_control_demos)/config/cartpole_controller.yaml +>>>>>>> ab810e7 (Renamed ign to gz (#67)) controller_manager @@ -210,16 +265,16 @@ cart_pole_controller: ``` #### Executing the examples -There are some examples in the `ign_ros2_control_demos` package. These examples allow to launch a cart in a 30 meter rail. +There are some examples in the `gz_ros2_control_demos` package. These examples allow to launch a cart in a 30 meter rail. You can run some of the example configurations by running the following commands: ```bash -ros2 launch ign_ros2_control_demos cart_example_position.launch.py -ros2 launch ign_ros2_control_demos cart_example_velocity.launch.py -ros2 launch ign_ros2_control_demos cart_example_effort.launch.py -ros2 launch ign_ros2_control_demos diff_drive_example.launch.py -ros2 launch ign_ros2_control_demos tricycle_drive_example.launch.py +ros2 launch gz_ros2_control_demos cart_example_position.launch.py +ros2 launch gz_ros2_control_demos cart_example_velocity.launch.py +ros2 launch gz_ros2_control_demos cart_example_effort.launch.py +ros2 launch gz_ros2_control_demos diff_drive_example.launch.py +ros2 launch gz_ros2_control_demos tricycle_drive_example.launch.py ``` Send example commands: @@ -227,9 +282,27 @@ Send example commands: When the Gazebo world is launched, you can run some of the following commands to move the cart. ```bash -ros2 run ign_ros2_control_demos example_position -ros2 run ign_ros2_control_demos example_velocity -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 +ros2 run gz_ros2_control_demos example_position +ros2 run gz_ros2_control_demos example_velocity +ros2 run gz_ros2_control_demos example_effort +ros2 run gz_ros2_control_demos example_diff_drive +ros2 run gz_ros2_control_demos example_tricycle_drive +``` +<<<<<<< HEAD +======= + +The following example shows parallel gripper with mimic joint: + +![](img/gripper.gif) + + +```bash +ros2 launch gz_ros2_control_demos gripper_mimic_joint_example.launch.py +``` + +Send example commands: + +```bash +ros2 run gz_ros2_control_demos example_gripper ``` +>>>>>>> ab810e7 (Renamed ign to gz (#67)) diff --git a/ign_ros2_control/CHANGELOG.rst b/gz_ros2_control/CHANGELOG.rst similarity index 100% rename from ign_ros2_control/CHANGELOG.rst rename to gz_ros2_control/CHANGELOG.rst diff --git a/ign_ros2_control/CMakeLists.txt b/gz_ros2_control/CMakeLists.txt similarity index 52% rename from ign_ros2_control/CMakeLists.txt rename to gz_ros2_control/CMakeLists.txt index f11c290f..5439c686 100644 --- a/ign_ros2_control/CMakeLists.txt +++ b/gz_ros2_control/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(ign_ros2_control) +project(gz_ros2_control) # Default to C11 if(NOT CMAKE_C_STANDARD) @@ -24,39 +24,41 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(yaml_cpp_vendor REQUIRED) -if("$ENV{IGNITION_VERSION}" STREQUAL "citadel") - find_package(ignition-gazebo3 REQUIRED) - set(IGN_GAZEBO_VER ${ignition-gazebo3_VERSION_MAJOR}) - message(STATUS "Compiling against Ignition Citadel") +set(GZ_PLUGIN) +set(GZ_SIM) -elseif("$ENV{IGNITION_VERSION}" STREQUAL "edifice") - find_package(ignition-gazebo5 REQUIRED) - set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) - message(STATUS "Compiling against Ignition Edifice") - -elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress") +if("$ENV{GZ_VERSION}" STREQUAL "fortress") find_package(ignition-gazebo6 REQUIRED) - set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) - message(STATUS "Compiling against Ignition Fortress") - + set(GZ_SIM_VER ${ignition-gazebo6_VERSION_MAJOR}) + message(STATUS "Compiling against Gazebo Fortress") + find_package(ignition-plugin1 REQUIRED) + set(GZ_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) + set(GZ_PLUGIN ignition-plugin${GZ_PLUGIN_VER}::register) + set(GZ_SIM ignition-gazebo${GZ_SIM_VER}::core) else() - find_package(ignition-gazebo6 REQUIRED) - set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) - message(STATUS "Compiling against Ignition Fortress") + find_package(gz-sim7 REQUIRED) + set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) + message(STATUS "Compiling against Gazebo Garden") + find_package(gz-plugin2 REQUIRED) + set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) + set(GZ_PLUGIN gz-plugin${GZ_PLUGIN_VER}::register) + set(GZ_SIM gz-sim${GZ_SIM_VER}::core) + add_definitions(-DGZ_HEADERS) endif() -find_package(ignition-plugin1 REQUIRED) -set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) +if("$ENV{ROS_DISTRO}" STREQUAL "rolling") + add_definitions(-DROLLING) +endif() include_directories(include) add_library(${PROJECT_NAME}-system SHARED - src/ign_ros2_control_plugin.cpp + src/gz_ros2_control_plugin.cpp ) target_link_libraries(${PROJECT_NAME}-system - ignition-gazebo${IGN_GAZEBO_VER}::core - ignition-plugin${IGN_PLUGIN_VER}::register + ${GZ_SIM} + ${GZ_PLUGIN} ) ament_target_dependencies(${PROJECT_NAME}-system ament_index_cpp @@ -70,21 +72,21 @@ ament_target_dependencies(${PROJECT_NAME}-system ######### -add_library(ign_hardware_plugins SHARED - src/ign_system.cpp +add_library(gz_hardware_plugins SHARED + src/gz_system.cpp ) -ament_target_dependencies(ign_hardware_plugins +ament_target_dependencies(gz_hardware_plugins rclcpp_lifecycle hardware_interface rclcpp ) -target_link_libraries(ign_hardware_plugins - ignition-gazebo${IGN_GAZEBO_VER}::core +target_link_libraries(gz_hardware_plugins + ${GZ_SIM} ) ## Install install(TARGETS - ign_hardware_plugins + gz_hardware_plugins ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -97,14 +99,14 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME} ign_hardware_plugins) +ament_export_libraries(${PROJECT_NAME} gz_hardware_plugins) # Install directories install(TARGETS ${PROJECT_NAME}-system DESTINATION lib ) -pluginlib_export_plugin_description_file(ign_ros2_control ign_hardware_plugins.xml) +pluginlib_export_plugin_description_file(gz_ros2_control gz_hardware_plugins.xml) # Setup the project ament_package() diff --git a/ign_ros2_control/LICENSE b/gz_ros2_control/LICENSE similarity index 100% rename from ign_ros2_control/LICENSE rename to gz_ros2_control/LICENSE diff --git a/gz_ros2_control/gz_hardware_plugins.xml b/gz_ros2_control/gz_hardware_plugins.xml new file mode 100644 index 00000000..40ae92fe --- /dev/null +++ b/gz_ros2_control/gz_hardware_plugins.xml @@ -0,0 +1,19 @@ + + + + GazeboPositionJoint + + + + + GazeboPositionJoint + + + + diff --git a/gz_ros2_control/include/gz_ros2_control/gz_ros2_control_plugin.hpp b/gz_ros2_control/include/gz_ros2_control/gz_ros2_control_plugin.hpp new file mode 100644 index 00000000..e7c26603 --- /dev/null +++ b/gz_ros2_control/include/gz_ros2_control/gz_ros2_control_plugin.hpp @@ -0,0 +1,68 @@ +// Copyright 2021 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. + +#ifndef GZ_ROS2_CONTROL__GZ_ROS2_CONTROL_PLUGIN_HPP_ +#define GZ_ROS2_CONTROL__GZ_ROS2_CONTROL_PLUGIN_HPP_ + +#include + +#ifdef GZ_HEADERS +#include +namespace sim = gz::sim; +#else +#include +namespace sim = ignition::gazebo; +#endif + +namespace gz_ros2_control +{ +// Forward declarations. +class GazeboSimROS2ControlPluginPrivate; + +class GazeboSimROS2ControlPlugin + : public sim::System, + public sim::ISystemConfigure, + public sim::ISystemPreUpdate, + public sim::ISystemPostUpdate +{ +public: + /// \brief Constructor + GazeboSimROS2ControlPlugin(); + + /// \brief Destructor + ~GazeboSimROS2ControlPlugin() override; + + // Documentation inherited + void Configure( + const sim::Entity & _entity, + const std::shared_ptr & _sdf, + sim::EntityComponentManager & _ecm, + sim::EventManager & _eventMgr) override; + + // Documentation inherited + void PreUpdate( + const sim::UpdateInfo & _info, + sim::EntityComponentManager & _ecm) override; + + void PostUpdate( + const sim::UpdateInfo & _info, + const sim::EntityComponentManager & _ecm) override; + +private: + /// \brief Private data pointer. + std::unique_ptr dataPtr; +}; +} // namespace gz_ros2_control + +#endif // GZ_ROS2_CONTROL__GZ_ROS2_CONTROL_PLUGIN_HPP_ diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp b/gz_ros2_control/include/gz_ros2_control/gz_system.hpp similarity index 81% rename from ign_ros2_control/include/ign_ros2_control/ign_system.hpp rename to gz_ros2_control/include/gz_ros2_control/gz_system.hpp index f766f3ec..8dc81f18 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp +++ b/gz_ros2_control/include/gz_ros2_control/gz_system.hpp @@ -13,29 +13,29 @@ // limitations under the License. -#ifndef IGN_ROS2_CONTROL__IGN_SYSTEM_HPP_ -#define IGN_ROS2_CONTROL__IGN_SYSTEM_HPP_ +#ifndef GZ_ROS2_CONTROL__GZ_SYSTEM_HPP_ +#define GZ_ROS2_CONTROL__GZ_SYSTEM_HPP_ #include #include #include #include -#include "ign_ros2_control/ign_system_interface.hpp" +#include "gz_ros2_control/gz_system_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -namespace ign_ros2_control +namespace gz_ros2_control { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; // Forward declaration -class IgnitionSystemPrivate; +class GazeboSimSystemPrivate; -// These class must inherit `ign_ros2_control::IgnitionSystemInterface` which implements a +// These class must inherit `gz_ros2_control::GazeboSimSystemInterface` which implements a // simulated `ros2_control` `hardware_interface::SystemInterface`. -class IgnitionSystem : public IgnitionSystemInterface +class GazeboSimSystem : public GazeboSimSystemInterface { public: // Documentation Inherited @@ -69,9 +69,9 @@ class IgnitionSystem : public IgnitionSystemInterface // Documentation Inherited bool initSim( rclcpp::Node::SharedPtr & model_nh, - std::map & joints, + std::map & joints, const hardware_interface::HardwareInfo & hardware_info, - ignition::gazebo::EntityComponentManager & _ecm, + sim::EntityComponentManager & _ecm, int & update_rate) override; private: @@ -82,9 +82,9 @@ class IgnitionSystem : public IgnitionSystemInterface const hardware_interface::HardwareInfo & hardware_info); /// \brief Private data class - std::unique_ptr dataPtr; + std::unique_ptr dataPtr; }; -} // namespace ign_ros2_control +} // namespace gz_ros2_control -#endif // IGN_ROS2_CONTROL__IGN_SYSTEM_HPP_ +#endif // GZ_ROS2_CONTROL__GZ_SYSTEM_HPP_ diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system_interface.hpp b/gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp similarity index 87% rename from ign_ros2_control/include/ign_ros2_control/ign_system_interface.hpp rename to gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp index 97e20db8..745864d3 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_system_interface.hpp +++ b/gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp @@ -13,22 +13,28 @@ // limitations under the License. -#ifndef IGN_ROS2_CONTROL__IGN_SYSTEM_INTERFACE_HPP_ -#define IGN_ROS2_CONTROL__IGN_SYSTEM_INTERFACE_HPP_ +#ifndef GZ_ROS2_CONTROL__GZ_SYSTEM_INTERFACE_HPP_ +#define GZ_ROS2_CONTROL__GZ_SYSTEM_INTERFACE_HPP_ #include #include #include #include +#ifdef GZ_HEADERS +#include +namespace sim = gz::sim; +#else #include +namespace sim = ignition::gazebo; +#endif #include #include #include -namespace ign_ros2_control +namespace gz_ros2_control { /// \brief This class allows us to handle flags easily, instead of using strings @@ -71,7 +77,7 @@ class SafeEnum }; // SystemInterface provides API-level access to read and command joint properties. -class IgnitionSystemInterface +class GazeboSimSystemInterface : public hardware_interface::SystemInterface { public: @@ -84,9 +90,9 @@ class IgnitionSystemInterface /// param[in] update_rate controller update rate virtual bool initSim( rclcpp::Node::SharedPtr & model_nh, - std::map & joints, + std::map & joints, const hardware_interface::HardwareInfo & hardware_info, - ignition::gazebo::EntityComponentManager & _ecm, + sim::EntityComponentManager & _ecm, int & update_rate) = 0; // Methods used to control a joint. @@ -104,6 +110,6 @@ class IgnitionSystemInterface rclcpp::Node::SharedPtr nh_; }; -} // namespace ign_ros2_control +} // namespace gz_ros2_control -#endif // IGN_ROS2_CONTROL__IGN_SYSTEM_INTERFACE_HPP_ +#endif // GZ_ROS2_CONTROL__GZ_SYSTEM_INTERFACE_HPP_ diff --git a/gz_ros2_control/package.xml b/gz_ros2_control/package.xml new file mode 100644 index 00000000..fb75b803 --- /dev/null +++ b/gz_ros2_control/package.xml @@ -0,0 +1,36 @@ + + + gz_ros2_control + 0.5.0 + Gazebo ros2_control package allows to control simulated robots using ros2_control framework. + Alejandro Hernández + Alejandro Hernández + Apache 2 + + ament_cmake + + ament_index_cpp + + gz-sim7 + ignition-gazebo6 + gz-sim7 + + gz-plugin2 + ignition-plugin + gz-plugin2 + pluginlib + rclcpp + yaml_cpp_vendor + rclcpp_lifecycle + hardware_interface + controller_manager + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + + diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp similarity index 76% rename from ign_ros2_control/src/ign_ros2_control_plugin.cpp rename to gz_ros2_control/src/gz_ros2_control_plugin.cpp index 7c78b16b..6a3ac958 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -12,20 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include #include #include #include #include +#ifdef GZ_HEADERS +#include +#include +#include +#include +#include +#include +#include +#else #include #include #include #include #include #include - #include +#endif + #include @@ -37,13 +49,13 @@ #include -#include "ign_ros2_control/ign_ros2_control_plugin.hpp" -#include "ign_ros2_control/ign_system.hpp" +#include "gz_ros2_control/gz_ros2_control_plugin.hpp" +#include "gz_ros2_control/gz_system.hpp" -namespace ign_ros2_control +namespace gz_ros2_control { ////////////////////////////////////////////////// -class IgnitionROS2ControlPluginPrivate +class GazeboSimROS2ControlPluginPrivate { public: /// \brief Get the URDF XML from the parameter server @@ -54,14 +66,14 @@ class IgnitionROS2ControlPluginPrivate /// joints are returned /// \param[in] _entity Entity of the model that the plugin is being /// configured for - /// \param[in] _ecm Ignition Entity Component Manager + /// \param[in] _ecm Gazebo Entity Component Manager /// \return List of entities containing all enabled joints - std::map GetEnabledJoints( - const ignition::gazebo::Entity & _entity, - ignition::gazebo::EntityComponentManager & _ecm) const; + std::map GetEnabledJoints( + const sim::Entity & _entity, + sim::EntityComponentManager & _ecm) const; /// \brief Entity ID for sensor within Gazebo. - ignition::gazebo::Entity entity_; + sim::Entity entity_; /// \brief Node Handles std::shared_ptr node_{nullptr}; @@ -80,7 +92,7 @@ class IgnitionROS2ControlPluginPrivate /// \brief Interface loader std::shared_ptr> + gz_ros2_control::GazeboSimSystemInterface>> robot_hw_sim_loader_{nullptr}; /// \brief Controller manager @@ -100,33 +112,33 @@ class IgnitionROS2ControlPluginPrivate rclcpp::Time((int64_t)0, RCL_ROS_TIME); /// \brief ECM pointer - ignition::gazebo::EntityComponentManager * ecm{nullptr}; + sim::EntityComponentManager * ecm{nullptr}; /// \brief controller update rate int update_rate; }; ////////////////////////////////////////////////// -std::map -IgnitionROS2ControlPluginPrivate::GetEnabledJoints( - const ignition::gazebo::Entity & _entity, - ignition::gazebo::EntityComponentManager & _ecm) const +std::map +GazeboSimROS2ControlPluginPrivate::GetEnabledJoints( + const sim::Entity & _entity, + sim::EntityComponentManager & _ecm) const { - std::map output; + std::map output; std::vector enabledJoints; // Get all available joints - auto jointEntities = _ecm.ChildrenByComponents(_entity, ignition::gazebo::components::Joint()); + auto jointEntities = _ecm.ChildrenByComponents(_entity, sim::components::Joint()); // Iterate over all joints and verify whether they can be enabled or not for (const auto & jointEntity : jointEntities) { - const auto jointName = _ecm.Component( + const auto jointName = _ecm.Component( jointEntity)->Data(); // Make sure the joint type is supported, i.e. it has exactly one // actuated axis - const auto * jointType = _ecm.Component(jointEntity); + const auto * jointType = _ecm.Component(jointEntity); switch (jointType->Data()) { case sdf::JointType::PRISMATIC: case sdf::JointType::REVOLUTE: @@ -140,7 +152,7 @@ IgnitionROS2ControlPluginPrivate::GetEnabledJoints( { RCLCPP_INFO( node_->get_logger(), - "[ign_ros2_control] Fixed joint [%s] (Entity=%lu)] is skipped", + "[gz_ros2_control] Fixed joint [%s] (Entity=%lu)] is skipped", jointName.c_str(), jointEntity); continue; } @@ -151,7 +163,7 @@ IgnitionROS2ControlPluginPrivate::GetEnabledJoints( { RCLCPP_WARN( node_->get_logger(), - "[ign_ros2_control] Joint [%s] (Entity=%lu)] is of unsupported type." + "[gz_ros2_control] Joint [%s] (Entity=%lu)] is of unsupported type." " Only joints with a single axis are supported.", jointName.c_str(), jointEntity); continue; @@ -160,7 +172,7 @@ IgnitionROS2ControlPluginPrivate::GetEnabledJoints( { RCLCPP_WARN( node_->get_logger(), - "[ign_ros2_control] Joint [%s] (Entity=%lu)] is of unknown type", + "[gz_ros2_control] Joint [%s] (Entity=%lu)] is of unknown type", jointName.c_str(), jointEntity); continue; } @@ -172,7 +184,7 @@ IgnitionROS2ControlPluginPrivate::GetEnabledJoints( } ////////////////////////////////////////////////// -std::string IgnitionROS2ControlPluginPrivate::getURDF() const +std::string GazeboSimROS2ControlPluginPrivate::getURDF() const { std::string urdf_string; @@ -215,7 +227,7 @@ std::string IgnitionROS2ControlPluginPrivate::getURDF() const break; } else { RCLCPP_ERROR( - node_->get_logger(), "ign_ros2_control plugin is waiting for model" + node_->get_logger(), "gz_ros2_control plugin is waiting for model" " URDF in parameter [%s] on the ROS param server.", this->robot_description_.c_str()); } @@ -227,13 +239,13 @@ std::string IgnitionROS2ControlPluginPrivate::getURDF() const } ////////////////////////////////////////////////// -IgnitionROS2ControlPlugin::IgnitionROS2ControlPlugin() -: dataPtr(std::make_unique()) +GazeboSimROS2ControlPlugin::GazeboSimROS2ControlPlugin() +: dataPtr(std::make_unique()) { } ////////////////////////////////////////////////// -IgnitionROS2ControlPlugin::~IgnitionROS2ControlPlugin() +GazeboSimROS2ControlPlugin::~GazeboSimROS2ControlPlugin() { // Stop controller manager thread this->dataPtr->stop_ = true; @@ -243,19 +255,19 @@ IgnitionROS2ControlPlugin::~IgnitionROS2ControlPlugin() } ////////////////////////////////////////////////// -void IgnitionROS2ControlPlugin::Configure( - const ignition::gazebo::Entity & _entity, +void GazeboSimROS2ControlPlugin::Configure( + const sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + sim::EntityComponentManager & _ecm, + sim::EventManager &) { // Make sure the controller is attached to a valid model - const auto model = ignition::gazebo::Model(_entity); + const auto model = sim::Model(_entity); if (!model.Valid(_ecm)) { RCLCPP_ERROR( this->dataPtr->node_->get_logger(), - "[Ignition ROS 2 Control] Failed to initialize because [%s] (Entity=%lu)] is not a model." - "Please make sure that Ignition ROS 2 Control is attached to a valid model.", + "[Gazebo ROS 2 Control] Failed to initialize because [%s] (Entity=%lu)] is not a model." + "Please make sure that Gazebo ROS 2 Control is attached to a valid model.", model.Name(_ecm).c_str(), _entity); return; } @@ -266,7 +278,7 @@ void IgnitionROS2ControlPlugin::Configure( if (paramFileName.empty()) { RCLCPP_ERROR( this->dataPtr->node_->get_logger(), - "Ignition ros2 control found an empty parameters file. Failed to initialize."); + "Gazebo ros2 control found an empty parameters file. Failed to initialize."); return; } @@ -318,7 +330,7 @@ void IgnitionROS2ControlPlugin::Configure( if (!rclcpp::ok()) { rclcpp::init(static_cast(argv.size()), argv.data()); - std::string node_name = "ignition_ros_control"; + std::string node_name = "gz_ros_control"; if (!controllerManagerPrefixNodeName.empty()) { node_name = controllerManagerPrefixNodeName + "_" + node_name; } @@ -336,7 +348,7 @@ void IgnitionROS2ControlPlugin::Configure( this->dataPtr->thread_executor_spin_ = std::thread(spin); RCLCPP_DEBUG_STREAM( - this->dataPtr->node_->get_logger(), "[Ignition ROS 2 Control] Setting up controller for [" << + this->dataPtr->node_->get_logger(), "[Gazebo Sim ROS 2 Control] Setting up controller for [" << model.Name(_ecm) << "] (Entity=" << _entity << ")]."); // Get list of enabled joints @@ -347,7 +359,7 @@ void IgnitionROS2ControlPlugin::Configure( if (enabledJoints.size() == 0) { RCLCPP_DEBUG_STREAM( this->dataPtr->node_->get_logger(), - "[Ignition ROS 2 Control] There are no available Joints."); + "[Gazebo ROS 2 Control] There are no available Joints."); return; } @@ -362,7 +374,7 @@ void IgnitionROS2ControlPlugin::Configure( } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM( this->dataPtr->node_->get_logger(), - "Error parsing URDF in ign_ros2_control plugin, plugin not active : " << ex.what()); + "Error parsing URDF in gz_ros2_control plugin, plugin not active : " << ex.what()); return; } @@ -371,9 +383,9 @@ void IgnitionROS2ControlPlugin::Configure( try { this->dataPtr->robot_hw_sim_loader_.reset( - new pluginlib::ClassLoader( - "ign_ros2_control", - "ign_ros2_control::IgnitionSystemInterface")); + new pluginlib::ClassLoader( + "gz_ros2_control", + "gz_ros2_control::GazeboSimSystemInterface")); } catch (pluginlib::LibraryLoadException & ex) { RCLCPP_ERROR( this->dataPtr->node_->get_logger(), "Failed to create robot simulation interface loader: %s ", @@ -382,11 +394,20 @@ void IgnitionROS2ControlPlugin::Configure( } for (unsigned int i = 0; i < control_hardware_info.size(); ++i) { +<<<<<<< HEAD:ign_ros2_control/src/ign_ros2_control_plugin.cpp std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type; auto ignitionSystem = std::unique_ptr( +======= +#ifndef ROLLING + std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type; +#else + std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_plugin_name; +#endif + auto gzSimSystem = std::unique_ptr( +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control/src/gz_ros2_control_plugin.cpp this->dataPtr->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); - if (!ignitionSystem->initSim( + if (!gzSimSystem->initSim( this->dataPtr->node_, enabledJoints, control_hardware_info[i], @@ -398,7 +419,7 @@ void IgnitionROS2ControlPlugin::Configure( return; } - resource_manager_->import_component(std::move(ignitionSystem), control_hardware_info[i]); + resource_manager_->import_component(std::move(gzSimSystem), control_hardware_info[i]); rclcpp_lifecycle::State state( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, @@ -432,9 +453,9 @@ void IgnitionROS2ControlPlugin::Configure( } ////////////////////////////////////////////////// -void IgnitionROS2ControlPlugin::PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & /*_ecm*/) +void GazeboSimROS2ControlPlugin::PreUpdate( + const sim::UpdateInfo & _info, + sim::EntityComponentManager & /*_ecm*/) { static bool warned{false}; if (!warned) { @@ -466,9 +487,9 @@ void IgnitionROS2ControlPlugin::PreUpdate( } ////////////////////////////////////////////////// -void IgnitionROS2ControlPlugin::PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & /*_ecm*/) +void GazeboSimROS2ControlPlugin::PostUpdate( + const sim::UpdateInfo & _info, + const sim::EntityComponentManager & /*_ecm*/) { // Get the simulation time and period rclcpp::Time sim_time_ros(std::chrono::duration_cast( @@ -477,18 +498,33 @@ void IgnitionROS2ControlPlugin::PostUpdate( if (sim_period >= this->dataPtr->control_period_) { this->dataPtr->last_update_sim_time_ros_ = sim_time_ros; - auto ign_controller_manager = - std::dynamic_pointer_cast( + auto gz_controller_manager = + std::dynamic_pointer_cast( this->dataPtr->controller_manager_); this->dataPtr->controller_manager_->read(sim_time_ros, sim_period); this->dataPtr->controller_manager_->update(sim_time_ros, sim_period); } } -} // namespace ign_ros2_control - +} // namespace gz_ros2_control + +#ifdef GZ_HEADERS +GZ_ADD_PLUGIN( + gz_ros2_control::GazeboSimROS2ControlPlugin, + gz::sim::System, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemConfigure, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPreUpdate, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPostUpdate) +GZ_ADD_PLUGIN_ALIAS( + gz_ros2_control::GazeboSimROS2ControlPlugin, + "ign_ros2_control::IgnitionROS2ControlPlugin") +#else IGNITION_ADD_PLUGIN( - ign_ros2_control::IgnitionROS2ControlPlugin, + gz_ros2_control::GazeboSimROS2ControlPlugin, ignition::gazebo::System, - ign_ros2_control::IgnitionROS2ControlPlugin::ISystemConfigure, - ign_ros2_control::IgnitionROS2ControlPlugin::ISystemPreUpdate, - ign_ros2_control::IgnitionROS2ControlPlugin::ISystemPostUpdate) + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemConfigure, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPreUpdate, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPostUpdate) +IGNITION_ADD_PLUGIN_ALIAS( + gz_ros2_control::GazeboSimROS2ControlPlugin, + "ign_ros2_control::IgnitionROS2ControlPlugin") +#endif diff --git a/ign_ros2_control/src/ign_system.cpp b/gz_ros2_control/src/gz_system.cpp similarity index 58% rename from ign_ros2_control/src/ign_system.cpp rename to gz_ros2_control/src/gz_system.cpp index 0566ea36..cbb711e9 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -12,9 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ign_ros2_control/ign_system.hpp" - -#include +#include "gz_ros2_control/gz_system.hpp" #include #include @@ -23,6 +21,28 @@ #include #include +#ifdef GZ_HEADERS +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#define GZ_TRANSPORT_NAMESPACE gz::transport:: +#define GZ_MSGS_NAMESPACE gz::msgs:: +#else +#include + #include #include #include @@ -36,9 +56,10 @@ #include #include #include - #include - +#define GZ_TRANSPORT_NAMESPACE ignition::transport:: +#define GZ_MSGS_NAMESPACE ignition::msgs:: +#endif #include @@ -66,10 +87,10 @@ struct jointData double joint_effort_cmd; /// \brief handles to the joints from within Gazebo - ignition::gazebo::Entity sim_joint; + sim::Entity sim_joint; /// \brief Control method defined in the URDF for each joint. - ign_ros2_control::IgnitionSystemInterface::ControlMethod joint_control_method; + gz_ros2_control::GazeboSimSystemInterface::ControlMethod joint_control_method; }; class ImuData @@ -82,16 +103,16 @@ class ImuData std::string topicName{}; /// \brief handles to the imu from within Gazebo - ignition::gazebo::Entity sim_imu_sensors_ = ignition::gazebo::kNullEntity; + sim::Entity sim_imu_sensors_ = sim::kNullEntity; /// \brief An array per IMU with 4 orientation, 3 angular velocity and 3 linear acceleration std::array imu_sensor_data_; /// \brief callback to get the IMU topic values - void OnIMU(const ignition::msgs::IMU & _msg); + void OnIMU(const GZ_MSGS_NAMESPACE IMU & _msg); }; -void ImuData::OnIMU(const ignition::msgs::IMU & _msg) +void ImuData::OnIMU(const GZ_MSGS_NAMESPACE IMU & _msg) { this->imu_sensor_data_[0] = _msg.orientation().x(); this->imu_sensor_data_[1] = _msg.orientation().y(); @@ -105,12 +126,12 @@ void ImuData::OnIMU(const ignition::msgs::IMU & _msg) this->imu_sensor_data_[9] = _msg.linear_acceleration().z(); } -class ign_ros2_control::IgnitionSystemPrivate +class gz_ros2_control::GazeboSimSystemPrivate { public: - IgnitionSystemPrivate() = default; + GazeboSimSystemPrivate() = default; - ~IgnitionSystemPrivate() = default; + ~GazeboSimSystemPrivate() = default; /// \brief Degrees od freedom. size_t n_dof_; @@ -131,26 +152,41 @@ class ign_ros2_control::IgnitionSystemPrivate /// \brief Entity component manager, ECM shouldn't be accessed outside those /// methods, otherwise the app will crash - ignition::gazebo::EntityComponentManager * ecm; + sim::EntityComponentManager * ecm; /// \brief controller update rate int * update_rate; +<<<<<<< HEAD:ign_ros2_control/src/ign_system.cpp /// \brief Ignition communication node. ignition::transport::Node node; +======= + /// \brief Gazebo communication node. + GZ_TRANSPORT_NAMESPACE Node node; + + /// \brief mapping of mimicked joints to index of joint they mimic + std::vector mimic_joints_; + + /// \brief Gain which converts position error to a velocity command + double position_proportional_gain_; +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control/src/gz_system.cpp }; -namespace ign_ros2_control +namespace gz_ros2_control { +<<<<<<< HEAD:ign_ros2_control/src/ign_system.cpp bool IgnitionSystem::initSim( +======= +bool GazeboSimSystem::initSim( +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control/src/gz_system.cpp rclcpp::Node::SharedPtr & model_nh, - std::map & enableJoints, + std::map & enableJoints, const hardware_interface::HardwareInfo & hardware_info, - ignition::gazebo::EntityComponentManager & _ecm, + sim::EntityComponentManager & _ecm, int & update_rate) { - this->dataPtr = std::make_unique(); + this->dataPtr = std::make_unique(); this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); this->nh_ = model_nh; @@ -171,31 +207,31 @@ bool IgnitionSystem::initSim( for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) { std::string joint_name = this->dataPtr->joints_[j].name = hardware_info.joints[j].name; - ignition::gazebo::Entity simjoint = enableJoints[joint_name]; + sim::Entity simjoint = enableJoints[joint_name]; this->dataPtr->joints_[j].sim_joint = simjoint; // Create joint position component if one doesn't exist if (!_ecm.EntityHasComponentType( simjoint, - ignition::gazebo::components::JointPosition().TypeId())) + sim::components::JointPosition().TypeId())) { - _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointPosition()); + _ecm.CreateComponent(simjoint, sim::components::JointPosition()); } // Create joint velocity component if one doesn't exist if (!_ecm.EntityHasComponentType( simjoint, - ignition::gazebo::components::JointVelocity().TypeId())) + sim::components::JointVelocity().TypeId())) { - _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointVelocity()); + _ecm.CreateComponent(simjoint, sim::components::JointVelocity()); } // Create joint force component if one doesn't exist if (!_ecm.EntityHasComponentType( simjoint, - ignition::gazebo::components::JointForce().TypeId())) + sim::components::JointForce().TypeId())) { - _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointForce()); + _ecm.CreateComponent(simjoint, sim::components::JointForce()); } // Accept this joint and continue configuration @@ -272,8 +308,12 @@ bool IgnitionSystem::initSim( if (!std::isnan(initial_velocity)) { this->dataPtr->joints_[j].joint_velocity_cmd = initial_velocity; } +<<<<<<< HEAD:ign_ros2_control/src/ign_system.cpp } else if (hardware_info.joints[j].command_interfaces[i].name == "effort") { this->dataPtr->joints_[j].joint_control_method |= EFFORT; +======= + } else if (joint_info.command_interfaces[i].name == "effort") { +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control/src/gz_system.cpp RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); this->dataPtr->command_interfaces_.emplace_back( joint_name, @@ -291,7 +331,7 @@ bool IgnitionSystem::initSim( return true; } -void IgnitionSystem::registerSensors( +void GazeboSimSystem::registerSensors( const hardware_interface::HardwareInfo & hardware_info) { // Collect gazebo sensor handles @@ -306,17 +346,17 @@ void IgnitionSystem::registerSensors( // So we have resize only once the structures where the data will be stored, and we can safely // use pointers to the structures - this->dataPtr->ecm->Each( - [&](const ignition::gazebo::Entity & _entity, - const ignition::gazebo::components::Imu *, - const ignition::gazebo::components::Name * _name) -> bool + this->dataPtr->ecm->Each( + [&](const sim::Entity & _entity, + const sim::components::Imu *, + const sim::components::Name * _name) -> bool { auto imuData = std::make_shared(); RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data()); auto sensorTopicComp = this->dataPtr->ecm->Component< - ignition::gazebo::components::SensorTopic>(_entity); + sim::components::SensorTopic>(_entity); if (sensorTopicComp) { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); } @@ -361,7 +401,11 @@ void IgnitionSystem::registerSensors( } CallbackReturn +<<<<<<< HEAD:ign_ros2_control/src/ign_system.cpp IgnitionSystem::on_init(const hardware_interface::HardwareInfo & actuator_info) +======= +GazeboSimSystem::on_init(const hardware_interface::HardwareInfo & actuator_info) +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control/src/gz_system.cpp { RCLCPP_WARN(this->nh_->get_logger(), "On init..."); if (hardware_interface::SystemInterface::on_init(actuator_info) != CallbackReturn::SUCCESS) { @@ -370,7 +414,7 @@ IgnitionSystem::on_init(const hardware_interface::HardwareInfo & actuator_info) return CallbackReturn::SUCCESS; } -CallbackReturn IgnitionSystem::on_configure( +CallbackReturn GazeboSimSystem::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO( @@ -380,48 +424,54 @@ CallbackReturn IgnitionSystem::on_configure( } std::vector -IgnitionSystem::export_state_interfaces() +GazeboSimSystem::export_state_interfaces() { return std::move(this->dataPtr->state_interfaces_); } std::vector -IgnitionSystem::export_command_interfaces() +GazeboSimSystem::export_command_interfaces() { return std::move(this->dataPtr->command_interfaces_); } -CallbackReturn IgnitionSystem::on_activate(const rclcpp_lifecycle::State & previous_state) +CallbackReturn GazeboSimSystem::on_activate(const rclcpp_lifecycle::State & previous_state) { return CallbackReturn::SUCCESS; return hardware_interface::SystemInterface::on_activate(previous_state); } -CallbackReturn IgnitionSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state) +CallbackReturn GazeboSimSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state) { return CallbackReturn::SUCCESS; return hardware_interface::SystemInterface::on_deactivate(previous_state); } +<<<<<<< HEAD:ign_ros2_control/src/ign_system.cpp hardware_interface::return_type IgnitionSystem::read( const rclcpp::Time & time, const rclcpp::Duration & period) +======= +hardware_interface::return_type GazeboSimSystem::read( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control/src/gz_system.cpp { for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { // Get the joint velocity const auto * jointVelocity = - this->dataPtr->ecm->Component( + this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); - // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 + // TODO(ahcorde): Revisit this part gazebosim/ign-physics#124 // Get the joint force // const auto * jointForce = - // _ecm.Component( + // _ecm.Component( // this->dataPtr->sim_joints_[j]); // Get the joint position const auto * jointPositions = - this->dataPtr->ecm->Component( + this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; @@ -432,7 +482,7 @@ hardware_interface::return_type IgnitionSystem::read( for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { if (this->dataPtr->imus_[i]->topicName.empty()) { auto sensorTopicComp = this->dataPtr->ecm->Component< - ignition::gazebo::components::SensorTopic>(this->dataPtr->imus_[i]->sim_imu_sensors_); + sim::components::SensorTopic>(this->dataPtr->imus_[i]->sim_imu_sensors_); if (sensorTopicComp) { this->dataPtr->imus_[i]->topicName = sensorTopicComp->Data(); RCLCPP_INFO_STREAM( @@ -448,23 +498,76 @@ hardware_interface::return_type IgnitionSystem::read( return hardware_interface::return_type::OK; } +<<<<<<< HEAD:ign_ros2_control/src/ign_system.cpp hardware_interface::return_type IgnitionSystem::write( const rclcpp::Time & time, const rclcpp::Duration & period) +======= +hardware_interface::return_type +GazeboSimSystem::perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +{ + for (unsigned int j = 0; j < this->dataPtr->joints_.size(); j++) { + for (const std::string & interface_name : stop_interfaces) { + // Clear joint control method bits corresponding to stop interfaces + if (interface_name == (this->dataPtr->joints_[j].name + "/" + + hardware_interface::HW_IF_POSITION)) + { + this->dataPtr->joints_[j].joint_control_method &= + static_cast(VELOCITY & EFFORT); + } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_VELOCITY)) + { + this->dataPtr->joints_[j].joint_control_method &= + static_cast(POSITION & EFFORT); + } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_EFFORT)) + { + this->dataPtr->joints_[j].joint_control_method &= + static_cast(POSITION & VELOCITY); + } + } + + // Set joint control method bits corresponding to start interfaces + for (const std::string & interface_name : start_interfaces) { + if (interface_name == (this->dataPtr->joints_[j].name + "/" + + hardware_interface::HW_IF_POSITION)) + { + this->dataPtr->joints_[j].joint_control_method |= POSITION; + } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_VELOCITY)) + { + this->dataPtr->joints_[j].joint_control_method |= VELOCITY; + } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_EFFORT)) + { + this->dataPtr->joints_[j].joint_control_method |= EFFORT; + } + } + } + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type GazeboSimSystem::write( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control/src/gz_system.cpp { for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { - if (!this->dataPtr->ecm->Component( + if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointVelocityCmd({0})); + sim::components::JointVelocityCmd({0})); } else { const auto jointVelCmd = - this->dataPtr->ecm->Component( + this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); - *jointVelCmd = ignition::gazebo::components::JointVelocityCmd( + *jointVelCmd = sim::components::JointVelocityCmd( {this->dataPtr->joints_[i].joint_velocity_cmd}); } } @@ -479,39 +582,141 @@ hardware_interface::return_type IgnitionSystem::write( double targetVel = -error; auto vel = - this->dataPtr->ecm->Component( + this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); if (vel == nullptr) { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, +<<<<<<< HEAD:ign_ros2_control/src/ign_system.cpp ignition::gazebo::components::JointVelocityCmd({targetVel})); +======= + sim::components::JointVelocityCmd({target_vel})); +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control/src/gz_system.cpp } else if (!vel->Data().empty()) { vel->Data()[0] = targetVel; } +<<<<<<< HEAD:ign_ros2_control/src/ign_system.cpp } if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { if (!this->dataPtr->ecm->Component( +======= + } else if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { + if (!this->dataPtr->ecm->Component( +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control/src/gz_system.cpp this->dataPtr->joints_[i].sim_joint)) { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointForceCmd({0})); + sim::components::JointForceCmd({0})); } else { const auto jointEffortCmd = - this->dataPtr->ecm->Component( + this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); - *jointEffortCmd = ignition::gazebo::components::JointForceCmd( + *jointEffortCmd = sim::components::JointForceCmd( {this->dataPtr->joints_[i].joint_effort_cmd}); } +<<<<<<< HEAD:ign_ros2_control/src/ign_system.cpp +======= + } else { + // Fallback case is a velocity command of zero + double target_vel = 0.0; + auto vel = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + if (vel == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, + sim::components::JointVelocityCmd({target_vel})); + } else if (!vel->Data().empty()) { + vel->Data()[0] = target_vel; + } else if (!vel->Data().empty()) { + vel->Data()[0] = target_vel; + } + } + } + + // set values of all mimic joints with respect to mimicked joint + for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { + for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { + if (mimic_interface == "position") { + // Get the joint position + double position_mimicked_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; + + double position_mimic_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + + double position_error = + position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier; + + double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate); + + auto vel = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + + if (vel == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + sim::components::JointVelocityCmd({velocity_sp})); + } else if (!vel->Data().empty()) { + vel->Data()[0] = velocity_sp; + } + } + if (mimic_interface == "velocity") { + // get the velocity of mimicked joint + double velocity_mimicked_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; + + if (!this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) + { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + sim::components::JointVelocityCmd({0})); + } else { + const auto jointVelCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointVelCmd = sim::components::JointVelocityCmd( + {mimic_joint.multiplier * velocity_mimicked_joint}); + } + } + if (mimic_interface == "effort") { + // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 + // Get the joint force + // const auto * jointForce = + // _ecm.Component( + // this->dataPtr->sim_joints_[j]); + if (!this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) + { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + sim::components::JointForceCmd({0})); + } else { + const auto jointEffortCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointEffortCmd = sim::components::JointForceCmd( + {mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort}); + } + } +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control/src/gz_system.cpp } } return hardware_interface::return_type::OK; } -} // namespace ign_ros2_control +} // namespace gz_ros2_control #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS( - ign_ros2_control::IgnitionSystem, ign_ros2_control::IgnitionSystemInterface) + gz_ros2_control::GazeboSimSystem, gz_ros2_control::GazeboSimSystemInterface) diff --git a/ign_ros2_control_demos/CHANGELOG.rst b/gz_ros2_control_demos/CHANGELOG.rst similarity index 100% rename from ign_ros2_control_demos/CHANGELOG.rst rename to gz_ros2_control_demos/CHANGELOG.rst diff --git a/ign_ros2_control_demos/CMakeLists.txt b/gz_ros2_control_demos/CMakeLists.txt similarity index 98% rename from ign_ros2_control_demos/CMakeLists.txt rename to gz_ros2_control_demos/CMakeLists.txt index 838a43d8..96e22fd7 100644 --- a/ign_ros2_control_demos/CMakeLists.txt +++ b/gz_ros2_control_demos/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5.0) -project(ign_ros2_control_demos) +project(gz_ros2_control_demos) # Default to C11 if(NOT CMAKE_C_STANDARD) diff --git a/ign_ros2_control_demos/config/cartpole_controller_effort.yaml b/gz_ros2_control_demos/config/cartpole_controller_effort.yaml similarity index 100% rename from ign_ros2_control_demos/config/cartpole_controller_effort.yaml rename to gz_ros2_control_demos/config/cartpole_controller_effort.yaml diff --git a/ign_ros2_control_demos/config/cartpole_controller_position.yaml b/gz_ros2_control_demos/config/cartpole_controller_position.yaml similarity index 93% rename from ign_ros2_control_demos/config/cartpole_controller_position.yaml rename to gz_ros2_control_demos/config/cartpole_controller_position.yaml index f51d2fa5..835a8360 100644 --- a/ign_ros2_control_demos/config/cartpole_controller_position.yaml +++ b/gz_ros2_control_demos/config/cartpole_controller_position.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 1000 # Hz joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController diff --git a/ign_ros2_control_demos/config/cartpole_controller_velocity.yaml b/gz_ros2_control_demos/config/cartpole_controller_velocity.yaml similarity index 100% rename from ign_ros2_control_demos/config/cartpole_controller_velocity.yaml rename to gz_ros2_control_demos/config/cartpole_controller_velocity.yaml diff --git a/ign_ros2_control_demos/config/diff_drive_controller_velocity.yaml b/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml similarity index 100% rename from ign_ros2_control_demos/config/diff_drive_controller_velocity.yaml rename to gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml diff --git a/gz_ros2_control_demos/config/gripper_controller.yaml b/gz_ros2_control_demos/config/gripper_controller.yaml new file mode 100644 index 00000000..d72296e1 --- /dev/null +++ b/gz_ros2_control_demos/config/gripper_controller.yaml @@ -0,0 +1,15 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + gripper_controller: + type: forward_command_controller/ForwardCommandController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +gripper_controller: + ros__parameters: + joints: + - right_finger_joint + interface_name: position diff --git a/ign_ros2_control_demos/config/tricycle_drive_controller.yaml b/gz_ros2_control_demos/config/tricycle_drive_controller.yaml similarity index 100% rename from ign_ros2_control_demos/config/tricycle_drive_controller.yaml rename to gz_ros2_control_demos/config/tricycle_drive_controller.yaml diff --git a/ign_ros2_control_demos/examples/example_diff_drive.cpp b/gz_ros2_control_demos/examples/example_diff_drive.cpp similarity index 100% rename from ign_ros2_control_demos/examples/example_diff_drive.cpp rename to gz_ros2_control_demos/examples/example_diff_drive.cpp diff --git a/ign_ros2_control_demos/examples/example_effort.cpp b/gz_ros2_control_demos/examples/example_effort.cpp similarity index 100% rename from ign_ros2_control_demos/examples/example_effort.cpp rename to gz_ros2_control_demos/examples/example_effort.cpp diff --git a/gz_ros2_control_demos/examples/example_gripper.cpp b/gz_ros2_control_demos/examples/example_gripper.cpp new file mode 100644 index 00000000..449cf6ab --- /dev/null +++ b/gz_ros2_control_demos/examples/example_gripper.cpp @@ -0,0 +1,61 @@ +// Copyright 2022 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. +// +// Author: Denis Štogl (Stogl Robotics Consulting) +// + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/float64_multi_array.hpp" + +std::shared_ptr node; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + node = std::make_shared("gripper_test_node"); + + auto publisher = node->create_publisher( + "/gripper_controller/commands", 10); + + RCLCPP_INFO(node->get_logger(), "node created"); + + std_msgs::msg::Float64MultiArray commands; + + using namespace std::chrono_literals; + + commands.data.push_back(0); + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0.38; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0.19; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + rclcpp::shutdown(); + + return 0; +} diff --git a/ign_ros2_control_demos/examples/example_position.cpp b/gz_ros2_control_demos/examples/example_position.cpp similarity index 100% rename from ign_ros2_control_demos/examples/example_position.cpp rename to gz_ros2_control_demos/examples/example_position.cpp diff --git a/ign_ros2_control_demos/examples/example_tricycle_drive.cpp b/gz_ros2_control_demos/examples/example_tricycle_drive.cpp similarity index 100% rename from ign_ros2_control_demos/examples/example_tricycle_drive.cpp rename to gz_ros2_control_demos/examples/example_tricycle_drive.cpp diff --git a/ign_ros2_control_demos/examples/example_velocity.cpp b/gz_ros2_control_demos/examples/example_velocity.cpp similarity index 100% rename from ign_ros2_control_demos/examples/example_velocity.cpp rename to gz_ros2_control_demos/examples/example_velocity.cpp diff --git a/ign_ros2_control_demos/launch/cart_example_effort.launch.py b/gz_ros2_control_demos/launch/cart_example_effort.launch.py similarity index 77% rename from ign_ros2_control_demos/launch/cart_example_effort.launch.py rename to gz_ros2_control_demos/launch/cart_example_effort.launch.py index 29031401..5e41c50c 100644 --- a/ign_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/gz_ros2_control_demos/launch/cart_example_effort.launch.py @@ -33,10 +33,10 @@ def generate_launch_description(): # Launch Arguments use_sim_time = LaunchConfiguration('use_sim_time', default=True) - ignition_ros2_control_demos_path = os.path.join( - get_package_share_directory('ign_ros2_control_demos')) + gz_ros2_control_demos_path = os.path.join( + get_package_share_directory('gz_ros2_control_demos')) - xacro_file = os.path.join(ignition_ros2_control_demos_path, + xacro_file = os.path.join(gz_ros2_control_demos_path, 'urdf', 'test_cart_effort.xacro.urdf') @@ -51,7 +51,11 @@ def generate_launch_description(): parameters=[params] ) +<<<<<<< HEAD:ign_ros2_control_demos/launch/cart_example_effort.launch.py ignition_spawn_entity = Node( +======= + gz_spawn_entity = Node( +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/launch/cart_example_effort.launch.py package='ros_gz_sim', executable='create', output='screen', @@ -66,7 +70,7 @@ def generate_launch_description(): output='screen' ) - load_joint_trajectory_controller = ExecuteProcess( + load_joint_effort_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'effort_controllers'], output='screen' ) @@ -75,23 +79,28 @@ def generate_launch_description(): # Launch gazebo environment IncludeLaunchDescription( PythonLaunchDescriptionSource( +<<<<<<< HEAD:ign_ros2_control_demos/launch/cart_example_effort.launch.py [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), +======= + [os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', 'gz_sim.launch.py')]), +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/launch/cart_example_effort.launch.py launch_arguments=[('gz_args', [' -r -v 3 empty.sdf'])]), RegisterEventHandler( event_handler=OnProcessExit( - target_action=ignition_spawn_entity, + target_action=gz_spawn_entity, on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( target_action=load_joint_state_broadcaster, - on_exit=[load_joint_trajectory_controller], + on_exit=[load_joint_effort_controller], ) ), node_robot_state_publisher, - ignition_spawn_entity, + gz_spawn_entity, # Launch Arguments DeclareLaunchArgument( 'use_sim_time', diff --git a/ign_ros2_control_demos/launch/cart_example_position.launch.py b/gz_ros2_control_demos/launch/cart_example_position.launch.py similarity index 80% rename from ign_ros2_control_demos/launch/cart_example_position.launch.py rename to gz_ros2_control_demos/launch/cart_example_position.launch.py index c1a8beb4..cdd760ab 100644 --- a/ign_ros2_control_demos/launch/cart_example_position.launch.py +++ b/gz_ros2_control_demos/launch/cart_example_position.launch.py @@ -33,10 +33,10 @@ def generate_launch_description(): # Launch Arguments use_sim_time = LaunchConfiguration('use_sim_time', default=True) - ignition_ros2_control_demos_path = os.path.join( - get_package_share_directory('ign_ros2_control_demos')) + gz_ros2_control_demos_path = os.path.join( + get_package_share_directory('gz_ros2_control_demos')) - xacro_file = os.path.join(ignition_ros2_control_demos_path, + xacro_file = os.path.join(gz_ros2_control_demos_path, 'urdf', 'test_cart_position.xacro.urdf') @@ -51,7 +51,11 @@ def generate_launch_description(): parameters=[params] ) +<<<<<<< HEAD:ign_ros2_control_demos/launch/cart_example_position.launch.py ignition_spawn_entity = Node( +======= + gz_spawn_entity = Node( +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/launch/cart_example_position.launch.py package='ros_gz_sim', executable='create', output='screen', @@ -76,12 +80,17 @@ def generate_launch_description(): # Launch gazebo environment IncludeLaunchDescription( PythonLaunchDescriptionSource( +<<<<<<< HEAD:ign_ros2_control_demos/launch/cart_example_position.launch.py [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), +======= + [os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', 'gz_sim.launch.py')]), +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/launch/cart_example_position.launch.py launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), RegisterEventHandler( event_handler=OnProcessExit( - target_action=ignition_spawn_entity, + target_action=gz_spawn_entity, on_exit=[load_joint_state_broadcaster], ) ), @@ -92,7 +101,7 @@ def generate_launch_description(): ) ), node_robot_state_publisher, - ignition_spawn_entity, + gz_spawn_entity, # Launch Arguments DeclareLaunchArgument( 'use_sim_time', diff --git a/ign_ros2_control_demos/launch/cart_example_velocity.launch.py b/gz_ros2_control_demos/launch/cart_example_velocity.launch.py similarity index 78% rename from ign_ros2_control_demos/launch/cart_example_velocity.launch.py rename to gz_ros2_control_demos/launch/cart_example_velocity.launch.py index 1a72809c..5dfd8c34 100644 --- a/ign_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/gz_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -33,10 +33,10 @@ def generate_launch_description(): # Launch Arguments use_sim_time = LaunchConfiguration('use_sim_time', default=True) - ignition_ros2_control_demos_path = os.path.join( - get_package_share_directory('ign_ros2_control_demos')) + gz_ros2_control_demos_path = os.path.join( + get_package_share_directory('gz_ros2_control_demos')) - xacro_file = os.path.join(ignition_ros2_control_demos_path, + xacro_file = os.path.join(gz_ros2_control_demos_path, 'urdf', 'test_cart_velocity.xacro.urdf') @@ -51,7 +51,11 @@ def generate_launch_description(): parameters=[params] ) +<<<<<<< HEAD:ign_ros2_control_demos/launch/cart_example_velocity.launch.py ignition_spawn_entity = Node( +======= + gz_spawn_entity = Node( +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/launch/cart_example_velocity.launch.py package='ros_gz_sim', executable='create', output='screen', @@ -66,7 +70,7 @@ def generate_launch_description(): output='screen' ) - load_joint_trajectory_controller = ExecuteProcess( + load_joint_velocity_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'velocity_controller'], output='screen' ) @@ -81,29 +85,34 @@ def generate_launch_description(): # Launch gazebo environment IncludeLaunchDescription( PythonLaunchDescriptionSource( +<<<<<<< HEAD:ign_ros2_control_demos/launch/cart_example_velocity.launch.py [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), +======= + [os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', 'gz_sim.launch.py')]), +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/launch/cart_example_velocity.launch.py launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), RegisterEventHandler( event_handler=OnProcessExit( - target_action=ignition_spawn_entity, + target_action=gz_spawn_entity, on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( target_action=load_joint_state_broadcaster, - on_exit=[load_joint_trajectory_controller], + on_exit=[load_joint_velocity_controller], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_trajectory_controller, + target_action=load_joint_velocity_controller, on_exit=[load_imu_sensor_broadcaster], ) ), node_robot_state_publisher, - ignition_spawn_entity, + gz_spawn_entity, # Launch Arguments DeclareLaunchArgument( 'use_sim_time', diff --git a/ign_ros2_control_demos/launch/diff_drive_example.launch.py b/gz_ros2_control_demos/launch/diff_drive_example.launch.py similarity index 80% rename from ign_ros2_control_demos/launch/diff_drive_example.launch.py rename to gz_ros2_control_demos/launch/diff_drive_example.launch.py index 9d9e801f..58217bbb 100644 --- a/ign_ros2_control_demos/launch/diff_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example.launch.py @@ -32,10 +32,10 @@ def generate_launch_description(): # Launch Arguments use_sim_time = LaunchConfiguration('use_sim_time', default=True) - ignition_ros2_control_demos_path = os.path.join( - get_package_share_directory('ign_ros2_control_demos')) + gz_ros2_control_demos_path = os.path.join( + get_package_share_directory('gz_ros2_control_demos')) - xacro_file = os.path.join(ignition_ros2_control_demos_path, + xacro_file = os.path.join(gz_ros2_control_demos_path, 'urdf', 'test_diff_drive.xacro.urdf') @@ -52,7 +52,11 @@ def generate_launch_description(): parameters=[params], ) +<<<<<<< HEAD:ign_ros2_control_demos/launch/diff_drive_example.launch.py ignition_spawn_entity = Node( +======= + gz_spawn_entity = Node( +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/launch/diff_drive_example.launch.py package='ros_gz_sim', executable='create', output='screen', @@ -77,12 +81,17 @@ def generate_launch_description(): # Launch gazebo environment IncludeLaunchDescription( PythonLaunchDescriptionSource( +<<<<<<< HEAD:ign_ros2_control_demos/launch/diff_drive_example.launch.py [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), +======= + [os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', 'gz_sim.launch.py')]), +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/launch/diff_drive_example.launch.py launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), RegisterEventHandler( event_handler=OnProcessExit( - target_action=ignition_spawn_entity, + target_action=gz_spawn_entity, on_exit=[load_joint_state_controller], ) ), @@ -93,7 +102,7 @@ def generate_launch_description(): ) ), node_robot_state_publisher, - ignition_spawn_entity, + gz_spawn_entity, # Launch Arguments DeclareLaunchArgument( 'use_sim_time', diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py b/gz_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py new file mode 100644 index 00000000..ea44bf1c --- /dev/null +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py @@ -0,0 +1,104 @@ +# Copyright 2022 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. +# +# Author: Denis Stogl (Stogl Robotics Consulting) +# + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + # Launch arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=True) + + gz_ros2_control_demos_path = os.path.join( + get_package_share_directory('gz_ros2_control_demos')) + + xacro_file = os.path.join(gz_ros2_control_demos_path, + 'urdf', + 'test_gripper_mimic_joint.xacro.urdf') + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=['-string', doc.toxml(), + '-name', 'gripper', + '-allow_renaming', 'true'], + ) + + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + load_gripper_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'gripper_controller'], + output='screen' + ) + + return LaunchDescription([ + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', 'gz_sim.launch.py')]), + launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gz_spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_gripper_controller], + ) + ), + node_robot_state_publisher, + gz_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/ign_ros2_control_demos/launch/tricycle_drive_example.launch.py b/gz_ros2_control_demos/launch/tricycle_drive_example.launch.py similarity index 81% rename from ign_ros2_control_demos/launch/tricycle_drive_example.launch.py rename to gz_ros2_control_demos/launch/tricycle_drive_example.launch.py index 152523b0..e17d5f61 100644 --- a/ign_ros2_control_demos/launch/tricycle_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/tricycle_drive_example.launch.py @@ -32,10 +32,10 @@ def generate_launch_description(): # Launch Arguments use_sim_time = LaunchConfiguration('use_sim_time', default=True) - ignition_ros2_control_demos_path = os.path.join( - get_package_share_directory('ign_ros2_control_demos')) + gz_ros2_control_demos_path = os.path.join( + get_package_share_directory('gz_ros2_control_demos')) - xacro_file = os.path.join(ignition_ros2_control_demos_path, + xacro_file = os.path.join(gz_ros2_control_demos_path, 'urdf', 'test_tricycle_drive.xacro.urdf') @@ -52,7 +52,11 @@ def generate_launch_description(): parameters=[params], ) +<<<<<<< HEAD:ign_ros2_control_demos/launch/tricycle_drive_example.launch.py ignition_spawn_entity = Node( +======= + gz_spawn_entity = Node( +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/launch/tricycle_drive_example.launch.py package='ros_gz_sim', executable='create', output='screen', @@ -79,10 +83,14 @@ def generate_launch_description(): PythonLaunchDescriptionSource( [os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')]), +<<<<<<< HEAD:ign_ros2_control_demos/launch/tricycle_drive_example.launch.py launch_arguments=[('ign_args', [' -r -v 4 empty.sdf'])]), +======= + launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/launch/tricycle_drive_example.launch.py RegisterEventHandler( event_handler=OnProcessExit( - target_action=ignition_spawn_entity, + target_action=gz_spawn_entity, on_exit=[load_joint_state_controller], ) ), @@ -93,7 +101,7 @@ def generate_launch_description(): ) ), node_robot_state_publisher, - ignition_spawn_entity, + gz_spawn_entity, # Launch Arguments DeclareLaunchArgument( 'use_sim_time', diff --git a/ign_ros2_control_demos/package.xml b/gz_ros2_control_demos/package.xml similarity index 77% rename from ign_ros2_control_demos/package.xml rename to gz_ros2_control_demos/package.xml index 497879cb..2ea9f062 100644 --- a/ign_ros2_control_demos/package.xml +++ b/gz_ros2_control_demos/package.xml @@ -1,8 +1,15 @@ +<<<<<<< HEAD:ign_ros2_control_demos/package.xml ign_ros2_control_demos 0.4.3 ign_ros2_control_demos +======= + + gz_ros2_control_demos + 0.5.0 + gz_ros2_control_demos +>>>>>>> ab810e7 (Renamed ign to gz (#67)):gz_ros2_control_demos/package.xml Alejandro Hernandez @@ -27,7 +34,7 @@ effort_controllers geometry_msgs hardware_interface - ign_ros2_control + gz_ros2_control imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller @@ -35,7 +42,9 @@ launch_ros rclcpp robot_state_publisher - ros_ign_gazebo + ros_gz_sim + ros_gz_sim + ros_ign_gazebo ros2controlcli std_msgs velocity_controllers diff --git a/ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf b/gz_ros2_control_demos/urdf/test_cart_effort.xacro.urdf similarity index 88% rename from ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf rename to gz_ros2_control_demos/urdf/test_cart_effort.xacro.urdf index 3738d08f..e0fa7673 100644 --- a/ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_cart_effort.xacro.urdf @@ -43,9 +43,9 @@ - + - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem @@ -80,8 +80,8 @@ - - $(find ign_ros2_control_demos)/config/cartpole_controller_effort.yaml + + $(find gz_ros2_control_demos)/config/cartpole_controller_effort.yaml diff --git a/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/gz_ros2_control_demos/urdf/test_cart_position.xacro.urdf similarity index 79% rename from ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf rename to gz_ros2_control_demos/urdf/test_cart_position.xacro.urdf index eeff47ec..a7faaac7 100644 --- a/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -12,6 +12,12 @@ + + + + + + @@ -24,6 +30,12 @@ + + + + + + @@ -43,9 +55,9 @@ - + - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem @@ -53,7 +65,7 @@ 15 - -5.0 + 5.0 @@ -82,8 +94,8 @@ - - $(find ign_ros2_control_demos)/config/cartpole_controller_position.yaml + + $(find gz_ros2_control_demos)/config/cartpole_controller_position.yaml diff --git a/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/gz_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf similarity index 89% rename from ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf rename to gz_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index 59a960a1..71b0c93f 100644 --- a/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -4,6 +4,7 @@ + @@ -66,7 +67,11 @@ - + + + + + @@ -111,9 +116,9 @@ - + - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem @@ -160,11 +165,11 @@ - - $(find ign_ros2_control_demos)/config/cartpole_controller_velocity.yaml + + $(find gz_ros2_control_demos)/config/cartpole_controller_velocity.yaml diff --git a/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf similarity index 92% rename from ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf rename to gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf index e608ed9e..7b77b090 100644 --- a/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_diff_drive.xacro.urdf @@ -132,9 +132,9 @@ - + - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem @@ -156,8 +156,8 @@ - - $(find ign_ros2_control_demos)/config/diff_drive_controller_velocity.yaml + + $(find gz_ros2_control_demos)/config/diff_drive_controller_velocity.yaml diff --git a/gz_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf b/gz_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf new file mode 100644 index 00000000..555bd493 --- /dev/null +++ b/gz_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + 0.15 + + + + + + right_finger_joint + 1 + + + + + + + + + + $(find gz_ros2_control_demos)/config/gripper_controller.yaml + + + diff --git a/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf similarity index 93% rename from ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf rename to gz_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf index 7d58323d..c0971c1a 100644 --- a/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf @@ -150,9 +150,9 @@ - + - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem @@ -167,8 +167,8 @@ - - $(find ign_ros2_control_demos)/config/tricycle_drive_controller.yaml + + $(find gz_ros2_control_demos)/config/tricycle_drive_controller.yaml diff --git a/ign_ros2_control/ign_hardware_plugins.xml b/ign_ros2_control/ign_hardware_plugins.xml deleted file mode 100644 index 3d245541..00000000 --- a/ign_ros2_control/ign_hardware_plugins.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - GazeboPositionJoint - - - diff --git a/ign_ros2_control/include/ign_ros2_control/ign_ros2_control_plugin.hpp b/ign_ros2_control/include/ign_ros2_control/ign_ros2_control_plugin.hpp deleted file mode 100644 index c7be9aef..00000000 --- a/ign_ros2_control/include/ign_ros2_control/ign_ros2_control_plugin.hpp +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright 2021 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. - -#ifndef IGN_ROS2_CONTROL__IGN_ROS2_CONTROL_PLUGIN_HPP_ -#define IGN_ROS2_CONTROL__IGN_ROS2_CONTROL_PLUGIN_HPP_ - -#include - -#include - -namespace ign_ros2_control -{ -// Forward declarations. -class IgnitionROS2ControlPluginPrivate; - -class IgnitionROS2ControlPlugin - : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate, - public ignition::gazebo::ISystemPostUpdate -{ -public: - /// \brief Constructor - IgnitionROS2ControlPlugin(); - - /// \brief Destructor - ~IgnitionROS2ControlPlugin() override; - - // Documentation inherited - void Configure( - const ignition::gazebo::Entity & _entity, - const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; - - // Documentation inherited - void PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) override; - - void PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) override; - -private: - /// \brief Private data pointer. - std::unique_ptr dataPtr; -}; -} // namespace ign_ros2_control - -#endif // IGN_ROS2_CONTROL__IGN_ROS2_CONTROL_PLUGIN_HPP_ diff --git a/img/gz_ros2_control.gif b/img/gz_ros2_control.gif new file mode 100644 index 00000000..65664bde Binary files /dev/null and b/img/gz_ros2_control.gif differ