Initial commit containing the whole driver

.gitmodules

+[submodule "abb_libegm"]
+	path = abb_libegm
+	url = https://github.com/ros-industrial/abb_libegm.git
+[submodule "abb_librws"]
+	path = abb_librws
+	url = https://github.com/ros-industrial/abb_librws.git

README.md

+# abb_egm_driver
+This repository provides a driver that allows to control ABB robots with ROS-Industrial via the External Guided Motion (EGM) interface. It complies with the [ROS Industrial Driver Specification](http://wiki.ros.org/Industrial/Industrial_Robot_Driver_Spec) and provides a driver node that enables both trajectory download and real-time motion streaming.
+To transmit the motions to the robot, this driver uses the [abb_libegm](https://github.com/ros-industrial/abb_libegm), which requires a fee-based ABB software module *689-1 External Guided Motion*. To operate the robot, for example to switch on the motors, the [abb_librws](https://github.com/ros-industrial/abb_librws) is used.
+Note the requirements of [abb_libegm](https://github.com/ros-industrial/abb_libegm) and [abb_librws](https://github.com/ros-industrial/abb_librws) regarding the required RobotWare version of the robot controller!
+
+## Installation in ROS Noetic and Ubuntu 20.04
+Update the operating system:
+
+    sudo apt update
+    sudo apt dist-upgrade
+
+Install required packages:
+
+    sudo apt install git swig libnlopt-cxx-dev ros-noetic-nlopt ros-noetic-catkin ros-noetic-moveit
+
+If a workspace does not yet exist in which the driver is to be used, create and initialize it:
+
+    mkdir catkin_ws
+    cd catḱin_ws
+    mkdir src
+    catkin init
+    catkin config --extend /opt/ros/noetic/
+    cd src
+
+There is no industrial-core package in Noetic yet, so it is cloned and a pull request is applied for compatibility with Noetic:
+
+    git clone https://github.com/ros-industrial/industrial_core.git
+    cd industrial_core
+    sudo apt install git-extras
+    git pr 258
+    cd ..
+
+Clone the EGM driver:
+
+    git clone --recursive https://gitlab.cvh-server.de/jweber/abb_egm_driver.git
+
+Build the project:
+
+    catkin build
+
+### Optional: trac_ik in Noetic
+If this driver is to be used with MoveIt, the package trac_ik is necessary, which does not yet exist in Noetic. However, it can be cloned and made compatible with Noetic through modifications:
+
+    cd src
+    git clone https://bitbucket.org/traclabs/trac_ik.git
+
+As described [here](https://bitbucket.org/traclabs/trac_ik/pull-requests/29/trac_ik_lib-fix-dependency-on-ubuntu-2004/diff), the missing dependencies must be added to **trac_ik/trac_ik_lib/package.xml**. Also, line 35 in **trac_ik/trac_ik_lib/include/nlopt_ik.hpp** must be changed to
+    
+    #include <nlopt/nlopt.hpp>
+
+## Installation in ROS Melodic and Ubuntu 18.04
+Update the operating system:
+
+    sudo apt update
+    sudo apt dist-upgrade
+
+Install required packages:
+
+    sudo apt install git swig libnlopt-dev ros-melodic-nlopt ros-melodic-catkin ros-melodic-moveit ros-melodic-industrial-core ros-melodic-trac-ik
+
+If a workspace does not yet exist in which the driver is to be used, create and initialize it:
+
+    mkdir catkin_ws
+    cd catḱin_ws
+    mkdir src
+    catkin init
+    catkin config --extend /opt/ros/melodic/
+    cd src
+
+Clone the EGM driver:
+
+    git clone --recursive https://gitlab.cvh-server.de/jweber/abb_egm_driver.git
+
+Build the project:
+
+    catkin build
+
+
+## Preparation of the robot
+In order to use this driver with the ABB robot, it must be configured with RobotStudio.
+The following steps show how to create a robot in RobotStudio and prepare it to work with this driver.
+
+1. Create new RobotStudio Solution: File → New → Solution with empty Station
+→ Create.
+2. Add your robot: Home → Add Library → select your robot.
+3. Add robot controller: Controller → Add Controller → Add Controller → select
+your controller. You can either choose a real robot controller on the network
+or create a virtual robot controller to simulate a robot. Make sure that the
+RobotWare version of the robot controller is newer than 6.07.01.
+4. Request write access for controller: Controller → Request Write Access and
+confirm write access on the control panel.
+5. Add a UDPUC transmission device: Controller → Configuration → Communication → Transmisson Protocoll, right-click → New Transmission Protocol, type
+UDPUC, Remote Adress xx.xx.xx.xx (IP of external computer using the abb_egm_driver), Remote Port
+number 6511 (Default port, which is also used in libegm). Rename the transmission device to “EGM_Transmission”.
+6. Add a new program module: Controller → RAPID (in the tree on the left side)
+→ T_ROB1 → right-click → New Module.
+7. Double click on new module, an empty RAPID program opens in the editor.
+Insert the RAPID EGM code from **abb_egm_driver/rapid/EGM_joint_trajectory.mod**. 
+Adjust the name of the transmission device in the line with EGMSetupUC to the name that was
+previously assigned to the newly created UDPUC transmission device.
+8. Request write access for controller: Controller → Release Write Access, confirm
+that the changes are to be applied.
+9. Restart robot controller: Controller → Restart.
+10. If you are using a virtual robot controller instead of a real robot controller, follow the instructions below.
+
+### Optional: Configuration to use a virtual robot controller
+RobotStudio can simulate robot controllers and the associated robots. For the virtual
+controller to be accessible via the RWS interface, it must be configured. To do this, follow [these](https://forums.robotstudio.com/discussion/12082/using-robotwebservices-to-access-a-remote-virtual-controller) configuration steps.
+
+## Driver preparation
+Download the [your robot]_support and [your robot]_movit_config packages for the ABB robot you are using ([ABB](https://github.com/ros-industrial/abb) or [ABB Experimental](https://github.com/ros-industrial/abb_experimental)). Perform the following steps. An example for the IRB1600 robot is available in this repository and can be used for guidance:
+1. Create a copy of the launch file **/src/[your robot]_support/robot_interface_download_[your robot].launch** with the name **/src/[your robot]_support/robot_interface_download_[your robot]_egm.launch**. In this file the parameters **egm_port** and **rws_ip_address** must be created according to the IRB1600 example and passed to **abb_egm_driver/launch/robot_interface.launch**. The **rws_ip_address** is the IP address of the robot (can be determined with the Tech Pendant) and the **egm_port** is the port over which the driver communicates with the robot controller via EGM (default is 6511).
+
+2. Create a copy of the launchfile **src/[your robot]_moveit_config/moveit_planning_execution.launch** with the name **src/[your robot]_moveit_config/moveit_planning_execution_egm.launch**. In this file
+
+        <include file="$(find abb_[your robot]_support)/launch/robot_interface_download_[your robot].launch" ></include>
+
+    must be changed to
+    
+        <include file="$(find abb_[your robot]_support)/launch/robot_interface_download_[your robot]_egm.launch" ></include>
+
+3. In the launchfile of your project the driver can now be started easily:
+
+        <include file="$(find [your robot]_moveit_config)/launch/moveit_planning_execution_egm.launch"></include>
+
+
+

abb_egm_driver/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(abb_egm_driver)
+add_definitions(-std=c++11)
+
+find_package(abb_libegm)
+set(TARGETS ${TARGETS} "abb_libegm::abb_libegm")
+get_target_property(abb_libegm_INCLUDE_DIR abb_libegm::abb_libegm INTERFACE_INCLUDE_DIRECTORIES)
+
+find_package(abb_librws)
+set(TARGETS ${TARGETS} "abb_librws::abb_librws")
+get_target_property(abb_librws_INCLUDE_DIR abb_librws::abb_librws INTERFACE_INCLUDE_DIRECTORIES)
+
+
+find_package(catkin REQUIRED COMPONENTS industrial_robot_client roscpp)
+
+catkin_package(
+  CATKIN_DEPENDS industrial_robot_client roscpp
+)
+
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${abb_libegm_INCLUDE_DIR}
+)
+
+
+
+add_executable(${PROJECT_NAME}_robot_node
+  src/abb_egm_robot_node.cpp
+  src/abb_egm_interface.cpp
+  src/abb_egm_robot_interface.cpp
+  src/abb_utils.cpp)
+target_link_libraries(${PROJECT_NAME}_robot_node
+  industrial_robot_client
+  ${catkin_LIBRARIES}
+  ${TARGETS})
+set_target_properties(
+  ${PROJECT_NAME}_robot_node
+  PROPERTIES OUTPUT_NAME robot_node PREFIX "")
+
+
+
+#install(
+#  TARGETS
+#    ${PROJECT_NAME}_robot_state
+#    ${PROJECT_NAME}_motion_download_interface
+#  DESTINATION
+#    ${CATKIN_PACKAGE_BIN_DESTINATION}
+#)
+
+#foreach(dir launch rapid)
+#  install(
+#    DIRECTORY ${dir}/
+#    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+#endforeach()

abb_egm_driver/include/abb_egm_driver/abb_egm_interface.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Southwest Research Institute
+ * 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 the Southwest Research Institute, 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 OWNER 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.
+ */
+#include <ros/ros.h>
+#include <abb_libegm/egm_trajectory_interface.h>
+
+class AbbEgmInterface
+{
+public:
+  AbbEgmInterface();
+  ~AbbEgmInterface();
+  bool init(int default_port = 6510);
+  abb::egm::EGMTrajectoryInterface* get_egm_interface();
+
+protected:
+  boost::asio::io_service io_service_;
+  boost::thread_group thread_group;
+  abb::egm::EGMTrajectoryInterface* egm_interface_;
+};
\ No newline at end of file

abb_egm_driver/include/abb_egm_driver/abb_egm_robot_interface.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Southwest Research Institute
+ * 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 the Southwest Research Institute, 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 OWNER 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.
+ */
+#include <ros/ros.h>
+#include <abb_libegm/egm_trajectory_interface.h>
+
+#include "control_msgs/FollowJointTrajectoryFeedback.h"
+#include "sensor_msgs/JointState.h"
+#include "industrial_msgs/RobotStatus.h"
+#include "industrial_msgs/CmdJointTrajectory.h"
+#include "industrial_msgs/StopMotion.h"
+
+#include "abb_egm_driver/abb_utils.h"
+#include "industrial_utils/param_utils.h"
+#include "industrial_robot_client/joint_trajectory_interface.h"
+
+#include <math.h>
+#include <thread>
+using industrial_utils::param::getJointNames;
+
+#define ROS_ERROR_RETURN(rtn,...) do {ROS_ERROR(__VA_ARGS__); return(rtn);} while(0)
+
+// Interface Modes
+#define INTERFACE_NONE 0
+#define INTERFACE_TRAJECTORY 1
+#define INTERFACE_STATIC_GOAL 2
+
+class AbbEgmRobotInterface
+{
+public:
+  AbbEgmRobotInterface();
+  AbbEgmRobotInterface(abb::egm::EGMTrajectoryInterface* egm_interface);
+  ~AbbEgmRobotInterface();
+
+  bool init();
+  bool init(const std::vector<std::string> &joint_names, const std::map<std::string, double> &velocity_limits);
+  
+  bool isCommunicatonError();
+  bool isEgmRunning();
+  bool isRapidRunning();
+  void start();
+  void stop();
+
+
+
+protected:
+  void run();
+  bool jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req, industrial_msgs::CmdJointTrajectory::Response &res);// service callback
+  void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);// subscriber callback
+  bool jointTrajectoryCall(const trajectory_msgs::JointTrajectoryConstPtr &msg);// function call
+  void jointCommandCB(const trajectory_msgs::JointTrajectoryPointConstPtr &msg);// subscriber callback
+  bool stopMotionCB(industrial_msgs::StopMotion::Request &req, industrial_msgs::StopMotion::Response &res);// service callback
+
+  bool send_to_robot(abb::egm::wrapper::trajectory::TrajectoryGoal robot_msgs);
+  bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr& traj, abb::egm::wrapper::trajectory::TrajectoryGoal* msgs);
+  bool transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out);
+  bool select(const std::vector<std::string>& ros_joint_names, const trajectory_msgs::JointTrajectoryPoint& ros_pt, const std::vector<std::string>& rbt_joint_names, trajectory_msgs::JointTrajectoryPoint* rbt_pt);
+  bool calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration);
+  bool is_valid(const trajectory_msgs::JointTrajectory &traj);
+  bool trajectoryStop(bool discard_trajectories);
+
+
+
+
+  bool process(abb::egm::wrapper::trajectory::ExecutionProgress* execution_progress);
+  bool create_messages(abb::egm::wrapper::trajectory::ExecutionProgress* execution_progress,
+                       control_msgs::FollowJointTrajectoryFeedback* control_state,
+                       sensor_msgs::JointState* sensor_state,
+                       industrial_msgs::RobotStatus* robot_state);
+  bool select(const std::vector<double>& all_joint_pos,
+              const std::vector<std::string>& all_joint_names,
+              std::vector<double>* pub_joint_pos,
+              std::vector<std::string>* pub_joint_names);
+  bool transform(const std::vector<double>& pos_in, std::vector<double>* pos_out);
+
+
+  double default_joint_pos_;  // default position to use for "dummy joints", if none specified
+  double default_duration_;   // default duration to use for joint commands, if no
+
+  double degreeToRadian(double in)
+  {
+    return in / 180.0 * M_PI;
+  }
+
+  double radianToDegree(double in)
+  {
+    return in / M_PI * 180.0;
+  }
+
+  ros::NodeHandle node_;
+  abb::egm::EGMTrajectoryInterface* egm_interface_;
+  bool J23_coupled_;
+  std::vector<std::string> joint_names_;
+  std::map<std::string, double> joint_vel_limits_;  // cache of max joint velocities from URDF
+
+  ros::Subscriber sub_joint_command_; 
+  ros::Subscriber sub_joint_trajectory_; // handle for joint-trajectory topic subscription
+  ros::ServiceServer srv_joint_trajectory_;  // handle for joint-trajectory service
+  ros::ServiceServer srv_stop_motion_;   // handle for stop_motion service
+
+  ros::Publisher pub_joint_control_state_;
+  ros::Publisher pub_joint_sensor_state_;
+  ros::Publisher pub_joint_robot_state_;
+  
+  ros::AsyncSpinner* spinner_; // Use 1 thread
+
+  std::thread *run_thread = nullptr;
+  bool communicationError = true;
+  bool egm_running = false;
+  bool rapid_running = false;
+  bool run_active = false;
+  int interface_mode = INTERFACE_NONE;  
+};
\ No newline at end of file

abb_egm_driver/include/abb_egm_driver/abb_utils.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2012, Southwest Research Institute
+ * 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 the Southwest Research Institute, 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 OWNER 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.
+ */
+
+#ifndef ABB_UTILS_H_
+#define ABB_UTILS_H_
+
+#include <vector>
+#include "trajectory_msgs/JointTrajectory.h"
+
+namespace abb
+{
+namespace utils
+{
+
+/**
+ * \brief Corrects for parallel linkage coupling between joints.
+ *
+ * \param[in] joints_in input joint angles
+ * \param[out] joints_out output joint angles
+ * \param[in] J23_factor  Linkage factor for J2-J3.
+ *   J3_out = J3_in + j23_factor * J2_in
+ */
+void linkage_transform(const std::vector<double>& joints_in, std::vector<double>* joints_out, double J23_factor=0);
+
+/**
+ * \brief Corrects for parallel linkage coupling between joints.
+ *
+ * \param[in] pt_in input joint trajectory point
+ * \param[out] pt_out output joint trajectory point
+ * \param[in] J23_factor  Linkage factor for J2-J3.
+ *   J3_out = J3_in + j23_factor * J2_in
+ */
+void linkage_transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out, double J23_factor=0);
+
+} //abb
+} //utils
+
+#endif /* ABB_UTILS_H_ */

