Add MultiMove support (#7)

* Switch to using EGMManager

* Each channel needs its own port number, add more comments

* Update abb.repos, hardware parameters, pack and go example, docs

* User specifies robotstudio_ip at launch

* Apply suggestions from code review

abb.repos

@@ -3,3 +3,11 @@ repositories:
     type: git
     url: https://github.com/ros-industrial/abb_libegm.git
     version: master
+  abb_librws:
+    type: git
+    url: https://github.com/ros-industrial/abb_librws.git
+    version: master
+  abb_egm_rws_managers:
+    type: git
+    url: https://github.com/ros-industrial/abb_egm_rws_managers.git
+    version: master

abb_bringup/launch/abb_control.launch.py

@@ -67,6 +67,14 @@ def generate_launch_description():
             description="Start robot with fake hardware mirroring command to its states.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robotstudio_ip",
+            default_value="None",
+            description="IP of RobotStudio computer. \
+            Used only if 'use_fake_hardware' parameter is false.",
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "fake_sensor_commands",
@@ -97,6 +105,7 @@ def generate_launch_description():
     prefix = LaunchConfiguration("prefix")
     use_fake_hardware = LaunchConfiguration("use_fake_hardware")
     fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    robotstudio_ip = LaunchConfiguration("robotstudio_ip")
     initial_joint_controller = LaunchConfiguration("initial_joint_controller")
     launch_rviz = LaunchConfiguration("launch_rviz")
 
@@ -117,6 +126,9 @@ def generate_launch_description():
             "fake_sensor_commands:=",
             fake_sensor_commands,
             " ",
+            "robotstudio_ip:=",
+            robotstudio_ip,
+            " ",
         ]
     )
     robot_description = {"robot_description": robot_description_content}

abb_hardware_interface/CMakeLists.txt

@@ -19,7 +19,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
-    abb_libegm
+    abb_egm_rws_managers
     hardware_interface
     pluginlib
     rclcpp
@@ -40,6 +40,7 @@ add_library(
   ${PROJECT_NAME}
   SHARED
   src/abb_system_position_only.cpp
+  src/utilities.cpp
 )
 ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 target_include_directories(

abb_hardware_interface/include/abb_hardware_interface/abb_system_position_only.hpp

@@ -14,21 +14,27 @@
 
 #pragma once
 
-#include <abb_libegm/egm_controller_interface.h>
+#include <abb_egm_rws_managers/egm_manager.h>
+#include <abb_egm_rws_managers/rws_manager.h>
+#include <abb_hardware_interface/visibility_control.h>
 
 #include <chrono>
+#include <cmath>
+#include <limits>
 #include <memory>
 #include <string>
 #include <vector>
 
-#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 "rclcpp/macros.hpp"
-#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
-#include "rclcpp_lifecycle/state.hpp"
-#include "abb_hardware_interface/visibility_control.h"
+#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/macros.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+
 
 using hardware_interface::return_type;
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
@@ -43,9 +49,6 @@ class ABBSystemPositionOnlyHardware : public hardware_interface::SystemInterface
   ROS2_CONTROL_DRIVER_PUBLIC
   CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
 
-  ROS2_CONTROL_DRIVER_PUBLIC
-  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
-
   ROS2_CONTROL_DRIVER_PUBLIC
   std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
 
@@ -55,35 +58,19 @@ class ABBSystemPositionOnlyHardware : public hardware_interface::SystemInterface
   ROS2_CONTROL_DRIVER_PUBLIC
   CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
 
-  ROS2_CONTROL_DRIVER_PUBLIC
-  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
-
   ROS2_CONTROL_DRIVER_PUBLIC
   return_type read() override;
 
   ROS2_CONTROL_DRIVER_PUBLIC
   return_type write() override;
 
 private:
-  // Hardware parameters
-  size_t robotstudio_port_;
-
-  // Store the command for the simulated robot
-  std::vector<double> hw_commands_, hw_states_;
-
   // EGM
-  int sequence_number_ = 0;
-  boost::asio::io_service io_service_;
-  boost::thread_group thread_group_;
-  std::unique_ptr<abb::egm::EGMControllerInterface> egm_interface_;
-
-  abb::egm::wrapper::Input input_;
-  abb::egm::wrapper::Output output_;
-  abb::egm::wrapper::Output output_pos_;
-  abb::egm::wrapper::Output output_vel_;
-  abb::egm::wrapper::Joints current_positions_;
-  abb::egm::wrapper::Joints current_velocities_;
-  abb::egm::wrapper::Joints initial_positions_;
+  abb::robot::RobotControllerDescription robot_controller_description_;
+  std::unique_ptr<abb::robot::EGMManager> egm_manager_;
+
+  // Store the state and commands for the robot(s)
+  abb::robot::MotionData motion_data_;
 };
 
 }  // namespace abb_hardware_interface

abb_hardware_interface/include/abb_hardware_interface/utilities.hpp

@@ -0,0 +1,69 @@
+/***********************************************************************************************************************
+ *
+ * Copyright (c) 2020, ABB Schweiz AG
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with
+ * or without modification, are permitted provided that
+ * the following conditions are met:
+ *
+ *    * Redistributions of source code must retain the
+ *      above copyright notice, this list of conditions
+ *      and the following disclaimer.
+ *    * Redistributions in binary form must reproduce the
+ *      above copyright notice, this list of conditions
+ *      and the following disclaimer in the documentation
+ *      and/or other materials provided with the
+ *      distribution.
+ *    * Neither the name of ABB nor the names of its
+ *      contributors may be used to endorse or promote
+ *      products derived from this software without
+ *      specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ***********************************************************************************************************************
+ */
+
+// This file is a modified copy from
+// https://github.com/ros-industrial/abb_robot_driver/blob/master/abb_robot_cpp_utilities/include/abb_robot_cpp_utilities/initialization.h
+
+#pragma once
+
+#include <abb_egm_rws_managers/rws_manager.h>
+
+namespace abb
+{
+namespace robot
+{
+namespace utilities
+{
+/**
+ * \brief Attempts to establish a connection to a robot controller's RWS server.
+ *
+ * If a connection is established, then a structured description of the robot controller is returned.
+ *
+ * \param rws_manager for handling the RWS communication with the robot controller.
+ * \param robot_controller_id for an identifier/nickname for the targeted robot controller.
+ * \param no_connection_timeout indicator whether to wait indefinitely on the robot controller.
+ *
+ * \return RobotControllerDescription of the robot controller.
+ *
+ * \throw std::runtime_error if unable to establish a connection.
+ */
+RobotControllerDescription establishRWSConnection(
+  RWSManager & rws_manager, const std::string & robot_controller_id,
+  const bool no_connection_timeout);
+
+}  // namespace utilities
+}  // namespace robot
+}  // namespace abb

abb_hardware_interface/package.xml

@@ -14,10 +14,10 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>abb_egm_rws_managers</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
-  <depend>abb_libegm</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>