Skip to content

Commit

Permalink
[Example 14] Modular robot with actuators not providing states (#334) (
Browse files Browse the repository at this point in the history
…#367)

Co-authored-by: Denis Štogl <denis@stogl.de>
Co-authored-by: Jack <jack.d.center@gmail.com>
Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
(cherry picked from commit 90fac3d)

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
  • Loading branch information
mergify[bot] and christophfroehlich authored Sep 29, 2023
1 parent ed33141 commit 0c4c5fd
Show file tree
Hide file tree
Showing 23 changed files with 1,518 additions and 4 deletions.
1 change: 1 addition & 0 deletions .github/workflows/ci-coverage-build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ jobs:
ros2_control_demo_example_9
ros2_control_demo_example_10
ros2_control_demo_example_12
ros2_control_demo_example_14

vcs-repo-file-url: |
https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_control_demos-not-released.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }}
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/ci-ros-lint.yml
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ jobs:
ros2_control_demo_example_9
ros2_control_demo_example_10
ros2_control_demo_example_12
ros2_control_demo_example_14

ament_lint_100:
name: ament_${{ matrix.linter }}
Expand Down Expand Up @@ -57,3 +58,4 @@ jobs:
ros2_control_demo_example_9
ros2_control_demo_example_10
ros2_control_demo_example_12
ros2_control_demo_example_14
2 changes: 2 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,8 @@ The following examples are part of this demo repository:

* Example 13: "Multi-robot example (tba.)"

* Example 14: ["Modular robots with actuators not providing states and with additional sensors"](example_14)

## Getting started

The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`.
Expand Down
12 changes: 10 additions & 2 deletions doc/index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,10 @@ Example 3: "RRBot with multiple interfaces"
Example 4: "Industrial robot with integrated sensor"
*RRBot* with an integrated sensor.

Example 5: "Industrial Robots with externally connected sensor"
Example 5: "Industrial robot with externally connected sensor"
*RRBot* with an externally connected sensor.

Example 6: "Modular Robots with separate communication to each actuator"
Example 6: "Modular robot 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"
Expand All @@ -70,9 +70,16 @@ Example 9: "Gazebo Classic"
Example 10: "GPIO interfaces"
Industrial robot with GPIO interfaces

Example 11: "Car-like robot using steering controller library (tba.)"

Example 12: "Controller chaining"
The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*.

Example 13: "Multi-robot example (tba.)"

Example 14: "Modular robots with actuators not providing states and with additional sensors"


.. _ros2_control_demos_install:

=====================
Expand Down Expand Up @@ -264,3 +271,4 @@ Examples
Example 9: Gazebo classic <../example_9/doc/userdoc.rst>
Example 10: Industrial robot with GPIO interfaces <../example_10/doc/userdoc.rst>
Example 12: Controller chaining <../example_12/doc/userdoc.rst>
Example 14: Modular robots with actuators not providing states <../example_14/doc/userdoc.rst>
75 changes: 75 additions & 0 deletions example_14/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@
cmake_minimum_required(VERSION 3.16)
project(ros2_control_demo_example_14 LANGUAGES CXX)

if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
add_compile_options(-Wall -Wextra)
endif()

# find dependencies
set(THIS_PACKAGE_INCLUDE_DEPENDS
hardware_interface
pluginlib
rclcpp
rclcpp_lifecycle
realtime_tools
)

# find dependencies
find_package(ament_cmake REQUIRED)
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()


## COMPILE
add_library(
ros2_control_demo_example_14
SHARED
hardware/rrbot_actuator_without_feedback.cpp
hardware/rrbot_sensor_for_position_feedback.cpp
)
target_compile_features(ros2_control_demo_example_14 PUBLIC cxx_std_17)
target_include_directories(ros2_control_demo_example_14 PUBLIC
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
$<INSTALL_INTERFACE:include/ros2_control_demo_example_14>
)
ament_target_dependencies(
ros2_control_demo_example_14 PUBLIC
${THIS_PACKAGE_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_14_BUILDING_DLL")

# Export hardware plugins
pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_14.xml)

# INSTALL
install(
DIRECTORY hardware/include/
DESTINATION include/ros2_control_demo_example_14
)
install(
DIRECTORY description/launch description/ros2_control description/urdf
DESTINATION share/ros2_control_demo_example_14
)
install(
DIRECTORY bringup/launch bringup/config
DESTINATION share/ros2_control_demo_example_14
)
install(TARGETS ros2_control_demo_example_14
EXPORT export_ros2_control_demo_example_14
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_14 HAS_LIBRARY_TARGET)
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
ament_package()
5 changes: 5 additions & 0 deletions example_14/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
# ros2_control_demo_example_14

The example shows how to implement robot hardware with actuators not providing states and with additional sensors.

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_14/doc/userdoc.html).
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
controller_manager:
ros__parameters:
update_rate: 100 # Hz

joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster

forward_velocity_controller:
type: forward_command_controller/ForwardCommandController


forward_velocity_controller:
ros__parameters:
joints:
- joint1
- joint2
interface_name: velocity
Original file line number Diff line number Diff line change
@@ -0,0 +1,153 @@
# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, RegisterEventHandler
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
# Declare arguments
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"slowdown", default_value="50.0", description="Slowdown factor of the RRbot."
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_controller",
default_value="forward_velocity_controller",
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"gui",
default_value="true",
description="Start RViz2 automatically with this launch file.",
)
)

# Initialize Arguments
prefix = LaunchConfiguration("prefix")
slowdown = LaunchConfiguration("slowdown")
robot_controller = LaunchConfiguration("robot_controller")
gui = LaunchConfiguration("gui")

# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[
FindPackageShare("ros2_control_demo_example_14"),
"urdf",
"rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro",
]
),
" ",
"prefix:=",
prefix,
" ",
"slowdown:=",
slowdown,
]
)
robot_description = {"robot_description": robot_description_content}

robot_controllers = PathJoinSubstitution(
[
FindPackageShare("ros2_control_demo_example_14"),
"config",
"rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml",
]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"]
)

control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
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],
condition=IfCondition(gui),
)

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=[robot_controller, "--controller-manager", "/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(declared_arguments + nodes)
Loading

0 comments on commit 0c4c5fd

Please sign in to comment.