diff --git a/kuka_rox_hardware_interface/CMakeLists.txt b/kuka_rox_hardware_interface/CMakeLists.txt index bec36c8a..5d967d44 100644 --- a/kuka_rox_hardware_interface/CMakeLists.txt +++ b/kuka_rox_hardware_interface/CMakeLists.txt @@ -14,7 +14,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# option(MOCK_KUKA_LIBS "Use mock classes instead of the kuka lib classes" ON) set(MOCK_KUKA_LIBS TRUE) find_package(ament_cmake REQUIRED) @@ -28,7 +27,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) @@ -73,7 +71,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 index ffdbdd47..1dad4906 100644 --- 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 index 0e5cd5d4..bab9bfa5 100644 --- 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 deleted file mode 100644 index c17cb76a..00000000 --- a/kuka_sunrise_driver/include/kuka_sunrise/internal/activatable_interface.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright 2020 Zoltán Rési -// -// 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_SUNRISE__INTERNAL__ACTIVATABLE_INTERFACE_HPP_ -#define KUKA_SUNRISE__INTERNAL__ACTIVATABLE_INTERFACE_HPP_ - -namespace kuka_sunrise -{ - -class ActivatableInterface -{ -public: - virtual bool activate() - { - is_active_ = true; - return true; - } - virtual bool deactivate() - { - is_active_ = false; - return true; - } - bool isActive() - { - return is_active_; - } - virtual ~ActivatableInterface() - { - } - -protected: - bool is_active_ = false; -}; - -} // namespace kuka_sunrise - -#endif // 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 deleted file mode 100644 index f2bb6e5b..00000000 --- a/kuka_sunrise_driver/include/kuka_sunrise/internal/serialization.hpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2020 Zoltán Rési -// -// 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_SUNRISE__INTERNAL__SERIALIZATION_HPP_ -#define KUKA_SUNRISE__INTERNAL__SERIALIZATION_HPP_ - -#include -#include -#include - -namespace kuka_sunrise -{ - -int serializeNext(int integer_in, std::vector & serialized_out) -{ - std::uint8_t * bytes = reinterpret_cast(&integer_in); - auto it = serialized_out.end(); - serialized_out.insert(it, bytes, bytes + sizeof(int)); - - // Redefine iterator because of possible invalidation - auto from_it = std::prev(serialized_out.end(), sizeof(int)); - std::reverse(from_it, serialized_out.end()); - // TODO(resizoltan): assert that int is 4 bytes long - // TODO(resizoltan): check endiannes - return sizeof(int); -} - -int deserializeNext(const std::vector & serialized_in, int & integer_out) -{ - if (serialized_in.size() < sizeof(int)) { - // TODO(resizoltan): error - } - std::vector serialized_copy = serialized_in; - integer_out = *(reinterpret_cast(serialized_copy.data())); - return sizeof(int); -} - -int serializeNext(double double_in, std::vector & serialized_out) -{ - std::uint8_t * bytes = reinterpret_cast(&double_in); - serialized_out.insert(serialized_out.end(), bytes, bytes + sizeof(double)); - - // Redefine iterator because of possible invalidation - auto from_it = std::prev(serialized_out.end(), sizeof(double)); - std::reverse(from_it, serialized_out.end()); - // TODO(resizoltan): assert that int is 4 bytes long - // TODO(resizoltan): check endiannes - return sizeof(int); -} - -int deserializeNext(const std::vector & serialized_in, double & double_out) -{ - if (serialized_in.size() < sizeof(double)) { - // TODO(resizoltan): error - } - std::vector serialized_copy = serialized_in; - double_out = *(reinterpret_cast(serialized_copy.data())); - return sizeof(int); -} - -} // namespace kuka_sunrise - -#endif // 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 deleted file mode 100644 index 60bb6fcd..00000000 --- a/kuka_sunrise_driver/include/kuka_sunrise/internal/service_tools.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2020 Zoltán Rési -// -// 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_SUNRISE__INTERNAL__SERVICE_TOOLS_HPP_ -#define KUKA_SUNRISE__INTERNAL__SERVICE_TOOLS_HPP_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" - -namespace kuka_sunrise -{ - -template -std::future_status wait_for_result(FutureT & future, WaitTimeT time_to_wait) -{ - auto end = std::chrono::steady_clock::now() + time_to_wait; - std::chrono::milliseconds wait_period(100); - std::future_status status = std::future_status::timeout; - do { - auto now = std::chrono::steady_clock::now(); - auto time_left = end - now; - if (time_left <= std::chrono::seconds(0)) { - break; - } - status = future.wait_for((time_left < wait_period) ? time_left : wait_period); - } while (rclcpp::ok() && status != std::future_status::ready); - return status; -} - -template -std::shared_ptr -sendRequest( - ClientT client, RequestT request, const uint32_t & service_timeout_ms = 2000, - const uint32_t & response_timeout_ms = 100) -{ - if (service_timeout_ms && !client->wait_for_service( - std::chrono::milliseconds(service_timeout_ms))) - { - printf("Wait for service failed\n"); - return nullptr; - } - auto future_result = client->async_send_request(request); - auto future_status = wait_for_result( - future_result, std::chrono::milliseconds(response_timeout_ms)); - if (future_status != std::future_status::ready) { - printf("Request timed out\n"); - return nullptr; - } - return future_result.get(); -} -} // namespace kuka_sunrise - -#endif // 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 index 5a4793bd..698a79fd 100644 --- 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 #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 #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 index 6001ef04..aa23c888 100644 --- 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 index d69b5e64..4d169645 100644 --- 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 #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 index f2da04dd..411a67fb 100644 --- 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;