diff --git a/README.md b/README.md index bfb7b9573..aa308504c 100644 --- a/README.md +++ b/README.md @@ -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`. diff --git a/doc/index.rst b/doc/index.rst index 2fde16280..bba0f06a2 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -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: @@ -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> diff --git a/example_14/CMakeLists.txt b/example_14/CMakeLists.txt new file mode 100644 index 000000000..87f4e0238 --- /dev/null +++ b/example_14/CMakeLists.txt @@ -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 +$ +$ +) +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() diff --git a/example_14/README.md b/example_14/README.md new file mode 100644 index 000000000..5848f89eb --- /dev/null +++ b/example_14/README.md @@ -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). diff --git a/example_14/bringup/config/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml b/example_14/bringup/config/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml new file mode 100644 index 000000000..981ca5647 --- /dev/null +++ b/example_14/bringup/config/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml @@ -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 diff --git a/example_14/bringup/launch/rrbot_base.launch.py b/example_14/bringup/launch/rrbot_base.launch.py new file mode 100644 index 000000000..7db3c4cf6 --- /dev/null +++ b/example_14/bringup/launch/rrbot_base.launch.py @@ -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) diff --git a/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py b/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py new file mode 100644 index 000000000..f5acd3a72 --- /dev/null +++ b/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py @@ -0,0 +1,63 @@ +# Copyright (c) 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, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +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, joint names in the controllers' configuration \ + also 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.", + ) + ) + + # Initialize Arguments + prefix = LaunchConfiguration("prefix") + slowdown = LaunchConfiguration("slowdown") + robot_controller = LaunchConfiguration("robot_controller") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), + launch_arguments={ + "controllers_file": "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml", + "description_file": "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro", + "prefix": prefix, + "use_fake_hardware": "false", + "slowdown": slowdown, + "robot_controller": robot_controller, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/example_14/description/launch/view_robot.launch.py b/example_14/description/launch/view_robot.launch.py new file mode 100644 index 000000000..1d671d762 --- /dev/null +++ b/example_14/description/launch/view_robot.launch.py @@ -0,0 +1,111 @@ +# 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 +from launch.conditions import IfCondition +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( + "description_package", + default_value="ros2_control_demo_example_6", + 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", + default_value="rrbot_modular_actuators.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start Rviz2 and Joint State Publisher gui automatically \ + with this launch file.", + ) + ) + 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.", + ) + ) + + # Initialize Arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + gui = LaunchConfiguration("gui") + prefix = LaunchConfiguration("prefix") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", "rrbot.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(gui), + ) + robot_state_publisher_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), + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro b/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro new file mode 100644 index 000000000..739ffc8e5 --- /dev/null +++ b/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro @@ -0,0 +1,67 @@ + + + + + + + + + + ros2_control_demo_example_14/RRBotSensorPositionFeedback + 1.0 + 2.0 + ${slowdown} + 23286 + + + + + + + + ros2_control_demo_example_14/RRBotSensorPositionFeedback + 1.0 + 2.0 + ${slowdown} + 23287 + + + + + + + + + ros2_control_demo_example_14/RRBotActuatorWithoutFeedback + 2.0 + 3.0 + 23286 + + + + -1 + 1 + + + + + + ros2_control_demo_example_14/RRBotActuatorWithoutFeedback + 2.0 + 3.0 + 23287 + + + + -1 + 1 + + + + + + + diff --git a/example_14/description/rviz/rrbot.rviz b/example_14/description/rviz/rrbot.rviz new file mode 100644 index 000000000..373bae55b --- /dev/null +++ b/example_14/description/rviz/rrbot.rviz @@ -0,0 +1,187 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 905 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8.443930625915527 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0044944556429982185 + Y: 1.0785865783691406 + Z: 2.4839563369750977 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.23039916157722473 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.150422096252441 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1137 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000041bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000041b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001100000041bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000041b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004fa0000041b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 23 diff --git a/example_14/description/urdf/rrbot.materials.xacro b/example_14/description/urdf/rrbot.materials.xacro new file mode 100644 index 000000000..eb8e0212c --- /dev/null +++ b/example_14/description/urdf/rrbot.materials.xacro @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_14/description/urdf/rrbot_description.urdf.xacro b/example_14/description/urdf/rrbot_description.urdf.xacro new file mode 100644 index 000000000..cfcb0388b --- /dev/null +++ b/example_14/description/urdf/rrbot_description.urdf.xacro @@ -0,0 +1,119 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_14/description/urdf/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro b/example_14/description/urdf/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro new file mode 100644 index 000000000..72dd198c3 --- /dev/null +++ b/example_14/description/urdf/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + diff --git a/example_14/doc/userdoc.rst b/example_14/doc/userdoc.rst new file mode 100644 index 000000000..bbfbec8e3 --- /dev/null +++ b/example_14/doc/userdoc.rst @@ -0,0 +1,162 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_6/doc/userdoc.rst + +.. _ros2_control_demos_example_14_userdoc: + +*********************************************************************** +Example 6: Modular Robots with separate communication to each actuator +*********************************************************************** + +The example shows how to implement robot hardware with separate communication to each actuator: + +* The communication is done on actuator level using proprietary or standardized API (e.g., canopen_402, Modbus, RS232, RS485). +* Data for all actuators is exchanged separately from each other. +* Examples: Mara, Arduino-based-robots + +This is implemented with a hardware interface of type ``hardware_interface::ActuatorInterface``. + +.. include:: ../../doc/run_from_docker.rst + +Tutorial steps +-------------------------- + +1. To check that *RRBot* descriptions are working properly use following launch commands + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_14 view_robot.launch.py + + .. note:: + + Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. + This happens because ``joint_state_publisher_gui`` node need some time to start. + The ``joint_state_publisher_gui`` provides a GUI to generate a random configuration for rrbot. It is immediately displayed in *RViz*. + + +2. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_14 rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py + + The launch file loads and starts the robot hardware, controllers and opens *RViz*. + In starting terminal you will see a lot of output from the hardware implementation showing its internal states. + This is only of exemplary purposes and should be avoided as much as possible in a hardware interface implementation. + + If you can see two orange and one yellow rectangle in in *RViz* everything has started properly. + Still, to be sure, let's introspect the control system before moving *RRBot*. + +3. Check if the hardware interface loaded properly, by opening another terminal and executing + + .. code-block:: shell + + ros2 control list_hardware_interfaces + + .. code-block:: shell + + command interfaces + joint1/position [available] [claimed] + joint2/position [available] [claimed] + state interfaces + joint1/position + joint2/position + + Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. + + Now, lets introspect the hardware components with + + .. code-block:: shell + + ros2 control list_hardware_components + + There are four hardware components, one for each actuator and one for each sensor: + + .. code-block:: shell + + Hardware Component 1 + name: RRBotModularJoint2 + type: actuator + plugin name: ros2_control_demo_example_14/RRBotActuatorWithoutFeedback + state: id=3 label=active + command interfaces + joint2/velocity [available] [claimed] + Hardware Component 2 + name: RRBotModularJoint1 + type: actuator + plugin name: ros2_control_demo_example_14/RRBotActuatorWithoutFeedback + state: id=3 label=active + command interfaces + joint1/velocity [available] [claimed] + Hardware Component 3 + name: RRBotModularPositionSensorJoint2 + type: sensor + plugin name: ros2_control_demo_example_14/RRBotSensorPositionFeedback + state: id=3 label=active + command interfaces + Hardware Component 4 + name: RRBotModularPositionSensorJoint1 + type: sensor + plugin name: ros2_control_demo_example_14/RRBotSensorPositionFeedback + state: id=3 label=active + command interfaces + + +4. Check if controllers are running + + .. code-block:: shell + + ros2 control list_controllers + + .. code-block:: shell + + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + forward_velocity_controller[forward_command_controller/ForwardCommandController] active + +5. If you get output from above you can send commands to *Forward Command Controller*: + + .. code-block:: shell + + ros2 topic pub /forward_velocity_controller/commands std_msgs/msg/Float64MultiArray "data: + - 5 + - 5" + + You should now see orange and yellow blocks moving in *RViz*. + Also, you should see changing states in the terminal where launch file is started, e.g. + + .. code-block:: shell + + [RRBotActuatorWithoutFeedback]: Writing command: 5.000000 + [RRBotActuatorWithoutFeedback]: Sending data command: 5 + [RRBotActuatorWithoutFeedback]: Joints successfully written! + [RRBotActuatorWithoutFeedback]: Writing command: 5.000000 + [RRBotActuatorWithoutFeedback]: Sending data command: 5 + [RRBotActuatorWithoutFeedback]: Joints successfully written! + [RRBotSensorPositionFeedback]: Reading... + [RRBotSensorPositionFeedback]: Got measured velocity 5.00000 + [RRBotSensorPositionFeedback]: Got state 0.25300 for joint 'joint1'! + [RRBotSensorPositionFeedback]: Joints successfully read! + [RRBotSensorPositionFeedback]: Reading... + [RRBotSensorPositionFeedback]: Got measured velocity 5.00000 + [RRBotSensorPositionFeedback]: Got state 0.25300 for joint 'joint2'! + [RRBotSensorPositionFeedback]: Joints successfully read! + + +Files used for this demos +-------------------------- + +* Launch file: `rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py `__ +* Controllers yaml: `rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml `__ +* URDF: `rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro `__ + + * ``ros2_control`` URDF tag: `rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro `__ + +* RViz configuration: `rrbot.rviz `__ + +* Hardware interface plugin: + + * `rrbot_actuator_without_feedback.cpp `__ + * `rrbot_sensor_for_position_feedback.cpp `__ + +Controllers from this demo +-------------------------- +* ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ +* ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp new file mode 100644 index 000000000..775d4d592 --- /dev/null +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp @@ -0,0 +1,90 @@ +// Copyright (c) 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. + +// +// Authors: Denis Stogl +// + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_14__RRBOT_ACTUATOR_WITHOUT_FEEDBACK_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_14__RRBOT_ACTUATOR_WITHOUT_FEEDBACK_HPP_ + +#include +#include +#include +#include +#include + +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "ros2_control_demo_example_14/visibility_control.h" + +namespace ros2_control_demo_example_14 +{ +class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(RRBotActuatorWithoutFeedback); + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Parameters for the RRBot simulation + double hw_start_sec_; + double hw_stop_sec_; + + // Store the command for the simulated robot + double hw_joint_command_; + + // Fake "mechanical connection" between actuator and sensor using sockets + struct sockaddr_in address_; + int socket_port_; + int sockoptval_ = 1; + int sock_; +}; + +} // namespace ros2_control_demo_example_14 + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_14__RRBOT_ACTUATOR_WITHOUT_FEEDBACK_HPP_ diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp new file mode 100644 index 000000000..20e9a51e9 --- /dev/null +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp @@ -0,0 +1,107 @@ +// Copyright (c) 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. + +// +// Authors: Denis Stogl +// + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_14__RRBOT_SENSOR_FOR_POSITION_FEEDBACK_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_14__RRBOT_SENSOR_FOR_POSITION_FEEDBACK_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor_interface.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "ros2_control_demo_example_14/visibility_control.h" + +namespace ros2_control_demo_example_14 +{ +class RRBotSensorPositionFeedback : public hardware_interface::SensorInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSensorPositionFeedback); + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Parameters for the RRBot simulation + double hw_start_sec_; + double hw_stop_sec_; + double hw_slowdown_; + + // Store the command for the simulated robot + double measured_velocity; // Local variable, but avoid initialization on each read + double last_measured_velocity_; + double hw_joint_state_; + + // Timestamps to calculate position for velocity + rclcpp::Clock clock_; + rclcpp::Time last_timestamp_; + rclcpp::Time current_timestamp; // Local variable, but avoid initialization on each read + + // Sync incoming commands between threads + realtime_tools::RealtimeBuffer rt_incomming_data_ptr_; + + // Create timer to checking incoming data on socket + std::thread incoming_data_thread_; + + // Fake "mechanical connection" between actuator and sensor using sockets + struct sockaddr_in address_; + int socket_port_; + int address_length_; + int obj_socket_; + int sockoptval_ = 1; + int sock_; +}; + +} // namespace ros2_control_demo_example_14 + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_14__RRBOT_SENSOR_FOR_POSITION_FEEDBACK_HPP_ diff --git a/example_14/hardware/include/ros2_control_demo_example_14/visibility_control.h b/example_14/hardware/include/ros2_control_demo_example_14/visibility_control.h new file mode 100644 index 000000000..d722ed528 --- /dev/null +++ b/example_14/hardware/include/ros2_control_demo_example_14/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2021 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_14__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_DEMO_EXAMPLE_14__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROS2_CONTROL_DEMO_EXAMPLE_14_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_14_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_14_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DEMO_EXAMPLE_14_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_DEMO_EXAMPLE_14_BUILDING_DLL +#define ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_14_EXPORT +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_14_IMPORT +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_14_LOCAL +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_14_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_14_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_14_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_14_LOCAL +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_14_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_14__VISIBILITY_CONTROL_H_ diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp new file mode 100644 index 000000000..5a9ba378c --- /dev/null +++ b/example_14/hardware/rrbot_actuator_without_feedback.cpp @@ -0,0 +1,206 @@ +// Copyright (c) 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. + +// +// Authors: Denis Stogl +// + +#include "ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_example_14 +{ +hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::ActuatorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + // START: This part here is for exemplary purposes - Please do not copy to your production code + hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + socket_port_ = std::stoi(info_.hardware_parameters["example_param_socket_port"]); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + hw_joint_command_ = std::numeric_limits::quiet_NaN(); + + const hardware_interface::ComponentInfo & joint = info_.joints[0]; + // RRBotActuatorWithoutFeedback has exactly one command interface and one joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + // START: This part here is for exemplary purposes - Please do not copy to your production code + // Initialize objects for fake mechanical connection + sock_ = socket(AF_INET, SOCK_STREAM, 0); + if (sock_ < 0) + { + RCLCPP_FATAL(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Creating socket failed."); + return hardware_interface::CallbackReturn::ERROR; + } + + auto server = gethostbyname("localhost"); + + address_.sin_family = AF_INET; + bcopy( + reinterpret_cast(server->h_addr), reinterpret_cast(&address_.sin_addr.s_addr), + server->h_length); + address_.sin_port = htons(socket_port_); + + RCLCPP_INFO( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Trying to connect to port %d.", + socket_port_); + if (connect(sock_, (struct sockaddr *)&address_, sizeof(address_)) < 0) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Connection over socket failed."); + return hardware_interface::CallbackReturn::ERROR; + } + RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Connected to socket"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_shutdown( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + shutdown(sock_, SHUT_RDWR); // shutdown socket + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +RRBotActuatorWithoutFeedback::export_state_interfaces() +{ + std::vector state_interfaces; + return state_interfaces; +} + +std::vector +RRBotActuatorWithoutFeedback::export_command_interfaces() +{ + std::vector command_interfaces; + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &hw_joint_command_)); + + return command_interfaces; +} + +hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // START: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Activating ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "%.1f seconds left...", + hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // set some default values for joints + if (std::isnan(hw_joint_command_)) + { + hw_joint_command_ = 0; + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // START: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Deactivating ...please wait..."); + + for (int i = 0; i < hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "%.1f seconds left...", hw_stop_sec_ - i); + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Successfully deactivated!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type RRBotActuatorWithoutFeedback::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type ros2_control_demo_example_14::RRBotActuatorWithoutFeedback::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // START: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Writing command: %f", hw_joint_command_); + + // Simulate sending commands to the hardware + std::ostringstream data; + data << hw_joint_command_; + RCLCPP_INFO( + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Sending data command: %s", + data.str().c_str()); + send(sock_, data.str().c_str(), strlen(data.str().c_str()), 0); + + RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Joints successfully written!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +} // namespace ros2_control_demo_example_14 + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_example_14::RRBotActuatorWithoutFeedback, hardware_interface::ActuatorInterface) diff --git a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp new file mode 100644 index 000000000..532e49112 --- /dev/null +++ b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp @@ -0,0 +1,284 @@ +// Copyright (c) 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. + +// +// Authors: Denis Stogl +// + +#include "ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/sensor_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_example_14 +{ +hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + // START: This part here is for exemplary purposes - Please do not copy to your production code + hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + hw_slowdown_ = std::stod(info_.hardware_parameters["example_param_hw_slowdown"]); + socket_port_ = std::stoi(info_.hardware_parameters["example_param_socket_port"]); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + hw_joint_state_ = std::numeric_limits::quiet_NaN(); + + const hardware_interface::ComponentInfo & joint = info_.joints[0]; + // RRBotSensorPositionFeedback has exactly one state interface and one joint + if (joint.state_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSensorPositionFeedback"), + "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSensorPositionFeedback"), + "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + clock_ = rclcpp::Clock(); + + // START: This part here is for exemplary purposes - Please do not copy to your production code + // Initialize objects for fake mechanical connection + obj_socket_ = socket(AF_INET, SOCK_STREAM, 0); + if (obj_socket_ < 0) + { + RCLCPP_FATAL(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Creating socket failed."); + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Setting socket options."); + if (setsockopt(obj_socket_, SOL_SOCKET, SO_REUSEADDR, &sockoptval_, sizeof(sockoptval_))) + { + RCLCPP_FATAL(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Setting socket failed."); + return hardware_interface::CallbackReturn::ERROR; + } + + address_length_ = sizeof(address_); + + address_.sin_family = AF_INET; + address_.sin_addr.s_addr = INADDR_ANY; + address_.sin_port = htons(socket_port_); + + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Binding to socket address."); + if (bind(obj_socket_, reinterpret_cast(&address_), sizeof(address_)) < 0) + { + RCLCPP_FATAL(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Binding to socket failed."); + return hardware_interface::CallbackReturn::ERROR; + } + + // Storage and Thread for incoming data + rt_incomming_data_ptr_.writeFromNonRT(std::numeric_limits::quiet_NaN()); + incoming_data_thread_ = std::thread( + [this]() + { + // Await and accept connection + RCLCPP_INFO( + rclcpp::get_logger("RRBotSensorPositionFeedback"), "Listening for connection on port %d.", + socket_port_); + if (listen(obj_socket_, 1) < 0) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSensorPositionFeedback"), "Cannot listen from the server."); + return hardware_interface::CallbackReturn::ERROR; + } + + sock_ = accept( + obj_socket_, reinterpret_cast(&address_), + reinterpret_cast(&address_length_)); + if (sock_ < 0) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSensorPositionFeedback"), "Cannot accept on the server."); + return hardware_interface::CallbackReturn::ERROR; + } + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Accepting on socket."); + + int incoming_data_read_rate = 1000; // Hz + RCLCPP_INFO( + rclcpp::get_logger("RRBotSensorPositionFeedback"), + "Creating thread for incoming data and read them with %d Hz to not miss any data.", + incoming_data_read_rate); + + // Variables for reading from a socket + const size_t reading_size_bytes = 1024; + char buffer[reading_size_bytes] = {0}; + + // Use nanoseconds to avoid chrono's rounding + std::this_thread::sleep_for(std::chrono::nanoseconds(1000000000 / incoming_data_read_rate)); + + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Receiving data"); + while (rclcpp::ok()) + { + if (recv(sock_, buffer, reading_size_bytes, 0) > 0) + { + RCLCPP_DEBUG( + rclcpp::get_logger("RRBotSensorPositionFeedback"), + "Read form buffer sockets data: '%s'", buffer); + + rt_incomming_data_ptr_.writeFromNonRT(std::stod(buffer)); + } + else + { + RCLCPP_INFO( + rclcpp::get_logger("RRBotSensorPositionFeedback"), + "Data not yet received from socket."); + rt_incomming_data_ptr_.writeFromNonRT(std::numeric_limits::quiet_NaN()); + } + + bzero(buffer, reading_size_bytes); + std::this_thread::sleep_for(std::chrono::nanoseconds(1000000000 / incoming_data_read_rate)); + } + }); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_shutdown( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + incoming_data_thread_.join(); // stop reading thread + shutdown(sock_, SHUT_RDWR); // shutdown socket + shutdown(obj_socket_, SHUT_RDWR); // shutdown socket + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +RRBotSensorPositionFeedback::export_state_interfaces() +{ + std::vector state_interfaces; + + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_state_)); + + return state_interfaces; +} + +hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // set some default values for joints + if (std::isnan(hw_joint_state_)) + { + hw_joint_state_ = 0; + } + last_measured_velocity_ = 0; + + // In general after a hardware is configured it can be read + last_timestamp_ = clock_.now(); + + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Configuration successful."); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // START: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Activating ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSensorPositionFeedback"), "%.1f seconds left...", hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // START: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Deactivating ...please wait..."); + + for (int i = 0; i < hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSensorPositionFeedback"), "%.1f seconds left...", hw_stop_sec_ - i); + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Successfully deactivated!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type RRBotSensorPositionFeedback::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + current_timestamp = clock_.now(); + rclcpp::Duration duration = current_timestamp - last_timestamp_; + last_timestamp_ = current_timestamp; + + // START: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Reading..."); + + // Simulate RRBot's movement + measured_velocity = *(rt_incomming_data_ptr_.readFromRT()); + if (!std::isnan(measured_velocity)) + { + last_measured_velocity_ = measured_velocity; + } + RCLCPP_INFO( + rclcpp::get_logger("RRBotSensorPositionFeedback"), "Got measured velocity %.5f", + measured_velocity); + hw_joint_state_ += (last_measured_velocity_ * duration.seconds()) / hw_slowdown_; + RCLCPP_INFO( + rclcpp::get_logger("RRBotSensorPositionFeedback"), "Got state %.5f for joint '%s'!", + hw_joint_state_, info_.joints[0].name.c_str()); + + RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Joints successfully read!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +} // namespace ros2_control_demo_example_14 + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_example_14::RRBotSensorPositionFeedback, hardware_interface::SensorInterface) diff --git a/example_14/package.xml b/example_14/package.xml new file mode 100644 index 000000000..2ec771b44 --- /dev/null +++ b/example_14/package.xml @@ -0,0 +1,38 @@ + + + + ros2_control_demo_example_14 + 0.0.0 + Modular Robots with actuators not providing states and with additional sensors. + + Denis Štogl + Bence Magyar + Christoph Froehlich + + Apache-2.0 + + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + + controller_manager + forward_command_controller + joint_state_broadcaster + joint_state_publisher_gui + robot_state_publisher + ros2_controllers_test_nodes + ros2controlcli + ros2launch + rviz2 + xacro + + ament_cmake_gtest + + + ament_cmake + + diff --git a/example_14/ros2_control_demo_example_14.xml b/example_14/ros2_control_demo_example_14.xml new file mode 100644 index 000000000..8cacc5459 --- /dev/null +++ b/example_14/ros2_control_demo_example_14.xml @@ -0,0 +1,16 @@ + + + + The ros2_control RRBot's actuators without feedback that can be controlled by different endpoints. + + + + + The ros2_control RRBot's position sensors that can be used with actuators without feedback. + + + diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml index f5fbf4a9a..b7e23be6e 100644 --- a/ros2_control_demos/package.xml +++ b/ros2_control_demos/package.xml @@ -20,8 +20,10 @@ ros2_control_demo_example_4 ros2_control_demo_example_5 ros2_control_demo_example_6 + ros2_control_demo_example_7 ros2_control_demo_example_8 ros2_control_demo_example_9 + ros2_control_demo_example_14 ament_cmake