Merge branch 'master' into feature/velocity_control_PR

controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp

@@ -20,7 +20,7 @@
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
-#include "kuka_driver_interfaces/srv/set_int.hpp"
+#include "kuka_driver_interfaces/msg/fri_configuration.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/time.hpp"
@@ -53,9 +53,9 @@ class FRIConfigurationController : public controller_interface::ControllerInterf
   FRI_CONFIGURATION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override;
 
 private:
-  rclcpp::Service<kuka_driver_interfaces::srv::SetInt>::SharedPtr receive_multiplier_service_;
+  rclcpp::Subscription<kuka_driver_interfaces::msg::FriConfiguration>::SharedPtr fri_config_sub_;
   int receive_multiplier_ = 1;
-  bool resend_multiplier_ = false;
+  int send_period_ms_ = 10;
 };
 }  // namespace kuka_controllers
 #endif  // FRI_CONFIGURATION_CONTROLLER__FRI_CONFIGURATION_CONTROLLER_HPP_

controllers/fri_configuration_controller/src/fri_configuration_controller.cpp

@@ -20,19 +20,13 @@ namespace kuka_controllers
 {
 controller_interface::CallbackReturn FRIConfigurationController::on_init()
 {
-  auto callback = [this](
-                    kuka_driver_interfaces::srv::SetInt::Request::SharedPtr request,
-                    kuka_driver_interfaces::srv::SetInt::Response::SharedPtr response)
+  auto callback = [this](const kuka_driver_interfaces::msg::FriConfiguration::SharedPtr msg)
   {
-    resend_multiplier_ = true;
-    receive_multiplier_ = request->data;
-    response->success = true;
+    receive_multiplier_ = msg->receive_multiplier;
+    send_period_ms_ = msg->send_period_ms;
   };
-  receive_multiplier_service_ = get_node()->create_service<kuka_driver_interfaces::srv::SetInt>(
-    "~/set_receive_multiplier", callback);
-  // TODO(Svastits): create service to get multiplier changes (or perpaps
-  // parameter??)
-  //   and set resend_multiplier_ to true in the callback
+  fri_config_sub_ = get_node()->create_subscription<kuka_driver_interfaces::msg::FriConfiguration>(
+    "~/set_fri_config", rclcpp::SystemDefaultsQoS(), callback);
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -43,6 +37,8 @@ FRIConfigurationController::command_interface_configuration() const
   config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
   config.names.emplace_back(
     std::string(hardware_interface::CONFIG_PREFIX) + "/" + hardware_interface::RECEIVE_MULTIPLIER);
+  config.names.emplace_back(
+    std::string(hardware_interface::CONFIG_PREFIX) + "/" + hardware_interface::SEND_PERIOD);
   return config;
 }
 
@@ -75,14 +71,9 @@ controller_interface::return_type FRIConfigurationController::update(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
   // TODO(Svastits): disable changes if HWIF is active
-  if (resend_multiplier_)
-  {
-    RCLCPP_INFO(
-      get_node()->get_logger(), "Changing receive multiplier of hardware interface to %i",
-      receive_multiplier_);
-    command_interfaces_[0].set_value(receive_multiplier_);
-    resend_multiplier_ = false;
-  }
+  command_interfaces_[0].set_value(receive_multiplier_);
+  command_interfaces_[1].set_value(send_period_ms_);
+
   return controller_interface::return_type::OK;
 }
 

controllers/control_mode_handler/CMakeLists.txt → controllers/kuka_control_mode_handler/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(control_mode_handler)
+project(kuka_control_mode_handler)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra)
@@ -13,7 +13,7 @@ find_package(kuka_drivers_core REQUIRED)
 include_directories(include)
 
 add_library(${PROJECT_NAME} SHARED
-  src/control_mode_handler.cpp)
+  src/kuka_control_mode_handler.cpp)
 
 target_include_directories(${PROJECT_NAME} PRIVATE
   include
@@ -24,7 +24,7 @@ ament_target_dependencies(${PROJECT_NAME} controller_interface std_msgs kuka_dri
 
 # 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 "CONTROL_MODE_HANDLER_BUILDING_LIBRARY")
+target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_CONTROL_MODE_HANDLER_BUILDING_LIBRARY")
 # prevent pluginlib from using boost
 target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 

controllers/control_mode_handler/controller_plugins.xml → controllers/kuka_control_mode_handler/controller_plugins.xml

@@ -1,4 +1,4 @@
-<library path="control_mode_handler">
+<library path="kuka_control_mode_handler">
   <class name="kuka_controllers/ControlModeHandler" type="kuka_controllers::ControlModeHandler" base_class_type="controller_interface::ControllerInterface">
     <description>
       This controller sets the control mode of KUKA robots in runtime

controllers/control_mode_handler/include/control_mode_handler/control_mode_handler.hpp → controllers/kuka_control_mode_handler/include/kuka_control_mode_handler/kuka_control_mode_handler.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef CONTROL_MODE_HANDLER__CONTROL_MODE_HANDLER_HPP_
-#define CONTROL_MODE_HANDLER__CONTROL_MODE_HANDLER_HPP_
+#ifndef KUKA_CONTROL_MODE_HANDLER__KUKA_CONTROL_MODE_HANDLER_HPP_
+#define KUKA_CONTROL_MODE_HANDLER__KUKA_CONTROL_MODE_HANDLER_HPP_
 
 #include <memory>
 #include <string>
@@ -26,37 +26,37 @@
 #include "rclcpp/time.hpp"
 #include "std_msgs/msg/u_int32.hpp"
 
-#include "control_mode_handler/visibility_control.h"
+#include "kuka_control_mode_handler/visibility_control.h"
 
 namespace kuka_controllers
 {
 class ControlModeHandler : public controller_interface::ControllerInterface
 {
 public:
-  CONTROL_MODE_HANDLER_PUBLIC controller_interface::InterfaceConfiguration
+  KUKA_CONTROL_MODE_HANDLER_PUBLIC controller_interface::InterfaceConfiguration
   command_interface_configuration() const override;
 
-  CONTROL_MODE_HANDLER_PUBLIC controller_interface::InterfaceConfiguration
+  KUKA_CONTROL_MODE_HANDLER_PUBLIC controller_interface::InterfaceConfiguration
   state_interface_configuration() const override;
 
-  CONTROL_MODE_HANDLER_PUBLIC controller_interface::return_type update(
+  KUKA_CONTROL_MODE_HANDLER_PUBLIC controller_interface::return_type update(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
-  CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_configure(
+  KUKA_CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_activate(
+  KUKA_CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_activate(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_deactivate(
+  KUKA_CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_deactivate(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_init() override;
+  KUKA_CONTROL_MODE_HANDLER_PUBLIC controller_interface::CallbackReturn on_init() override;
 
 private:
   rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr control_mode_subscriber_;
   kuka_drivers_core::ControlMode control_mode_ =
     kuka_drivers_core::ControlMode::CONTROL_MODE_UNSPECIFIED;
 };
 }  // namespace kuka_controllers
-#endif  // CONTROL_MODE_HANDLER__CONTROL_MODE_HANDLER_HPP_
+#endif  // KUKA_CONTROL_MODE_HANDLER__KUKA_CONTROL_MODE_HANDLER_HPP_

controllers/kuka_control_mode_handler/include/kuka_control_mode_handler/visibility_control.h

@@ -0,0 +1,49 @@
+//    Copyright 2023 Aron Svastits
+//
+//    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 KUKA_CONTROL_MODE_HANDLER__VISIBILITY_CONTROL_H_
+#define KUKA_CONTROL_MODE_HANDLER__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 KUKA_CONTROL_MODE_HANDLER_EXPORT __attribute__((dllexport))
+#define KUKA_CONTROL_MODE_HANDLER_IMPORT __attribute__((dllimport))
+#else
+#define KUKA_CONTROL_MODE_HANDLER_EXPORT __declspec(dllexport)
+#define KUKA_CONTROL_MODE_HANDLER_IMPORT __declspec(dllimport)
+#endif
+#ifdef KUKA_CONTROL_MODE_HANDLER_BUILDING_LIBRARY
+#define KUKA_CONTROL_MODE_HANDLER_PUBLIC KUKA_CONTROL_MODE_HANDLER_EXPORT
+#else
+#define KUKA_CONTROL_MODE_HANDLER_PUBLIC KUKA_CONTROL_MODE_HANDLER_IMPORT
+#endif
+#define KUKA_CONTROL_MODE_HANDLER_PUBLIC_TYPE KUKA_CONTROL_MODE_HANDLER_PUBLIC
+#define KUKA_CONTROL_MODE_HANDLER_LOCAL
+#else
+#define KUKA_CONTROL_MODE_HANDLER_EXPORT __attribute__((visibility("default")))
+#define KUKA_CONTROL_MODE_HANDLER_IMPORT
+#if __GNUC__ >= 4
+#define KUKA_CONTROL_MODE_HANDLER_PUBLIC __attribute__((visibility("default")))
+#define KUKA_CONTROL_MODE_HANDLER_LOCAL __attribute__((visibility("hidden")))
+#else
+#define KUKA_CONTROL_MODE_HANDLER_PUBLIC
+#define KUKA_CONTROL_MODE_HANDLER_LOCAL
+#endif
+#define KUKA_CONTROL_MODE_HANDLER_PUBLIC_TYPE
+#endif
+
+#endif  // KUKA_CONTROL_MODE_HANDLER__VISIBILITY_CONTROL_H_

controllers/control_mode_handler/package.xml → controllers/kuka_control_mode_handler/package.xml

@@ -1,7 +1,7 @@
 <?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>control_mode_handler</name>
+  <name>kuka_control_mode_handler</name>
   <version>0.9.0</version>
   <description>Controller for setting the control mode of KUKA robots in runtime</description>
 

controllers/control_mode_handler/src/control_mode_handler.cpp → controllers/kuka_control_mode_handler/src/kuka_control_mode_handler.cpp

@@ -14,7 +14,7 @@
 
 #include "kuka_drivers_core/hardware_interface_types.hpp"
 
-#include "control_mode_handler/control_mode_handler.hpp"
+#include "kuka_control_mode_handler/kuka_control_mode_handler.hpp"
 
 namespace kuka_controllers
 {

controllers/kuka_controllers/package.xml

@@ -8,8 +8,8 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <exec_depend>control_mode_handler</exec_depend>
-  <exec_depend>event_broadcaster</exec_depend>
+  <exec_depend>kuka_control_mode_handler</exec_depend>
+  <exec_depend>kuka_event_broadcaster</exec_depend>
   <exec_depend>fri_configuration_controller</exec_depend>
   <exec_depend>fri_state_broadcaster</exec_depend>
   <exec_depend>joint_group_impedance_controller</exec_depend>

controllers/event_broadcaster/CMakeLists.txt → controllers/kuka_event_broadcaster/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(event_broadcaster)
+project(kuka_event_broadcaster)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra)
@@ -13,7 +13,7 @@ find_package(kuka_drivers_core REQUIRED)
 include_directories(include)
 
 add_library(${PROJECT_NAME} SHARED
-  src/event_broadcaster.cpp)
+  src/kuka_event_broadcaster.cpp)
 
 target_include_directories(${PROJECT_NAME} PRIVATE
   include
@@ -24,7 +24,7 @@ ament_target_dependencies(${PROJECT_NAME} controller_interface kuka_drivers_core
 
 # 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 "EVENT_BROADCASTER_BUILDING_LIBRARY")
+target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_EVENT_BROADCASTER_BUILDING_LIBRARY")
 # prevent pluginlib from using boost
 target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 

controllers/event_broadcaster/controller_plugins.xml → controllers/kuka_event_broadcaster/controller_plugins.xml

@@ -1,4 +1,4 @@
-<library path="event_broadcaster">
+<library path="kuka_event_broadcaster">
   <class name="kuka_controllers/EventBroadcaster" type="kuka_controllers::EventBroadcaster" base_class_type="controller_interface::ControllerInterface">
     <description>
       This broadcaster publishes the state changes of ECI