Merge pull request #58 from kroshu/fix/combine_parameters

Fix/combine parameters

kuka_driver_examples/eci_demo/launch/iisy_circle.launch.py

@@ -54,7 +54,7 @@ def generate_launch_description():
     robot_description = {'robot_description': robot_description_config}
 
     rviz_config_file = get_package_share_directory(
-        'kuka_iisy_support') + "/launch/urdf.rviz"
+        'kuka_iisy_support') + "/config/urdf_planning_scene.rviz"
 
     controller_config = (get_package_share_directory('kuka_rox_hw_interface') +
                          "/config/ros2_controller_config.yaml")

kuka_rox_hardware_interface/CMakeLists.txt

@@ -14,6 +14,11 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+# A few (transitive) dependencies are available only in one of the dependencies
+# Set flag to suppress linker error
+# TODO: find better solution
+set(CMAKE_EXE_LINKER_FLAGS "-Wl,--copy-dt-needed-entries")
+
 set(MOCK_KUKA_LIBS TRUE)
 
 find_package(ament_cmake REQUIRED)
@@ -40,13 +45,15 @@ if(NOT MOCK_KUKA_LIBS)
   message("Using real kuka libs")
   add_definitions(-DNON_MOCK_SETUP)
 else()
+  find_package(Protobuf)
   include_directories(include/mock)
   add_library(os-core-udp-communication SHARED
     src/mock/os-core-udp-communication/udp_replier_mock.cpp
     src/mock/os-core-udp-communication/udp_socket_mock.cpp)
 
   add_library(motion-services-ecs-proto-api-cpp INTERFACE)
   target_include_directories(motion-services-ecs-proto-api-cpp INTERFACE include/mock)
+  target_link_libraries(motion-services-ecs-proto-api-cpp INTERFACE protobuf::libprotobuf)
 
   add_library(motion-external-proto-api-nanopb INTERFACE)
   target_include_directories(motion-external-proto-api-nanopb INTERFACE include/mock)
@@ -72,7 +79,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 kroshu_ros2_core::communication_helpers)
+target_link_libraries(robot_manager_node kroshu_ros2_core::communication_helpers motion-services-ecs-proto-api-cpp)
 
 pluginlib_export_plugin_description_file(hardware_interface kuka_rox_hw_interface.xml)
 

kuka_rox_hardware_interface/config/eci_config.yaml

@@ -0,0 +1,17 @@
+robot_manager:
+  ros__parameters:
+    control_mode: 1
+    client_ip: "0.0.0.0"
+    controller_ip: "0.0.0.0"
+    position_controller_name: "joint_trajectory_controller"
+    impedance_controller_name: "joint_impedance_controller"
+    torque_controller_name: "effort_controller"
+
+# Control mode enums:
+#  1 - position control
+#  2 - cartesian overlay (not supported yet)
+#  3 - joint impedance control
+#  4 - cartesian impedance control (not supported yet)
+#  5 - torque control  
+
+

kuka_rox_hardware_interface/include/kuka_rox_hw_interface/robot_manager_node.hpp

@@ -29,16 +29,18 @@
 #include "std_msgs/msg/bool.hpp"
 
 #include "communication_helpers/service_tools.hpp"
-
 #include "kroshu_ros2_core/ROS2BaseLCNode.hpp"
 