abb_egm_driver/launch/robot_interface.launch

+<launch>
+  <!-- port of EGM-Server -->
+  <arg name="egm_port" doc="Port of EGM-Server. The same port must be specified in Robot Studio (Controller -> Configuration -> Communication -> Transmisson Protocoll -> UDPUC Device)"/>
+
+  <!-- rws_ip_address: IPV4 address of IRC5 robot controller -->
+  <arg name="rws_ip_address" doc="IP address of IRC5 robot controller" />
+  
+  <!-- J23_coupled: set TRUE to apply correction for J2/J3 parallel linkage -->
+  <arg name="J23_coupled" default="false" doc="If true, compensate for J2-J3 parallel linkage" />
+
+  <!-- copy the specified arguments to the Parameter Server, for use by nodes below -->
+  <param name="J23_coupled" type="bool" value="$(arg J23_coupled)"/>
+  <param name="egm_port" value="$(arg egm_port)"/>
+  <param name="rws_ip_address" type="str" value="$(arg rws_ip_address)"/>
+  
+  <!-- robot_node: publishes joint positions and robot-state data, subscribe trajectories and positions and send them to the robot -->
+  <node pkg="abb_egm_driver" type="robot_node" name="robot_node" output="screen"/>
+  
+  <!-- joint_trajectory_action: provides actionlib interface for high-level robot control -->
+   <node pkg="industrial_robot_client" type="joint_trajectory_action" name="joint_trajectory_action"/>
+
+</launch>

