[Example 7] Full example for 6 revolute joint robot (backport #201)

README.md

@@ -32,24 +32,34 @@ The following examples are part of this demo repository:
    *RRBot* with an integrated sensor.
 
 
-* Example 5: ["Industrial Robots with externally connected sensor"](example_5)
+* Example 5: ["Industrial robots with externally connected sensor"](example_5)
 
    *RRBot* with an externally connected sensor.
 
-* Example 6: ["Modular Robots with separate communication to each actuator"](example_6)
+* Example 6: ["Modular robots with separate communication to each actuator"](example_6)
 
    The example shows how to implement robot hardware with separate communication to each actuator.
 
-* Example 7: "Multi-robot example (tba.)"
+* Example 7: ["6-DOF robot"](example_7)
+
+   A full tutorial for a 6 DOF robot for intermediate ROS 2 users.
 
 * Example 8: ["Using transmissions"](example_8)
 
    *RRBot* with an exposed transmission interface.
 
-* Example 9: ["Gazebo Classic Simulation"](example_9)
+* Example 9: ["Gazebo classic simulation"](example_9)
 
    Demonstrates how to switch between simulation and hardware.
 
+* Example 10: "RRbot with GPIO interfaces (tba.)"
+
+* Example 11: "Car-like robot using steering controller library (tba.)"
+
+* Example 12: "Controller chaining example (tba.)"
+
+* Example 13: "Multi-robot example (tba.)"
+
 ## Getting started
 
 The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`.

doc/index.rst

@@ -7,7 +7,7 @@ Demos
 #################
 
 This `GitHub Repository <https://github.com/ros-controls/ros2_control_demos>`_
-provides templates for the development of ros2_control-enabled robots and a simple simulations to demonstrate and prove ros2_control concepts.
+provides templates for the development of ``ros2_control``-enabled robots and simple simulations to demonstrate and prove ``ros2_control`` concepts.
 
 If you want to have a rather step by step manual how to do things with ``ros2_control`` checkout `ros-control/roscon2022_workshop <https://github.com/ros-controls/roscon2022_workshop>`_ repository.
 
@@ -22,6 +22,7 @@ This repository demonstrates the following ``ros2_control`` concepts:
   * Loading the configuration and starting a robot using launch files.
   * Control of a differential mobile base *DiffBot*.
   * Control of two joints of *RRBot*.
+  * Control of a 6-degrees-of-freedom robot.
   * Implementing a controller switching strategy for a robot.
   * Using joint limits and transmission concepts in ``ros2_control``.
 
@@ -35,38 +36,34 @@ The repository has two other goals:
 2. The repository is a validation environment for ``ros2_control`` concepts, which can only be tested during run-time (e.g., execution of controllers by the controller manager, communication between robot hardware and controllers).
 
 =====================
-Example Overview
+Examples Overview
 =====================
 
 Example 1: RRBot
    *RRBot* - or ''Revolute-Revolute Manipulator Robot'' - a simple position controlled robot with one hardware interface. This example also demonstrates the switching between different controllers.
 
-
 Example 2: DiffBot
    *DiffBot*, or ''Differential Mobile Robot'', is a simple mobile base with differential drive.
    The robot is basically a box moving according to differential drive kinematics.
 
-
 Example 3: "RRBot with multiple interfaces"
    *RRBot* with multiple interfaces.
 
-
 Example 4: "Industrial robot with integrated sensor"
    *RRBot* with an integrated sensor.
 
-
 Example 5: "Industrial Robots with externally connected sensor"
    *RRBot* with an externally connected sensor.
 
-
 Example 6: "Modular Robots with separate communication to each actuator"
    The example shows how to implement robot hardware with separate communication to each actuator.
 
+Example 7: "6-DOF robot"
+   A full tutorial for a 6 DOF robot for intermediate ROS 2 users.
 
 Example 8: "Using transmissions"
    *RRBot* with an exposed transmission interface.
 
-
 Example 9: "Gazebo Classic"
    Demonstrates how to switch between simulation and hardware.
 
@@ -252,7 +249,8 @@ Examples
    Example 2: DiffBot <../example_2/doc/userdoc.rst>
    Example 3: RRBot with multiple interfaces <../example_3/doc/userdoc.rst>
    Example 4: Industrial robot with integrated sensor <../example_4/doc/userdoc.rst>
-   Example 5: Industrial Robots with externally connected sensor <../example_5/doc/userdoc.rst>
-   Example 6: Modular Robots with separate communication to each actuator <../example_6/doc/userdoc.rst>
+   Example 5: Industrial robots with externally connected sensor <../example_5/doc/userdoc.rst>
+   Example 6: Modular robots with separate communication to each actuator <../example_6/doc/userdoc.rst>
+   Example 7: Full tutorial with a 6DOF robot <../example_7/doc/userdoc.rst>
    Example 8: Using transmissions <../example_8/doc/userdoc.rst>
-   Example 9: Gazebo Classic <../example_9/doc/userdoc.rst>
+   Example 9: Gazebo classic <../example_9/doc/userdoc.rst>

example_7/CMakeLists.txt

@@ -0,0 +1,109 @@
+cmake_minimum_required(VERSION 3.16)
+project(ros2_control_demo_example_7 LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+# find dependencies
+set(HW_IF_INCLUDE_DEPENDS
+  pluginlib
+  rcpputils
+  hardware_interface
+)
+set(REF_GEN_INCLUDE_DEPENDS
+  kdl_parser
+  rclcpp
+  trajectory_msgs
+)
+set(CONTROLLER_INCLUDE_DEPENDS
+  pluginlib
+  rcpputils
+  controller_interface
+  realtime_tools
+  trajectory_msgs
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${HW_IF_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+foreach(Dependency IN ITEMS ${REF_GEN_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+foreach(Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+
+## COMPILE
+add_executable(send_trajectory reference_generator/send_trajectory.cpp)
+
+ament_target_dependencies(
+  send_trajectory PUBLIC
+  ${REF_GEN_INCLUDE_DEPENDS}
+)
+
+add_library(
+  ros2_control_demo_example_7
+  SHARED
+  hardware/r6bot_hardware.cpp
+  controller/r6bot_controller.cpp
+)
+
+target_compile_features(ros2_control_demo_example_7 PUBLIC cxx_std_17)
+target_include_directories(ros2_control_demo_example_7 PUBLIC
+$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
+$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/controller/include>
+$<INSTALL_INTERFACE:include/ros2_control_demo_example_7>
+)
+ament_target_dependencies(
+  ros2_control_demo_example_7 PUBLIC
+  ${HW_IF_INCLUDE_DEPENDS}
+  ${CONTROLLER_INCLUDE_DEPENDS}
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_EXAMPLE_7_BUILDING_DLL")
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_7.xml)
+# Export controller plugins
+pluginlib_export_plugin_description_file(controller_interface ros2_control_demo_example_7.xml)
+
+# INSTALL
+install(
+  DIRECTORY hardware/include/
+  DESTINATION include/ros2_control_demo_example_7
+)
+install(
+  DIRECTORY description/launch description/ros2_control description/urdf description/rviz description/srdf  description/meshes
+  DESTINATION share/ros2_control_demo_example_7
+)
+install(
+  DIRECTORY bringup/launch bringup/config
+  DESTINATION share/ros2_control_demo_example_7
+)
+
+install(
+    TARGETS send_trajectory
+    RUNTIME DESTINATION lib/ros2_control_demo_example_7
+)
+
+install(TARGETS ros2_control_demo_example_7
+  EXPORT export_ros2_control_demo_example_7
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+endif()
+
+## EXPORTS
+ament_export_targets(export_ros2_control_demo_example_7 HAS_LIBRARY_TARGET)
+ament_export_dependencies(${HW_IF_INCLUDE_DEPENDS} ${REF_GEN_INCLUDE_DEPENDS} ${CONTROLLER_INCLUDE_DEPENDS})
+ament_package()

example_7/README.md

@@ -0,0 +1,12 @@
+# ros2_control_demo_example_7
+
+A full tutorial for a 6 DOF robot for intermediate ROS 2 users.
+
+It consists of the following:
+* bringup: launch files and ros2_controller configuration
+* controller: a controller for the 6-DOF robot
+* description: the 6-DOF robot description
+* hardware: ros2_control hardware interface
+* reference_generator: A KDL-based reference generator for a fixed trajectory
+
+Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_7/doc/userdoc.html).

example_7/bringup/config/r6bot_controller.yaml

@@ -0,0 +1,28 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    r6bot_controller:
+      type: ros2_control_demo_example_7/RobotController
+
+
+r6bot_controller:
+  ros__parameters:
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+
+    command_interfaces:
+      - position
+      - velocity
+
+    state_interfaces:
+      - position
+      - velocity

example_7/bringup/launch/r6bot_controller.launch.py

@@ -0,0 +1,114 @@
+# Copyright 2023 ros2_control Development Team
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("ros2_control_demo_example_7"),
+                    "urdf",
+                    "r6bot.urdf.xacro",
+                ]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("ros2_control_demo_example_7"),
+            "config",
+            "r6bot_controller.yaml",
+        ]
+    )
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("ros2_control_demo_example_7"), "rviz", "view_robot.rviz"]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, robot_controllers],
+        remappings=[
+            (
+                "/forward_position_controller/commands",
+                "/position_commands",
+            ),
+        ],
+        output="both",
+    )
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    robot_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["r6bot_controller", "-c", "/controller_manager"],
+    )
+
+    # Delay rviz start after `joint_state_broadcaster`
+    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[rviz_node],
+        )
+    )
+
+    # Delay start of robot_controller after `joint_state_broadcaster`
+    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[robot_controller_spawner],
+        )
+    )
+
+    nodes = [
+        control_node,
+        robot_state_pub_node,
+        joint_state_broadcaster_spawner,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
+    ]
+
+    return LaunchDescription(nodes)