Remove robot manager code duplications (#122)

* add ros2 control tools to core * snake_case file names * uset tools * finish iiqka restruct * remove sendrequests * remove some logs * fix build * clean up includes + lint * fix bug * combine conditions * lint --------- Co-authored-by: Aron Svastits <svastits1@gmail.com>
kroshu · Dec 14, 2023 · dc4674a · dc4674a
1 parent 0244938
commit dc4674a
Show file tree

Hide file tree

Showing 24 changed files with 260 additions and 348 deletions.
diff --git a/kuka_drivers_core/CMakeLists.txt b/kuka_drivers_core/CMakeLists.txt
@@ -22,10 +22,10 @@ find_package(lifecycle_msgs REQUIRED)
 find_package(controller_manager REQUIRED)
 
 add_library(kuka_drivers_core SHARED
-  src/ROS2BaseNode.cpp
-  src/ROS2BaseLCNode.cpp
-  src/ParameterHandler.cpp
-  src/ControllerHandler.cpp
+  src/ros2_base_node.cpp
+  src/ros2_base_lc_node.cpp
+  src/parameter_handler.cpp
+  src/controller_handler.cpp
 )
 ament_target_dependencies(kuka_drivers_core rclcpp rclcpp_lifecycle lifecycle_msgs)
 

diff --git a/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp b/kuka_drivers_core/include/communication_helpers/ros2_control_tools.hpp
@@ -0,0 +1,67 @@
+// Copyright 2020 Áron 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 COMMUNICATION_HELPERS__ROS2_CONTROL_TOOLS_HPP_
+#define COMMUNICATION_HELPERS__ROS2_CONTROL_TOOLS_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
+#include "controller_manager_msgs/srv/switch_controller.hpp"
+#include "communication_helpers/service_tools.hpp"
+
+namespace kuka_drivers_core
+{
+
+bool changeHardwareState(
+  rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr client,
+  const std::string & hardware_name, uint8_t state, int timeout_ms = 2000)
+{
+  auto hw_request =
+    std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>();
+  hw_request->name = hardware_name;
+  hw_request->target_state.id = state;
+  auto hw_response = sendRequest<controller_manager_msgs::srv::SetHardwareComponentState::Response>(
+    client, hw_request, 0, timeout_ms);
+  if (!hw_response || !hw_response->ok) {
+    return false;
+  }
+  return true;
+}
+
+bool changeControllerState(
+  rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr client,
+  const std::vector<std::string> & activate_controllers,
+  const std::vector<std::string> & deactivate_controllers,
+  int32_t strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT)
+{
+  auto controller_request =
+    std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
+  controller_request->strictness = strictness;
+  controller_request->activate_controllers = activate_controllers;
+  controller_request->deactivate_controllers = deactivate_controllers;
+
+  auto controller_response = sendRequest<controller_manager_msgs::srv::SwitchController::Response>(
+    client, controller_request, 0, 2000);
+  if (!controller_response || !controller_response->ok) {
+    return false;
+  }
+  return true;
+}
+}  // namespace kuka_drivers_core
+
+#endif  // COMMUNICATION_HELPERS__ROS2_CONTROL_TOOLS_HPP_
diff --git a/...include/kuka_drivers_core/ControlMode.hpp → ...nclude/kuka_drivers_core/control_mode.hpp b/...include/kuka_drivers_core/ControlMode.hpp → ...nclude/kuka_drivers_core/control_mode.hpp
@@ -13,8 +13,8 @@
 // limitations under the License.
 
 
-#ifndef KUKA_DRIVERS_CORE__CONTROLMODE_HPP_
-#define KUKA_DRIVERS_CORE__CONTROLMODE_HPP_
+#ifndef KUKA_DRIVERS_CORE__CONTROL_MODE_HPP_
+#define KUKA_DRIVERS_CORE__CONTROL_MODE_HPP_
 
 namespace kuka_drivers_core
 {
@@ -51,4 +51,4 @@ enum class ControllerType : std::uint8_t
 };
 }  // namespace kuka_drivers_core
 