+#include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h"
+
 namespace kuka_rox
 {
-
 class RobotManagerNode : public kroshu_ros2_core::ROS2BaseLCNode
 {
 public:
   RobotManagerNode();
+  ~RobotManagerNode();
+
 
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_configure(const rclcpp_lifecycle::State &) override;
@@ -52,33 +54,34 @@ class RobotManagerNode : public kroshu_ros2_core::ROS2BaseLCNode
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_deactivate(const rclcpp_lifecycle::State &) override;
 
-  bool onControlModeChangeRequest(const std::string & control_mode);
-  bool onControllerNameChangeRequest(
-    const std::string & controller_name,
-    const std::string & controller_name_param);
+  bool onControlModeChangeRequest(int control_mode);
 
 private:
+  void ObserveControl();
+
   rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr
     change_hardware_state_client_;
   rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr
     change_controller_state_client_;
   rclcpp::CallbackGroup::SharedPtr cbg_;
   std::vector<std::string> controller_names_;
-  std::map<std::string, std::vector<std::string>> control_mode_map_;
+  std::map<int, std::vector<std::string>> control_mode_map_;
+
+  std::thread observe_thread_;
+  std::atomic<bool> terminate_{false};
+#ifdef NON_MOCK_SETUP
+  std::unique_ptr<kuka::ecs::v1::ExternalControlService::Stub> stub_;
+  std::unique_ptr<grpc::ClientContext> context_;
+#endif
 
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Bool>> is_configured_pub_;
   std_msgs::msg::Bool is_configured_msg_;
 
-
-  const std::string POSITION_CONTROL = "position";
-  const std::string TORQUE_CONTROL = "torque";
-  const std::string IMPEDANCE_CONTROL = "impedance";
-
-  const std::string POSITION_CONTROLLER_NAME_PARAM = "position_controller_name";
-  const std::string IMPEDANCE_CONTROLLER_NAME_PARAM = "impedance_controller_name";
-  const std::string TORQUE_CONTROLLER_NAME_PARAM = "torque_controller_name";
-
-  static constexpr bool is_joint_imp_contr_ = true;
+  // There are two kinds of control modes with different number of necessary interfaces to be set:
+  //  - in standard modes (position, torque), only the control signal to the used interface (1)
+  //  - in impedance modes, the setpoint and the parameters describing the behaviour (2)
+  static constexpr int STANDARD_MODE_IF_SIZE = 1;
+  static constexpr int IMPEDANCE_MODE_IF_SIZE = 2;
 };
 
 }  // namespace kuka_rox

kuka_rox_hardware_interface/include/kuka_rox_hw_interface/rox_hardware_interface.hpp

@@ -58,6 +58,8 @@ class KukaRoXHardwareInterface : public hardware_interface::SystemInterface
 
   std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
 
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
   CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
 
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
@@ -68,9 +70,9 @@ class KukaRoXHardwareInterface : public hardware_interface::SystemInterface
 
   bool isActive() const;
 
+private:
   void ObserveControl();
 
-private:
   bool is_active_ = false;
   bool msg_received_ = false;
 
@@ -88,7 +90,6 @@ class KukaRoXHardwareInterface : public hardware_interface::SystemInterface
   std::unique_ptr<grpc::ClientContext> context_;
 #endif
 
-
   std::thread observe_thread_;
   std::atomic<bool> terminate_{false};
   std::mutex observe_mutex_;

kuka_rox_hardware_interface/launch/startup.launch.py

@@ -29,8 +29,8 @@ def generate_launch_description():
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             PathJoinSubstitution(
-                [FindPackageShare("kuka_rox_hw_interface"),
-                 "config", "iisy.urdf.xacro"]
+                [FindPackageShare("kuka_iisy_support"),
+                 "urdf", "iisy.urdf.xacro"]
             ),
             " ",
         ]
@@ -50,6 +50,9 @@ def generate_launch_description():
     joint_imp_controller_config = (get_package_share_directory('kuka_rox_hw_interface') +
                                    "/config/joint_impedance_controller_config.yaml")
 
+    eci_config = (get_package_share_directory('kuka_rox_hw_interface') +
+                  "/config/eci_config.yaml")
+
     controller_manager_node = '/controller_manager'
 
     return LaunchDescription([
@@ -63,10 +66,7 @@ def generate_launch_description():
             namespace='',
             package="kuka_rox_hw_interface",
             executable="robot_manager_node",
-            parameters=[
-                {'position_controller_name': 'joint_trajectory_controller'},
-                {'impedance_controller_name': 'joint_impedance_controller'},
-                {'torque_controller_name': 'effort_controller'}]
+            parameters=[eci_config]
         ),
         Node(
             package='robot_state_publisher',

kuka_rox_hardware_interface/launch/startup_with_rviz.launch.py

@@ -24,7 +24,7 @@
 
 def generate_launch_description():
     rviz_config_file = os.path.join(
-        get_package_share_directory('kuka_iisy_support'), 'launch', 'urdf_wo_planning_scene.rviz')
+        get_package_share_directory('kuka_iisy_support'), 'config', 'simple_urdf.rviz')
 
     startup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
         [get_package_share_directory('kuka_rox_hw_interface'), '/launch/startup.launch.py']))