abb_egm_driver/package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>abb_egm_driver</name>
+  <version>1.4.0</version>
+  <description>
+   <p>
+     ROS-Industrial nodes for interfacing with ABB robot controllers using External Guided Motion (EGM) interface.
+   </p>
+   <p>
+     This package is part of the ROS-Industrial program and contains nodes 
+     for interfacing with ABB industrial robot controllers.
+   </p>
+  </description>
+  <author>Jan Weber</author>
+  <maintainer email="jan.weber@hs-bochum.de">Jan Weber</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>abb_libegm</depend>
+  <depend>industrial_robot_client</depend>
+
+</package>

abb_egm_driver/rapid/EGM_joint_trajectory.mod

+MODULE EGM_joint_trajectory  
+    PROC main()
+        !TPWrite "Start EGM interface";
+
+        ! Identifier for the EGM handler.
+        VAR egmident egm_id;
+    
+        ! Limits for convergance.
+        VAR egm_minmax cond_angle := [-0.1, 0.1]; ! degree
+        
+        ! Register an EGM id.
+        EGMGetId egm_id;
+        EGMSetupUC ROB_1, egm_id, "default", "EGM_Transmission", \Joint, \CommTimeout:=5;
+    
+        ! Prepare for an EGM communication session.
+        EGMActJoint egm_id \StreamStart \J1:=cond_angle \J2:=cond_angle \J3:=cond_angle
+                \J4:=cond_angle \J5:=cond_angle \J6:=cond_angle \LpFilter:=15 
+                \SampleRate:=4 \MaxSpeedDeviation:=25.0;
+        WHILE TRUE DO                
+            ! Start the EGM communication session.
+            EGMRunJoint egm_id, EGM_STOP_HOLD,\J1 \J2 \J3 \J4 \J5 \J6 \CondTime:=9999999;
+        ENDWHILE
+            
+        ERROR
+            TPWrite "Error!";
+            IF ERRNO = ERR_UDPUC_COMM THEN
+                TPWrite "Communication timedout";
+                TRYNEXT;
+            ENDIF
+    ENDPROC
+ENDMODULE  
\ No newline at end of file

