diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index f7016c9d4..3a7009360 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -37,6 +37,7 @@ jobs: ros2_control_demo_example_10 ros2_control_demo_example_11 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 }} diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 685c8886c..a87d10281 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -32,6 +32,7 @@ jobs: ros2_control_demo_example_10 ros2_control_demo_example_11 ros2_control_demo_example_12 + ros2_control_demo_example_14 ament_lint_100: name: ament_${{ matrix.linter }} @@ -59,3 +60,4 @@ jobs: ros2_control_demo_example_8 ros2_control_demo_example_9 ros2_control_demo_example_12 + ros2_control_demo_example_14 diff --git a/README.md b/README.md index 43f17b8fe..2e24f4820 100644 --- a/README.md +++ b/README.md @@ -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`. diff --git a/doc/index.rst b/doc/index.rst index f26d88d23..6cd03cede 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -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" @@ -69,13 +69,18 @@ Example 9: "Gazebo Classic" Example 10: "GPIO interfaces" Industrial robot with GPIO interfaces - + Example 11: "CarlikeBot" - *CarlikeBot* with an Ackermann steering controller would be similar to how a car would be controlled. + *CarlikeBot* with an Ackermann steering controller 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: ===================== @@ -268,3 +273,4 @@ Examples Example 10: Industrial robot with GPIO interfaces <../example_10/doc/userdoc.rst> Example 11: CarlikeBot <../example_11/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> diff --git a/example_14/CMakeLists.txt b/example_14/CMakeLists.txt new file mode 100644 index 000000000..3b7e7b153 --- /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 + 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_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..f988a5c70 --- /dev/null +++ b/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py @@ -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) 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..4bcb91972 --- /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_description", + 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_without_feedback_sensors_for_position_feedback.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("ros2_control_demo_example_14"), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rrbot/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/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..eb1409220 --- /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..d51fa8f37 --- /dev/null +++ b/example_14/doc/userdoc.rst @@ -0,0 +1,163 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_14/doc/userdoc.rst + +.. _ros2_control_demos_example_14_userdoc: + +************************************************************** +Example 14: Modular robot with actuators not providing states +************************************************************** + +The example shows how to implement robot hardware with separate communication to each actuator as well as separate sensors for position feedback: + +* The communication is done on actuator level using proprietary or standardized API (e.g., canopen_402, Modbus, RS232, RS485). +* Data for all actuators and sensors is exchanged separately from each other +* Examples: Arduino-based-robots, custom robots + +This is implemented with hardware interfaces of type ``hardware_interface::ActuatorInterface`` and ``hardware_interface::SensorInterface``. + +.. 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, let's 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 `__ + + * Description: `rrbot_description.urdf.xacro `__ + * ``ros2_control`` URDF tag: `rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro `__ + +* RViz configuration: `rrbot.rviz `__ + +* Hardware interface plugins: + + * `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/example_4/doc/userdoc.rst b/example_4/doc/userdoc.rst index b0909a28b..5f289beca 100644 --- a/example_4/doc/userdoc.rst +++ b/example_4/doc/userdoc.rst @@ -66,6 +66,30 @@ Tutorial steps Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. + Now, let's introspect the hardware components with + + .. code-block:: shell + + ros2 control list_hardware_components -v + + There is a single hardware component for the robot providing the command and state interfaces: + + .. code-block:: shell + + Hardware Component 1 + name: RRBotSystemWithSensor + type: system + plugin name: ros2_control_demo_example_4/RRBotSystemWithSensorHardware + state: id=3 label=active + command interfaces + joint1/position [available] [claimed] + joint2/position [available] [claimed] + state interfaces + joint1/position [available] + joint2/position [available] + tcp_fts_sensor/force.x [available] + tcp_fts_sensor/torque.z [available] + 4. Check if controllers are running .. code-block:: shell diff --git a/example_5/doc/userdoc.rst b/example_5/doc/userdoc.rst index 5a3ccc524..f308c8ad5 100644 --- a/example_5/doc/userdoc.rst +++ b/example_5/doc/userdoc.rst @@ -69,6 +69,31 @@ Tutorial steps Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. + Now, let's introspect the hardware components with + + .. code-block:: shell + + ros2 control list_hardware_components + + There are two hardware components, one for the robot and one for the sensor: + + .. code-block:: shell + + Hardware Component 1 + name: ExternalRRBotFTSensor + type: sensor + plugin name: ros2_control_demo_example_5/ExternalRRBotForceTorqueSensorHardware + state: id=3 label=active + command interfaces + Hardware Component 2 + name: RRBotSystemPositionOnly + type: system + plugin name: ros2_control_demo_example_5/RRBotSystemPositionOnlyHardware + state: id=3 label=active + command interfaces + joint1/position [available] [claimed] + joint2/position [available] [claimed] + 4. Check if controllers are running .. code-block:: shell diff --git a/example_6/doc/userdoc.rst b/example_6/doc/userdoc.rst index eb98b843b..da083f974 100644 --- a/example_6/doc/userdoc.rst +++ b/example_6/doc/userdoc.rst @@ -62,6 +62,31 @@ Tutorial steps Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. + Now, let's introspect the hardware components with + + .. code-block:: shell + + ros2 control list_hardware_components + + There are two 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_6/RRBotModularJoint + state: id=3 label=active + command interfaces + joint2/position [available] [claimed] + Hardware Component 2 + name: RRBotModularJoint1 + type: actuator + plugin name: ros2_control_demo_example_6/RRBotModularJoint + state: id=3 label=active + command interfaces + joint1/position [available] [claimed] + 4. Check if controllers are running .. code-block:: shell @@ -70,8 +95,8 @@ Tutorial steps .. code-block:: shell - forward_position_controller[forward_command_controller/ForwardCommandController] - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] + forward_position_controller[forward_command_controller/ForwardCommandController] active + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active 5. If you get output from above you can send commands to *Forward Command Controller*, either: diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml index ecb08c780..2df0f206e 100644 --- a/ros2_control_demos/package.xml +++ b/ros2_control_demos/package.xml @@ -20,11 +20,13 @@ 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_10 ros2_control_demo_example_11 ros2_control_demo_example_12 + ros2_control_demo_example_14 ament_cmake