├── .github └── workflows │ └── ci_bionic.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include └── abb_driver │ └── abb_utils.h ├── launch └── robot_interface.launch ├── package.xml ├── rapid ├── ROS_common.sys ├── ROS_messages.sys ├── ROS_motion.mod ├── ROS_motionServer.mod ├── ROS_socket.sys └── ROS_stateServer.mod └── src ├── abb_joint_downloader_node.cpp ├── abb_robot_state_node.cpp └── abb_utils.cpp /.github/workflows/ci_bionic.yml: -------------------------------------------------------------------------------- 1 | name: CI - Ubuntu Bionic 2 | 3 | on: 4 | # direct pushes to protected branches are not supported 5 | pull_request: 6 | # run every day, at 6am UTC 7 | schedule: 8 | - cron: '0 6 * * *' 9 | # allow manually starting this workflow 10 | workflow_dispatch: 11 | 12 | jobs: 13 | industrial_ci: 14 | name: ROS Melodic (${{ matrix.ros_repo }}) 15 | runs-on: ubuntu-20.04 16 | 17 | strategy: 18 | matrix: 19 | ros_distro: [ melodic ] 20 | ros_repo: [ main, testing ] 21 | 22 | env: 23 | CCACHE_DIR: "${{ github.workspace }}/.ccache" 24 | 25 | steps: 26 | - name: Fetch repository 27 | uses: actions/checkout@v3 28 | 29 | - name: ccache cache 30 | uses: actions/cache@v3 31 | with: 32 | path: ${{ env.CCACHE_DIR }} 33 | # we always want the ccache cache to be persisted, as we cannot easily 34 | # determine whether dependencies have changed, and ccache will manage 35 | # updating the cache for us. Adding 'run_id' to the key will force an 36 | # upload at the end of the job. 37 | key: ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }}-${{github.run_id}} 38 | restore-keys: | 39 | ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }} 40 | 41 | - name: Run industrial_ci 42 | uses: ros-industrial/industrial_ci@master 43 | env: 44 | ROS_DISTRO: ${{ matrix.ros_distro }} 45 | ROS_REPO: ${{ matrix.ros_repo }} 46 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package abb_driver 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.4.0 (2020-06-22) 6 | ------------------ 7 | * First release of stand-alone ``abb_driver`` from new repository. 8 | * Contributors: gavanderhoorn 9 | 10 | 1.3.1 (2019-09-17) 11 | ------------------ 12 | * Add robot status publishing to Rapid driver (`#168 `_) 13 | * Fix is_near check (`#155 `_) 14 | * Add support for external linear axes to abb_driver (`#150 `_) 15 | * Use the 'doc' attribute on 'arg' elements. (`#143 `_) 16 | * Update maintainers (`#139 `_) 17 | * Contributors: Gonzalo Casas, Harsh Deshpande, JD Yamokoski, Keerthana Manivannan, gavanderhoorn 18 | 19 | 1.3.0 (2017-05-27) 20 | ------------------ 21 | * kinetic-devel release of ros-industrial/abb 22 | * Contributors: AustinDeric 23 | 24 | 1.2.1 (2017-03-27) 25 | ------------------ 26 | * No changes 27 | 28 | 1.2.0 (2015-06-06) 29 | ------------------ 30 | * No changes 31 | 32 | 1.1.9 (2015-04-07) 33 | ------------------ 34 | * No changes 35 | 36 | 1.1.8 (2015-04-06) 37 | ------------------ 38 | * No changes 39 | 40 | 1.1.7 (2015-04-01) 41 | ------------------ 42 | * Merged hydro branch 43 | - Updated CHANGELOG.rst and package.xml files 44 | * Contributors: Levi Armstrong 45 | 46 | 1.1.6 (2015-03-17) 47 | ------------------ 48 | * Fix typos and links in CHANGELOG.rst 49 | * Contributors: Levi Armstrong 50 | 51 | 1.1.5 (2015-03-17) 52 | ------------------ 53 | * driver: ROS_motionServer.mod was checking for wrong comm type to send reply 54 | message, causing driver to quit working after stop command. 55 | Fix `#42 `_. 56 | * Contributors: Levi Armstrong 57 | 58 | 1.1.4 (2014-12-14) 59 | ------------------ 60 | * No changes 61 | 62 | 1.1.3 (2014-09-05) 63 | ------------------ 64 | * driver: reintroduce coupling factor default. 65 | Reverts 3765cd6. 66 | * Bump versions. 67 | * driver: remove default for J23 coupling parameter. 68 | Users should explicitly provide this on the command line, or use one 69 | of the convenience launchfiles provided with the support packages. 70 | It is of critical importance that this parameter is set to the 71 | correct value, and should therefore not be supplied a default in 72 | a (for end-users) file with low visibility. 73 | * driver: move driver (Rapid and nodes) into separate package. 74 | Node sources, headers and launch files copied from abb_common. 75 | * Contributors: gavanderhoorn 76 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(abb_driver) 3 | 4 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 5 | 6 | find_package(catkin REQUIRED COMPONENTS industrial_robot_client simple_message) 7 | 8 | catkin_package( 9 | CATKIN_DEPENDS industrial_robot_client 10 | ) 11 | 12 | include_directories( 13 | include 14 | ${catkin_INCLUDE_DIRS} 15 | ) 16 | 17 | 18 | add_executable(${PROJECT_NAME}_robot_state 19 | src/abb_robot_state_node.cpp 20 | src/abb_utils.cpp) 21 | target_link_libraries(${PROJECT_NAME}_robot_state 22 | industrial_robot_client 23 | simple_message 24 | ${catkin_LIBRARIES}) 25 | set_target_properties( 26 | ${PROJECT_NAME}_robot_state 27 | PROPERTIES OUTPUT_NAME robot_state PREFIX "") 28 | target_compile_definitions( 29 | ${PROJECT_NAME}_robot_state 30 | PRIVATE 31 | ${industrial_robot_client_DEFINITIONS}) 32 | 33 | add_executable(${PROJECT_NAME}_motion_download_interface 34 | src/abb_joint_downloader_node.cpp 35 | src/abb_utils.cpp) 36 | target_link_libraries(${PROJECT_NAME}_motion_download_interface 37 | industrial_robot_client 38 | simple_message 39 | ${catkin_LIBRARIES}) 40 | set_target_properties(${PROJECT_NAME}_motion_download_interface 41 | PROPERTIES OUTPUT_NAME motion_download_interface PREFIX "") 42 | target_compile_definitions( 43 | ${PROJECT_NAME}_motion_download_interface 44 | PRIVATE 45 | ${industrial_robot_client_DEFINITIONS}) 46 | 47 | install( 48 | TARGETS 49 | ${PROJECT_NAME}_robot_state 50 | ${PROJECT_NAME}_motion_download_interface 51 | DESTINATION 52 | ${CATKIN_PACKAGE_BIN_DESTINATION} 53 | ) 54 | 55 | foreach(dir launch rapid) 56 | install( 57 | DIRECTORY ${dir}/ 58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 59 | endforeach() 60 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # abb_driver 2 | 3 | [![Github Issues](https://img.shields.io/github/issues/ros-industrial/abb_driver.svg)](http://github.com/ros-industrial/abb_driver/issues) 4 | 5 | [![license - bsd 3 clause](https://img.shields.io/:license-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) 6 | 7 | [![support level: community](https://img.shields.io/badge/support%20level-community-lightgray.png)](http://rosindustrial.org/news/2016/10/7/better-supporting-a-growing-ros-industrial-software-platform) 8 | 9 | [ROS-Industrial][] RAPID based driver for ABB IRC5 controllers. 10 | 11 | 12 | ## Contents 13 | 14 | This repository contains a simple, RAPID based ROS driver for ABB industrial robots connected to IRC5 controllers. 15 | 16 | The driver is largely manipulator agnostic, and is expected to work with any ABB manipulator compatible with an IRC5 controller. 17 | 18 | For more information, refer to the [ROS wiki][]. 19 | 20 | Note: this is not a new development, but a migration of the old `abb_driver` package to a separate repository. See [Origin and history](#origin-and-history) for more information. 21 | 22 | 23 | ## TOC 24 | 25 | 1. [Status](#status) 26 | 1. [Performance](#performance) 27 | 1. [Requirements](#requirements) 28 | 1. [Releases and supported ROS distributions](#releases-and-supported-ros-distributions) 29 | 1. [Installation](#installation) 30 | 1. [Building from source](#building-from-source) 31 | 1. [Usage](#usage) 32 | 1. [Origin and history](#origin-and-history) 33 | 34 | 35 | ## Status 36 | 37 | This package is usable as-is, but is not feature complete. 38 | Only joint motion can be commanded and there is no support for IO, Cartesian motion or any other more advanced RAPID functionality. 39 | 40 | No significant development is planned, as focus has shifted to [abb_robot_driver][] (with [abb_libegm][] and [abb_librws][]). 41 | 42 | Community contributed usability enhancements and new features will however be accepted and merged. 43 | 44 | 45 | ## Performance 46 | 47 | Performance of the driver (of both the ROS and RAPID components) is deemed sufficient for mildly dynamic workloads (ie: pick-and-place and relatively slow motion). 48 | Processes for which control of the robot's position and velocity must achieve high resolution in both time and space are not supported by this driver. 49 | 50 | Users are encouraged to consider using [abb_libegm][] and [abb_librws][] instead. 51 | 52 | 53 | ## Requirements 54 | 55 | Please refer to the [ROS wiki][] for information on supported controllers, required RobotWare versions and required controller options. 56 | 57 | 58 | ## Releases and supported ROS distributions 59 | 60 | This repository follows the main [abb][] repository as far as development and maintenance policies, branching strategies and release scheduling. 61 | Branch naming follows the ROS distribution they are compatible with. `-devel` branches may be unstable. 62 | 63 | `abb_driver` has been successfully built from sources with both ROS Kinetic and ROS Melodic, on Ubuntu Xenial and Ubuntu Bionic respectively. 64 | Only `amd64` architectures have been tested. 65 | Other combinations of OS, ROS versions and architectures may work, but have not been tested. 66 | 67 | Binary packages are provided for ROS Kinetic and ROS Melodic on all platforms supported by the ROS buildfarm for those ROS releases. 68 | Refer to [REP 3: Target Platforms][], *Kinetic Kame* and *Melodic Morenia* sections for more information on which platforms are supported. 69 | 70 | 71 | ## Installation 72 | 73 | On Ubuntu, installation via `apt` is recommended over building the package from source. 74 | 75 | For ROS Melodic, the following command installs the driver and all of its dependencies (after having configured the ROS package repositories): 76 | 77 | ``` 78 | sudo apt install ros-melodic-abb-driver 79 | ``` 80 | 81 | When using ROS Kinetic, replace `melodic` with `kinetic`. 82 | 83 | Instructions to build the driver from source, refer to the next section. 84 | 85 | 86 | ## Building from source 87 | 88 | ### On newer (or older) versions of ROS 89 | 90 | Building `abb_driver` on Ubuntu Xenial/ROS Kinetic and Ubuntu Bionic/ROS Melodic systems is supported. This will require creating a Catkin workspace, cloning this repository, installing all required dependencies and finally building the workspace. 91 | 92 | ### Catkin tools 93 | 94 | It is recommended to use [catkin_tools][] instead of the default [catkin][] when building ROS workspaces. `catkin_tools` provides a number of benefits over regular `catkin_make` and will be used in the instructions below. The package can be built using `catkin_make` however: use `catkin_make` in place of `catkin build` where appropriate. 95 | 96 | ### Building the package 97 | 98 | The following instructions assume that a [Catkin workspace][] has been created at `$HOME/catkin_ws` and that the *source space* is at `$HOME/catkin_ws/src`. Update paths appropriately if they are different on the build machine. 99 | 100 | These instructions build the `kinetic-devel` branch on a ROS Kinetic system: 101 | 102 | ```bash 103 | # change to the root of the Catkin workspace 104 | cd $HOME/catkin_ws 105 | 106 | # retrieve the latest development version of the abb_driver repository. If you'd rather 107 | # use the latest released version, replace 'kinetic-devel' with 'kinetic' 108 | git clone -b kinetic-devel https://github.com/ros-industrial/abb_driver.git src/abb_driver 109 | 110 | # check for and install missing build dependencies. 111 | 112 | # first: update the local database 113 | rosdep update 114 | 115 | # now install dependencies, again using rosdep. 116 | # Note: this may install additional packages, depending on the software already present 117 | # on the machine. 118 | # Be sure to change 'kinetic' to whichever ROS version you are using 119 | rosdep install --from-paths src/ --ignore-src --rosdistro kinetic 120 | 121 | # build the workspace (using catkin_tools) 122 | catkin build 123 | ``` 124 | 125 | ### Activating the workspace 126 | 127 | Finally, activate the workspace to get access to the packages just built: 128 | 129 | ```bash 130 | source $HOME/catkin_ws/devel/setup.bash 131 | ``` 132 | 133 | At this point the package should be usable (ie: `roslaunch` should be able to auto-complete `abb_driver`). In case the workspace contains additional packages (ie: not from this repository), those should also still be available. 134 | 135 | 136 | ## Usage 137 | 138 | Refer to [Working With ROS-Industrial Robot Support Packages][] for information on how to use the files provided by the driver. See also the other pages on the [ROS wiki][]. 139 | 140 | Refer to the [tutorials][] for information on installation and configuration of the controller-specific software components. 141 | 142 | 143 | ## Origin and history 144 | 145 | This package was extracted from the main [abb][] repository. 146 | Refer to [ros-industrial/abb#179][] for rationale and a description of the workflow. 147 | 148 | As `ros-industrial/abb` contained fairly large files which would no longer be relevant for the extracted package, `git-filter-branch` was used to prune everything unrelated to the driver. This has rewritten history. 149 | 150 | Commits and file provenance however have been retained as much as possible, and references to issues and PRs have been updated wherever possible. 151 | Only the `kinetic-devel` branch was migrated: others can still be found at `ros-industrial/abb`. 152 | Open issues on the tracker of `ros-industrial/abb` against `abb_driver` have been transferred to the new repository. 153 | 154 | Note: as commit history has been altered, commit hashes will have changed. 155 | References in issues and comments on commits in this repository from before the migration will therefor no longer link to their respective commits. 156 | 157 | 158 | 159 | 160 | 161 | 162 | [ROS-Industrial]: http://wiki.ros.org/Industrial 163 | [ROS wiki]: http://wiki.ros.org/abb_driver 164 | [abb]: https://github.com/ros-industrial/abb 165 | [ros-industrial/abb#179]: https://github.com/ros-industrial/abb/issues/179 166 | [abb_robot_driver]: https://github.com/ros-industrial/abb_robot_driver 167 | [abb_libegm]: https://github.com/ros-industrial/abb_libegm 168 | [abb_librws]: https://github.com/ros-industrial/abb_librws 169 | [REP 3: Target Platforms]: https://ros.org/reps/rep-0003.html 170 | [Catkin workspace]: http://wiki.ros.org/catkin/Tutorials/create_a_workspace 171 | [catkin]: http://wiki.ros.org/catkin 172 | [catkin_tools]: https://catkin-tools.readthedocs.io/en/latest 173 | [Working With ROS-Industrial Robot Support Packages]: http://wiki.ros.org/Industrial/Tutorials/WorkingWithRosIndustrialRobotSupportPackages 174 | [tutorials]: http://wiki.ros.org/abb/Tutorials 175 | 176 | -------------------------------------------------------------------------------- /include/abb_driver/abb_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2012, Southwest Research Institute 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Southwest Research Institute, nor the names 16 | * of its contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef ABB_UTILS_H_ 33 | #define ABB_UTILS_H_ 34 | 35 | #include 36 | #include "trajectory_msgs/JointTrajectory.h" 37 | 38 | namespace abb 39 | { 40 | namespace utils 41 | { 42 | 43 | /** 44 | * \brief Corrects for parallel linkage coupling between joints. 45 | * 46 | * \param[in] joints_in input joint angles 47 | * \param[out] joints_out output joint angles 48 | * \param[in] J23_factor Linkage factor for J2-J3. 49 | * J3_out = J3_in + j23_factor * J2_in 50 | */ 51 | void linkage_transform(const std::vector& joints_in, std::vector* joints_out, double J23_factor=0); 52 | 53 | /** 54 | * \brief Corrects for parallel linkage coupling between joints. 55 | * 56 | * \param[in] pt_in input joint trajectory point 57 | * \param[out] pt_out output joint trajectory point 58 | * \param[in] J23_factor Linkage factor for J2-J3. 59 | * J3_out = J3_in + j23_factor * J2_in 60 | */ 61 | void linkage_transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out, double J23_factor=0); 62 | 63 | } //abb 64 | } //utils 65 | 66 | #endif /* ABB_UTILS_H_ */ 67 | -------------------------------------------------------------------------------- /launch/robot_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | abb_driver 4 | 1.4.0 5 | 6 |