-#endif  // KUKA_DRIVERS_CORE__CONTROLMODE_HPP_
+#endif  // KUKA_DRIVERS_CORE__CONTROL_MODE_HPP_
diff --git a/...e/kuka_drivers_core/ControllerHandler.hpp → .../kuka_drivers_core/controller_handler.hpp b/...e/kuka_drivers_core/ControllerHandler.hpp → .../kuka_drivers_core/controller_handler.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef KUKA_DRIVERS_CORE__CONTROLLERHANDLER_HPP_
-#define KUKA_DRIVERS_CORE__CONTROLLERHANDLER_HPP_
+#ifndef KUKA_DRIVERS_CORE__CONTROLLER_HANDLER_HPP_
+#define KUKA_DRIVERS_CORE__CONTROLLER_HANDLER_HPP_
 
 #include <string>
 #include <vector>
@@ -22,7 +22,7 @@
 #include <set>
 
 #include "rclcpp/rclcpp.hpp"
-#include "ControlMode.hpp"
+#include "control_mode.hpp"
 
 namespace kuka_drivers_core
 {
@@ -128,4 +128,4 @@ class ControllerHandler
 }  // namespace kuka_drivers_core
 
 
-#endif  // KUKA_DRIVERS_CORE__CONTROLLERHANDLER_HPP_
+#endif  // KUKA_DRIVERS_CORE__CONTROLLER_HANDLER_HPP_
diff --git a/...de/kuka_drivers_core/ParameterHandler.hpp → ...e/kuka_drivers_core/parameter_handler.hpp b/...de/kuka_drivers_core/ParameterHandler.hpp → ...e/kuka_drivers_core/parameter_handler.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef KUKA_DRIVERS_CORE__PARAMETERHANDLER_HPP_
-#define KUKA_DRIVERS_CORE__PARAMETERHANDLER_HPP_
+#ifndef KUKA_DRIVERS_CORE__PARAMETER_HANDLER_HPP_
+#define KUKA_DRIVERS_CORE__PARAMETER_HANDLER_HPP_
 
 #include <functional>
 #include <memory>
@@ -180,4 +180,4 @@ class ParameterHandler
 }  // namespace kuka_drivers_core
 
 
-#endif  // KUKA_DRIVERS_CORE__PARAMETERHANDLER_HPP_
+#endif  // KUKA_DRIVERS_CORE__PARAMETER_HANDLER_HPP_
diff --git a/...lude/kuka_drivers_core/ROS2BaseLCNode.hpp → ...e/kuka_drivers_core/ros2_base_lc_node.hpp b/...lude/kuka_drivers_core/ROS2BaseLCNode.hpp → ...e/kuka_drivers_core/ros2_base_lc_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef KUKA_DRIVERS_CORE__ROS2BASELCNODE_HPP_
-#define KUKA_DRIVERS_CORE__ROS2BASELCNODE_HPP_
+#ifndef KUKA_DRIVERS_CORE__ROS2_BASE_LC_NODE_HPP_
+#define KUKA_DRIVERS_CORE__ROS2_BASE_LC_NODE_HPP_
 
 #include <string>
 #include <map>
@@ -23,7 +23,7 @@
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 
-#include "kuka_drivers_core/ParameterHandler.hpp"
+#include "kuka_drivers_core/parameter_handler.hpp"
 
 namespace kuka_drivers_core
 {
@@ -90,4 +90,4 @@ class ROS2BaseLCNode : public rclcpp_lifecycle::LifecycleNode
 }  // namespace kuka_drivers_core
 
 
-#endif  // KUKA_DRIVERS_CORE__ROS2BASELCNODE_HPP_
+#endif  // KUKA_DRIVERS_CORE__ROS2_BASE_LC_NODE_HPP_
diff --git a/...nclude/kuka_drivers_core/ROS2BaseNode.hpp → ...lude/kuka_drivers_core/ros2_base_node.hpp b/...nclude/kuka_drivers_core/ROS2BaseNode.hpp → ...lude/kuka_drivers_core/ros2_base_node.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef KUKA_DRIVERS_CORE__ROS2BASENODE_HPP_
-#define KUKA_DRIVERS_CORE__ROS2BASENODE_HPP_
+#ifndef KUKA_DRIVERS_CORE__ROS2_BASE_NODE_HPP_
+#define KUKA_DRIVERS_CORE__ROS2_BASE_NODE_HPP_
 
 #include <string>
 #include <map>
@@ -24,7 +24,7 @@
 #include "rclcpp/node.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 
