Merge pull request robotology#53 from elandini84/feature/ros2_control…

…/forward_pos

Added a first ros2_control hardware interface

.gitignore

@@ -30,3 +30,5 @@ compile_commands.json
 /.cache/
 /ros2_interfaces_ws/install/
 /ros2_interfaces_ws/log/
+/ros2_components_ws/install/
+/ros2_components_ws/log/

resources/CMakeLists.txt

@@ -2,3 +2,4 @@
 # SPDX-License-Identifier: BSD-3-Clause
 
 add_subdirectory(ros2_frameTransform_config)
+add_subdirectory(ros2_control_demo)

resources/ros2_control_demo/CMakeLists.txt

@@ -0,0 +1,8 @@
+# SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
+# SPDX-License-Identifier: BSD-3-Clause
+
+set(RESNAME ros2_control_demo)
+
+file(GLOB apps ${CMAKE_CURRENT_SOURCE_DIR}/scripts/*.xml)
+
+yarp_install(FILES ${apps}    DESTINATION ${YARP-DEVICES-ROS2_APPLICATIONS_INSTALL_DIR})

resources/ros2_control_demo/scripts/ros2_control_R1SN003.xml

@@ -0,0 +1,34 @@
+<application>
+   <name>ros2_control_demo_R1SN003</name>
+   <dependencies>
+   </dependencies>
+
+   <!-- modules -->
+
+   <module>
+      <name>yarprobotinterface</name>
+      <parameters></parameters>
+      <workdir>/home/user1/robotology/robots-configuration/R1SN003/</workdir>
+      <node>r1-base</node>
+   </module>
+
+   <module>
+      <name>ros2</name>
+      <parameters>launch cer_ros2_control cer_R1SN003_neck_pos_only.launch.py</parameters>
+      <node>r1-base</node>
+   </module>
+
+   <module>
+      <name>yarpmotorgui</name>
+      <parameters></parameters>
+      <workdir>/home/user1/robotology/robots-configuration/R1SN003/</workdir>
+      <node>console</node>
+   </module>
+
+   <module>
+      <name>rviz2</name>
+      <parameters>-d cer_config.rviz</parameters>
+      <workdir>/home/user1/robotology/cer-sim/colcon_ws/src/cer_rviz2/rviz2</workdir>
+      <node>console</node>
+   </module>
+</application>

ros2_components_ws/src/cb_hw_fw_pos/CMakeLists.txt

@@ -0,0 +1,97 @@
+cmake_minimum_required(VERSION 3.8)
+project(cb_hw_fw_pos)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rcpputils REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(yarp_control_msgs REQUIRED)
+
+add_library(${PROJECT_NAME}
+  SHARED
+  src/cb_hw_fw_pos.cpp
+  )
+target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_include_directories(
+  ${PROJECT_NAME}
+  PRIVATE
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  lifecycle_msgs
+  control_msgs
+  rcpputils
+  rcutils
+  yarp_control_msgs
+)
+
+# 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 "CB_HW_FW_POS_BUILDING_LIBRARY")
+
+#target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+pluginlib_export_plugin_description_file(hardware_interface ${PROJECT_NAME}.xml)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(
+  include
+)
+
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+
+ament_export_targets(
+ export_${PROJECT_NAME}
+)
+
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+
+ament_package()

ros2_components_ws/src/cb_hw_fw_pos/cb_hw_fw_pos.xml

@@ -0,0 +1,9 @@
+<library path="cb_hw_fw_pos">
+    <class name="cb_hw_fw_pos/CbHwFwPos"
+           type="cb_hw_fw_pos::CbHwFwPos"
+           base_class_type="hardware_interface::SystemInterface">
+        <description>
+            Hardware interface to forward position commands to yarp controlBoard_nws_ros2
+        </description>
+    </class>
+</library>

ros2_components_ws/src/cb_hw_fw_pos/include/cb_hw_fw_pos/cb_hw_fw_pos.hpp

@@ -0,0 +1,134 @@
+#ifndef CB_HW_FW_POS__CB_HW_FW_POS_HPP_
+#define CB_HW_FW_POS__CB_HW_FW_POS_HPP_
+
+#include <string>
+#include <vector>
+
+#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 "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include <rclcpp/rclcpp.hpp>
+
+//Custom ros2 interfaces
+#include <yarp_control_msgs/srv/get_control_modes.hpp>
+#include <yarp_control_msgs/srv/get_position.hpp>
+#include <yarp_control_msgs/srv/get_velocity.hpp>
+#include <yarp_control_msgs/srv/set_control_modes.hpp>
+#include <yarp_control_msgs/srv/get_available_control_modes.hpp>
+#include <yarp_control_msgs/srv/get_joints_names.hpp>
+#include <yarp_control_msgs/msg/position.hpp>
+#include <yarp_control_msgs/msg/velocity.hpp>
+#include <yarp_control_msgs/msg/position_direct.hpp>
+
+#include <mutex>
+
+#include "cb_hw_fw_pos/visibility_control.h"
+
+namespace cb_hw_fw_pos {
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+/*
+ * ROS2 ros2_control hardare interface for sending position commands to a controlBoard_nws_ros2.
+ * It has been created to allow controlling a YARP based robot through ros2_control tools
+ * Since it is a "write-only" interface, it cannot be used with controllers that require to get
+ * data back from the hardware component.
+ *
+ * ---
+ *
+ * This interface can either publish continuously the values stored in "CbHwFwPos::m_hwCommandsPositions"
+ * basically locking, as long as the interface is active, the joints in position or it can publish it only
+ * if the stored values change.
+ */
+
+class CbHwFwPos : public hardware_interface::SystemInterface
+{
+private:
+    bool         m_active{false};
+    bool         m_continuousPosWrite{true};
+    std::string  m_nodeName;             // name of the rosNode
+    std::string  m_msgs_name;            // prefix for control_board_nws_ros2 services and topics
+    // ControlBoard_nws_ros2 related topics and services names
+    std::string  m_posTopicName;                 // Position commands topic
+    std::string  m_getModesClientName;           // Service client for joints current control modes
+    std::string  m_getPositionClientName;        // Service client to get current position values
+    std::string  m_getJointsNamesClientName;     // Service client to get the available joints names
+    mutable std::mutex       m_cmdMutex;
+    std::vector<std::string> m_jointNames; // name of the joints
+
+    // Command interfaces
+    // Store the commands for the robot
+    std::vector<double> m_hwCommandsPositions;
+    std::vector<double> m_oldPositions; // This array has to be used in order to avoid sending
+                                        // the same position commans over and over
+    std::vector<std::string> m_modes; // Current joints control modes.
+                                      // We have to assume that the joints do not change
+                                      // control mode without this hw interface class knowing.
+                                      // In other words, the only way to switch control mode
+                                      // should be via "perform_command_mode_switch" method.
+
+    // Ros2 related attributes
+    rclcpp::Node::SharedPtr m_node;
+
+    //  yarp_control_msgs clients and publisher
+    rclcpp::Publisher<yarp_control_msgs::msg::Position>::SharedPtr              m_posPublisher;
+    rclcpp::Client<yarp_control_msgs::srv::GetJointsNames>::SharedPtr           m_getJointsNamesClient;
+    rclcpp::Client<yarp_control_msgs::srv::GetControlModes>::SharedPtr          m_getControlModesClient;
+    rclcpp::Client<yarp_control_msgs::srv::GetPosition>::SharedPtr              m_getPositionClient;
+
+    // Internal functions
+    /* Check whether or not the joints specified in the ros2 robot configuration files are actually
+     * available on the robot
+     */
+    bool _checkJoints(const std::vector<hardware_interface::ComponentInfo>& joints);
+    /* It initializes the m_hwCommandsPositions and m_oldPositions arrays and checks that the
+     * command interfaces specified in the Component infos are only "position"
+     */
+    CallbackReturn _initExportableInterfaces(const std::vector<hardware_interface::ComponentInfo>& joints);
+    /* Simply read the current position values
+     */
+    CallbackReturn _getHWCurrentValues();
+
+public:
+    RCLCPP_SHARED_PTR_DEFINITIONS(CbHwFwPos)
+    CbHwFwPos();
+    ~CbHwFwPos();
+
+    // SystemInterface virtual functions
+    CB_HW_FW_POS_PUBLIC
+    CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+
+    CB_HW_FW_POS_PUBLIC
+    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+    CB_HW_FW_POS_PUBLIC
+    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+    CB_HW_FW_POS_PUBLIC
+    hardware_interface::return_type prepare_command_mode_switch(const std::vector<std::string> & start_interfaces, const std::vector<std::string> & stop_interfaces) override;
+
+    CB_HW_FW_POS_PUBLIC
+    hardware_interface::return_type perform_command_mode_switch(const std::vector<std::string> & /*start_interfaces*/, const std::vector<std::string> & /*stop_interfaces*/) override;
+
+    CB_HW_FW_POS_PUBLIC
+    hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+    CB_HW_FW_POS_PUBLIC
+    hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+    CB_HW_FW_POS_PUBLIC
+    hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+    CB_HW_FW_POS_PUBLIC
+    hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
+
+};
+
+}  // namespace cb_hw_fw_pos
+
+#endif  // CB_HW_FW_POS__CB_HW_INTERFACE_HPP_