abb_egm_driver/src/abb_egm_interface.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Southwest Research Institute
+ * 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 the Southwest Research Institute, 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 OWNER 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.
+ */
+
+
+#include "abb_egm_driver/abb_egm_interface.h"
+
+/**
+ * @brief Default constructor for the EGM interface
+ */
+AbbEgmInterface::AbbEgmInterface()
+{
+}
+
+/**
+ * @brief Destructor for the EGM interface
+ */
+AbbEgmInterface::~AbbEgmInterface()
+{
+  delete this->egm_interface_;
+}
+
+/**
+ * @brief Initializes the EGM interface and 
+ * establishes the EGM connection to the robot.
+ * 
+ * @param default_port TPort of the EGM interface on the robot controller
+ *
+ * @return True if the EGM connection was successfully 
+ * established, otherwise False
+ *  */
+bool AbbEgmInterface::init(int default_port)
+{
+  int port;
+  ros::param::param<int>("egm_port", port, default_port);
+  if (port <= 0)
+  {
+    ROS_ERROR("No valid EGM port found.  Please set ROS 'egm_port' param");
+    return false;
+  }
+
+  // Create the interface with which the robot will be controlled
+  this->egm_interface_ = new abb::egm::EGMTrajectoryInterface(io_service_, port);
+
+  if(!egm_interface_->isInitialized())
+  {
+    ROS_ERROR("EGM interface failed to initialize (e.g. due to port already bound)"); 
+    return false;
+  }
+
+  // Spin up a thread to run the io_service of the EGM interface.
+  thread_group.create_thread(boost::bind(&boost::asio::io_service::run, &io_service_));
+
+  ROS_INFO("Wait for the EGM communication session to start...");
+  bool wait = true;
+  while(ros::ok() && wait)
+  {
+    if(egm_interface_->isConnected())
+    {
+      if(egm_interface_->getStatus().rapid_execution_state() == abb::egm::wrapper::Status_RAPIDExecutionState_RAPID_UNDEFINED)
+      {
+        ROS_WARN("RAPID execution state is UNDEFINED (might happen first time after controller start/restart). I will try to restart RAPID once");
+        return false;
+      }
+      else
+      {
+        wait = egm_interface_->getStatus().rapid_execution_state() != abb::egm::wrapper::Status_RAPIDExecutionState_RAPID_RUNNING;
+      }
+    }
+    ros::Duration(0.5).sleep();
+  }
+  ROS_INFO("EGM communication ready");
+  return true;
+
+}
+
+/**
+ * @brief Returns the EGM interface
+ * 
+ * @return The EGM interface
+ *  */
+abb::egm::EGMTrajectoryInterface* AbbEgmInterface::get_egm_interface()
+  {
+    return this->egm_interface_;
+  }
\ No newline at end of file

