Skip to content

Commit

Permalink
visibility control
Browse files Browse the repository at this point in the history
  • Loading branch information
Aron Svastits committed Nov 23, 2023
1 parent dfe9174 commit 6ff90af
Show file tree
Hide file tree
Showing 8 changed files with 102 additions and 92 deletions.
13 changes: 9 additions & 4 deletions kuka_iiqka_eac_driver/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -71,11 +71,16 @@ else()
remove_definitions(-DNON_MOCK_SETUP)
endif()

add_library(kuka_iiqka_eac_driver SHARED
add_library(${PROJECT_NAME} SHARED
src/hardware_interface.cpp
)
ament_target_dependencies(kuka_iiqka_eac_driver rclcpp sensor_msgs hardware_interface)
target_link_libraries(kuka_iiqka_eac_driver motion-external-proto-api-nanopb motion-services-ecs-proto-api-cpp

# 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 "KUKA_IIQKA_EAC_DRIVER_BUILDING_LIBRARY")

ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface)
target_link_libraries(${PROJECT_NAME} motion-external-proto-api-nanopb motion-services-ecs-proto-api-cpp
motion-services-ecs-proto-api-nanopb yaml-cpp kuka::os-core-udp-communication kuka::nanopb-helpers)


Expand All @@ -86,7 +91,7 @@ target_link_libraries(robot_manager_node kuka_drivers_core::communication_helper

pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml)

install(TARGETS kuka_iiqka_eac_driver robot_manager_node
install(TARGETS ${PROJECT_NAME} robot_manager_node
DESTINATION lib/${PROJECT_NAME})

install(DIRECTORY config launch
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@
#include "nanopb/kuka/ecs/v1/motion_state_external.pb.hh"
#include "os-core-udp-communication/replier.h"

#include "kuka_iiqka_eac_driver/visibility_control.h"

using hardware_interface::return_type;
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

Expand All @@ -52,24 +54,34 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface
public:
RCLCPP_SHARED_PTR_DEFINITIONS(KukaEACHardwareInterface)

CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo & info)
override;

std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
KUKA_IIQKA_EAC_DRIVER_PUBLIC std::vector<hardware_interface::StateInterface>
export_state_interfaces() override;

std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
KUKA_IIQKA_EAC_DRIVER_PUBLIC std::vector<hardware_interface::CommandInterface>
export_command_interfaces() override;

CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override;

CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;

CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override;

return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
KUKA_IIQKA_EAC_DRIVER_PUBLIC return_type read(
const rclcpp::Time & time,
const rclcpp::Duration & period) override;

return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
KUKA_IIQKA_EAC_DRIVER_PUBLIC return_type write(
const rclcpp::Time & time,
const rclcpp::Duration & period) override;

private:
void ObserveControl();
KUKA_IIQKA_EAC_DRIVER_LOCAL void ObserveControl();

bool is_active_ = false;
bool msg_received_ = false;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,23 +1,16 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
// Copyright 2023 Á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
// 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
// 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.

/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
// 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_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_
#define KUKA_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_
Expand All @@ -33,7 +26,7 @@
#define KUKA_IIQKA_EAC_DRIVER_EXPORT __declspec(dllexport)
#define KUKA_IIQKA_EAC_DRIVER_IMPORT __declspec(dllimport)
#endif
#ifdef KUKA_IIQKA_EAC_DRIVER_BUILDING_DLL
#ifdef KUKA_IIQKA_EAC_DRIVER_BUILDING_LIBRARY
#define KUKA_IIQKA_EAC_DRIVER_PUBLIC KUKA_IIQKA_EAC_DRIVER_EXPORT
#else
#define KUKA_IIQKA_EAC_DRIVER_PUBLIC KUKA_IIQKA_EAC_DRIVER_IMPORT
Expand All @@ -50,7 +43,7 @@
#define KUKA_IIQKA_EAC_DRIVER_PUBLIC
#define KUKA_IIQKA_EAC_DRIVER_LOCAL
#endif
#define KUKA_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_
#define KUKA_IIQKA_EAC_DRIVER_PUBLIC_TYPE
#endif

#endif // KUKA_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_
#endif // KUKA_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_
2 changes: 1 addition & 1 deletion kuka_kss_rsi_driver/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ add_library(${PROJECT_NAME} SHARED

# 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 "JOINT_STATE_BROADCASTER_BUILDING_DLL")
target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_KSS_RSI_DRIVER_BUILDING_LIBRARY")
# prevent pluginlib from using boost
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

Expand Down
Original file line number Diff line number Diff line change
@@ -1,23 +1,16 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
// Copyright 2023 Á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
// 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
// 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.

/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
// 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_KSS_RSI_DRIVER__VISIBILITY_CONTROL_H_
#define KUKA_KSS_RSI_DRIVER__VISIBILITY_CONTROL_H_
Expand All @@ -33,7 +26,7 @@
#define KUKA_KSS_RSI_DRIVER_EXPORT __declspec(dllexport)
#define KUKA_KSS_RSI_DRIVER_IMPORT __declspec(dllimport)
#endif
#ifdef KUKA_KSS_RSI_DRIVER_BUILDING_DLL
#ifdef KUKA_KSS_RSI_DRIVER_BUILDING_LIBRARY
#define KUKA_KSS_RSI_DRIVER_PUBLIC KUKA_KSS_RSI_DRIVER_EXPORT
#else
#define KUKA_KSS_RSI_DRIVER_PUBLIC KUKA_KSS_RSI_DRIVER_IMPORT
Expand All @@ -53,4 +46,4 @@
#define KUKA_KSS_RSI_DRIVER_PUBLIC_TYPE
#endif

#endif // KUKA_KSS_RSI_DRIVER__VISIBILITY_CONTROL_H_
#endif // KUKA_KSS_RSI_DRIVER__VISIBILITY_CONTROL_H_
13 changes: 9 additions & 4 deletions kuka_sunrise_fri_driver/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -72,11 +72,16 @@ target_link_libraries(fri_client_sdk PRIVATE protobuf-nanopb)
install(DIRECTORY include/fri_client_sdk DESTINATION include)
install(FILES ${private_headers} DESTINATION include)

add_library(kuka_fri_hw_interface SHARED
add_library(${PROJECT_NAME} SHARED
src/hardware_interface.cpp
)
ament_target_dependencies(kuka_fri_hw_interface kuka_driver_interfaces rclcpp rclcpp_lifecycle hardware_interface)
target_link_libraries(kuka_fri_hw_interface fri_client_sdk)

# 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 "KUKA_SUNRISE_FRI_DRIVER_BUILDING_LIBRARY")

ament_target_dependencies(${PROJECT_NAME} kuka_driver_interfaces rclcpp rclcpp_lifecycle hardware_interface)
target_link_libraries(${PROJECT_NAME} fri_client_sdk)

add_library(configuration_manager SHARED
src/connection_helpers/configuration_manager.cpp)
Expand All @@ -92,7 +97,7 @@ target_link_libraries(robot_manager_node

pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml)

install(TARGETS fri_connection fri_client_sdk kuka_fri_hw_interface robot_manager_node
install(TARGETS ${PROJECT_NAME} fri_connection fri_client_sdk robot_manager_node
DESTINATION lib/${PROJECT_NAME})

install(DIRECTORY launch config
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,8 @@
#include "fri_client_sdk/friUdpConnection.h"
#include "fri_client_sdk/friClientIf.h"

#include "kuka_sunrise_fri_driver/visibility_control.h"


#include "pluginlib/class_list_macros.hpp"

Expand All @@ -55,26 +57,33 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface,
public KUKA::FRI::LBRClient
{
public:
KukaFRIHardwareInterface()
: client_application_(udp_connection_, *this) {}
CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
RCLCPP_SHARED_PTR_DEFINITIONS(KukaFRIHardwareInterface)

std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

hardware_interface::return_type read(
KUKA_SUNRISE_FRI_DRIVER_PUBLIC KukaFRIHardwareInterface()
: client_application_(udp_connection_, *this) {}
KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_init(
const hardware_interface::HardwareInfo & info) override;
KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;
KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override;

KUKA_SUNRISE_FRI_DRIVER_PUBLIC std::vector<hardware_interface::StateInterface>
export_state_interfaces() override;
KUKA_SUNRISE_FRI_DRIVER_PUBLIC std::vector<hardware_interface::CommandInterface>
export_command_interfaces() override;

KUKA_SUNRISE_FRI_DRIVER_PUBLIC hardware_interface::return_type read(
const rclcpp::Time & time,
const rclcpp::Duration & period) override;
hardware_interface::return_type write(
KUKA_SUNRISE_FRI_DRIVER_PUBLIC hardware_interface::return_type write(
const rclcpp::Time & time,
const rclcpp::Duration & period) override;
void updateCommand(const rclcpp::Time & stamp);
bool setReceiveMultiplier(int receive_multiplier);
KUKA_SUNRISE_FRI_DRIVER_PUBLIC void updateCommand(const rclcpp::Time & stamp);
KUKA_SUNRISE_FRI_DRIVER_PUBLIC bool setReceiveMultiplier(int receive_multiplier);

void waitForCommand() final;
void command() final;
KUKA_SUNRISE_FRI_DRIVER_PUBLIC void waitForCommand() final;
KUKA_SUNRISE_FRI_DRIVER_PUBLIC void command() final;

class InvalidGPIOTypeException : public std::runtime_error
{
Expand Down Expand Up @@ -124,7 +133,7 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface,

RobotState robot_state_;

IOTypes getType(const std::string & type_string) const
KUKA_SUNRISE_FRI_DRIVER_LOCAL IOTypes getType(const std::string & type_string) const
{
auto it = types.find(type_string);
if (it != types.end()) {
Expand Down
Original file line number Diff line number Diff line change
@@ -1,23 +1,16 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
// Copyright 2023 Á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
// 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
// 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.

/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
// 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_FRI_DRIVER__VISIBILITY_CONTROL_H_
#define KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_
Expand All @@ -33,7 +26,7 @@
#define KUKA_SUNRISE_FRI_DRIVER_EXPORT __declspec(dllexport)
#define KUKA_SUNRISE_FRI_DRIVER_IMPORT __declspec(dllimport)
#endif
#ifdef KUKA_SUNRISE_FRI_DRIVER_BUILDING_DLL
#ifdef KUKA_SUNRISE_FRI_DRIVER_BUILDING_LIBRARY
#define KUKA_SUNRISE_FRI_DRIVER_PUBLIC KUKA_SUNRISE_FRI_DRIVER_EXPORT
#else
#define KUKA_SUNRISE_FRI_DRIVER_PUBLIC KUKA_SUNRISE_FRI_DRIVER_IMPORT
Expand All @@ -50,7 +43,7 @@
#define KUKA_SUNRISE_FRI_DRIVER_PUBLIC
#define KUKA_SUNRISE_FRI_DRIVER_LOCAL
#endif
#define KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_
#define KUKA_SUNRISE_FRI_DRIVER_PUBLIC_TYPE
#endif

#endif // KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_
#endif // KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_

0 comments on commit 6ff90af

Please sign in to comment.