ros2_components_ws/src/cb_hw_fw_pos/include/cb_hw_fw_pos/visibility_control.h

@@ -0,0 +1,35 @@
+#ifndef CB_HW_FW_POS__VISIBILITY_CONTROL_H_
+#define CB_HW_FW_POS__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 CB_HW_FW_POS_EXPORT __attribute__ ((dllexport))
+    #define CB_HW_FW_POS_IMPORT __attribute__ ((dllimport))
+  #else
+    #define CB_HW_FW_POS_EXPORT __declspec(dllexport)
+    #define CB_HW_FW_POS_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef CB_HW_FW_POS_BUILDING_LIBRARY
+    #define CB_HW_FW_POS_PUBLIC CB_HW_FW_POS_EXPORT
+  #else
+    #define CB_HW_FW_POS_PUBLIC CB_HW_FW_POS_IMPORT
+  #endif
+  #define CB_HW_FW_POS_PUBLIC_TYPE CB_HW_FW_POS_PUBLIC
+  #define CB_HW_FW_POS_LOCAL
+#else
+  #define CB_HW_FW_POS_EXPORT __attribute__ ((visibility("default")))
+  #define CB_HW_FW_POS_IMPORT
+  #if __GNUC__ >= 4
+    #define CB_HW_FW_POS_PUBLIC __attribute__ ((visibility("default")))
+    #define CB_HW_FW_POS_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define CB_HW_FW_POS_PUBLIC
+    #define CB_HW_FW_POS_LOCAL
+  #endif
+  #define CB_HW_FW_POS_PUBLIC_TYPE
+#endif
+
+#endif  // CB_HW_FW_POS__VISIBILITY_CONTROL_H_

ros2_components_ws/src/cb_hw_fw_pos/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cb_hw_fw_pos</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">user1</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>control_msgs</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>rcpputils</depend>
+  <depend>rcutils</depend>
+  <depend>hardware_interface</depend>
+  <depend>yarp_control_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>