Merge pull request #50 from kroshu/feature/ros2_control

move comm helpers to kroshu core
kroshu · Jan 27, 2023 · 26b87ab · 26b87ab
2 parents 182d2b9 + 2b2a14a
commit 26b87ab
Show file tree

Hide file tree

Showing 12 changed files with 15 additions and 226 deletions.
diff --git a/kuka_rox_hardware_interface/CMakeLists.txt b/kuka_rox_hardware_interface/CMakeLists.txt
@@ -37,7 +37,6 @@ find_package(controller_manager REQUIRED)
 find_package(controller_manager_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
-find_package(kuka_sunrise) # TODO: move internal lib to another package
 include_directories(include)
 
 if(NOT MOCK_KUKA_LIBS)
@@ -84,7 +83,7 @@ target_link_libraries(rox_control_node kuka_rox_hw_interface)
 add_executable(robot_manager_node
   src/robot_manager_node.cpp)
 ament_target_dependencies(robot_manager_node rclcpp kroshu_ros2_core sensor_msgs controller_manager controller_manager_msgs)
-target_link_libraries(robot_manager_node kuka_rox_hw_interface kuka_sunrise::internal)
+target_link_libraries(robot_manager_node kuka_rox_hw_interface kroshu_ros2_core::communication_helpers)
 
 pluginlib_export_plugin_description_file(hardware_interface kuka_rox_hw_interface.xml)
 

diff --git a/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/robot_manager_node.hpp b/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/robot_manager_node.hpp
@@ -27,7 +27,7 @@
 #include "controller_manager_msgs/srv/switch_controller.hpp"
 #include "std_msgs/msg/bool.hpp"
 
-#include "kuka_sunrise/internal/service_tools.hpp"
+#include "communication_helpers/service_tools.hpp"
 
 #include "kroshu_ros2_core/ROS2BaseLCNode.hpp"
 

diff --git a/kuka_sunrise_driver/CMakeLists.txt b/kuka_sunrise_driver/CMakeLists.txt
@@ -33,20 +33,11 @@ find_package(controller_manager_msgs)
 
 include_directories(include)
 
-add_library(internal SHARED
-  include/kuka_sunrise/internal/activatable_interface.hpp
-  include/kuka_sunrise/internal/serialization.hpp
-  include/kuka_sunrise/internal/service_tools.hpp)
-ament_target_dependencies(internal rclcpp)
-set_target_properties(internal PROPERTIES LINKER_LANGUAGE CXX)
-
-ament_export_targets(internal HAS_LIBRARY_TARGET)
-ament_export_dependencies(rclcpp)
-
-
 add_library(robot_manager SHARED
   src/kuka_sunrise/robot_manager.cpp
   src/kuka_sunrise/tcp_connection.cpp)
+target_link_libraries(robot_manager
+  kroshu_ros2_core::communication_helpers)
 
 add_library(kuka_fri_hw_interface SHARED
   src/kuka_sunrise/kuka_fri_hardware_interface.cpp
@@ -81,14 +72,6 @@ install(DIRECTORY launch config
 install(DIRECTORY config
   DESTINATION share/${PROJECT_NAME})
 
-install(DIRECTORY include/kuka_sunrise/internal DESTINATION include/kuka_sunrise)
-install(
-  TARGETS internal
-  EXPORT internal
-  LIBRARY DESTINATION lib
-  INCLUDES DESTINATION include
-)
-
 if(BUILD_TESTING)
   find_package(ament_cmake_copyright REQUIRED)
   find_package(ament_cmake_cppcheck REQUIRED)

diff --git a/kuka_sunrise_driver/include/kuka_sunrise/internal/activatable_interface.hpp b/kuka_sunrise_driver/include/kuka_sunrise/internal/activatable_interface.hpp
diff --git a/kuka_sunrise_driver/include/kuka_sunrise/internal/serialization.hpp b/kuka_sunrise_driver/include/kuka_sunrise/internal/serialization.hpp
diff --git a/kuka_sunrise_driver/include/kuka_sunrise/internal/service_tools.hpp b/kuka_sunrise_driver/include/kuka_sunrise/internal/service_tools.hpp
diff --git a/kuka_sunrise_driver/include/kuka_sunrise/kuka_fri_hardware_interface.hpp b/kuka_sunrise_driver/include/kuka_sunrise/kuka_fri_hardware_interface.hpp
@@ -27,7 +27,6 @@
 #include <hardware_interface/types/hardware_interface_type_values.hpp>
 
 #include "kuka_driver_interfaces/srv/set_int.hpp"
-#include "kuka_sunrise/internal/activatable_interface.hpp"
 #include "fri/friLBRClient.h"
 #include "fri/HWIFClientApplication.hpp"
 #include "fri/friUdpConnection.h"
@@ -53,9 +52,8 @@ static std::unordered_map<std::string,
 {{"analog", IOTypes::ANALOG}, {"digital", IOTypes::DIGITAL}, {"boolean", IOTypes::BOOLEAN}};
 
 class KUKAFRIHardwareInterface : public hardware_interface::SystemInterface,
-  public KUKA::FRI::LBRClient,
-  public ActivatableInterface  // TODO(Svastits): is this necessary in current state?
-{
+  public KUKA::FRI::LBRClient
+  {
 public:
   KUKAFRIHardwareInterface()
   : client_application_(udp_connection_, *this) {}
@@ -89,6 +87,7 @@ class KUKAFRIHardwareInterface : public hardware_interface::SystemInterface,
   };
 
 private:
+  bool is_active_ = false;
   KUKA::FRI::HWIFClientApplication client_application_;
   KUKA::FRI::UdpConnection udp_connection_;
 

diff --git a/kuka_sunrise_driver/include/kuka_sunrise/robot_manager_node.hpp b/kuka_sunrise_driver/include/kuka_sunrise/robot_manager_node.hpp
@@ -31,15 +31,14 @@
 
 #include "kuka_sunrise/robot_manager.hpp"
 #include "kuka_sunrise/configuration_manager.hpp"
-#include "kuka_sunrise/internal/activatable_interface.hpp"
-#include "kuka_sunrise/internal/service_tools.hpp"
+#include "communication_helpers/service_tools.hpp"
 
 #include "kroshu_ros2_core/ROS2BaseLCNode.hpp"
 
 namespace kuka_sunrise
 {
 
-class RobotManagerNode : public kroshu_ros2_core::ROS2BaseLCNode, public ActivatableInterface
+class RobotManagerNode : public kroshu_ros2_core::ROS2BaseLCNode
 {
 public:
   RobotManagerNode();

diff --git a/kuka_sunrise_driver/src/kuka_sunrise/configuration_manager.cpp b/kuka_sunrise_driver/src/kuka_sunrise/configuration_manager.cpp
@@ -17,7 +17,7 @@
 #include <vector>
 
 #include "kuka_sunrise/configuration_manager.hpp"
-#include "kuka_sunrise/internal/service_tools.hpp"
+#include "communication_helpers/service_tools.hpp"
 #include "kuka_sunrise/robot_manager.hpp"
 
 namespace kuka_sunrise

diff --git a/kuka_sunrise_driver/src/kuka_sunrise/kuka_fri_hardware_interface.cpp b/kuka_sunrise_driver/src/kuka_sunrise/kuka_fri_hardware_interface.cpp
@@ -125,14 +125,14 @@ CallbackReturn KUKAFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta
     RCLCPP_ERROR(rclcpp::get_logger("KUKAFRIHardwareInterface"), "Could not connect");
     return CallbackReturn::FAILURE;
   }
-  this->ActivatableInterface::activate();
+  is_active_= true;
   return CallbackReturn::SUCCESS;
 }
 
 CallbackReturn KUKAFRIHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
 {
   client_application_.disconnect();
-  this->ActivatableInterface::deactivate();
+  is_active_= false;
   return CallbackReturn::SUCCESS;
 }
 

diff --git a/kuka_sunrise_driver/src/kuka_sunrise/robot_manager.cpp b/kuka_sunrise_driver/src/kuka_sunrise/robot_manager.cpp
@@ -18,7 +18,7 @@
 #include <chrono>
 
 #include "kuka_sunrise/robot_manager.hpp"
-#include "kuka_sunrise/internal/serialization.hpp"
+#include "communication_helpers/serialization.hpp"
 #include "kuka_sunrise/tcp_connection.hpp"
 
 namespace kuka_sunrise

diff --git a/kuka_sunrise_driver/src/kuka_sunrise/robot_manager_node.cpp b/kuka_sunrise_driver/src/kuka_sunrise/robot_manager_node.cpp
@@ -300,7 +300,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)
     return ERROR;
   }
 
-  if (this->isActive() && !this->deactivateControl()) {
+  if (!this->deactivateControl()) {
     RCLCPP_ERROR(get_logger(), "Could not deactivate control");
     return ERROR;
   }
@@ -350,14 +350,12 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)
 
 bool RobotManagerNode::activateControl()
 {
-  this->ActivatableInterface::activate();
   if (!robot_manager_->isConnected()) {
     RCLCPP_ERROR(get_logger(), "Not connected");
     return false;
   }
 
   if (!robot_manager_->activateControl()) {
-    this->ActivatableInterface::deactivate();
     RCLCPP_ERROR(get_logger(), "Could not activate control");
     return false;
   }
@@ -374,11 +372,10 @@ bool RobotManagerNode::deactivateControl()
     return false;
   }
 
-  if (this->isActive() && !robot_manager_->deactivateControl()) {
+  if (!robot_manager_->deactivateControl()) {
     RCLCPP_ERROR(get_logger(), "Could not deactivate control");
     return false;
   }
-  this->ActivatableInterface::deactivate();
 
   std_msgs::msg::Bool command_state;
   command_state.data = false;