Skip to content

Commit

Permalink
Add example 14 from #147
Browse files Browse the repository at this point in the history
Co-authored-by: Denis Štogl <[email protected]>
Co-authored-by: Jack <[email protected]>
  • Loading branch information
3 people committed Aug 5, 2023
1 parent 09b357a commit 34f43fd
Show file tree
Hide file tree
Showing 22 changed files with 1,915 additions and 0 deletions.
2 changes: 2 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,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
11 changes: 11 additions & 0 deletions doc/index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,16 @@ Example 8: "Using transmissions"
Example 9: "Gazebo Classic"
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.)"

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


.. _ros2_control_demos_install:

Expand Down Expand Up @@ -254,3 +264,4 @@ Examples
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 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 description/rviz
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
216 changes: 216 additions & 0 deletions example_14/bringup/launch/rrbot_base.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,216 @@
# 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(
"runtime_config_package",
default_value="ros2_control_demo_example_14",
description='Package with the controller\'s configuration in "config" folder. \
Usually the argument is not set, it enables use of a custom setup.',
)
)
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="rrbot_controllers.yaml",
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="ros2_control_demo_example_14",
description="Description package with robot URDF/xacro files. Usually the argument \
is not set, it enables use of a custom description.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
description="URDF/XACRO description file with the robot.",
)
)
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(
"use_gazebo",
default_value="false",
description="Start robot in Gazebo simulation.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="true",
description="Start robot with fake hardware mirroring command to its states.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"mock_sensor_commands",
default_value="false",
description="Enable fake command interfaces for sensors used for simple simulations. \
Used only if 'use_fake_hardware' parameter is true.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"slowdown", default_value="3.0", description="Slowdown factor of the RRbot."
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_controller",
default_value="forward_position_controller",
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"start_rviz",
default_value="true",
description="Start RViz2 automatically with this launch file.",
)
)

# Initialize Arguments
runtime_config_package = LaunchConfiguration("runtime_config_package")
controllers_file = LaunchConfiguration("controllers_file")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
use_gazebo = LaunchConfiguration("use_gazebo")
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
mock_sensor_commands = LaunchConfiguration("mock_sensor_commands")
slowdown = LaunchConfiguration("slowdown")
robot_controller = LaunchConfiguration("robot_controller")
start_rviz = LaunchConfiguration("start_rviz")

# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "urdf", description_file]
),
" ",
"prefix:=",
prefix,
" ",
"use_gazebo:=",
use_gazebo,
" ",
"use_fake_hardware:=",
use_fake_hardware,
" ",
"mock_sensor_commands:=",
mock_sensor_commands,
" ",
"slowdown:=",
slowdown,
]
)
robot_description = {"robot_description": robot_description_content}

robot_controllers = PathJoinSubstitution(
[
FindPackageShare(runtime_config_package),
"config",
controllers_file,
]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_package), "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(start_rviz),
)

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, "-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(declared_arguments + nodes)
Loading

0 comments on commit 34f43fd

Please sign in to comment.