7 | ROS-Industrial nodes for interfacing with ABB robot controllers. 8 |

9 |

10 | This package is part of the ROS-Industrial program and contains nodes 11 | for interfacing with ABB industrial robot controllers. 12 |

13 | 14 | Edward Venator 15 | Jeremy Zoss 16 | Shaun Edwards 17 | Levi Armstrong (Southwest Research Institute) 18 | BSD 19 | 20 | http://wiki.ros.org/abb_driver 21 | https://github.com/ros-industrial/abb_driver/issues 22 | https://github.com/ros-industrial/abb_driver 23 | 24 | catkin 25 | 26 | simple_message 27 | industrial_robot_client 28 | 29 | 30 | -------------------------------------------------------------------------------- /rapid/ROS_common.sys: -------------------------------------------------------------------------------- 1 | MODULE ROS_common(SYSMODULE) 2 | 3 | ! Software License Agreement (BSD License) 4 | ! 5 | ! Copyright (c) 2012, Edward Venator, Case Western Reserve University 6 | ! Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute 7 | ! All rights reserved. 8 | ! 9 | ! Redistribution and use in source and binary forms, with or without modification, 10 | ! are permitted provided that the following conditions are met: 11 | ! 12 | ! Redistributions of source code must retain the above copyright notice, this 13 | ! list of conditions and the following disclaimer. 14 | ! Redistributions in binary form must reproduce the above copyright notice, this 15 | ! list of conditions and the following disclaimer in the documentation 16 | ! and/or other materials provided with the distribution. 17 | ! Neither the name of the Case Western Reserve University nor the names of its contributors 18 | ! may be used to endorse or promote products derived from this software without 19 | ! specific prior written permission. 20 | ! 21 | ! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 22 | ! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 23 | ! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 24 | ! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | ! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 26 | ! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 27 | ! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | ! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 29 | ! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | RECORD ROS_joint_trajectory_pt 32 | robjoint joint_pos; 33 | extjoint extax_pos; 34 | num duration; 35 | ENDRECORD 36 | 37 | CONST num MAX_TRAJ_LENGTH := 100; 38 | 39 | ! These variables should use TestAndSet read/write protection to prevent conflicts 40 | PERS bool ROS_trajectory_lock := false; 41 | PERS ROS_joint_trajectory_pt ROS_trajectory{MAX_TRAJ_LENGTH}; 42 | PERS num ROS_trajectory_size := 0; 43 | PERS bool ROS_new_trajectory := false; ! can safely READ, but should use lock to WRITE 44 | 45 | ENDMODULE 46 | -------------------------------------------------------------------------------- /rapid/ROS_messages.sys: -------------------------------------------------------------------------------- 1 | MODULE ROS_messages(SYSMODULE) 2 | ! Software License Agreement (BSD License) 3 | ! 4 | ! Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute 5 | ! All rights reserved. 6 | ! 7 | ! Redistribution and use in source and binary forms, with or without modification, 8 | ! are permitted provided that the following conditions are met: 9 | ! 10 | ! Redistributions of source code must retain the above copyright notice, this 11 | ! list of conditions and the following disclaimer. 12 | ! Redistributions in binary form must reproduce the above copyright notice, this 13 | ! list of conditions and the following disclaimer in the documentation 14 | ! and/or other materials provided with the distribution. 15 | ! Neither the name of the Case Western Reserve University nor the names of its contributors 16 | ! may be used to endorse or promote products derived from this software without 17 | ! specific prior written permission. 18 | ! 19 | ! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 20 | ! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 21 | ! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 22 | ! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 23 | ! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 24 | ! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | ! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | ! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 27 | ! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | RECORD ROS_msg_header 30 | num msg_type; 31 | num comm_type; 32 | num reply_code; 33 | ENDRECORD 34 | 35 | RECORD ROS_msg 36 | ROS_msg_header header; 37 | rawbytes data; 38 | ENDRECORD 39 | 40 | RECORD ROS_msg_joint_traj_pt 41 | ROS_msg_header header; 42 | num sequence_id; 43 | robjoint joints; ! in DEGREES 44 | extjoint ext_axes; 45 | num velocity; 46 | num duration; 47 | ENDRECORD 48 | 49 | RECORD ROS_msg_joint_data 50 | ROS_msg_header header; 51 | num sequence_id; 52 | robjoint joints; ! in DEGREES 53 | extjoint ext_axes; 54 | ENDRECORD 55 | 56 | RECORD ROS_msg_robot_status 57 | ROS_msg_header header; 58 | num sequence_id; 59 | num drives_powered; 60 | num e_stopped; 61 | num error_code; 62 | num in_error; 63 | num in_motion; 64 | num mode; 65 | num motion_possible; 66 | ENDRECORD 67 | 68 | ! Message Type Codes (from simple_message/simple_message.h) 69 | CONST num ROS_MSG_TYPE_INVALID := 0; 70 | CONST num ROS_MSG_TYPE_JOINT := 10; ! joint-position feedback 71 | CONST num ROS_MSG_TYPE_JOINT_TRAJ_PT := 11; ! joint-trajectory-point (for path downloading) 72 | CONST num ROS_MSG_TYPE_STATUS := 13; ! robot status message (for reporting the robot state) 73 | CONST num ROS_COM_TYPE_TOPIC := 1; 74 | CONST num ROS_COM_TYPE_SRV_REQ := 2; 75 | CONST num ROS_COM_TYPE_SRV_REPLY := 3; 76 | CONST num ROS_REPLY_TYPE_INVALID := 0; 77 | CONST num ROS_REPLY_TYPE_SUCCESS := 1; 78 | CONST num ROS_REPLY_TYPE_FAILURE := 2; 79 | 80 | ! "Special" Sequence Codes (from simple_message/joint_traj_pt.h) 81 | CONST num ROS_TRAJECTORY_START_DOWNLOAD := -1; 82 | CONST num ROS_TRAJECTORY_END := -3; 83 | CONST num ROS_TRAJECTORY_STOP := -4; 84 | 85 | ! Robot mode codes (from industrial_msgs/RobotMode.msg) 86 | CONST num ROS_ROBOT_MODE_UNKNOWN := -1; ! Unknown or unavailable 87 | CONST num ROS_ROBOT_MODE_MANUAL := 1; ! Teach OR manual mode 88 | CONST num ROS_ROBOT_MODE_AUTO := 2; ! Automatic mode 89 | 90 | ! Tri-state values (from inudstrial/TriState.msg) 91 | CONST num ROS_TRISTATE_UNKNOWN := -1; 92 | CONST num ROS_TRISTATE_TRUE := 1; 93 | CONST num ROS_TRISTATE_ON := 1; 94 | CONST num ROS_TRISTATE_FALSE := 0; 95 | CONST num ROS_TRISTATE_OFF := 0; 96 | 97 | ! Other message constants 98 | CONST num ROS_MSG_MAX_JOINTS := 10; ! from joint_data.h 99 | 100 | PROC ROS_receive_msg_joint_traj_pt(VAR socketdev client_socket, VAR ROS_msg_joint_traj_pt message, \num wait_time) 101 | VAR ROS_msg raw_message; 102 | 103 | ! Read raw message data 104 | IF Present(wait_time) THEN 105 | ROS_receive_msg client_socket, raw_message, \wait_time:=wait_time; 106 | ELSE 107 | ROS_receive_msg client_socket, raw_message; 108 | ENDIF 109 | 110 | ! Integrity Check: Message Type 111 | IF (raw_message.header.msg_type <> ROS_MSG_TYPE_JOINT_TRAJ_PT) THEN 112 | ErrWrite \W, "ROS Socket Type Mismatch", "Unexpected message type", 113 | \RL2:="expected: " + ValToStr(ROS_MSG_TYPE_JOINT_TRAJ_PT), 114 | \RL3:="received: " + ValToStr(raw_message.header.msg_type); 115 | RAISE ERR_ARGVALERR; ! TBD: define specific error code 116 | ENDIF 117 | 118 | ! Integrity Check: Data Size 119 | IF (RawBytesLen(raw_message.data) < 52) THEN 120 | ErrWrite \W, "ROS Socket Missing Data", "Insufficient data for joint_trajectory_pt", 121 | \RL2:="expected: 52", 122 | \RL3:="received: " + ValToStr(RawBytesLen(raw_message.data)); 123 | RAISE ERR_OUTOFBND; ! TBD: define specific error code 124 | ENDIF 125 | 126 | ! Copy Header data 127 | message.header := raw_message.header; 128 | 129 | ! Unpack data fields 130 | UnpackRawBytes raw_message.data, 1, message.sequence_id, \IntX:=DINT; 131 | UnpackRawBytes raw_message.data, 5, message.joints.rax_1, \Float4; 132 | UnpackRawBytes raw_message.data, 9, message.joints.rax_2, \Float4; 133 | UnpackRawBytes raw_message.data, 13, message.joints.rax_3, \Float4; 134 | UnpackRawBytes raw_message.data, 17, message.joints.rax_4, \Float4; 135 | UnpackRawBytes raw_message.data, 21, message.joints.rax_5, \Float4; 136 | UnpackRawBytes raw_message.data, 25, message.joints.rax_6, \Float4; 137 | UnpackRawBytes raw_message.data, 29, message.ext_axes.eax_a, \Float4; 138 | UnpackRawBytes raw_message.data, 33, message.ext_axes.eax_b, \Float4; 139 | UnpackRawBytes raw_message.data, 37, message.ext_axes.eax_c, \Float4; 140 | UnpackRawBytes raw_message.data, 41, message.ext_axes.eax_d, \Float4; 141 | UnpackRawBytes raw_message.data, 45, message.velocity, \Float4; 142 | UnpackRawBytes raw_message.data, 49, message.duration, \Float4; 143 | 144 | ! Convert data from ROS units to ABB units 145 | message.joints := rad2deg_robjoint(message.joints); 146 | message.ext_axes := m2mm_extjoint(message.ext_axes); 147 | ! TBD: convert velocity 148 | 149 | ERROR 150 | RAISE; ! raise errors to calling code 151 | ENDPROC 152 | 153 | PROC ROS_send_msg_joint_data(VAR socketdev client_socket, ROS_msg_joint_data message) 154 | VAR ROS_msg raw_message; 155 | VAR robjoint ROS_joints; 156 | VAR extjoint ROS_ext_axes; 157 | VAR num i; 158 | 159 | ! Force message header to the correct values 160 | raw_message.header.msg_type := ROS_MSG_TYPE_JOINT; 161 | raw_message.header.comm_type := ROS_COM_TYPE_TOPIC; 162 | raw_message.header.reply_code := ROS_REPLY_TYPE_INVALID; 163 | 164 | ! Convert data from ABB units to ROS units 165 | ROS_joints := deg2rad_robjoint(message.joints); 166 | ROS_ext_axes := mm2m_extjoint(message.ext_axes); 167 | 168 | ! Pack data into message 169 | PackRawBytes message.sequence_id, raw_message.data, 1, \IntX:=DINT; 170 | PackRawBytes ROS_joints.rax_1, raw_message.data, 5, \Float4; 171 | PackRawBytes ROS_joints.rax_2, raw_message.data, 9, \Float4; 172 | PackRawBytes ROS_joints.rax_3, raw_message.data, 13, \Float4; 173 | PackRawBytes ROS_joints.rax_4, raw_message.data, 17, \Float4; 174 | PackRawBytes ROS_joints.rax_5, raw_message.data, 21, \Float4; 175 | PackRawBytes ROS_joints.rax_6, raw_message.data, 25, \Float4; 176 | PackRawBytes ROS_ext_axes.eax_a, raw_message.data, 29, \Float4; 177 | PackRawBytes ROS_ext_axes.eax_b, raw_message.data, 33, \Float4; 178 | PackRawBytes ROS_ext_axes.eax_c, raw_message.data, 37, \Float4; 179 | PackRawBytes ROS_ext_axes.eax_d, raw_message.data, 41, \Float4; 180 | 181 | ROS_send_msg client_socket, raw_message; 182 | 183 | ERROR 184 | RAISE; ! raise errors to calling code 185 | ENDPROC 186 | 187 | PROC ROS_send_msg_robot_status(VAR socketdev client_socket, ROS_msg_robot_status message) 188 | VAR ROS_msg raw_message; 189 | 190 | ! Force message header to the correct values 191 | raw_message.header.msg_type := message.header.msg_type; 192 | raw_message.header.comm_type := message.header.comm_type; 193 | raw_message.header.reply_code := message.header.reply_code; 194 | 195 | ! Pack data into message 196 | PackRawBytes message.drives_powered, raw_message.data, 1, \IntX:=DINT; 197 | PackRawBytes message.e_stopped, raw_message.data, 5, \IntX:=DINT; 198 | PackRawBytes message.error_code, raw_message.data, 9, \IntX:=DINT; 199 | PackRawBytes message.in_error, raw_message.data, 13, \IntX:=DINT; 200 | PackRawBytes message.in_motion, raw_message.data, 17, \IntX:=DINT; 201 | PackRawBytes message.mode, raw_message.data, 21, \IntX:=DINT; 202 | PackRawBytes message.motion_possible, raw_message.data, 25, \IntX:=DINT; 203 | 204 | ROS_send_msg client_socket, raw_message; 205 | 206 | ERROR 207 | RAISE; ! raise errors to calling code 208 | ENDPROC 209 | 210 | LOCAL FUNC num deg2rad(num deg) 211 | RETURN deg * pi / 180; 212 | ENDFUNC 213 | 214 | LOCAL FUNC num connected_eax(num eax) 215 | ! The value 9E9 is defined for axes which are not connected 216 | IF eax <> 9E9 THEN 217 | RETURN eax; 218 | ENDIF 219 | RETURN 0; 220 | ENDFUNC 221 | 222 | ! Returns only connected external axes in METERS 223 | LOCAL FUNC extjoint mm2m_extjoint(extjoint all_eax) 224 | VAR extjoint eax; 225 | eax.eax_a := connected_eax(all_eax.eax_a) / 1000; 226 | eax.eax_b := connected_eax(all_eax.eax_b) / 1000; 227 | eax.eax_c := connected_eax(all_eax.eax_c) / 1000; 228 | eax.eax_d := connected_eax(all_eax.eax_d) / 1000; 229 | RETURN eax; 230 | ENDFUNC 231 | 232 | ! Returns external axes in MILLIMETERS 233 | LOCAL FUNC extjoint m2mm_extjoint(extjoint eax_in_m) 234 | VAR extjoint eax_in_mm; 235 | eax_in_mm.eax_a := eax_in_m.eax_a * 1000; 236 | eax_in_mm.eax_b := eax_in_m.eax_b * 1000; 237 | eax_in_mm.eax_c := eax_in_m.eax_c * 1000; 238 | eax_in_mm.eax_d := eax_in_m.eax_d * 1000; 239 | 240 | RETURN eax_in_mm; 241 | ENDFUNC 242 | 243 | LOCAL FUNC robjoint deg2rad_robjoint(robjoint deg) 244 | VAR robjoint rad; 245 | rad.rax_1 := deg2rad(deg.rax_1); 246 | rad.rax_2 := deg2rad(deg.rax_2); 247 | rad.rax_3 := deg2rad(deg.rax_3); 248 | rad.rax_4 := deg2rad(deg.rax_4); 249 | rad.rax_5 := deg2rad(deg.rax_5); 250 | rad.rax_6 := deg2rad(deg.rax_6); 251 | 252 | RETURN rad; 253 | ENDFUNC 254 | 255 | LOCAL FUNC num rad2deg(num rad) 256 | RETURN rad * 180 / pi; 257 | ENDFUNC 258 | 259 | LOCAL FUNC robjoint rad2deg_robjoint(robjoint rad) 260 | VAR robjoint deg; 261 | deg.rax_1 := rad2deg(rad.rax_1); 262 | deg.rax_2 := rad2deg(rad.rax_2); 263 | deg.rax_3 := rad2deg(rad.rax_3); 264 | deg.rax_4 := rad2deg(rad.rax_4); 265 | deg.rax_5 := rad2deg(rad.rax_5); 266 | deg.rax_6 := rad2deg(rad.rax_6); 267 | 268 | RETURN deg; 269 | ENDFUNC 270 | 271 | ENDMODULE 272 | -------------------------------------------------------------------------------- /rapid/ROS_motion.mod: -------------------------------------------------------------------------------- 1 | MODULE ROS_motion 2 | 3 | ! Software License Agreement (BSD License) 4 | ! 5 | ! Copyright (c) 2012, Edward Venator, Case Western Reserve University 6 | ! Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute 7 | ! All rights reserved. 8 | ! 9 | ! Redistribution and use in source and binary forms, with or without modification, 10 | ! are permitted provided that the following conditions are met: 11 | ! 12 | ! Redistributions of source code must retain the above copyright notice, this 13 | ! list of conditions and the following disclaimer. 14 | ! Redistributions in binary form must reproduce the above copyright notice, this 15 | ! list of conditions and the following disclaimer in the documentation 16 | ! and/or other materials provided with the distribution. 17 | ! Neither the name of the Case Western Reserve University nor the names of its contributors 18 | ! may be used to endorse or promote products derived from this software without 19 | ! specific prior written permission. 20 | ! 21 | ! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 22 | ! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 23 | ! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 24 | ! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | ! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 26 | ! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 27 | ! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | ! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 29 | ! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | LOCAL CONST zonedata DEFAULT_CORNER_DIST := z10; 32 | LOCAL VAR ROS_joint_trajectory_pt trajectory{MAX_TRAJ_LENGTH}; 33 | LOCAL VAR num trajectory_size := 0; 34 | LOCAL VAR intnum intr_new_trajectory; 35 | 36 | PROC main() 37 | VAR num current_index; 38 | VAR jointtarget target; 39 | VAR speeddata move_speed := v10; ! default speed 40 | VAR zonedata stop_mode; 41 | VAR bool skip_move; 42 | 43 | ! Set up interrupt to watch for new trajectory 44 | IDelete intr_new_trajectory; ! clear interrupt handler, in case restarted with ExitCycle 45 | CONNECT intr_new_trajectory WITH new_trajectory_handler; 46 | IPers ROS_new_trajectory, intr_new_trajectory; 47 | 48 | WHILE true DO 49 | ! Check for new Trajectory 50 | IF (ROS_new_trajectory) 51 | init_trajectory; 52 | 53 | ! execute all points in this trajectory 54 | IF (trajectory_size > 0) THEN 55 | FOR current_index FROM 1 TO trajectory_size DO 56 | target.robax := trajectory{current_index}.joint_pos; 57 | target.extax := trajectory{current_index}.extax_pos; 58 | 59 | skip_move := (current_index = 1) AND is_near(target, 0.1, 0.1); 60 | 61 | stop_mode := DEFAULT_CORNER_DIST; ! assume we're smoothing between points 62 | IF (current_index = trajectory_size) stop_mode := fine; ! stop at path end 63 | 64 | ! Execute move command 65 | IF (NOT skip_move) 66 | MoveAbsJ target, move_speed, \T:=trajectory{current_index}.duration, stop_mode, tool0; 67 | ENDFOR 68 | 69 | trajectory_size := 0; ! trajectory done 70 | ENDIF 71 | 72 | WaitTime 0.05; ! Throttle loop while waiting for new command 73 | ENDWHILE 74 | ERROR 75 | ErrWrite \W, "Motion Error", "Error executing motion. Aborting trajectory."; 76 | abort_trajectory; 77 | ENDPROC 78 | 79 | LOCAL PROC init_trajectory() 80 | clear_path; ! cancel any active motions 81 | 82 | WaitTestAndSet ROS_trajectory_lock; ! acquire data-lock 83 | trajectory := ROS_trajectory; ! copy to local var 84 | trajectory_size := ROS_trajectory_size; ! copy to local var 85 | ROS_new_trajectory := FALSE; 86 | ROS_trajectory_lock := FALSE; ! release data-lock 87 | ENDPROC 88 | 89 | LOCAL FUNC bool is_near(jointtarget target, num deg_tol, num mm_tol) 90 | VAR jointtarget curr_jnt; 91 | 92 | curr_jnt := CJointT(); 93 | 94 | ! either an external axis is unconfigured/not present OR if it is, then it must be close enough 95 | RETURN ( ABS(curr_jnt.robax.rax_1 - target.robax.rax_1) < deg_tol ) 96 | AND ( ABS(curr_jnt.robax.rax_2 - target.robax.rax_2) < deg_tol ) 97 | AND ( ABS(curr_jnt.robax.rax_3 - target.robax.rax_3) < deg_tol ) 98 | AND ( ABS(curr_jnt.robax.rax_4 - target.robax.rax_4) < deg_tol ) 99 | AND ( ABS(curr_jnt.robax.rax_5 - target.robax.rax_5) < deg_tol ) 100 | AND ( ABS(curr_jnt.robax.rax_6 - target.robax.rax_6) < deg_tol ) 101 | AND ( (curr_jnt.extax.eax_a = 9E9) OR (ABS(curr_jnt.extax.eax_a - target.extax.eax_a) < mm_tol) ) 102 | AND ( (curr_jnt.extax.eax_b = 9E9) OR (ABS(curr_jnt.extax.eax_b - target.extax.eax_b) < mm_tol) ) 103 | AND ( (curr_jnt.extax.eax_c = 9E9) OR (ABS(curr_jnt.extax.eax_c - target.extax.eax_c) < mm_tol) ) 104 | AND ( (curr_jnt.extax.eax_d = 9E9) OR (ABS(curr_jnt.extax.eax_d - target.extax.eax_d) < mm_tol) ); 105 | ENDFUNC 106 | 107 | LOCAL PROC abort_trajectory() 108 | trajectory_size := 0; ! "clear" local trajectory 109 | clear_path; 110 | ExitCycle; ! restart program 111 | ENDPROC 112 | 113 | LOCAL PROC clear_path() 114 | IF ( NOT (IsStopMoveAct(\FromMoveTask) OR IsStopMoveAct(\FromNonMoveTask)) ) 115 | StopMove; ! stop any active motions 116 | ClearPath; ! clear queued motion commands 117 | StartMove; ! re-enable motions 118 | ENDPROC 119 | 120 | LOCAL TRAP new_trajectory_handler 121 | IF (NOT ROS_new_trajectory) RETURN; 122 | 123 | abort_trajectory; 124 | ENDTRAP 125 | 126 | ENDMODULE 127 | -------------------------------------------------------------------------------- /rapid/ROS_motionServer.mod: -------------------------------------------------------------------------------- 1 | MODULE ROS_motionServer 2 | 3 | ! Software License Agreement (BSD License) 4 | ! 5 | ! Copyright (c) 2012, Edward Venator, Case Western Reserve University 6 | ! Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute 7 | ! All rights reserved. 8 | ! 9 | ! Redistribution and use in source and binary forms, with or without modification, 10 | ! are permitted provided that the following conditions are met: 11 | ! 12 | ! Redistributions of source code must retain the above copyright notice, this 13 | ! list of conditions and the following disclaimer. 14 | ! Redistributions in binary form must reproduce the above copyright notice, this 15 | ! list of conditions and the following disclaimer in the documentation 16 | ! and/or other materials provided with the distribution. 17 | ! Neither the name of the Case Western Reserve University nor the names of its contributors 18 | ! may be used to endorse or promote products derived from this software without 19 | ! specific prior written permission. 20 | ! 21 | ! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 22 | ! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 23 | ! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 24 | ! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | ! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 26 | ! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 27 | ! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | ! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 29 | ! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | LOCAL CONST num server_port := 11000; 32 | 33 | LOCAL VAR socketdev server_socket; 34 | LOCAL VAR socketdev client_socket; 35 | LOCAL VAR ROS_joint_trajectory_pt trajectory{MAX_TRAJ_LENGTH}; 36 | LOCAL VAR num trajectory_size; 37 | 38 | PROC main() 39 | VAR ROS_msg_joint_traj_pt message; 40 | 41 | TPWrite "MotionServer: Waiting for connection."; 42 | ROS_init_socket server_socket, server_port; 43 | ROS_wait_for_client server_socket, client_socket; 44 | 45 | WHILE ( true ) DO 46 | ! Recieve Joint Trajectory Pt Message 47 | ROS_receive_msg_joint_traj_pt client_socket, message; 48 | trajectory_pt_callback message; 49 | ENDWHILE 50 | 51 | ERROR (ERR_SOCK_TIMEOUT, ERR_SOCK_CLOSED) 52 | IF (ERRNO=ERR_SOCK_TIMEOUT) OR (ERRNO=ERR_SOCK_CLOSED) THEN 53 | SkipWarn; ! TBD: include this error data in the message logged below? 54 | ErrWrite \W, "ROS MotionServer disconnect", "Connection lost. Resetting socket."; 55 | ExitCycle; ! restart program 56 | ELSE 57 | TRYNEXT; 58 | ENDIF 59 | UNDO 60 | IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket; 61 | IF (SocketGetStatus(server_socket) <> SOCKET_CLOSED) SocketClose server_socket; 62 | ENDPROC 63 | 64 | LOCAL PROC trajectory_pt_callback(ROS_msg_joint_traj_pt message) 65 | VAR ROS_joint_trajectory_pt point; 66 | VAR jointtarget current_pos; 67 | VAR ROS_msg reply_msg; 68 | 69 | point := [message.joints, message.ext_axes, message.duration]; 70 | 71 | ! use sequence_id to signal start/end of trajectory download 72 | TEST message.sequence_id 73 | CASE ROS_TRAJECTORY_START_DOWNLOAD: 74 | TPWrite "Traj START received"; 75 | trajectory_size := 0; ! Reset trajectory size 76 | add_traj_pt point; ! Add this point to the trajectory 77 | CASE ROS_TRAJECTORY_END: 78 | TPWrite "Traj END received"; 79 | add_traj_pt point; ! Add this point to the trajectory 80 | activate_trajectory; 81 | CASE ROS_TRAJECTORY_STOP: 82 | TPWrite "Traj STOP received"; 83 | trajectory_size := 0; ! empty trajectory 84 | activate_trajectory; 85 | StopMove; ClearPath; StartMove; ! redundant, but re-issue stop command just to be safe 86 | DEFAULT: 87 | add_traj_pt point; ! Add this point to the trajectory 88 | ENDTEST 89 | 90 | ! send reply, if requested 91 | IF (message.header.comm_type = ROS_COM_TYPE_SRV_REQ) THEN 92 | reply_msg.header := [ROS_MSG_TYPE_JOINT_TRAJ_PT, ROS_COM_TYPE_SRV_REPLY, ROS_REPLY_TYPE_SUCCESS]; 93 | ROS_send_msg client_socket, reply_msg; 94 | ENDIF 95 | 96 | ERROR 97 | RAISE; ! raise errors to calling code 98 | ENDPROC 99 | 100 | LOCAL PROC add_traj_pt(ROS_joint_trajectory_pt point) 101 | IF (trajectory_size = MAX_TRAJ_LENGTH) THEN 102 | ErrWrite \W, "Too Many Trajectory Points", "Trajectory has already reached its maximum size", 103 | \RL2:="max_size = " + ValToStr(MAX_TRAJ_LENGTH); 104 | ELSE 105 | Incr trajectory_size; 106 | trajectory{trajectory_size} := point; !Add this point to the trajectory 107 | ENDIF 108 | ENDPROC 109 | 110 | LOCAL PROC activate_trajectory() 111 | WaitTestAndSet ROS_trajectory_lock; ! acquire data-lock 112 | TPWrite "Sending " + ValToStr(trajectory_size) + " points to MOTION task"; 113 | ROS_trajectory := trajectory; 114 | ROS_trajectory_size := trajectory_size; 115 | ROS_new_trajectory := TRUE; 116 | ROS_trajectory_lock := FALSE; ! release data-lock 117 | ENDPROC 118 | 119 | ENDMODULE 120 | -------------------------------------------------------------------------------- /rapid/ROS_socket.sys: -------------------------------------------------------------------------------- 1 | MODULE ROS_socket(SYSMODULE) 2 | ! Software License Agreement (BSD License) 3 | ! 4 | ! Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute 5 | ! Copyright (c) 2012, Edward Venator, Case Western Reserve University 6 | ! All rights reserved. 7 | ! 8 | ! Redistribution and use in source and binary forms, with or without modification, 9 | ! are permitted provided that the following conditions are met: 10 | ! 11 | ! Redistributions of source code must retain the above copyright notice, this 12 | ! list of conditions and the following disclaimer. 13 | ! Redistributions in binary form must reproduce the above copyright notice, this 14 | ! list of conditions and the following disclaimer in the documentation 15 | ! and/or other materials provided with the distribution. 16 | ! Neither the name of the Case Western Reserve University nor the names of its contributors 17 | ! may be used to endorse or promote products derived from this software without 18 | ! specific prior written permission. 19 | ! 20 | ! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 21 | ! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 22 | ! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 23 | ! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | ! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 25 | ! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 26 | ! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | ! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 28 | ! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | PROC ROS_init_socket(VAR socketdev server_socket, num port) 31 | IF (SocketGetStatus(server_socket) = SOCKET_CLOSED) SocketCreate server_socket; 32 | IF (SocketGetStatus(server_socket) = SOCKET_CREATED) SocketBind server_socket, GetSysInfo(\LanIp), port; 33 | IF (SocketGetStatus(server_socket) = SOCKET_BOUND) SocketListen server_socket; 34 | 35 | ERROR 36 | RAISE; ! raise errors to calling code 37 | ENDPROC 38 | 39 | PROC ROS_wait_for_client(VAR socketdev server_socket, VAR socketdev client_socket, \num wait_time) 40 | VAR string client_ip; 41 | VAR num time_val := WAIT_MAX; ! default to wait-forever 42 | 43 | IF Present(wait_time) time_val := wait_time; 44 | 45 | IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket; 46 | WaitUntil (SocketGetStatus(client_socket) = SOCKET_CLOSED); 47 | 48 | SocketAccept server_socket, client_socket, \ClientAddress:=client_ip, \Time:=time_val; 49 | TPWrite "Client at "+client_ip+" connected."; 50 | 51 | ERROR 52 | RAISE; ! raise errors to calling code 53 | ENDPROC 54 | 55 | PROC ROS_receive_msg(VAR socketdev client_socket, VAR ROS_msg message, \num wait_time) 56 | VAR rawbytes buffer; 57 | VAR num time_val := WAIT_MAX; ! default to wait-forever 58 | VAR num bytes_rcvd; 59 | VAR num msg_length; 60 | 61 | ClearRawBytes buffer; 62 | IF Present(wait_time) time_val := wait_time; 63 | 64 | ! TBD: need to determine whether this handles split/merged messages correctly 65 | 66 | ! Read prefix INT (total message length) 67 | SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=4, \Time:=time_val; 68 | UnpackRawBytes buffer, 1, msg_length, \IntX:=UDINT; 69 | 70 | ! Read remaining message bytes 71 | SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=msg_length, \NoRecBytes:=bytes_rcvd, \Time:=time_val; 72 | IF (bytes_rcvd <> msg_length) THEN 73 | ErrWrite \W, "ROS Socket Recv Failed", "Did not receive expected # of bytes.", 74 | \RL2:="Expected: " + ValToStr(msg_length), 75 | \RL3:="Received: " + ValToStr(bytes_rcvd); 76 | RETURN; 77 | ENDIF 78 | 79 | ! Unpack message header/data 80 | UnpackRawBytes buffer, 1, message.header.msg_type, \IntX:=DINT; 81 | UnpackRawBytes buffer, 5, message.header.comm_type, \IntX:=DINT; 82 | UnpackRawBytes buffer, 9, message.header.reply_code, \IntX:=DINT; 83 | CopyRawBytes buffer, 13, message.data, 1; 84 | 85 | ERROR 86 | RAISE; ! raise errors to calling code 87 | ENDPROC 88 | 89 | PROC ROS_send_msg(VAR socketdev client_socket, VAR ROS_msg message) 90 | VAR rawbytes buffer; 91 | 92 | PackRawBytes 12 + RawBytesLen(message.data), buffer, 1, \IntX := UDINT; ! Packet length (excluding this prefix) 93 | PackRawBytes message.header.msg_type, buffer, 5, \IntX := DINT; ! Message type 94 | PackRawBytes message.header.comm_type, buffer, 9, \IntX := DINT; ! Comm type 95 | PackRawBytes message.header.reply_code, buffer, 13, \IntX := DINT; ! Reply code 96 | CopyRawBytes message.data, 1, buffer, 17; ! Message data 97 | 98 | SocketSend client_socket \RawData:=buffer; 99 | 100 | ERROR 101 | RAISE; ! raise errors to calling code 102 | ENDPROC 103 | 104 | ENDMODULE 105 | -------------------------------------------------------------------------------- /rapid/ROS_stateServer.mod: -------------------------------------------------------------------------------- 1 | MODULE ROS_stateServer 2 | 3 | ! Software License Agreement (BSD License) 4 | ! 5 | ! Copyright (c) 2012, Edward Venator, Case Western Reserve University 6 | ! Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute 7 | ! All rights reserved. 8 | ! 9 | ! Redistribution and use in source and binary forms, with or without modification, 10 | ! are permitted provided that the following conditions are met: 11 | ! 12 | ! Redistributions of source code must retain the above copyright notice, this 13 | ! list of conditions and the following disclaimer. 14 | ! Redistributions in binary form must reproduce the above copyright notice, this 15 | ! list of conditions and the following disclaimer in the documentation 16 | ! and/or other materials provided with the distribution. 17 | ! Neither the name of the Case Western Reserve University nor the names of its contributors 18 | ! may be used to endorse or promote products derived from this software without 19 | ! specific prior written permission. 20 | ! 21 | ! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 22 | ! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 23 | ! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 24 | ! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | ! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 26 | ! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 27 | ! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | ! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 29 | ! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | LOCAL CONST num server_port := 11002; 32 | LOCAL CONST num update_rate := 0.10; ! broadcast rate (sec) 33 | 34 | LOCAL VAR socketdev server_socket; 35 | LOCAL VAR socketdev client_socket; 36 | 37 | PROC main() 38 | 39 | TPWrite "StateServer: Waiting for connection."; 40 | ROS_init_socket server_socket, server_port; 41 | ROS_wait_for_client server_socket, client_socket; 42 | 43 | WHILE (TRUE) DO 44 | send_joints; 45 | send_status; 46 | WaitTime update_rate; 47 | ENDWHILE 48 | 49 | ERROR (ERR_SOCK_TIMEOUT, ERR_SOCK_CLOSED) 50 | IF (ERRNO=ERR_SOCK_TIMEOUT) OR (ERRNO=ERR_SOCK_CLOSED) THEN 51 | SkipWarn; ! TBD: include this error data in the message logged below? 52 | ErrWrite \W, "ROS StateServer disconnect", "Connection lost. Waiting for new connection."; 53 | ExitCycle; ! restart program 54 | ELSE 55 | TRYNEXT; 56 | ENDIF 57 | UNDO 58 | ENDPROC 59 | 60 | LOCAL PROC send_joints() 61 | VAR ROS_msg_joint_data message; 62 | VAR jointtarget joints; 63 | 64 | ! get current joint position (degrees) 65 | joints := CJointT(); 66 | 67 | ! create message 68 | message.header := [ROS_MSG_TYPE_JOINT, ROS_COM_TYPE_TOPIC, ROS_REPLY_TYPE_INVALID]; 69 | message.sequence_id := 0; 70 | message.joints := joints.robax; 71 | message.ext_axes := joints.extax; 72 | 73 | ! send message to client 74 | ROS_send_msg_joint_data client_socket, message; 75 | 76 | ERROR 77 | RAISE; ! raise errors to calling code 78 | ENDPROC 79 | 80 | ! signalExecutionError : System Output 81 | ! signalMotionPossible : System Output 82 | ! signalMotorOn : System Output 83 | ! signalRobotActive : System Output 84 | ! signalRobotEStop : System Output 85 | ! signalRobotNotMoving : System Output 86 | ! signalRosMotionTaskExecuting : System Output 87 | LOCAL PROC send_status() 88 | VAR ROS_msg_robot_status message; 89 | 90 | ! get current joint position (degrees) 91 | ! joints := CJointT(); 92 | 93 | ! create message 94 | message.header := [ROS_MSG_TYPE_STATUS, ROS_COM_TYPE_TOPIC, ROS_REPLY_TYPE_INVALID]; 95 | message.sequence_id := 0; 96 | 97 | ! default values 98 | message.mode := ROS_ROBOT_MODE_UNKNOWN; 99 | message.e_stopped := ROS_TRISTATE_UNKNOWN; 100 | message.drives_powered := ROS_TRISTATE_UNKNOWN; 101 | message.error_code := ROS_TRISTATE_UNKNOWN; 102 | message.in_error := ROS_TRISTATE_UNKNOWN; 103 | message.in_motion := ROS_TRISTATE_UNKNOWN; 104 | message.motion_possible := ROS_TRISTATE_UNKNOWN; 105 | 106 | ! Get operating mode 107 | TEST OpMode() 108 | CASE OP_AUTO: 109 | message.mode := ROS_ROBOT_MODE_AUTO; 110 | CASE OP_MAN_PROG, OP_MAN_TEST: 111 | message.mode := ROS_ROBOT_MODE_MANUAL; 112 | CASE OP_UNDEF: 113 | message.mode := ROS_ROBOT_MODE_UNKNOWN; 114 | ENDTEST 115 | 116 | ! Get E-stop status 117 | IF DOutput(signalRobotEStop) = 1 THEN 118 | message.e_stopped := ROS_TRISTATE_ON; 119 | ELSE 120 | message.e_stopped := ROS_TRISTATE_OFF; 121 | ENDIF 122 | 123 | ! Get whether motors have power 124 | IF DOutput(signalMotorOn) = 1 THEN 125 | message.drives_powered := ROS_TRISTATE_TRUE; 126 | ELSE 127 | message.drives_powered := ROS_TRISTATE_FALSE; 128 | ENDIF 129 | 130 | ! Determine in_error and set error_code if in_error is true 131 | if DOutput(signalExecutionError) = 1 THEN 132 | message.in_error := ROS_TRISTATE_TRUE; 133 | message.error_code := ERRNO; 134 | ELSE 135 | message.in_error := ROS_TRISTATE_FALSE; 136 | message.error_code := 0; 137 | ENDIF 138 | 139 | ! Get in_motion 140 | IF DOutput(signalRobotNotMoving) = 1 THEN 141 | message.in_motion := ROS_TRISTATE_FALSE; 142 | ELSE 143 | message.in_motion := ROS_TRISTATE_TRUE; 144 | ENDIF 145 | 146 | ! Get whether motion is possible 147 | if (DOutput(signalMotionPossible) = 1) AND 148 | (DOutput(signalRobotActive) = 1) AND 149 | (DOutput(signalMotorOn) = 1) AND 150 | (DOutput(signalRosMotionTaskExecuting) = 1) THEN 151 | message.motion_possible := ROS_TRISTATE_TRUE; 152 | ELSE 153 | message.motion_possible := ROS_TRISTATE_FALSE; 154 | ENDIF 155 | 156 | ! send message to client 157 | ROS_send_msg_robot_status client_socket, message; 158 | 159 | ERROR 160 | RAISE; ! raise errors to calling code 161 | ENDPROC 162 | 163 | ENDMODULE 164 | -------------------------------------------------------------------------------- /src/abb_joint_downloader_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2012, Southwest Research Institute 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Southwest Research Institute, nor the names 16 | * of its contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include "abb_driver/abb_utils.h" 33 | #include "industrial_robot_client/joint_trajectory_downloader.h" 34 | #include "industrial_utils/param_utils.h" 35 | 36 | using industrial_robot_client::joint_trajectory_downloader::JointTrajectoryDownloader; 37 | namespace StandardSocketPorts = industrial::simple_socket::StandardSocketPorts; 38 | 39 | class ABB_JointTrajectoryDownloader : public JointTrajectoryDownloader 40 | { 41 | using JointTrajectoryDownloader::init; // so base-class init() stays visible 42 | 43 | bool J23_coupled_; 44 | 45 | public: 46 | bool init(std::string default_ip = "", int default_port = StandardSocketPorts::MOTION) 47 | { 48 | if (!JointTrajectoryDownloader::init(default_ip, default_port)) // call base-class init() 49 | return false; 50 | 51 | if (ros::param::has("J23_coupled")) 52 | ros::param::get("J23_coupled", this->J23_coupled_); 53 | else 54 | J23_coupled_ = false; 55 | 56 | return true; 57 | } 58 | 59 | bool transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out) 60 | { 61 | // correct for parallel linkage effects, if desired 62 | // - use POSITIVE factor for joint->motor correction 63 | abb::utils::linkage_transform(pt_in, pt_out, J23_coupled_ ? +1:0 ); 64 | 65 | return true; 66 | } 67 | 68 | bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity) 69 | { 70 | *rbt_velocity = 0; // unused by ABB driver 71 | return true; 72 | } 73 | }; 74 | 75 | int main(int argc, char** argv) 76 | { 77 | // initialize node 78 | ros::init(argc, argv, "motion_interface"); 79 | 80 | // launch the default JointTrajectoryDownloader connection/handlers 81 | ABB_JointTrajectoryDownloader motionInterface; 82 | motionInterface.init(); 83 | motionInterface.run(); 84 | 85 | return 0; 86 | } 87 | -------------------------------------------------------------------------------- /src/abb_robot_state_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2012, Southwest Research Institute 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Southwest Research Institute, nor the names 16 | * of its contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include "abb_driver/abb_utils.h" 33 | #include "industrial_robot_client/robot_state_interface.h" 34 | #include "industrial_utils/param_utils.h" 35 | 36 | using industrial_robot_client::robot_state_interface::RobotStateInterface; 37 | using industrial_robot_client::joint_relay_handler::JointRelayHandler; 38 | 39 | class ABB_JointRelayHandler : public JointRelayHandler 40 | { 41 | bool J23_coupled_; 42 | 43 | public: 44 | ABB_JointRelayHandler() : JointRelayHandler() 45 | { 46 | if (ros::param::has("J23_coupled")) 47 | ros::param::get("J23_coupled", this->J23_coupled_); 48 | else 49 | J23_coupled_ = false; 50 | } 51 | 52 | bool transform(const std::vector& pos_in, std::vector* pos_out) 53 | { 54 | // correct for parallel linkage effects, if desired 55 | // - use NEGATIVE factor for motor->joint correction 56 | abb::utils::linkage_transform(pos_in, pos_out, J23_coupled_ ? -1:0 ); 57 | 58 | return true; 59 | } 60 | }; 61 | 62 | int main(int argc, char** argv) 63 | { 64 | // initialize node 65 | ros::init(argc, argv, "state_interface"); 66 | 67 | // launch the default Robot State Interface connection/handlers 68 | RobotStateInterface rsi; 69 | rsi.init(); 70 | 71 | // replace the JointRelayHandler with ABB-version 72 | ABB_JointRelayHandler jointHandler; // for joint-linkage correction 73 | std::vector joint_names = rsi.get_joint_names(); 74 | jointHandler.init(rsi.get_connection(), joint_names); 75 | rsi.add_handler(&jointHandler); 76 | 77 | // run the node 78 | rsi.run(); 79 | 80 | return 0; 81 | } 82 | -------------------------------------------------------------------------------- /src/abb_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2012, Southwest Research Institute 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Southwest Research Institute, nor the names 16 | * of its contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include "abb_driver/abb_utils.h" 33 | #include "ros/ros.h" 34 | 35 | namespace abb 36 | { 37 | namespace utils 38 | { 39 | 40 | // TBD: This transform should also account for velocity/acceleration affects due to linkage, so that velocity calculation is accurate 41 | void linkage_transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out, double J23_factor) 42 | { 43 | *pt_out = pt_in; 44 | linkage_transform(pt_in.positions, &(pt_out->positions), J23_factor); 45 | } 46 | 47 | void linkage_transform(const std::vector& points_in, std::vector* points_out, double J23_factor) 48 | { 49 | ROS_ASSERT(points_in.size() > 3); 50 | 51 | *points_out = points_in; 52 | points_out->at(2) += J23_factor * points_out->at(1); 53 | } 54 | 55 | } //abb 56 | 57 | } //utils 58 | --------------------------------------------------------------------------------