abb_egm_driver/src/abb_egm_robot_node.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2012, Southwest Research Institute
+ * 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 the Southwest Research Institute, 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 OWNER 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.
+ */
+#include <ros/ros.h>
+#include <signal.h>
+#include <arpa/inet.h>
+
+#include "abb_egm_driver/abb_egm_interface.h"
+#include "abb_egm_driver/abb_egm_robot_interface.h"
+#include <abb_librws/rws_interface.h>
+
+abb::rws::RWSInterface* rws_interface = nullptr;
+AbbEgmInterface* egm_interface = nullptr;
+AbbEgmRobotInterface* robot_interface = nullptr;
+volatile bool ctrl_c_pressed = false;
+
+/**
+ * @brief Interrupt handler that is called when CTRL-C is pressed and 
+ * sets the global flag ctrl_c_pressed. This flag is used, for example, 
+ * to abort loops and terminate the program.
+ */
+void mySigintHandler(int sig)
+{
+  ctrl_c_pressed = true;
+  ROS_INFO("The user has aborted by pressing CTRL-C");
+}
+
+/**
+ * @brief Stops the robot by stopping the execution of RAPID and 
+ * turning off the robot's motors.
+ * 
+ * @param rws_int The RWS interface with which the robot can be controlled
+ *
+ * @return True if the robot could be stopped successfully, otherwise False.
+ */
+bool shutdownRobot(abb::rws::RWSInterface* rws_int)
+{
+  bool success = true;
+  //stop RAPID
+  int cnt = 0;
+  ROS_INFO("Stop RAPID...");
+  while((rws_int->isRAPIDRunning() == abb::rws::TriBool::Values::TRUE_VALUE) && (cnt++ < 2))
+  {
+    if(!rws_int->stopRAPIDExecution())
+      ROS_INFO("Failed to send stopRAPIDExecution command, retry...");
+      ros::Duration(0.5).sleep();
+  }
+  if(rws_int->isRAPIDRunning() == abb::rws::TriBool::Values::TRUE_VALUE)
+  {
+    ROS_WARN_STREAM("Failed to stop RAPID! isRAPIDRunning = " << rws_int->isRAPIDRunning());
+    success = false;
+  }
+  
+  //stop motors
+  cnt = 0;
+  ROS_INFO("Stop motors...");
+  while((rws_int->isMotorsOn() == abb::rws::TriBool::Values::TRUE_VALUE) && (cnt++ < 2))
+  {
+    if(!rws_int->setMotorsOff())
+      ROS_INFO("Failed to send setMotorsOff command, retry...");
+      ros::Duration(0.5).sleep();
+  }
+  if(rws_int->isMotorsOn() == abb::rws::TriBool::Values::TRUE_VALUE)
+  {
+    ROS_WARN_STREAM("Failed to stop motors! isMotorsOn = " << rws_int->isMotorsOn());
+    success = false;
+  }
+  return success;
+}
+
+/**
+ * @brief Restarts RAPID by stopping RAPID, resetting the 
+ * program pointer, and starting RAPID.
+ * 
+ * @param rws_int The RWS interface with which the robot can be controlled
+ *
+ */
+void restartRapid(abb::rws::RWSInterface* rws_int)
+{
+    do
+    {
+      if(!rws_int->stopRAPIDExecution())
+        ROS_INFO("Failed to send stopRAPIDExecution command, retry...");
+      ros::Duration(1.0).sleep();
+    }while(!ctrl_c_pressed && rws_int->isRAPIDRunning() == abb::rws::TriBool::Values::TRUE_VALUE && !ctrl_c_pressed);
+
+    while(!ctrl_c_pressed && !rws_int->resetRAPIDProgramPointer() && !ctrl_c_pressed)
+    {
+      ROS_INFO("Failed to reset program pointer, retry...");
+      ros::Duration(1.0).sleep();
+    }
+
+    do
+    {
+      if(!rws_int->startRAPIDExecution())
+        ROS_INFO("Failed to send startRAPIDExecution command, retry...");
+      ros::Duration(1.0).sleep();
+    }while(!ctrl_c_pressed && rws_int->isRAPIDRunning() != abb::rws::TriBool::Values::TRUE_VALUE && !ctrl_c_pressed);
+
+}
+
+/**
+ * @brief Prepares the robot so that it is ready to work with the driver. 
+ * It is first checked if the RWS interface is connected and the robot 
+ * is in automatic mode. Then the motors are switched on and RAPID is started.
+ * 
+ * @param rws_int The RWS interface with which the robot can be controlled
+ *
+ * @return True if the robot is ready, otherwise False
+ */
+bool prepareRobot(abb::rws::RWSInterface* rws_int)
+{
+  //as long as RWS is not connected
+  while(!ctrl_c_pressed && !rws_int->collectRuntimeInfo().rws_connected && !ctrl_c_pressed)
+  {
+    //wait for RWS connection
+    ROS_INFO("Wait for RWS connection...");
+    ros::Duration(0.5).sleep(); 
+  }
+  
+  //As long as the robot is not in automatic mode
+  while(!ctrl_c_pressed && rws_int->isAutoMode() != abb::rws::TriBool::Values::TRUE_VALUE && !ctrl_c_pressed)
+  {
+    ROS_INFO("IRC is not in automatic mode! You should set the robot controller in automatic mode now!");
+    ros::Duration(1.0).sleep();
+  }
+
+  //Check, if motors are switched on. If not, switch them on.
+  while(!ctrl_c_pressed && rws_int->isMotorsOn() != abb::rws::TriBool::Values::TRUE_VALUE && !ctrl_c_pressed)
+  {
+    if(!rws_int->setMotorsOn())
+    {
+      ROS_INFO("Failed to switch on motors, retry...");
+    }
+    ros::Duration(1.0).sleep();
+  }
+
+  //Check, if RAPID is running. If not, reset program pointer and start RAPID!
+  if(!ctrl_c_pressed && rws_int->isRAPIDRunning() != abb::rws::TriBool::Values::TRUE_VALUE)
+  {
+    while(!ctrl_c_pressed && !rws_int->resetRAPIDProgramPointer() && !ctrl_c_pressed)
+    {
+      ROS_INFO("Failed to reset program pointer, retry...");
+      ros::Duration(1.0).sleep();
+    }
+
+    do
+    {
+      if(!rws_int->startRAPIDExecution())
+        ROS_INFO("Failed to send startRAPIDExecution command, retry...");
+      ros::Duration(1.0).sleep();
+    }while(!ctrl_c_pressed && rws_int->isRAPIDRunning() != abb::rws::TriBool::Values::TRUE_VALUE && !ctrl_c_pressed);
+
+  }
+
+  if(ctrl_c_pressed)
+  {
+    return false;
+  }
+  else
+  {
+    bool success = rws_interface->collectRuntimeInfo().rws_connected &&
+            rws_interface->isAutoMode() == abb::rws::TriBool::Values::TRUE_VALUE && 
+            rws_interface->isMotorsOn() == abb::rws::TriBool::Values::TRUE_VALUE && 
+            rws_interface->isRAPIDRunning() == abb::rws::TriBool::Values::TRUE_VALUE;
+
+    if(!success)
+    {
+      ROS_INFO("Failed to initialize RWS:");
+      ROS_INFO_STREAM("rws_connected: " << rws_interface->collectRuntimeInfo().rws_connected);
+      ROS_INFO_STREAM("automatic mode: " << rws_interface->isAutoMode());
+      ROS_INFO_STREAM("motors on: " << rws_interface->isMotorsOn());
+      ROS_INFO_STREAM("RAPID running: " << rws_interface->isRAPIDRunning());
+    }
+    return success;
+  }
+}
+
+/**
+ * @brief Checks if the string ip_adress contains a valid IPV4 address.
+ * 
+ * @param rws_int The string containing the IP address to be checked.
+ *
+ * @return True if valid, otherwise False
+ */
+bool validateIpAddress(std::string &ipAddress)
+{
+    struct sockaddr_in sa;
+    int result = inet_pton(AF_INET, ipAddress.c_str(), &(sa.sin_addr));
+    return result != 0;
+}
+
+/**
+ * @brief The main method that starts the EGM driver
+ */
+int main(int argc, char** argv)
+{
+  signal(SIGINT, mySigintHandler);
+  // initialize node
+  ros::init(argc, argv, "robot_node", ros::init_options::NoSigintHandler);
+  ros::NodeHandle n;
+  
+  //get and validate RWS ip address
+  std::string rws_ip_address;
+  if(!n.getParam("rws_ip_address", rws_ip_address))
+  {
+    ROS_ERROR("Param rws_ip_address not available!");
+    return 0;
+  }
+  if(!validateIpAddress(rws_ip_address))
+  {
+    ROS_ERROR("rws_ip_address = '%s' not valid!", rws_ip_address.c_str());
+    return 0;
+  }
+
+  bool begin = true;
+  ros::Rate r(10);
+  while(!ctrl_c_pressed)
+  {
+    //when at the beginning or if there are communication problems
+    if(begin || robot_interface->isCommunicatonError())
+    {
+      begin = false;
+
+      // Create the RWS interface
+      if(!rws_interface)
+      {
+        rws_interface = new abb::rws::RWSInterface(rws_ip_address);
+      }
+
+      // Prepare the robot so that it is in the right state to work with the driver
+      if(prepareRobot(rws_interface))
+      {
+        // Create the RWS interface
+        if(!egm_interface)
+        {
+          egm_interface = new AbbEgmInterface();
+          // Initialize the EGM interface. If it fails, restart RAPID and try again. 
+          // After switching on the robot controller, it often happens that the initialization fails.
+          // This can be fixed by restarting RAPID.
+          if(!egm_interface->init()) restartRapid(rws_interface);
+          if(!egm_interface->init()) break;
+        }
+
+        // Create the Robot interface
+        if(!robot_interface)
+        {
+          //prepare robot interface, this will subscribe ROS topics and services
+          robot_interface = new AbbEgmRobotInterface(egm_interface->get_egm_interface());
+          if(!robot_interface->init()) break;
+
+          //start Robot interface thread, this will publish robot states in ROS
+          robot_interface->start();
+          ros::Duration(1.0).sleep();
+        }
+      }
+    }
+
+    // if something goes wrong
+    if(!robot_interface->isEgmRunning() || !robot_interface->isRapidRunning())
+    {
+      if(!robot_interface->isEgmRunning())ROS_WARN("The EGM interface stopped working.");
+      if(!robot_interface->isRapidRunning())ROS_WARN("RAPID stopped working.");
+      ROS_WARN("Check the status of the robot, fix the problems that occurred (for example, emergency stop pressed, SafeZone violated) and then restart the driver (CTRL-C)");
+      ROS_WARN("Note: After an emergency stop, the motors must be activated manually using the button on the robot controller!");
+      while(!ctrl_c_pressed){ros::Duration(1.0).sleep();}     
+    }
+
+    // Sleep at a rate of 10Hz
+    r.sleep();
+  }
+
+  // CTRL-C was pressed, the driver should shut down
+
+  //stop incoming motions, stop subscriber, stop thread
+  delete robot_interface; 
+
+  //stop RAPID, switch off motors
+  if(rws_interface)
+    shutdownRobot(rws_interface);
+
+  delete egm_interface;
+  delete rws_interface;
+  ros::shutdown();
+
+  return 0;
+}

abb_egm_driver/src/abb_utils.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2012, Southwest Research Institute
+ * 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 the Southwest Research Institute, 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 OWNER 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.
+ */
+
+#include "abb_egm_driver/abb_utils.h"
+#include "ros/ros.h"
+
+namespace abb
+{
+namespace utils
+{
+
+// TBD: This transform should also account for velocity/acceleration affects due to linkage, so that velocity calculation is accurate
+void linkage_transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out, double J23_factor)
+{
+  *pt_out = pt_in;
+  linkage_transform(pt_in.positions, &(pt_out->positions), J23_factor);
+}
+
+void linkage_transform(const std::vector<double>& points_in, std::vector<double>* points_out, double J23_factor)
+{
+  ROS_ASSERT(points_in.size() > 3);
+
+  *points_out = points_in;
+  points_out->at(2) += J23_factor * points_out->at(1);
+}
+
+} //abb
+
+} //utils

abb_libegm @ e0b8c3b0

+Subproject commit e0b8c3b0a906a50a7ad8d1d1b465a4343369c4e8

abb_librws @ 3a5914ef

+Subproject commit 3a5914ef383a18e99c14f90831528ffda943cf97