diff --git a/README.md b/README.md index 3be1354c8..8a070d0cf 100644 --- a/README.md +++ b/README.md @@ -475,6 +475,30 @@ Available controllers: Commanding the robot: see the commands below. +### Example 6: "Industrial Robots with an exposed transmission interface" + +Files: + - Launch file: [rrbot_transmissions_system_position_only.launch.py](ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py) + - Controllers yaml: [rrbot_controllers.yaml](ros2_control_demo_bringup/config/rrbot_controllers.yaml) + - URDF: [rrbot_transmissions_system_position_only.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro) + - `ros2_control` URDF tag: [rrbot_transmissions_system_position_only.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro) + +Interfaces: + - Command interfaces: + - joint1/position + - joint2/position + - State interfaces: + - joint1/position + - joint2/position + +Available controllers: + - `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + - `forward_position_controller[forward_command_controller/ForwardCommandController]` (position) + +Moving the robot: + - see below description of `forward_position_controller` + + ## Controllers and moving hardware To move the robot you should load and start controllers. diff --git a/example_1/README.rst b/example_1/README.rst index 5e4db7b46..cbe3f9971 100644 --- a/example_1/README.rst +++ b/example_1/README.rst @@ -5,7 +5,7 @@ TODO(destogl): This is not adjusted yet!! *RRBot*, or ''Revolute-Revolute Manipulator Robot'', is a simple 3-linkage, 2-joint arm that we will use to demonstrate various features. It is essentially a double inverted pendulum and demonstrates some fun control concepts within a simulator and was originally introduced for Gazebo tutorials. -The *RRBot* URDF files can be found in the `urdf` folder of `rrbot_description` package. +The *RRBot* URDF files can be found in the ``urdf`` folder of ``rrbot_description`` package. 1. To check that *RRBot* descriptions are working properly use following launch commands: @@ -13,9 +13,9 @@ The *RRBot* URDF files can be found in the `urdf` folder of `rrbot_description` ``` ros2 launch rrbot_description 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`. + **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``. 1. To check that *RRBot* descriptions are working properly use following launch commands: @@ -24,37 +24,37 @@ The *RRBot* URDF files can be found in the `urdf` folder of `rrbot_description` ``` ros2 launch rrbot_description 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`. + **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``. 1. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with: ``` ros2 launch ros2_control_demo_bringup rrbot.launch.py ``` - The launch file loads and starts the robot hardware, controllers and opens `RViz`. + 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. + 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*. 1. Check if the hardware interface loaded properly, by opening another terminal and executing: ``` ros2 control list_hardware_interfaces ``` - You should get: - ``` - command interfaces + + You should get:: + + command interfaces joint1/position [claimed] joint2/position [claimed] - state interfaces - joint1/position - joint2/position + state interfaces + joint1/position + joint2/position - ``` - Marker `[claimed]` by command interfaces means that a controller has access to command *RRBot*. + Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. 1. Check is controllers are running: ``` @@ -78,7 +78,7 @@ The *RRBot* URDF files can be found in the `urdf` folder of `rrbot_description` ``` ros2 launch ros2_control_demo_bringup test_forward_position_controller.launch.py ``` - You should now see orange and yellow blocks moving in `RViz`. + 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. @@ -94,5 +94,5 @@ Files used for this demos: Controllers from this demo: - - `Joint State Broadcaster` ([`ros2_controllers` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/joint_state_broadcaster/doc/userdoc.html) - - `Forward Command Controller` ([`ros2_controllers` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/forward_command_controller/doc/userdoc.html) + - ``Joint State Broadcaster`` ([``ros2_controllers`` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/joint_state_broadcaster/doc/userdoc.html) + - ``Forward Command Controller`` ([``ros2_controllers`` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/forward_command_controller/doc/userdoc.html) diff --git a/example_1/hardware/rrbot.hpp b/example_1/hardware/rrbot.hpp index cdf22b62e..1ba370fc3 100644 --- a/example_1/hardware/rrbot.hpp +++ b/example_1/hardware/rrbot.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_ -#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_ +#ifndef EXAMPLE_1__HARDWARE__RRBOT_HPP_ +#define EXAMPLE_1__HARDWARE__RRBOT_HPP_ #include #include @@ -78,4 +78,4 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa } // namespace ros2_control_demo_hardware -#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_ +#endif // EXAMPLE_1__HARDWARE__RRBOT_HPP_ diff --git a/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py b/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py new file mode 100644 index 000000000..6d3d0dca3 --- /dev/null +++ b/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py @@ -0,0 +1,70 @@ +# 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, 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 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_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 + prefix = LaunchConfiguration("prefix") + slowdown = LaunchConfiguration("slowdown") + robot_controller = LaunchConfiguration("robot_controller") + start_rviz = LaunchConfiguration("start_rviz") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), + launch_arguments={ + "description_file": "rrbot_transmissions_system_position_only.urdf.xacro", + "prefix": prefix, + "slowdown": slowdown, + "robot_controller": robot_controller, + "start_rviz": start_rviz, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro new file mode 100644 index 000000000..590b21c9f --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro @@ -0,0 +1,50 @@ + + + + + + + + + ros2_control_demo_hardware/RRBotTransmissionsSystemPositionOnlyHardware + ${slowdown} + + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + transmission_interface/SimpleTransmission + + + 2.0 + 0.0 + + + + + transmission_interface/SimpleTransmission + + + 4.0 + 0.0 + + + + + + + diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro new file mode 100644 index 000000000..f9fd6347c --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index e27bb5658..4cbe811ea 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(transmission_interface REQUIRED) ## COMPILE @@ -26,6 +27,7 @@ add_library( src/rrbot_system_position_only.cpp src/rrbot_system_multi_interface.cpp src/rrbot_system_with_sensor.cpp + src/rrbot_transmissions_system_position_only.cpp src/rrbot_actuator.cpp src/external_rrbot_force_torque_sensor.cpp ) @@ -40,13 +42,14 @@ ament_target_dependencies( pluginlib rclcpp rclcpp_lifecycle + transmission_interface ) # 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_HARDWARE_BUILDING_DLL") -# Export hardware pligins +# Export hardware plugins pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_hardware.xml) # INSTALL @@ -75,5 +78,6 @@ ament_export_dependencies( pluginlib rclcpp rclcpp_lifecycle + transmission_interface ) ament_package() diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp new file mode 100644 index 000000000..80e3692f5 --- /dev/null +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -0,0 +1,96 @@ +// Copyright 2020 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. + +#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_ +#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_ + +#include +#include +#include +#include + +#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/clock.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "ros2_control_demo_hardware/visibility_control.h" +#include "transmission_interface/transmission.hpp" + +namespace ros2_control_demo_hardware +{ +class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface::SystemInterface +{ +public: + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::unique_ptr logger_; + std::unique_ptr clock_; + + // parameters for the RRBot simulation + double actuator_slowdown_; + + // transmissions + std::vector> transmissions_; + + struct InterfaceData + { + explicit InterfaceData(const std::string & name); + + std::string name_; + double command_; + double state_; + + // this is the "sink" that will be part of the transmission Joint/Actuator handles + double transmission_passthrough_; + }; + std::vector joint_interfaces_; + std::vector actuator_interfaces_; +}; + +} // namespace ros2_control_demo_hardware + +#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_ diff --git a/ros2_control_demo_hardware/package.xml b/ros2_control_demo_hardware/package.xml index 866c80bab..4e996853f 100644 --- a/ros2_control_demo_hardware/package.xml +++ b/ros2_control_demo_hardware/package.xml @@ -15,6 +15,7 @@ pluginlib rclcpp rclcpp_lifecycle + transmission_interface ament_cmake_gtest diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index 519d54456..1456dc50a 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -41,4 +41,11 @@ The ros2_control RRBot example using external Force Torque Sensor where it is separate from the RRBot. + + + The ros2_control minimal robot protocol with transmissions. + + diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp new file mode 100644 index 000000000..929a39618 --- /dev/null +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -0,0 +1,343 @@ +// Copyright 2020 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. + +#include "ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logging.hpp" +#include "transmission_interface/simple_transmission_loader.hpp" +#include "transmission_interface/transmission.hpp" +#include "transmission_interface/transmission_interface_exception.hpp" + +namespace ros2_control_demo_hardware +{ + +constexpr double kNaN = std::numeric_limits::quiet_NaN(); + +hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + logger_ = std::make_unique( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware")); + + clock_ = std::make_unique(); + + RCLCPP_INFO(*logger_, "Initializing..."); + + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + actuator_slowdown_ = std::stod(info_.hardware_parameters["actuator_slowdown"]); + + const auto num_joints = std::accumulate( + info_.transmissions.begin(), info_.transmissions.end(), 0ul, + [](const auto & acc, const auto & trans_info) { return acc + trans_info.joints.size(); }); + + const auto num_actuators = std::accumulate( + info_.transmissions.begin(), info_.transmissions.end(), 0ul, + [](const auto & acc, const auto & trans_info) { return acc + trans_info.actuators.size(); }); + + // reserve the space needed for joint and actuator data structures + joint_interfaces_.reserve(num_joints); + actuator_interfaces_.reserve(num_actuators); + + // create transmissions, joint and actuator handles + auto transmission_loader = transmission_interface::SimpleTransmissionLoader(); + + for (const auto & transmission_info : info_.transmissions) + { + // only simple transmissions are supported in this demo + if (transmission_info.type != "transmission_interface/SimpleTransmission") + { + RCLCPP_FATAL( + *logger_, "Transmission '%s' of type '%s' not supported in this demo", + transmission_info.name.c_str(), transmission_info.type.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + std::shared_ptr transmission; + try + { + transmission = transmission_loader.load(transmission_info); + } + catch (const transmission_interface::TransmissionInterfaceException & exc) + { + RCLCPP_FATAL( + *logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); + return hardware_interface::CallbackReturn::ERROR; + } + + std::vector joint_handles; + for (const auto & joint_info : transmission_info.joints) + { + // this demo supports only one interface per joint + if (!(joint_info.interfaces.size() == 1 && + joint_info.interfaces[0] == hardware_interface::HW_IF_POSITION)) + { + RCLCPP_FATAL( + *logger_, "Invalid transmission joint '%s' configuration for this demo", + joint_info.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + const auto joint_interface = + joint_interfaces_.insert(joint_interfaces_.end(), InterfaceData(joint_info.name)); + + transmission_interface::JointHandle joint_handle( + joint_info.name, hardware_interface::HW_IF_POSITION, + &joint_interface->transmission_passthrough_); + joint_handles.push_back(joint_handle); + } + + std::vector actuator_handles; + for (const auto & actuator_info : transmission_info.actuators) + { + // no check for actuators types + + const auto actuator_interface = + actuator_interfaces_.insert(actuator_interfaces_.end(), InterfaceData(actuator_info.name)); + transmission_interface::ActuatorHandle actuator_handle( + actuator_info.name, hardware_interface::HW_IF_POSITION, + &actuator_interface->transmission_passthrough_); + actuator_handles.push_back(actuator_handle); + } + + /// @note no need to store the joint and actuator handles, the transmission + /// will keep whatever info it needs after is done with them + + try + { + transmission->configure(joint_handles, actuator_handles); + } + catch (const transmission_interface::TransmissionInterfaceException & exc) + { + RCLCPP_FATAL( + *logger_, "Error while configuring %s: %s", transmission_info.name.c_str(), exc.what()); + return hardware_interface::CallbackReturn::ERROR; + } + + transmissions_.push_back(transmission); + } + + RCLCPP_INFO(*logger_, "Initialization successful"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(*logger_, "Configuring..."); + + auto reset_interfaces = [](std::vector & interfaces) + { + for (auto & interface_data : interfaces) + { + interface_data.command_ = 0.0; + interface_data.state_ = 0.0; + interface_data.transmission_passthrough_ = kNaN; + } + }; + + reset_interfaces(joint_interfaces_); + reset_interfaces(actuator_interfaces_); + + RCLCPP_INFO(*logger_, "Configuration successful"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +RRBotTransmissionsSystemPositionOnlyHardware::export_state_interfaces() +{ + std::vector state_interfaces; + for (const auto & joint : info_.joints) + { + /// @pre all joint interfaces exist, checked in on_init() + auto joint_interface = std::find_if( + joint_interfaces_.begin(), joint_interfaces_.end(), + [&](const InterfaceData & interface) { return interface.name_ == joint.name; }); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->state_)); + } + return state_interfaces; +} + +std::vector +RRBotTransmissionsSystemPositionOnlyHardware::export_command_interfaces() +{ + std::vector command_interfaces; + for (const auto & joint : info_.joints) + { + /// @pre all joint interfaces exist, checked in on_init() + auto joint_interface = std::find_if( + joint_interfaces_.begin(), joint_interfaces_.end(), + [&](const InterfaceData & interface) { return interface.name_ == joint.name; }); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->command_)); + } + return command_interfaces; +} + +hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(*logger_, "Activating..."); + RCLCPP_INFO(*logger_, "Activation successful"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(*logger_, "Deactivating..."); + RCLCPP_INFO(*logger_, "Deactivation successful"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // actuator: state -> transmission + std::for_each( + actuator_interfaces_.begin(), actuator_interfaces_.end(), + [](auto & actuator_interface) + { actuator_interface.transmission_passthrough_ = actuator_interface.state_; }); + + // transmission: actuator -> joint + std::for_each( + transmissions_.begin(), transmissions_.end(), + [](auto & transmission) { transmission->actuator_to_joint(); }); + + // joint: transmission -> state + std::for_each( + joint_interfaces_.begin(), joint_interfaces_.end(), + [](auto & joint_interface) + { joint_interface.state_ = joint_interface.transmission_passthrough_; }); + + // log state data + std::stringstream ss; + ss << "State data:"; + for (const auto & transmission_info : info_.transmissions) + { + // again, this only for simple transmissions, we know there is only one joint + const auto joint_interface = std::find_if( + joint_interfaces_.cbegin(), joint_interfaces_.cend(), + [&](const auto & joint_interface) + { return joint_interface.name_ == transmission_info.joints[0].name; }); + + const auto actuator_interface = std::find_if( + actuator_interfaces_.cbegin(), actuator_interfaces_.cend(), + [&](const auto & actuator_interface) + { return actuator_interface.name_ == transmission_info.actuators[0].name; }); + + const auto & reduction = transmission_info.joints[0].mechanical_reduction; + + ss << std::endl + << "\t" << joint_interface->name_ << ": " << joint_interface->state_ << " <-- " + << transmission_info.name << "(R=" << reduction << ") <-- " << actuator_interface->name_ + << ": " << actuator_interface->state_; + } + RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // joint: command -> transmission + std::for_each( + joint_interfaces_.begin(), joint_interfaces_.end(), + [](auto & joint_interface) + { joint_interface.transmission_passthrough_ = joint_interface.command_; }); + + // transmission: joint -> actuator + std::for_each( + transmissions_.begin(), transmissions_.end(), + [](auto & transmission) { transmission->joint_to_actuator(); }); + + // actuator: transmission -> command + std::for_each( + actuator_interfaces_.begin(), actuator_interfaces_.end(), + [](auto & actuator_interface) + { actuator_interface.command_ = actuator_interface.transmission_passthrough_; }); + + // simulate motor motion + std::for_each( + actuator_interfaces_.begin(), actuator_interfaces_.end(), + [&](auto & actuator_interface) + { + actuator_interface.state_ = + actuator_interface.state_ + + (actuator_interface.command_ - actuator_interface.state_) / actuator_slowdown_; + }); + + // log command data + std::stringstream ss; + ss << "Command data:"; + for (const auto & transmission_info : info_.transmissions) + { + // again, this only for simple transmissions, we know there is only one joint + const auto joint_interface = std::find_if( + joint_interfaces_.cbegin(), joint_interfaces_.cend(), + [&](const auto & joint_interface) + { return joint_interface.name_ == transmission_info.joints[0].name; }); + + const auto actuator_interface = std::find_if( + actuator_interfaces_.cbegin(), actuator_interfaces_.cend(), + [&](const auto & actuator_interface) + { return actuator_interface.name_ == transmission_info.actuators[0].name; }); + + const auto & reduction = transmission_info.joints[0].mechanical_reduction; + + ss << std::endl + << "\t" << joint_interface->name_ << ": " << joint_interface->command_ << " --> " + << transmission_info.name << "(R=" << reduction << ") --> " << actuator_interface->name_ + << ": " << actuator_interface->command_; + } + RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); + + return hardware_interface::return_type::OK; +} + +RRBotTransmissionsSystemPositionOnlyHardware::InterfaceData::InterfaceData(const std::string & name) +: name_(name), command_(kNaN), state_(kNaN), transmission_passthrough_(kNaN) +{ +} + +} // namespace ros2_control_demo_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_hardware::RRBotTransmissionsSystemPositionOnlyHardware, + hardware_interface::SystemInterface)