-#include "kuka_drivers_core/ParameterHandler.hpp"
+#include "kuka_drivers_core/parameter_handler.hpp"
 
 namespace kuka_drivers_core
 {
@@ -65,4 +65,4 @@ class ROS2BaseNode : public rclcpp::Node
 };
 }  // namespace kuka_drivers_core
 
-#endif  // KUKA_DRIVERS_CORE__ROS2BASENODE_HPP_
+#endif  // KUKA_DRIVERS_CORE__ROS2_BASE_NODE_HPP_
diff --git a/kuka_drivers_core/src/ControllerHandler.cpp → kuka_drivers_core/src/controller_handler.cpp b/kuka_drivers_core/src/ControllerHandler.cpp → kuka_drivers_core/src/controller_handler.cpp
@@ -15,7 +15,7 @@
 #include <vector>
 #include <utility>
 
-#include "kuka_drivers_core/ControllerHandler.hpp"
+#include "kuka_drivers_core/controller_handler.hpp"
 
 namespace kuka_drivers_core
 {

diff --git a/kuka_drivers_core/src/ParameterHandler.cpp → kuka_drivers_core/src/parameter_handler.cpp b/kuka_drivers_core/src/ParameterHandler.cpp → kuka_drivers_core/src/parameter_handler.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kuka_drivers_core/ParameterHandler.hpp"
+#include "kuka_drivers_core/parameter_handler.hpp"
 
 #include <string>
 #include <vector>

diff --git a/kuka_drivers_core/src/ROS2BaseLCNode.cpp → kuka_drivers_core/src/ros2_base_lc_node.cpp b/kuka_drivers_core/src/ROS2BaseLCNode.cpp → kuka_drivers_core/src/ros2_base_lc_node.cpp
@@ -16,7 +16,7 @@
 #include <vector>
 #include <memory>
 
-#include "kuka_drivers_core/ROS2BaseLCNode.hpp"
+#include "kuka_drivers_core/ros2_base_lc_node.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 namespace kuka_drivers_core

diff --git a/kuka_drivers_core/src/ROS2BaseNode.cpp → kuka_drivers_core/src/ros2_base_node.cpp b/kuka_drivers_core/src/ROS2BaseNode.cpp → kuka_drivers_core/src/ros2_base_node.cpp
@@ -16,7 +16,7 @@
 #include <vector>
 #include <memory>
 
-#include "kuka_drivers_core/ROS2BaseNode.hpp"
+#include "kuka_drivers_core/ros2_base_node.hpp"
 
 
 namespace kuka_drivers_core

diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp
@@ -23,17 +23,11 @@
 #include <mutex>
 #include <thread>
 
-#include "rclcpp/macros.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 "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-
 #include "pluginlib/class_list_macros.hpp"
+#include "hardware_interface/system_interface.hpp"
 
 #include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h"
 #include "nanopb/kuka/core/motion/joint.pb.hh"

diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp
@@ -22,16 +22,13 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/client.hpp"
-#include "lifecycle_msgs/srv/change_state.hpp"
-#include "lifecycle_msgs/msg/state.hpp"
-#include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
-#include "controller_manager_msgs/srv/switch_controller.hpp"
 #include "std_msgs/msg/bool.hpp"
 #include "std_msgs/msg/u_int32.hpp"
+#include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
+#include "controller_manager_msgs/srv/switch_controller.hpp"
 
-#include "communication_helpers/service_tools.hpp"
-#include "kuka_drivers_core/ROS2BaseLCNode.hpp"
-#include "kuka_drivers_core/ControllerHandler.hpp"
+#include "kuka_drivers_core/controller_handler.hpp"
+#include "kuka_drivers_core/ros2_base_lc_node.hpp"
 
 #include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h"
 
@@ -43,7 +40,6 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode
   RobotManagerNode();
   ~RobotManagerNode();
 
-
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_configure(const rclcpp_lifecycle::State &) override;
 

diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp
@@ -12,16 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kuka_iiqka_eac_driver/hardware_interface.hpp"
-
 #include <grpcpp/create_channel.h>
-
 #include <stdexcept>
 #include <string>
 #include <vector>
 
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
 #include "nanopb-helpers/nanopb_serialization_helper.h"
 
+#include "kuka_iiqka_eac_driver/hardware_interface.hpp"
+
 using namespace kuka::ecs::v1;  // NOLINT
 
 using os::core::udp::communication::Socket;